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

first implementation of the UpdateGlobalThrust function

Browse Source
This commit is contained in:
templeofshadow
2026-06-27 17:34:10 -05:00
parent ba5888bf1a
commit ae0e728d3e
1 changed files with 116 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

130
main.lua
View File

@@ -33,6 +33,33 @@ Thrusters = {
}
} -- Table of thruster tables
ThrustDirections = {
Angular = {
Negative = {
x = "pitchdown",
y = "yawport",
z = "rollport"
},
Positive = {
x = "pitchup",
y = "yawstar",
z = "rollstar"
}
},
Lateral = {
Negative = {
x = "star",
y = "down",
z = "aft"
},
Positive = {
x = "port",
y = "up",
z = "fore"
}
}
}
SensorData = {}
ThrusterTypes = {
@@ -563,9 +590,46 @@ function PollGimbal()
SensorData.Gimbal = {}
Angles = gimbalSensor.getAngles()
AngularRates = gimbalSensor.getAngularRates()
LinearAcceleration = gimbalSensor.getLinearAcceleration()
Angles = {
xAngle = 0, -- pitch angle
zAngle = 0 -- roll angle
}
for i, v in ipairs(gimbalSensor.getAngles) do
if i == 0 then
Angles.xAngle = v
elseif i == 1 then
Angles.zAngle = v
end
end
AngularRates = {
wx = 0, -- pitch rate
wy = 0, -- yaw rate
wz = 0 -- roll rate
}
for i, v in ipairs(gimbalSensor.getAngularRates) do
if i == 0 then
AngularRates.wx = v
elseif i == 1 then
AngularRates.wy = v
elseif i == 2 then
AngularRates.wz = v
end
end
LinearAcceleration = {
ax = 0, -- port-starboard axis
ay = 0, -- up-down axis
az = 0 -- fore-aft axis
}
for i, v in ipairs(gimbalSensor.getLinearAcceleration) do
if i == 0 then
LinearAcceleration.ax = v
elseif i == 1 then
LinearAcceleration.ay = v
elseif i == 2 then
LinearAcceleration.az = v
end
end
SensorData.Gimbal.Angles = Angles
SensorData.Gimbal.AngularRates = AngularRates
@@ -577,13 +641,18 @@ function PollGimbal()
end
end
-- the sigmoid is probably the best function to use for determining how much thrust to apply based on the current velocity or angular rate
function Sigmoid(x)
local e = 2.718281828459045
return 1 / (1 + e^(-x))
end
-- this sigmoid is modified to return a value between -1 and 1, and inverted so that the output is negative when the input is positive, and vice versa
-- the idea is to take the velocity or angular rate, and return a value that can be used to determine how much thrust to apply in the opposite direction to counteract it
-- then, the output can be given to the update thrust function which will propogate the value to the thrusters
function CustomSigmoid(x)
return ((2 * Sigmoid(x)) - 1) * -1
return ((-2 * Sigmoid(x)) + 1)
end
-- thruster is a thruster type, power is a vector from 0.0 to 1.0
@@ -626,11 +695,24 @@ function SetThrusterPower(thruster, power)
end
end
function GetThrusterPower(thruster)
if thruster.type == "rotator" then
return thruster.power
end
if thruster.type == "thruster" then
return thruster.thruster.getPower()
end
end
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
local desiredLateralThrustVectors = {}
for f, v in pairs(VelocityVectors) do
for f, v in pairs(SensorData.Velocity.Raw) do
desiredLateralThrustVectors[f] = CustomSigmoid(v)
end
@@ -638,11 +720,9 @@ function UpdateGlobalThrust()
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
end
if f == "y" then
elseif f == "y" then
if v > 0 then table.insert(thrustDirections, { down = v }) else table.insert(thrustDirections, { up = math.abs(v) }) end
end
if f == "z" then
elseif f == "z" then
if v > 0 then table.insert(thrustDirections, { fore = v }) else table.insert(thrustDirections, { aft = math.abs(v) }) end
end
end
@@ -650,19 +730,41 @@ function UpdateGlobalThrust()
for _, d in pairs(thrustDirections) do
for _, t in pairs(Thrusters) do
if t.affectVectors.lateral == d then
SetThrusterPower(t, d)
local correctedPower = GetThrusterPower(t) + d
if correctedPower > 1 then correctedPower = 1 end
if correctedPower < 0 then correctedPower = 0 end
SetThrusterPower(t, correctedPower)
end
end
end
-- angular thrust
local desiredAngluarThrustVectors = {}
local desiredAngularThrustVectors = {}
for f, v in pairs(AngularRates) do
desiredAngluarThrustVectors[f] = CustomSigmoid(v)
for f, v in pairs(SensorData.Gimbal.AngularRates) do
desiredAngularThrustVectors[f] = CustomSigmoid(v)
end
-- Need to complete this function implementation based on docs.md requirements
for f, v in pairs(desiredAngularThrustVectors) do
if f == "wx" then
if v > 0 then table.insert(thrustDirections, { pitchdown = v }) else table.insert(thrustDirections, { pitchup = math.abs(v) }) end
elseif f == "wy" then
if v > 0 then table.insert(thrustDirections, { yawport = v }) else table.insert(thrustDirections, { yawstar = math.abs(v) }) end
elseif f == "wz" then
if v > 0 then table.insert(thrustDirections, { rollport = v }) else table.insert(thrustDirections, { rollstar = math.abs(v) }) end
end
end
for _, d in pairs(thrustDirections) do
for _, t in pairs(Thrusters) do
if t.affectVectors.angular == d then
local correctedPower = GetThrusterPower(t) + d
if correctedPower > 1 then correctedPower = 1 end
if correctedPower < 0 then correctedPower = 0 end
SetThrusterPower(t, correctedPower)
end
end
end
end
function Init()