Stop Missions blowing past desired speeds

ArduCopter/mode.h

@@ -750,6 +750,10 @@ class ModeAuto : public Mode {
         float up;     // desired speed upwards in m/s. 0 if unset
         float down;   // desired speed downwards in m/s. 0 if unset
     } desired_speed_override;
+
+    // method to process do_change_speed before we calculate SCurve:
+    void do_change_speed_lookahead(uint16_t index);
+
 };
 
 #if AUTOTUNE_ENABLED == ENABLED

ArduCopter/mode_auto.cpp

@@ -1539,6 +1539,8 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
     // get waypoint's location from command and send to wp_nav
     const Location target_loc = loc_from_cmd(cmd, default_loc);
 
+    do_change_speed_lookahead(cmd.index+1);
+
     if (!wp_start(target_loc)) {
         // failure to set next destination can only be because of missing terrain data
         copter.failsafe_terrain_on_event();
@@ -1558,6 +1560,21 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
     }
 }
 
+// if a DO_CHANGE_SPEED is found then process it immediately.  This
+// prevents the vehicle creating an accel segment before it processes
+// the command.
+void ModeAuto::do_change_speed_lookahead(uint16_t index)
+{
+    AP_Mission::Mission_Command cmd;
+    if (!mission.get_next_do_cmd(index, cmd)) {
+        return;
+    }
+    if (cmd.id != MAV_CMD_DO_CHANGE_SPEED) {
+        return;
+    }
+    do_change_speed(cmd);
+}
+
 // checks the next mission command and adds it as a destination if necessary
 // supports both straight line and spline waypoints
 // cmd should be the current command
@@ -1915,6 +1932,7 @@ void ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd)
 // Do (Now) commands
 /********************************************************************************/
 
+// note that we extract a lot of this same information in segment_speed
 void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd)
 {
     if (cmd.content.speed.target_ms > 0) {

Tools/autotest/arducopter.py

@@ -10007,6 +10007,71 @@ def DO_CHANGE_SPEED(self):
 
         self.wait_disarmed()
 
+    def DO_CHANGE_SPEED_SCurve(self):
+        '''make sure speed is honoured quickly'''
+        alt = 10
+        self.upload_simple_relhome_mission([
+            (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, alt),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 40, 0, alt),
+            self.create_MISSION_ITEM_INT(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, p2=1.5),
+            (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
+        ])
+        self.set_parameter('AUTO_OPTIONS', 3)
+        self.change_mode('AUTO')
+        self.wait_ready_to_arm()
+
+        self.context_push()
+        self.progress("Installing hook to watch maximum speed")
+        lg = vehicle_test_suite.TestSuite.ValidateGroundSpeed(self, 0, 3)
+        self.install_message_hook_context(lg)
+        self.arm_vehicle()
+        self.progress("Wait for vehicle to start moving")
+        self.wait_groundspeed(0.75, 10)
+        self.progress("Ensuring vehicle does not slow back down again")
+        mg = vehicle_test_suite.TestSuite.ValidateGroundSpeed(self, 0.5, 1.75)
+        self.install_message_hook_context(mg)
+
+        # RTL speeds up, so we stop watching the speed when we are entering it
+        self.wait_current_waypoint(2, timeout=10)
+        self.wait_distance_to_nav_target(10, 20, timeout=300)
+        self.context_pop()
+        self.wait_current_waypoint(4, timeout=300)
+
+        self.wait_disarmed(timeout=300)
+
+    def DO_CHANGE_SPEED_SCurveNext(self):
+        '''make sure speed is honoured quickly for waypoint past first'''
+        alt = 10
+        self.upload_simple_relhome_mission([
+            (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, alt),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 40, 0, alt),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 80, 0, alt),
+            self.create_MISSION_ITEM_INT(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, p2=1.5),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 180, 0, alt),
+            (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
+        ])
+        self.set_parameter('AUTO_OPTIONS', 3)
+        self.change_mode('AUTO')
+        self.wait_ready_to_arm()
+
+        self.context_push()
+        self.arm_vehicle()
+        # this is the 0-40m segment:
+        self.wait_groundspeed(6, 8)
+        self.progress("Ensuring vehicle does not slow back down again")
+        mg = vehicle_test_suite.TestSuite.ValidateGroundSpeed(self, 0.5, 10)
+        self.install_message_hook_context(mg)
+
+        # this is the 80-180m segment:
+        self.wait_groundspeed(1.4, 1.6)
+        self.assert_current_waypoint(3)
+
+        # RTL speeds up, so we stop watching the speed when we are entering it
+        self.wait_distance_to_nav_target(10, 20, timeout=300)
+        self.context_pop()
+
+        self.wait_disarmed(timeout=300)
+
     def AUTO_LAND_TO_BRAKE(self):
         '''ensure terrain altitude is taken into account when braking'''
         self.set_parameters({
@@ -11769,6 +11834,8 @@ def tests2b(self):  # this block currently around 9.5mins here
             self.GroundEffectCompensation_touchDownExpected,
             self.GroundEffectCompensation_takeOffExpected,
             self.DO_CHANGE_SPEED,
+            self.DO_CHANGE_SPEED_SCurve,
+            self.DO_CHANGE_SPEED_SCurveNext,
             self.MISSION_START,
             self.AUTO_LAND_TO_BRAKE,
             self.WPNAV_SPEED,

Tools/autotest/vehicle_test_suite.py

@@ -3521,6 +3521,37 @@ class ValidateAHRS3AgainstSimState(ValidateIntPositionAgainstSimState):
         def __init__(self, suite, **kwargs):
             super(TestSuite.ValidateAHRS3AgainstSimState, self).__init__(suite, 'AHRS3', **kwargs)
 
+    class ValidateRange(MessageHook):
+        '''monitors vehicle Groundspeed and snure it stays between limits'''
+        def __init__(self, suite, minimum_value, maximum_value):
+            super(TestSuite.ValidateRange, self).__init__(suite)
+            self.minimum_value = minimum_value
+            self.maximum_value = maximum_value
+            self.last_print = 0
+            self.min_print_interval = 1  # seconds
+
+        def process_value(self, value):
+            if time.time() - self.last_print > self.min_print_interval:
+                self.progress(f"{self.progress_prefix()}: groundspeed (min={self.minimum_value} <= current={value} <= {self.maximum_value})")  # noqa:501
+                self.last_print = time.time()
+
+            if value < self.minimum_value:
+                raise NotAchievedException(f"Value {value} below minimum {self.minimum_value}")
+            if value > self.maximum_value:
+                raise NotAchievedException(f"Value {value} above maximum {self.maximum_value}")
+
+        def hook_removed(self):
+            self.progress("groundspeed was maintained")
+
+    class ValidateGroundSpeed(ValidateRange):
+        def process(self, mav, m):
+            if m.get_type() != "VFR_HUD":
+                return
+            self.process_value(m.groundspeed)
+
+        def progress_prefix(self):
+            return "VGS: "
+
     def message_hook(self, mav, msg):
         """Called as each mavlink msg is received."""
 #        print("msg: %s" % str(msg))

libraries/AP_Mission/AP_Mission.h

@@ -778,6 +778,11 @@ class AP_Mission
     }
 #endif
 
+    ///     returns true if found, false if not found
+    ///     stops and returns false if it hits another navigation command before it finds the first do or conditional command
+    ///     accounts for do_jump commands but never increments the jump's num_times_run (get_next_nav_cmd is responsible for this)
+    bool get_next_do_cmd(uint16_t start_index, Mission_Command& cmd);
+
 private:
     static AP_Mission *_singleton;
 
@@ -832,11 +837,6 @@ class AP_Mission
     bool get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool increment_jump_num_times_if_found, bool send_gcs_msg = true);
 
     /// get_next_do_cmd - gets next "do" or "conditional" command after start_index
-    ///     returns true if found, false if not found
-    ///     stops and returns false if it hits another navigation command before it finds the first do or conditional command
-    ///     accounts for do_jump commands but never increments the jump's num_times_run (get_next_nav_cmd is responsible for this)
-    bool get_next_do_cmd(uint16_t start_index, Mission_Command& cmd);
-
     ///
     /// jump handling methods
     ///