temple/caero-attitude-control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
templeofshadow
2026-06-29 13:52:38 -05:00
parent de7decceaa
commit d57bcf0ad4
1 changed files with 80 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

143
main.lua
View File

@@ -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()