Rover: add get_desired_location method

APMrover2/mode.h

@@ -104,6 +104,10 @@ public:
     // return distance (in meters) to destination
     virtual float get_distance_to_destination() const { return 0.0f; }
 
+    // return desired location (used in Guided, Auto, RTL, etc)
+    // return true on success, false if there is no valid destination
+    virtual bool get_desired_location(Location& destination) const WARN_IF_UNUSED { return false; }
+
     // set desired location (used in Guided, Auto)
     //   next_leg_bearing_cd should be heading to the following waypoint (used to slow the vehicle in order to make the turn)
     virtual bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) WARN_IF_UNUSED;
@@ -252,7 +256,8 @@ public:
     // return distance (in meters) to destination
     float get_distance_to_destination() const override;
 
-    // set desired location
+    // get or set desired location
+    bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
     bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) override WARN_IF_UNUSED;
     bool reached_destination() const override;
 
@@ -366,8 +371,11 @@ public:
     // return true if vehicle has reached destination
     bool reached_destination() const override;
 
-    // set desired location, heading and speed
+    // get or set desired location
+    bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
     bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) override WARN_IF_UNUSED;
+
+    // set desired heading and speed
     void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) override;
 
     // set desired heading-delta, turn-rate and speed
@@ -448,6 +456,9 @@ public:
     float nav_bearing() const override { return _desired_yaw_cd * 0.01f; }
     float crosstrack_error() const override { return 0.0f; }
 
+    // return desired location
+    bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
+
     // return distance (in meters) to destination
     float get_distance_to_destination() const override { return _distance_to_destination; }
 
@@ -495,6 +506,10 @@ public:
     // attributes of the mode
     bool is_autopilot_mode() const override { return true; }
 
+    // return desired location
+    bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
+
+    // return distance (in meters) to destination
     float get_distance_to_destination() const override { return _distance_to_destination; }
     bool reached_destination() const override;
 
@@ -518,6 +533,10 @@ public:
     // attributes of the mode
     bool is_autopilot_mode() const override { return true; }
 
+    // return desired location
+    bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
+
+    // return distance (in meters) to destination
     float get_distance_to_destination() const override { return _distance_to_destination; }
     bool reached_destination() const override { return smart_rtl_state == SmartRTL_StopAtHome; }
 
@@ -597,6 +616,9 @@ public:
     float nav_bearing() const override { return wp_bearing(); }
     float crosstrack_error() const override { return 0.0f; }
 
+    // return desired location
+    bool get_desired_location(Location& destination) const override WARN_IF_UNUSED { return false; }
+
     // return distance (in meters) to destination
     float get_distance_to_destination() const override;
 

APMrover2/mode_auto.cpp

@@ -131,6 +131,31 @@ float ModeAuto::get_distance_to_destination() const
     return 0.0f;
 }
 
+// get desired location
+bool ModeAuto::get_desired_location(Location& destination) const
+{
+    switch (_submode) {
+    case Auto_WP:
+        if (g2.wp_nav.is_destination_valid()) {
+            destination = g2.wp_nav.get_destination();
+            return true;
+        }
+        return false;
+    case Auto_HeadingAndSpeed:
+        // no desired location for this submode
+        return false;
+    case Auto_RTL:
+        return rover.mode_rtl.get_desired_location(destination);
+    case Auto_Loiter:
+        return rover.mode_loiter.get_desired_location(destination);
+    case Auto_Guided:
+        return rover.mode_guided.get_desired_location(destination);\
+    }
+
+    // we should never reach here but just in case
+    return false;
+}
+
 // set desired location to drive to
 bool ModeAuto::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
 {

APMrover2/mode_guided.cpp

@@ -145,6 +145,29 @@ bool ModeGuided::reached_destination() const
     return true;
 }
 
+// get desired location
+bool ModeGuided::get_desired_location(Location& destination) const
+{
+    switch (_guided_mode) {
+    case Guided_WP:
+        if (g2.wp_nav.is_destination_valid()) {
+            destination = g2.wp_nav.get_destination();
+            return true;
+        }
+        return false;
+    case Guided_HeadingAndSpeed:
+    case Guided_TurnRateAndSpeed:
+        // not supported in these submodes
+        return false;
+    case Guided_Loiter:
+        // get destination from loiter
+        return rover.mode_loiter.get_desired_location(destination);
+    }
+
+    // should never get here but just in case
+    return false;
+}
+
 // set desired location
 bool ModeGuided::set_desired_location(const struct Location& destination,
                                       float next_leg_bearing_cd)

APMrover2/mode_loiter.cpp

@@ -52,3 +52,10 @@ void ModeLoiter::update()
     calc_steering_to_heading(_desired_yaw_cd);
     calc_throttle(_desired_speed, true);
 }
+
+// get desired location
+bool ModeLoiter::get_desired_location(Location& destination) const
+{
+    destination = _destination;
+    return true;
+}

APMrover2/mode_rtl.cpp

@@ -58,6 +58,16 @@ void ModeRTL::update()
     }
 }
 
+// get desired location
+bool ModeRTL::get_desired_location(Location& destination) const
+{
+    if (g2.wp_nav.is_destination_valid()) {
+        destination = g2.wp_nav.get_destination();
+        return true;
+    }
+    return false;
+}
+
 bool ModeRTL::reached_destination() const
 {
     return g2.wp_nav.reached_destination();

APMrover2/mode_smart_rtl.cpp

@@ -86,6 +86,26 @@ void ModeSmartRTL::update()
     }
 }
 
+// get desired location
+bool ModeSmartRTL::get_desired_location(Location& destination) const
+{
+    switch (smart_rtl_state) {
+    case SmartRTL_WaitForPathCleanup:
+        return false;
+    case SmartRTL_PathFollow:
+        if (g2.wp_nav.is_destination_valid()) {
+            destination = g2.wp_nav.get_destination();
+            return true;
+        }
+        return false;
+    case SmartRTL_StopAtHome:
+    case SmartRTL_Failure:
+        return false;
+    }
+    // should never reach here but just in case
+    return false;
+}
+
 // save current position for use by the smart_rtl flight mode
 void ModeSmartRTL::save_position()
 {