diff --git a/main.lua b/main.lua
index e0ac5c0..abdc486 100644
--- a/main.lua
+++ b/main.lua
@@ -847,10 +847,16 @@ function UpdateGlobalThrust()
     end
     
     local throttleOutput = PIDs.AltitudePID:update(Config.Autopilot.AutopilotDesiredAltitude or 0, SensorData.Altitude.Altitude or 0, dt)
+    local pitchOutput    = PIDs.PitchAttitudePID:update(0, SensorData.Gimbal.Angles.xAngle, dt)
+    local rollOutput     = PIDs.RollAttitudePID:update(0, SensorData.Gimbal.Angles.zAngle, dt)
+
+    local yawOutput          = PIDs.YawAttitudePID:update(Config.Autopilot.AutopilotDesiredHeading or 0, SensorData.NavTable.Heading, 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)
+    local pitchRateOutput    = PIDs.PitchRatePID:update(Config.Autopilot.AutopilotDesiredPitchRate or 0, SensorData.Gimbal.AngularRates.wx or 0, dt)
+    local rollRateOutput     = PIDs.RollRatePID:update(Config.Autopilot.AutopilotDesiredRollRate or 0, SensorData.Gimbal.AngularRates.wz or 0, dt)
+    local yawRateOutput      = PIDs.YawRatePID:update(Config.Autopilot.AutopilotDesiredYawRate or 0, SensorData.Gimbal.AngularRates.wy or 0, dt)
+
+
 
 
     for _, thruster in pairs(Thrusters) do
@@ -858,13 +864,13 @@ function UpdateGlobalThrust()
 
         -- Calculate desired thrust based on affectVectors and PID outputs
         if thruster.affectVectors.angular.pitch ~= nil and thruster.primary_pitch_thruster then
-            desiredThrust = desiredThrust + pitchOutput
+            desiredThrust = desiredThrust + pitchOutput + pitchRateOutput
         end
         if thruster.affectVectors.angular.roll ~= nil and thruster.primary_roll_thruster then
-            desiredThrust = desiredThrust + rollOutput
+            desiredThrust = desiredThrust + rollOutput + rollRateOutput
         end
         if thruster.affectVectors.angular.yaw ~= nil and thruster.primary_yaw_thruster then
-            desiredThrust = desiredThrust + yawOutput
+            desiredThrust = desiredThrust + yawRateOutput
         end
 
         if thruster.affectVectors.lateral.y == "up" then