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

add heading correction to other sensor poll functions

Browse Source
This commit is contained in:
templeofshadow
2026-06-29 21:42:26 -05:00
parent 85fd1d0d85
commit d07b952d66
1 changed files with 56 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
main.lua
View File

@@ -565,6 +565,19 @@ function PollVelocity()
local vsAxis = velsensor.getAxis()
local vsVelocity = velsensor.getVelocity()
-- correct for heading correction value
if Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
if vsAxis == "x" then vsAxis = "z" end
if vsAxis == "z" then vsAxis = "x" end
elseif Config.SensorCorrection.Heading == 180 or Config.SensorCorrection.Heading == -180 then
vsVelocity = -vsVelocity
elseif Config.SensorCorrection.Heading == -90 or Config.SensorCorrection.Heading == 270 then
if vsAxis == "x" then vsAxis = "z" end
if vsAxis == "z" then vsAxis = "x" end
vsVelocity = -vsVelocity
end
VelocityVectors[vsAxis] = vsVelocity
SensorData.Velocity.Raw = VelocityVectors
@@ -671,6 +684,20 @@ function PollGimbal()
end
end
-- correct for heading correction value
if Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
local temp = Angles.xAngle
Angles.xAngle = Angles.zAngle
Angles.zAngle = temp
elseif Config.SensorCorrection.Heading == 180 or Config.SensorCorrection.Heading == -180 then
Angles.xAngle = -Angles.xAngle
Angles.zAngle = -Angles.zAngle
elseif Config.SensorCorrection.Heading == -90 or Config.SensorCorrection.Heading == 270 then
local temp = Angles.xAngle
Angles.xAngle = -Angles.zAngle
Angles.zAngle = -temp
end
AngularRates = {
wx = 0, -- pitch rate
wy = 0, -- yaw rate
@@ -685,6 +712,21 @@ function PollGimbal()
AngularRates.wz = v
end
end
-- correct for heading correction value
if Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
local temp = AngularRates.wx
AngularRates.wx = AngularRates.wz
AngularRates.wz = temp
elseif Config.SensorCorrection.Heading == 180 or Config.SensorCorrection.Heading == -180 then
AngularRates.wx = -AngularRates.wx
AngularRates.wz = -AngularRates.wz
elseif Config.SensorCorrection.Heading == -90 or Config.SensorCorrection.Heading == 270 then
local temp = AngularRates.wx
AngularRates.wx = -AngularRates.wz
AngularRates.wz = -temp
end
LinearAcceleration = {
ax = 0, -- port-starboard axis
ay = 0, -- up-down axis
@@ -700,6 +742,20 @@ function PollGimbal()
end
end
-- correct for heading correction value
if Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
local temp = LinearAcceleration.ax
LinearAcceleration.ax = LinearAcceleration.az
LinearAcceleration.az = temp
elseif Config.SensorCorrection.Heading == 180 or Config.SensorCorrection.Heading == -180 then
LinearAcceleration.ax = -LinearAcceleration.ax
LinearAcceleration.az = -LinearAcceleration.az
elseif Config.SensorCorrection.Heading == -90 or Config.SensorCorrection.Heading == 270 then
local temp = LinearAcceleration.ax
LinearAcceleration.ax = -LinearAcceleration.az
LinearAcceleration.az = -temp
end
SensorData.Gimbal.Angles = Angles
SensorData.Gimbal.AngularRates = AngularRates
SensorData.Gimbal.LinearAcceleration = LinearAcceleration
@@ -858,9 +914,6 @@ function UpdateGlobalThrust()
if Config.Autopilot.AutopilotDesiredHeading ~= nil then yawOutput = PIDs.YawAttitudePID:update(Config.Autopilot.AutopilotDesiredHeading, SensorData.NavTable.Heading, dt) end
local yawRateOutput = PIDs.YawRatePID:update(Config.Autopilot.AutopilotDesiredYawRate or 0, SensorData.Gimbal.AngularRates.wy or 0, dt)
for _, thruster in pairs(Thrusters) do
local desiredThrust = 0