diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 9ade4f158c..ad0e2faacd 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1142,6 +1142,7 @@ protected: const char *name4() const override { return "FOLL"; } uint32_t wp_distance() const override; int32_t wp_bearing() const override; + bool get_wp(Location_Class &loc) 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 45b72d6d1c..2fc4d057d1 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -160,4 +160,16 @@ int32_t Copter::ModeFollow::wp_bearing() const return g2.follow.get_bearing_to_target() * 100; } +/* + get target position for mavlink reporting + */ +bool Copter::ModeFollow::get_wp(Location_Class &loc) +{ + float dist = g2.follow.get_distance_to_target(); + float bearing = g2.follow.get_bearing_to_target(); + loc = copter.current_loc; + location_update(loc, bearing, dist); + return true; +} + #endif // MODE_FOLLOW_ENABLED == ENABLED