Rover: add WP_SPEED and RTL_SPEED

This separates the default/maximum speed used in Auto, Guided, RTL and SmartRTL from the CRUISE_SPEED which is used as the base for the speed-to-throttle controller (along with CRUISE_THROTTLE)

APMrover2/GCS_Mavlink.cpp

@@ -926,7 +926,8 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             }
 
             // send yaw change and target speed to guided mode controller
-            float target_speed = constrain_float(packet.param2 * rover.g.speed_cruise, -rover.g.speed_cruise, rover.g.speed_cruise);
+            const float speed_max = rover.control_mode->get_speed_default();
+            const float target_speed = constrain_float(packet.param2 * speed_max, -speed_max, speed_max);
             rover.mode_guided.set_desired_heading_delta_and_speed(packet.param1, target_speed);
             result = MAV_RESULT_ACCEPTED;
             break;
@@ -1043,7 +1044,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
 
             // convert thrust to ground speed
             packet.thrust = constrain_float(packet.thrust, -1.0f, 1.0f);
-            float target_speed = rover.g.speed_cruise * packet.thrust;
+            const float target_speed = rover.control_mode->get_speed_default() * packet.thrust;
 
             // if the body_yaw_rate field is ignored, convert quaternion to heading
             if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_YAW_RATE_IGNORE) != 0) {
@@ -1114,8 +1115,9 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
 
             // consume velocity and convert to target speed and heading
             if (!vel_ignore) {
+                const float speed_max = rover.control_mode->get_speed_default();
                 // convert vector length into a speed
-                target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -rover.g.speed_cruise, rover.g.speed_cruise);
+                target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
                 // convert vector direction to target yaw
                 target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;
 
@@ -1215,8 +1217,9 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
 
             // consume velocity and convert to target speed and heading
             if (!vel_ignore) {
+                const float speed_max = rover.control_mode->get_speed_default();
                 // convert vector length into a speed
-                target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -rover.g.speed_cruise, rover.g.speed_cruise);
+                target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
                 // convert vector direction to target yaw
                 target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;
 

APMrover2/Parameters.cpp

@@ -517,6 +517,24 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: ../libraries/AP_SmartRTL/AP_SmartRTL.cpp
     AP_SUBGROUPINFO(smart_rtl, "SRTL_", 13, ParametersG2, AP_SmartRTL),
 
+    // @Param: WP_SPEED
+    // @DisplayName: Waypoint speed default
+    // @Description: Waypoint speed default.  If zero use CRUISE_SPEED.
+    // @Units: m/s
+    // @Range: 0 100
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("WP_SPEED", 14, ParametersG2, wp_speed, 0.0f),
+
+    // @Param: RTL_SPEED
+    // @DisplayName: Return-to-Launch speed default
+    // @Description: Return-to-Launch speed default.  If zero use WP_SPEED or CRUISE_SPEED.
+    // @Units: m/s
+    // @Range: 0 100
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("RTL_SPEED", 15, ParametersG2, rtl_speed, 0.0f),
+
     AP_GROUPEND
 };
 

APMrover2/Parameters.h

@@ -322,6 +322,10 @@ public:
 
     // Safe RTL library
     AP_SmartRTL smart_rtl;
+
+    // default speeds for auto, rtl
+    AP_Float wp_speed;
+    AP_Float rtl_speed;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/commands_logic.cpp

@@ -268,7 +268,8 @@ void Rover::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd)
     }
 
     // set auto target
-    mode_auto.set_desired_heading_and_speed(desired_heading_cd, constrain_float(cmd.content.set_yaw_speed.speed, -g.speed_cruise, g.speed_cruise));
+    const float speed_max = control_mode->get_speed_default();
+    mode_auto.set_desired_heading_and_speed(desired_heading_cd, constrain_float(cmd.content.set_yaw_speed.speed, -speed_max, speed_max));
 }
 
 /********************************************************************************/
@@ -378,14 +379,10 @@ bool Rover::verify_within_distance()
 
 void Rover::do_change_speed(const AP_Mission::Mission_Command& cmd)
 {
-    if (cmd.content.speed.target_ms > 0.0f) {
+    // set speed for active mode
-        g.speed_cruise.set(cmd.content.speed.target_ms);
+    if ((cmd.content.speed.target_ms >= 0.0f) && (cmd.content.speed.target_ms <= rover.control_mode->get_speed_default())) {
-        gcs().send_text(MAV_SEVERITY_INFO, "Cruise speed: %.1f m/s", static_cast<double>(g.speed_cruise.get()));
+        control_mode->set_desired_speed(cmd.content.speed.target_ms);
-    }
+        gcs().send_text(MAV_SEVERITY_INFO, "speed: %.1f m/s", static_cast<double>(cmd.content.speed.target_ms));
-
-    if (cmd.content.speed.throttle_pct > 0.0f && cmd.content.speed.throttle_pct <= 100.0f) {
-        g.throttle_cruise.set(cmd.content.speed.throttle_pct);
-        gcs().send_text(MAV_SEVERITY_INFO, "Cruise throttle: %.1f", static_cast<double>(g.throttle_cruise.get()));
     }
 }
 

APMrover2/mode.cpp

@@ -130,7 +130,6 @@ void Mode::set_desired_location(const struct Location& destination, float next_l
     // record targets
     _origin = rover.current_loc;
     _destination = destination;
-    _desired_speed = g.speed_cruise;
 
     // initialise distance
     _distance_to_destination = get_distance(_origin, _destination);
@@ -177,6 +176,24 @@ void Mode::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
     _desired_speed = target_speed;
 }
 
+// get default speed for this mode (held in (CRUISE_SPEED, WP_SPEED or RTL_SPEED)
+float Mode::get_speed_default(bool rtl) const
+{
+    if (rtl && is_positive(g2.rtl_speed)) {
+        return g2.rtl_speed;
+    } else if (is_positive(g2.wp_speed)) {
+        return g2.wp_speed;
+    } else {
+        return g.speed_cruise;
+    }
+}
+
+// restore desired speed to default from parameter values (CRUISE_SPEED or WP_SPEED)
+void Mode::set_desired_speed_to_default(bool rtl)
+{
+    _desired_speed = get_speed_default(rtl);
+}
+
 void Mode::calc_throttle(float target_speed, bool nudge_allowed)
 {
     // add in speed nudging

APMrover2/mode.h

@@ -84,6 +84,17 @@ public:
     // get speed error in m/s, returns zero for modes that do not control speed
     float speed_error() const { return _speed_error; }
 
+    // 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
+    void set_desired_speed(float speed) { _desired_speed = 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);
+
     // Navigation control variables
     // The instantaneous desired lateral acceleration in m/s/s
     float lateral_acceleration;

APMrover2/mode_auto.cpp

@@ -15,6 +15,9 @@ bool ModeAuto::_enter()
         return false;
     }
 
+    // initialise waypoint speed
+    set_desired_speed_to_default();
+
     // init location target
     set_desired_location(rover.current_loc);
 

APMrover2/mode_guided.cpp

@@ -3,10 +3,10 @@
 
 bool ModeGuided::_enter()
 {
-    /*
+    // initialise waypoint speed
-      when entering guided mode we set the target as the current
+    set_desired_speed_to_default();
-      location. This matches the behaviour of the copter code.
+
-    */
+    // when entering guided mode we set the target as the current location.
     lateral_acceleration = 0.0f;
     set_desired_location(rover.current_loc);
 

APMrover2/mode_rtl.cpp

@@ -8,6 +8,9 @@ bool ModeRTL::_enter()
         return false;
     }
 
+    // initialise waypoint speed
+    set_desired_speed_to_default(true);
+
     // set destination
     set_desired_location(rover.home);
 

APMrover2/mode_smart_rtl.cpp

@@ -14,6 +14,9 @@ bool ModeSmartRTL::_enter()
         return false;
     }
 
+    // initialise waypoint speed
+    set_desired_speed_to_default(true);
+
     // RTL never reverses
     rover.set_reverse(false);