begin reimplementation of UpdateGlobalThrust based on PIDs instead of the old system

docs.md

@@ -194,6 +194,12 @@ Every cycle:
 - Velocity Sensor `getAxis()`, `getVelocity`
 - Poll input signal strengths for global downwards thrust, and fore and aft thrust
 
+### Math
+
+#### Calculate Counteractive Thrust
+
+TODO: fill in
+
 ### Data Structures
 
 #### Config
@@ -241,6 +247,8 @@ Config = {
 }
 ```
 
+#### SensorData
+
 The member variables of the sensor table is as follows:
 
 ```lua
@@ -284,11 +292,21 @@ SensorData = {
 }
 ```
 
+#### Thrusters
+
 The member variables of the Thruster table are as follows (using some example thrusters):
 
 ```lua
 Thrusters = {
     gyroscopic_propeller_bearing_0 = {
+        -- primary attitude thruster?
+        primary_pitch_thruster = true,
+        primary_roll_thruster = true,
+        primary_yaw_thruster = false,
+        -- primary lateral thruster?
+        primary_altitude_thruster = true,
+        primary_fore_thruster = false,
+        primary_aft_thruster = false,
         type = "rotator",
         name = "gyroscopic_propeller_bearing_0",
         thruster = nil,
@@ -308,6 +326,14 @@ Thrusters = {
         power = nil
     },
     gyroscopic_propeller_bearing_1 = {
+        -- primary attitude thruster?
+        primary_pitch_thruster = true,
+        primary_roll_thruster = true,
+        primary_yaw_thruster = false,
+        -- primary lateral thruster?
+        primary_altitude_thruster = true,
+        primary_fore_thruster = false,
+        primary_aft_thruster = false,
         type = "rotator",
         name = "gyroscopic_propeller_bearing_1",
         thruster = nil,
@@ -327,6 +353,14 @@ Thrusters = {
         power = nil
     },
     gyroscopic_propeller_bearing_2 = {
+        -- primary attitude thruster?
+        primary_pitch_thruster = true,
+        primary_roll_thruster = true,
+        primary_yaw_thruster = false,
+        -- primary lateral thruster?
+        primary_altitude_thruster = true,
+        primary_fore_thruster = false,
+        primary_aft_thruster = false,
         type = "rotator",
         name = "gyroscopic_propeller_bearing_2",
         thruster = nil,
@@ -346,6 +380,14 @@ Thrusters = {
         power = nil
     },
     gyroscopic_propeller_bearing_3 = {
+        -- primary attitude thruster?
+        primary_pitch_thruster = true,
+        primary_roll_thruster = true,
+        primary_yaw_thruster = false,
+        -- primary lateral thruster?
+        primary_altitude_thruster = true,
+        primary_fore_thruster = false,
+        primary_aft_thruster = false,
         type = "rotator",
         name = "gyroscopic_propeller_bearing_3",
         thruster = nil,

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
     
-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")
+    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)
+
+
+    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
+        if thruster.affectVectors.angular.roll ~= nil and thruster.primary_roll_thruster then
+            desiredThrust = desiredThrust + rollOutput
+        end
+        if thruster.affectVectors.angular.yaw ~= nil and thruster.primary_yaw_thruster then
+            desiredThrust = desiredThrust + yawOutput
         end
 
-    -- TODO: Refactor this
-
-    local desiredLateralThrustVectors = {}
-
-    for f, v in pairs(SensorData.Velocity.Raw) do
-        desiredLateralThrustVectors[f] = CustomSigmoid(v)
+        if thruster.affectVectors.lateral.y == "up" then
+            desiredThrust = desiredThrust + throttleOutput 
         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
+        -- 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]
 
-    ]]
-
-    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
+        SetThrusterPower(thruster, normalizedThrust)
     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")
-        end
-        return
-    end
-
-    -- TODO: Implement this
 end
 
 function Init()

thrusters.txt.example

@@ -1,53 +1,101 @@
 return {
     gyroscopic_propeller_bearing_0 = {
+        primary_pitch_thruster = true,
+        primary_roll_thruster = true,
+        primary_yaw_thruster = false,
+        primary_altitude_thruster = true,
+        primary_fore_thruster = false,
+        primary_aft_thruster = false,
         type = "rotator",
         name = "gyroscopic_propeller_bearing_0",
         thruster = nil,
         transmission = nil,
         affectVectors = {
+            angular = {
               yaw = nil,
               pitch = "up",
               roll = "star",
-            lateral = nil
+            },
+            lateral = {
+              x = nil,
+              y = "up",
+              z = nil
+            }
         },
         power = nil
     },
     gyroscopic_propeller_bearing_1 = {
+        primary_pitch_thruster = true,
+        primary_roll_thruster = true,
+        primary_yaw_thruster = false,
+        primary_altitude_thruster = true,
+        primary_fore_thruster = false,
+        primary_aft_thruster = false,
         type = "rotator",
         name = "gyroscopic_propeller_bearing_1",
         thruster = nil,
         transmission = nil,
         affectVectors = {
+            angular = {
               yaw = nil,
               pitch = "up",
               roll = "port",
-            lateral = nil
+            },
+            lateral = {
+              x = nil,
+              y = "up",
+              z = nil
+            }
         },
         power = nil
     },
     gyroscopic_propeller_bearing_2 = {
+        primary_pitch_thruster = true,
+        primary_roll_thruster = true,
+        primary_yaw_thruster = false,
+        primary_altitude_thruster = true,
+        primary_fore_thruster = false,
+        primary_aft_thruster = false,
         type = "rotator",
         name = "gyroscopic_propeller_bearing_2",
         thruster = nil,
         transmission = nil,
         affectVectors = {
+            angular = {
               yaw = nil,
               pitch = "down",
               roll = "port",
-            lateral = nil
+            },
+            lateral = {
+              x = nil,
+              y = "up",
+              z = nil
+            }
         },
         power = nil
     },
     gyroscopic_propeller_bearing_3 = {
+        primary_pitch_thruster = true,
+        primary_roll_thruster = true,
+        primary_yaw_thruster = false,
+        primary_altitude_thruster = true,
+        primary_fore_thruster = false,
+        primary_aft_thruster = false,
         type = "rotator",
         name = "gyroscopic_propeller_bearing_3",
         thruster = nil,
         transmission = nil,
         affectVectors = {
+            angular = {
               yaw = nil,
               pitch = "down",
               roll = "star",
-            lateral = nil
+            },
+            lateral = {
+              x = nil,
+              y = "up",
+              z = nil
+            }
         },
         power = nil
     }