From 95bc9cca790aa8296052bb191f7e2b4c6a2767f1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 22 Jan 2021 09:19:38 +1100 Subject: [PATCH] ArduPlane: use ground_course in preference to ground_course_cd --- ArduPlane/commands_logic.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index e5df746af0..1e93289724 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -460,7 +460,7 @@ void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd) } else if (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) { // use gps ground course based bearing hold steer_state.hold_course_cd = -1; - bearing = AP::gps().ground_course_cd() * 0.01f; + bearing = AP::gps().ground_course(); next_WP_loc.offset_bearing(bearing, 1000); // push it out 1km } else { // use yaw based bearing hold @@ -513,7 +513,7 @@ bool Plane::verify_takeoff() // course. This keeps wings level until we are ready to // rotate, and also allows us to cope with arbitrary // compass errors for auto takeoff - float takeoff_course = wrap_PI(radians(gps.ground_course_cd()*0.01f)) - steer_state.locked_course_err; + float takeoff_course = wrap_PI(radians(gps.ground_course())) - steer_state.locked_course_err; takeoff_course = wrap_PI(takeoff_course); steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100); gcs().send_text(MAV_SEVERITY_INFO, "Holding course %d at %.1fm/s (%.1f)",