From 791b6effddb7d5bf06ac7b9e4c0ec13c494b617e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 25 Apr 2019 11:48:39 +0900 Subject: [PATCH] Copter: fix wp distance and bearing reporting during LOITER_TURNS --- ArduCopter/mode_auto.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 505ae1c294..5c48685b3d 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -573,12 +573,26 @@ bool Copter::ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd) uint32_t Copter::ModeAuto::wp_distance() const { - return wp_nav->get_wp_distance_to_destination(); + switch (_mode) { + case Auto_Circle: + return copter.circle_nav->get_distance_to_target(); + case Auto_WP: + case Auto_CircleMoveToEdge: + default: + return wp_nav->get_wp_distance_to_destination(); + } } int32_t Copter::ModeAuto::wp_bearing() const { - return wp_nav->get_wp_bearing_to_destination(); + switch (_mode) { + case Auto_Circle: + return copter.circle_nav->get_bearing_to_target(); + case Auto_WP: + case Auto_CircleMoveToEdge: + default: + return wp_nav->get_wp_bearing_to_destination(); + } } bool Copter::ModeAuto::get_wp(Location& destination)