diff --git a/main.lua b/main.lua
index 8496e0b..28d4c56 100644
--- a/main.lua
+++ b/main.lua
@@ -847,13 +847,15 @@ 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 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 pitchOutput        = PIDs.PitchAttitudePID:update(0, SensorData.Gimbal.Angles.xAngle, dt)
+    local pitchRateOutput    = PIDs.PitchRatePID:update(pitchOutput, SensorData.Gimbal.AngularRates.wx or 0, dt)
+
+    local rollOutput         = PIDs.RollAttitudePID:update(0, SensorData.Gimbal.Angles.zAngle, dt)
+    local rollRateOutput     = PIDs.RollRatePID:update(rollOutput, SensorData.Gimbal.AngularRates.wz or 0, dt)
+
+    local yawOutput = 5
+    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)