Rover: move set_desired_speed to each mode

APMrover2/mode.cpp

@@ -209,17 +209,6 @@ bool Mode::set_desired_location(const struct Location& destination, float next_l
     return true;
 }
 
-// set desired heading and speed
-void Mode::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
-{
-    // handle initialisation
-    _reached_destination = false;
-
-    // record targets
-    _desired_yaw_cd = yaw_angle_cd;
-    _desired_speed = target_speed;
-}
-
 // get default speed for this mode (held in WP_SPEED or RTL_SPEED)
 float Mode::get_speed_default(bool rtl) const
 {
@@ -230,22 +219,6 @@ float Mode::get_speed_default(bool rtl) const
     return g2.wp_nav.get_default_speed();
 }
 
-// restore desired speed to default from parameter values (WP_SPEED)
-void Mode::set_desired_speed_to_default(bool rtl)
-{
-    _desired_speed = get_speed_default(rtl);
-}
-
-// set desired speed in m/s
-bool Mode::set_desired_speed(float speed)
-{
-    if (!is_negative(speed)) {
-        _desired_speed = speed;
-        return true;
-    }
-    return false;
-}
-
 // execute the mission in reverse (i.e. backing up)
 void Mode::set_reversed(bool value)
 {

APMrover2/mode.h

@@ -115,19 +115,12 @@ public:
     // 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; }
 
-    // set desired heading and speed - supported in Auto and Guided modes
-    virtual void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed);
-
     // get default speed for this mode (held in CRUISE_SPEED, WP_SPEED or RTL_SPEED)
     // rtl argument should be true if called from RTL or SmartRTL modes (handled here to avoid duplication)
     float get_speed_default(bool rtl = false) const;
 
     // set desired speed in m/s
-    bool set_desired_speed(float speed);
-
-    // restore desired speed to default from parameter values (CRUISE_SPEED or WP_SPEED)
-    // rtl argument should be true if called from RTL or SmartRTL modes (handled here to avoid duplication)
-    void set_desired_speed_to_default(bool rtl = false);
+    virtual bool set_desired_speed(float speed) { return false; }
 
     // execute the mission in reverse (i.e. backing up)
     void set_reversed(bool value);
@@ -207,7 +200,6 @@ protected:
     float _distance_to_destination; // distance from vehicle to final destination in meters
     bool _reached_destination;  // true once the vehicle has reached the destination
     float _desired_yaw_cd;      // desired yaw in centi-degrees.  used in Auto, Guided and Loiter
-    float _desired_speed;       // desired speed in m/s
 };
 
 
@@ -258,9 +250,8 @@ public:
     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
-    void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) override;
-    bool reached_heading();
+    // set desired speed in m/s
+    bool set_desired_speed(float speed) override;
 
     // start RTL (within auto)
     void start_RTL();
@@ -325,6 +316,9 @@ private:
     };
 
     bool auto_triggered;        // true when auto has been triggered to start
+
+    // HeadingAndSpeed sub mode variables
+    float _desired_speed;   // desired speed in HeadingAndSpeed submode
     bool _reached_heading;  // true when vehicle has reached desired heading in TurnToHeading sub mode
 
     // Loiter control
@@ -376,12 +370,15 @@ public:
     // return true if vehicle has reached destination
     bool reached_destination() const override;
 
+    // set desired speed in m/s
+    bool set_desired_speed(float speed) override;
+
     // 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;
+    void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed);
 
     // set desired heading-delta, turn-rate and speed
     void set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed);
@@ -414,6 +411,7 @@ protected:
     uint32_t _des_att_time_ms;  // system time last call to set_desired_attitude was made (used for timeout)
     float _desired_yaw_rate_cds;// target turn rate centi-degrees per second
     bool sent_notification;     // used to send one time notification to ground station
+    float _desired_speed;       // desired speed used only in HeadingAndSpeed submode
 
     // limits
     struct {
@@ -472,6 +470,7 @@ protected:
     bool _enter() override;
 
     Location _destination;      // target location to hold position around
+    float _desired_speed;       // desired speed (ramped down from initial speed to zero)
 };
 
 class ModeManual : public Mode
@@ -518,6 +517,9 @@ public:
     float get_distance_to_destination() const override { return _distance_to_destination; }
     bool reached_destination() const override;
 
+    // set desired speed in m/s
+    bool set_desired_speed(float speed) override;
+
 protected:
 
     bool _enter() override;
@@ -547,6 +549,9 @@ public:
     float get_distance_to_destination() const override { return _distance_to_destination; }
     bool reached_destination() const override { return smart_rtl_state == SmartRTL_StopAtHome; }
 
+    // set desired speed in m/s
+    bool set_desired_speed(float speed) override;
+
     // save current position for use by the smart_rtl flight mode
     void save_position();
 
@@ -631,10 +636,15 @@ public:
     // return distance (in meters) to destination
     float get_distance_to_destination() const override;
 
+    // set desired speed in m/s
+    bool set_desired_speed(float speed) override;
+
 protected:
 
     bool _enter() override;
     void _exit() override;
+
+    float _desired_speed;       // desired speed in m/s
 };
 
 class ModeSimple : public Mode

APMrover2/mode_auto.cpp

@@ -202,24 +202,28 @@ bool ModeAuto::reached_destination() const
     return true;
 }
 
-// set desired heading in centidegrees (vehicle will turn to this heading)
-void ModeAuto::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
+// set desired speed in m/s
+bool ModeAuto::set_desired_speed(float speed)
 {
-    // call parent
-    Mode::set_desired_heading_and_speed(yaw_angle_cd, target_speed);
-
-    _submode = Auto_HeadingAndSpeed;
-    _reached_heading = false;
-}
-
-// return true if vehicle has reached desired heading
-bool ModeAuto::reached_heading()
-{
-    if (_submode == Auto_HeadingAndSpeed) {
-        return _reached_heading;
+    switch (_submode) {
+    case Auto_WP:
+    case Auto_Stop:
+        if (!is_negative(speed)) {
+            g2.wp_nav.set_desired_speed(speed);
+            return true;
         }
-    // we should never reach here but just in case, return true to allow missions to continue
+        return false;
+    case Auto_HeadingAndSpeed:
+        _desired_speed = speed;
         return true;
+    case Auto_RTL:
+        return rover.mode_rtl.set_desired_speed(speed);
+    case Auto_Loiter:
+        return rover.mode_loiter.set_desired_speed(speed);
+    case Auto_Guided:
+        return rover.mode_guided.set_desired_speed(speed);
+    }
+    return false;
 }
 
 // start RTL (within auto)
@@ -604,9 +608,13 @@ void ModeAuto::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd)
         desired_heading_cd = cmd.content.set_yaw_speed.angle_deg * 100.0f;
     }
 
-    // set auto target
+    // set targets
     const float speed_max = g2.wp_nav.get_default_speed();
-    set_desired_heading_and_speed(desired_heading_cd, constrain_float(cmd.content.set_yaw_speed.speed, -speed_max, speed_max));
+    _desired_speed = constrain_float(cmd.content.set_yaw_speed.speed, -speed_max, speed_max);
+    _desired_yaw_cd = desired_heading_cd;
+    _reached_heading = false;
+    _reached_destination = false;
+    _submode = Auto_HeadingAndSpeed;
 }
 
 /********************************************************************************/
@@ -700,7 +708,11 @@ bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
 // verify_yaw - return true if we have reached the desired heading
 bool ModeAuto::verify_nav_set_yaw_speed()
 {
-    return reached_heading();
+    if (_submode == Auto_HeadingAndSpeed) {
+        return _reached_heading;
+    }
+    // we should never reach here but just in case, return true to allow missions to continue
+    return true;
 }
 
 /********************************************************************************/

APMrover2/mode_follow.cpp

@@ -8,8 +8,8 @@ bool ModeFollow::_enter()
         return false;
     }
 
-    // initialise waypoint speed
-    set_desired_speed_to_default();
+    // initialise speed to waypoint speed
+    _desired_speed = g2.wp_nav.get_default_speed();
 
     return true;
 }
@@ -85,3 +85,13 @@ float ModeFollow::get_distance_to_destination() const
 {
     return g2.follow.get_distance_to_target();
 }
+
+// set desired speed in m/s
+bool ModeFollow::set_desired_speed(float speed)
+{
+    if (is_negative(speed)) {
+        return false;
+    }
+    _desired_speed = speed;
+    return true;
+}

APMrover2/mode_guided.cpp

@@ -144,6 +144,26 @@ bool ModeGuided::reached_destination() const
     return true;
 }
 
+// set desired speed in m/s
+bool ModeGuided::set_desired_speed(float speed)
+{
+    switch (_guided_mode) {
+    case Guided_WP:
+        if (!is_negative(speed)) {
+            g2.wp_nav.set_desired_speed(speed);
+            return true;
+        }
+        return false;
+    case Guided_HeadingAndSpeed:
+    case Guided_TurnRateAndSpeed:
+        // speed is set from mavlink message
+        return false;
+    case Guided_Loiter:
+        return rover.mode_loiter.set_desired_speed(speed);
+    }
+    return false;
+}
+
 // get desired location
 bool ModeGuided::get_desired_location(Location& destination) const
 {
@@ -185,10 +205,7 @@ bool ModeGuided::set_desired_location(const struct Location& destination,
 // set desired attitude
 void ModeGuided::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
 {
-    // call parent
-    Mode::set_desired_heading_and_speed(yaw_angle_cd, target_speed);
-
-    // handle guided specific initialisation and logging
+    // initialisation and logging
     _guided_mode = ModeGuided::Guided_HeadingAndSpeed;
     _des_att_time_ms = AP_HAL::millis();
     _reached_destination = false;

APMrover2/mode_rtl.cpp

@@ -81,3 +81,13 @@ bool ModeRTL::reached_destination() const
 {
     return g2.wp_nav.reached_destination();
 }
+
+// set desired speed in m/s
+bool ModeRTL::set_desired_speed(float speed)
+{
+    if (is_negative(speed)) {
+        return false;
+    }
+    g2.wp_nav.set_desired_speed(speed);
+    return true;
+}

APMrover2/mode_smart_rtl.cpp

@@ -115,6 +115,16 @@ bool ModeSmartRTL::get_desired_location(Location& destination) const
     return false;
 }
 
+// set desired speed in m/s
+bool ModeSmartRTL::set_desired_speed(float speed)
+{
+    if (is_negative(speed)) {
+        return false;
+    }
+    g2.wp_nav.set_desired_speed(speed);
+    return true;
+}
+
 // save current position for use by the smart_rtl flight mode
 void ModeSmartRTL::save_position()
 {