From e83d5a6fc7ae57452cb8bad5b88057bbf40c4a09 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Tue, 19 Jun 2018 08:01:44 -0400 Subject: [PATCH] Fix gps rescue heading edge case The yaw heading calculation didn't handle edge cases like 180 and -180 well. Also cleaned up coding style. --- src/main/flight/gps_rescue.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 491c0f1c44..aeb1f4f9ce 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -394,10 +394,11 @@ void setBearing(int16_t deg) { int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - deg; - if (dif <= -180) + if (dif <= -180) { dif += 360; - if (dif >= +180) + } else if (dif > 180) { dif -= 360; + } dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);