From 358069df246361d104bf25b1d52e0a5fd861effc Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 22 Jan 2021 09:19:38 +1100 Subject: [PATCH] AP_AHRS: use ground_course in preference to ground_course_cd --- libraries/AP_AHRS/AP_AHRS.cpp | 2 +- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 0057c10353..e135188bda 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -246,7 +246,7 @@ Vector2f AP_AHRS::groundspeed_vector(void) // Generate estimate of ground speed vector using GPS if (gotGPS) { - const float cog = radians(AP::gps().ground_course_cd()*0.01f); + const float cog = radians(AP::gps().ground_course()); gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * AP::gps().ground_speed(); } // If both ADS and GPS data is available, apply a complementary filter diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 759167986c..ba608027af 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -528,7 +528,7 @@ AP_AHRS_DCM::drift_correction_yaw(void) yaw_deltat = (_gps.last_fix_time_ms() - _gps_last_update) * 1.0e-3f; _gps_last_update = _gps.last_fix_time_ms(); new_value = true; - const float gps_course_rad = ToRad(_gps.ground_course_cd() * 0.01f); + const float gps_course_rad = ToRad(_gps.ground_course()); const float yaw_error_rad = wrap_PI(gps_course_rad - yaw); yaw_error = sinf(yaw_error_rad); @@ -1041,7 +1041,7 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const if (_flags.fly_forward && _have_position) { float gps_delay_sec = 0; _gps.get_lag(gps_delay_sec); - loc.offset_bearing(_gps.ground_course_cd() * 0.01f, _gps.ground_speed() * gps_delay_sec); + loc.offset_bearing(_gps.ground_course(), _gps.ground_speed() * gps_delay_sec); } return _have_position; }