diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 6d0a07d672..8cc3e8b0fc 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1252,6 +1252,8 @@ protected: const char *name() const override { return "FOLLOW"; } const char *name4() const override { return "FOLL"; } + uint32_t wp_distance() const override; + int32_t wp_bearing() const override; uint32_t last_log_ms; // system time of last time desired velocity was logging }; diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index 782b4369af..45b72d6d1c 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -150,4 +150,14 @@ void Copter::ModeFollow::run() Copter::ModeGuided::run(); } +uint32_t Copter::ModeFollow::wp_distance() const +{ + return g2.follow.get_distance_to_target() * 100; +} + +int32_t Copter::ModeFollow::wp_bearing() const +{ + return g2.follow.get_bearing_to_target() * 100; +} + #endif // MODE_FOLLOW_ENABLED == ENABLED