From d07b952d663022bde94ce5a52d1a2e22332b4e22 Mon Sep 17 00:00:00 2001
From: templeofshadow <temple@templeofshadow.org>
Date: Mon, 29 Jun 2026 21:42:26 -0500
Subject: [PATCH] add heading correction to other sensor poll functions

---
 main.lua | 59 +++++++++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 56 insertions(+), 3 deletions(-)

diff --git a/main.lua b/main.lua
index 803e41c..21df886 100644
--- a/main.lua
+++ b/main.lua
@@ -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