diff --git a/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c b/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c index c3b852a39..7da7bf24b 100644 --- a/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c +++ b/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c @@ -158,7 +158,11 @@ static void stabilizationTask(void* parameters) ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP)) { if(stabSettings.YawMode == STABILIZATIONSETTINGS_YAWMODE_RATE) { // rate stabilization on yaw - yawChange = (attitudeActual.Yaw - yawPrevious) / dT; + // Avoid a huge (wrong) change if we crossed 0/360 + yawChange = attitudeActual.Yaw - yawPrevious; + if (yawChange>180.0) yawChange -= 360; + if (yawChange<-180.0) yawChange += 360; + yawChange = yawChange / dT; yawPrevious = attitudeActual.Yaw; yawError = bound(attitudeDesired.Yaw, -stabSettings.YawMax, stabSettings.YawMax) - yawChange; } else { // heading stabilization