Rover: handle AR_WPNav failing to set desired location

APMrover2/GCS_Mavlink.cpp

@@ -511,8 +511,7 @@ bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd)
     }
 
     // make any new wp uploaded instant (in case we are already in Guided mode)
-    rover.mode_guided.set_desired_location(cmd.content.location);
+    return rover.mode_guided.set_desired_location(cmd.content.location);
-    return true;
 }
 
 void GCS_MAVLINK_Rover::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
@@ -824,7 +823,10 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             // set guided mode targets
             if (!pos_ignore) {
                 // consume position target
-                rover.mode_guided.set_desired_location(target_loc);
+                if (!rover.mode_guided.set_desired_location(target_loc)) {
+                    // GCS will need to monitor desired location to
+                    // see if they are having an effect.
+                }
             } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
                 // consume velocity
                 rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
@@ -926,7 +928,10 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             // set guided mode targets
             if (!pos_ignore) {
                 // consume position target
-                rover.mode_guided.set_desired_location(target_loc);
+                if (!rover.mode_guided.set_desired_location(target_loc)) {
+                    // GCS will just need to look at desired location
+                    // outputs to see if it having an effect.
+                }
             } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
                 // consume velocity
                 rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);

APMrover2/mode.cpp

@@ -196,13 +196,17 @@ float Mode::get_desired_lat_accel() const
 }
 
 // set desired location
-void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
+bool Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
 {
-    g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd);
+    if (!g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd)) {
+        return false;
+    }
 
     // initialise distance
     _distance_to_destination = g2.wp_nav.get_distance_to_destination();
     _reached_destination = false;
+
+    return true;
 }
 
 // set desired heading and speed

APMrover2/mode.h

@@ -106,7 +106,7 @@ public:
 
     // 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 void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN);
+    virtual bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) WARN_IF_UNUSED;
 
     // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
     virtual bool reached_destination() const { return true; }
@@ -253,7 +253,7 @@ public:
     float get_distance_to_destination() const override;
 
     // set desired location
-    void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) override;
+    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;
 
     // heading and speed control
@@ -294,7 +294,7 @@ private:
 
     bool verify_command(const AP_Mission::Mission_Command& cmd);
     void do_RTL(void);
-    void do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination);
+    bool do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination);
     void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
     void do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd);
     bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
@@ -367,7 +367,7 @@ public:
     bool reached_destination() const override;
 
     // set desired location, heading and speed
-    void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) override;
+    bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) override WARN_IF_UNUSED;
     void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) override;
 
     // set desired heading-delta, turn-rate and speed

APMrover2/mode_auto.cpp

@@ -11,12 +11,14 @@ bool ModeAuto::_enter()
         return false;
     }
 
+    // init location target
+    if (!g2.wp_nav.set_desired_location(rover.current_loc)) {
+        return false;
+    }
+
     // initialise waypoint speed
     g2.wp_nav.set_desired_speed_to_default();
 
-    // init location target
-    g2.wp_nav.set_desired_location(rover.current_loc);
-
     // other initialisation
     auto_triggered = false;
 
@@ -130,12 +132,16 @@ float ModeAuto::get_distance_to_destination() const
 }
 
 // set desired location to drive to
-void ModeAuto::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
+bool ModeAuto::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
 {
     // call parent
-    Mode::set_desired_location(destination, next_leg_bearing_cd);
+    if (!Mode::set_desired_location(destination, next_leg_bearing_cd)) {
+        return false;
+    }
 
     _submode = Auto_WP;
+
+    return true;
 }
 
 // return true if vehicle has reached or even passed destination
@@ -299,8 +305,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
 
     switch (cmd.id) {
     case MAV_CMD_NAV_WAYPOINT:  // Navigate to Waypoint
-        do_nav_wp(cmd, false);
+        return do_nav_wp(cmd, false);
-        break;
 
     case MAV_CMD_NAV_RETURN_TO_LAUNCH:
         do_RTL();
@@ -308,8 +313,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
 
     case MAV_CMD_NAV_LOITER_UNLIM:  // Loiter indefinitely
     case MAV_CMD_NAV_LOITER_TIME:   // Loiter for specified time
-        do_nav_wp(cmd, true);
+        return do_nav_wp(cmd, true);
-        break;
 
     case MAV_CMD_NAV_GUIDED_ENABLE: // accept navigation commands from external nav computer
         do_nav_guided_enable(cmd);
@@ -481,17 +485,8 @@ void ModeAuto::do_RTL(void)
     start_RTL();
 }
 
-void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination)
+bool ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination)
 {
-    // just starting so we haven't previously reached the waypoint
-    previously_reached_wp = false;
-
-    // this will be used to remember the time in millis after we reach or pass the WP.
-    loiter_start_time = 0;
-
-    // this is the delay, stored in seconds
-    loiter_duration = cmd.p1;
-
     // get heading to following waypoint (auto mode reduces speed to allow corning without large overshoot)
     // in case of non-zero loiter duration, we provide heading-unknown to signal we should stop at the point
     float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN;
@@ -502,7 +497,20 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_sto
     // retrieve and sanitize target location
     Location cmdloc = cmd.content.location;
     cmdloc.sanitize(rover.current_loc);
-    set_desired_location(cmdloc, next_leg_bearing_cd);
+    if (!set_desired_location(cmdloc, next_leg_bearing_cd)) {
+        return false;
+    }
+
+    // just starting so we haven't previously reached the waypoint
+    previously_reached_wp = false;
+
+    // this will be used to remember the time in millis after we reach or pass the WP.
+    loiter_start_time = 0;
+
+    // this is the delay, stored in seconds
+    loiter_duration = cmd.p1;
+
+    return true;
 }
 
 // start guided within auto to allow external navigation system to control vehicle

APMrover2/mode_guided.cpp

@@ -146,7 +146,7 @@ bool ModeGuided::reached_destination() const
 }
 
 // set desired location
-void ModeGuided::set_desired_location(const struct Location& destination,
+bool ModeGuided::set_desired_location(const struct Location& destination,
                                       float next_leg_bearing_cd)
 {
     if (g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd)) {
@@ -154,7 +154,9 @@ void ModeGuided::set_desired_location(const struct Location& destination,
         // handle guided specific initialisation and logging
         _guided_mode = ModeGuided::Guided_WP;
         rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_desired_speed(), 0.0f, 0.0f));
+        return true;
     }
+    return false;
 }
 
 // set desired attitude

APMrover2/mode_rtl.cpp

@@ -8,6 +8,18 @@ bool ModeRTL::_enter()
         return false;
     }
 
+    // set target to the closest rally point or home
+#if AP_RALLY == ENABLED
+    if (!g2.wp_nav.set_desired_location(g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt))) {
+        return false;
+    }
+#else
+    // set destination
+    if (!g2.wp_nav.set_desired_location(rover.home)) {
+        return false;
+    }
+#endif
+
     // initialise waypoint speed
     if (is_positive(g2.rtl_speed)) {
         g2.wp_nav.set_desired_speed(g2.rtl_speed);
@@ -15,14 +27,6 @@ bool ModeRTL::_enter()
         g2.wp_nav.set_desired_speed_to_default();
     }
 
-    // set target to the closest rally point or home
-#if AP_RALLY == ENABLED
-    g2.wp_nav.set_desired_location(g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt));
-#else
-    // set destination
-    g2.wp_nav.set_desired_location(rover.home);
-#endif
-
     sent_notification = false;
 
     return true;