Copter: add look-ahead for DO_CHANGE_SPEED commands

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) {