From 1535405744ccb8f7f4af0a12f85805a26ef1a04d Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 8 Aug 2024 14:47:15 +1000
Subject: [PATCH] Copter: add look-ahead for DO_CHANGE_SPEED commands

---
 ArduCopter/mode.h        |  4 ++++
 ArduCopter/mode_auto.cpp | 18 ++++++++++++++++++
 2 files changed, 22 insertions(+)

diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index 11b2a64410c4c..9637c5a511c94 100644
--- a/ArduCopter/mode.h
+++ b/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
diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
index 0da35567d39a9..81b588362f568 100644
--- a/ArduCopter/mode_auto.cpp
+++ b/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) {