From a7931b1581cd935d2b2dfbd80258359500af15e6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Nov 2013 13:57:51 +1100 Subject: [PATCH] AP_AHRS: fixed wrapping bug in GPS based heading error this bug could cause the GPS based yaw to use an expensive reset far too often --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 4e8f6ec0af..6e619b0363 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -367,7 +367,7 @@ AP_AHRS_DCM::drift_correction_yaw(void) _gps_last_update = _gps->last_fix_time; new_value = true; float gps_course_rad = ToRad(_gps->ground_course_cd * 0.01f); - float yaw_error_rad = gps_course_rad - yaw; + float yaw_error_rad = wrap_PI(gps_course_rad - yaw); yaw_error = sinf(yaw_error_rad); /* reset yaw to match GPS heading under any of the