From 8f272978964235fd54e445cc379f57a43ad0bab8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 May 2012 12:14:52 +1000 Subject: [PATCH] DCM: fixed a bug when using GPS for yaw correction When using GPS for yaw correction we need to apply the x and y omegaI corrections from the _omega_I_sum in the period before we get to the minimum ground speed for GPS yaw correction. Otherwise we get a large sudden omega_I change on takeoff. --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index bc286a0d28..fe689d856e 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -429,7 +429,7 @@ AP_AHRS_DCM::drift_correction(float deltat) // lost the yaw reference sensor completely we don't // keep using a stale offset _omega_yaw_P *= 0.97; - return; + goto check_sum_time; } // ensure the course error is scaled from -PI to PI @@ -461,6 +461,7 @@ AP_AHRS_DCM::drift_correction(float deltat) _error_yaw_sum += error_course; _error_yaw_count++; +check_sum_time: if (_omega_I_sum_time > 10) { // every 10 seconds we apply the accumulated // _omega_I_sum changes to _omega_I. We do this to