From 6606ab4996d31e38b227a8c9abe633159db0b207 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 21 Jul 2021 10:08:05 +0930 Subject: [PATCH] Copter: Guided and Loiter mode returns Crosstrack error --- ArduCopter/mode.h | 1 + ArduCopter/mode_guided.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index c663529fcb..f8b8ff6190 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1028,6 +1028,7 @@ protected: uint32_t wp_distance() const override; int32_t wp_bearing() const override; + float crosstrack_error() const override { return pos_control->crosstrack_error();} #if PRECISION_LANDING == ENABLED bool do_precision_loiter(); diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 204b1970ab..001706acaa 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -1024,6 +1024,7 @@ float ModeGuided::crosstrack_error() const case SubMode::Accel: case SubMode::VelAccel: case SubMode::PosVelAccel: + return pos_control->crosstrack_error(); case SubMode::Angle: // no track to have a crosstrack to return 0;