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