Guided Mode support MAV_CMD_DO_CHANGE_SPEED

ArduCopter/GCS_Mavlink.cpp

@@ -830,13 +830,21 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
         // param4 : unused
         if (packet.param2 > 0.0f) {
             if (packet.param1 > 2.9f) { // 3 = speed down
-                copter.wp_nav->set_speed_down(packet.param2 * 100.0f);
+                if (copter.flightmode->set_speed_down(packet.param2 * 100.0f)) {
+                    return MAV_RESULT_ACCEPTED;
+                }
+                return MAV_RESULT_FAILED;
             } else if (packet.param1 > 1.9f) { // 2 = speed up
-                copter.wp_nav->set_speed_up(packet.param2 * 100.0f);
+                if (copter.flightmode->set_speed_up(packet.param2 * 100.0f)) {
+                    return MAV_RESULT_ACCEPTED;
+                }
+                return MAV_RESULT_FAILED;
             } else {
-                copter.wp_nav->set_speed_xy(packet.param2 * 100.0f);
+                if (copter.flightmode->set_speed_xy(packet.param2 * 100.0f)) {
+                    return MAV_RESULT_ACCEPTED;
+                }
+                return MAV_RESULT_FAILED;
             }
-            return MAV_RESULT_ACCEPTED;
         }
         return MAV_RESULT_FAILED;
 

ArduCopter/mode.h

@@ -87,6 +87,11 @@ public:
     virtual uint32_t wp_distance() const { return 0; }
     virtual float crosstrack_error() const { return 0.0f;}
 
+    // functions to support MAV_CMD_DO_CHANGE_SPEED
+    virtual bool set_speed_xy(float speed_xy_cms) {return false;}
+    virtual bool set_speed_up(float speed_xy_cms) {return false;}
+    virtual bool set_speed_down(float speed_xy_cms) {return false;}
+
     int32_t get_alt_above_ground_cm(void);
 
     // pilot input processing
@@ -443,6 +448,10 @@ public:
     bool is_taking_off() const override;
     bool use_pilot_yaw() const override;
 
+    bool set_speed_xy(float speed_xy_cms) override;
+    bool set_speed_up(float speed_up_cms) override;
+    bool set_speed_down(float speed_down_cms) override;
+
     bool requires_terrain_failsafe() const override { return true; }
 
     // return true if this flight mode supports user takeoff
@@ -968,6 +977,10 @@ public:
 
     bool is_taking_off() const override;
     
+    bool set_speed_xy(float speed_xy_cms) override;
+    bool set_speed_up(float speed_up_cms) override;
+    bool set_speed_down(float speed_down_cms) override;
+
     // initialises position controller to implement take-off
     // takeoff_alt_cm is interpreted as alt-above-home (in cm) or alt-above-terrain if a rangefinder is available
     bool do_user_takeoff_start(float takeoff_alt_cm) override;

ArduCopter/mode_auto.cpp

@@ -562,6 +562,24 @@ bool ModeAuto::use_pilot_yaw(void) const
     return (copter.g2.auto_options.get() & uint32_t(Options::IgnorePilotYaw)) == 0;
 }
 
+bool ModeAuto::set_speed_xy(float speed_xy_cms)
+{
+    copter.wp_nav->set_speed_xy(speed_xy_cms);
+    return true;
+}
+
+bool ModeAuto::set_speed_up(float speed_up_cms)
+{
+    copter.wp_nav->set_speed_up(speed_up_cms);
+    return true;
+}
+
+bool ModeAuto::set_speed_down(float speed_down_cms)
+{
+    copter.wp_nav->set_speed_down(speed_down_cms);
+    return true;
+}
+
 // start_command - this function will be called when the ap_mission lib wishes to start a new command
 bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
 {

ArduCopter/mode_guided.cpp

@@ -284,6 +284,30 @@ bool ModeGuided::is_taking_off() const
     return guided_mode == SubMode::TakeOff && !takeoff_complete;
 }
 
+bool ModeGuided::set_speed_xy(float speed_xy_cms)
+{
+    // initialise horizontal speed, acceleration
+    pos_control->set_max_speed_accel_xy(speed_xy_cms, wp_nav->get_wp_acceleration());
+    pos_control->set_correction_speed_accel_xy(speed_xy_cms, wp_nav->get_wp_acceleration());
+    return true;
+}
+
+bool ModeGuided::set_speed_up(float speed_up_cms)
+{
+    // initialize vertical speeds and acceleration
+    pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), speed_up_cms, wp_nav->get_accel_z());
+    pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), speed_up_cms, wp_nav->get_accel_z());
+    return true;
+}
+
+bool ModeGuided::set_speed_down(float speed_down_cms)
+{
+    // initialize vertical speeds and acceleration
+    pos_control->set_max_speed_accel_z(speed_down_cms, wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
+    pos_control->set_correction_speed_accel_z(speed_down_cms, wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
+    return true;
+}
+
 // initialise guided mode's angle controller
 void ModeGuided::angle_control_start()
 {