From d57bcf0ad4bd0fe8fa52365e0b91d6e3270e4dcd Mon Sep 17 00:00:00 2001 From: templeofshadow Date: Mon, 29 Jun 2026 13:52:38 -0500 Subject: [PATCH] begin reimplementation of UpdateGlobalThrust based on PIDs instead of the old system --- main.lua | 143 +++++++++++++++++++++++++++++++------------------------ 1 file changed, 80 insertions(+), 63 deletions(-) diff --git a/main.lua b/main.lua index 096ea97..788e53c 100644 --- a/main.lua +++ b/main.lua @@ -153,6 +153,7 @@ end Config.Autopilot = { AutopilotEngaged = false, AutoForeAft = false, + AutopilotDesiredAltitude = nil, AutopilotDesiredSpeed = nil, AutopilotDesiredHeading = nil } @@ -702,80 +703,96 @@ function GetThrusterPower(thruster) end end +-- PID systems for stabilization and autopilot +LastTime = os.epoch("utc") + +function GetDeltaTime() + local currentTime = os.epoch("utc") + local dt = (currentTime - LastTime) / 1000 -- convert milliseconds to seconds + LastTime = currentTime + if dt <= 0 then dt = 0.001 end -- prevent division by zero + return dt +end + +function CreatePID(kp, ki, kd) + return { + kp = kp, + ki = ki, + kd = kd, + integral = 0, lastError = 0, + update = function(self, setpoint, pv, dt) + local error = setpoint - pv + self.integral = self.integral + (error * dt) + local derivative = (error - self.lastError) / dt + self.lastError = error + return (self.kp * error) + (self.ki * self.integral) + (self.kd * derivative) + end + } +end + +-- tune these for the best values for each vessel +PIDs = { +-- angular + -- attitude PIDs + RollAttitudePID = CreatePID(0.1, 0.01, 0.05), + PitchAttitudePID = CreatePID(0.1, 0.01, 0.05), + YawAttitudePID = CreatePID(0.1, 0.01, 0.05), + + -- rate PIDs + RollRatePID = CreatePID(0.1, 0.01, 0.05), + PitchRatePID = CreatePID(0.1, 0.01, 0.05), + YawRatePID = CreatePID(0.1, 0.01, 0.05), + +-- lateral + -- value PIDs + AltitudePID = CreatePID(0.1, 0.01, 0.05), + PortStarPID = CreatePID(0.1, 0.01, 0.05), + ForeAftPID = CreatePID(0.1, 0.01, 0.05), + + -- rate PIDs + VerticalRatePID = CreatePID(0.1, 0.01, 0.05), + PortStarRatePID = CreatePID(0.1, 0.01, 0.05), + ForeAftRatePID = CreatePID(0.1, 0.01, 0.05) +} + function UpdateGlobalThrust() -- for each thruster, there is a table of affectVectors that determine in what directions the thruster can apply thrust -- each thruster has a power value from 0 to 1, which is the amount of thrust the thruster is currently applying -- depending on the desired thrust vectors, some counteractive desired thrust vectors will be ignored - -- lateral thrust - UpdateGlobalLateralThrust() + local dt = GetDeltaTime() - -- angular thrust - UpdateGlobalAngularThrust() -end + + local throttleOutput = PIDs.AltitudePID:update(Config.Autopilot.AutopilotDesiredAltitude or 0, SensorData.Altitude.Altitude or 0, dt) + + local pitchOutput = PIDs.PitchRatePID:update(Config.Autopilot.AutopilotDesiredPitchRate or 0, SensorData.Gimbal.AngularRates.wx or 0, dt) + local rollOutput = PIDs.RollRatePID:update(Config.Autopilot.AutopilotDesiredRollRate or 0, SensorData.Gimbal.AngularRates.wz or 0, dt) + local yawOutput = PIDs.YawRatePID:update(Config.Autopilot.AutopilotDesiredYawRate or 0, SensorData.Gimbal.AngularRates.wy or 0, dt) -function UpdateGlobalLateralThrust() - -- lateral thrust - -- verify that SensorData.Velocity.Raw is not nil - if SensorData.Velocity == nil or SensorData.Velocity.Raw == nil then - if Config.Debug then - print("DEBUG: SensorData.Velocity.Raw or SensorData.Velocity is nil, skipping UpdateGlobalLateralThrust") + + for _, thruster in pairs(Thrusters) do + local desiredThrust = 0 + + -- Calculate desired thrust based on affectVectors and PID outputs + if thruster.affectVectors.angular.pitch ~= nil and thruster.primary_pitch_thruster then + desiredThrust = desiredThrust + pitchOutput end - return - end - - -- TODO: Refactor this - - local desiredLateralThrustVectors = {} - - for f, v in pairs(SensorData.Velocity.Raw) do - desiredLateralThrustVectors[f] = CustomSigmoid(v) - end - - --[[ - desiredLateralThrustVectors is a table that is similar to SensorData.Velocity.Raw, but with values between -1 and 1. - essentially, it reverses the velocity vector and then clamps the range between -1 and 1, so that the value can be used for thrust power - - ]] - - local thrustDirections = {} - for f, v in pairs(desiredLateralThrustVectors) do - if f == "x" then - if v > 0 then table.insert(thrustDirections, { port = v }) else table.insert(thrustDirections, { star = math.abs(v) }) end - elseif f == "y" then - if v > 0 then table.insert(thrustDirections, { down = v }) else table.insert(thrustDirections, { up = math.abs(v) }) end - elseif f == "z" then - if v > 0 then table.insert(thrustDirections, { fore = v }) else table.insert(thrustDirections, { aft = math.abs(v) }) end + if thruster.affectVectors.angular.roll ~= nil and thruster.primary_roll_thruster then + desiredThrust = desiredThrust + rollOutput end - end - - --[[ - thrustDirections is an array of tables, each table has a key that is the direction of the desired thrust, and a value that is the magnitude of the desired thrust - like so: - thrustDirections = { - { port = 0.5 }, - { down = 0.1 }, - { aft = 0.6 } - } - there will only ever be three tables in thrustDirections, as there are only three axes of movement, and each axis can only have one desired thrust direction at a time - (angular thrust is the same, but with different keys for the tables, like pitchup/pitchdown, yawport/yawstar, rollport/rollstar) - - ]] - - -end - -function UpdateGlobalAngularThrust() - -- angular thrust - -- verify that SensorData.Gimbal.AngularRates is not nil - if SensorData.Gimbal == nil or SensorData.Gimbal.AngularRates == nil then - if Config.Debug then - print("DEBUG: SensorData.Gimbal.AngularRates or SensorData.Gimbal is nil, skipping UpdateGlobalAngularThrust") + if thruster.affectVectors.angular.yaw ~= nil and thruster.primary_yaw_thruster then + desiredThrust = desiredThrust + yawOutput end - return - end - -- TODO: Implement this + if thruster.affectVectors.lateral.y == "up" then + desiredThrust = desiredThrust + throttleOutput + end + + -- Normalize the desired thrust to be between 0 and 1 + local normalizedThrust = math.max(0, math.min(1, (desiredThrust + 1) / 2)) -- Convert from [-1, 1] to [0, 1] + + SetThrusterPower(thruster, normalizedThrust) + end end function Init()