From d30891e3d155c97ea32ab9a3af5a2350251ad04d Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Fri, 15 Apr 2022 16:07:11 -0700 Subject: [PATCH] Plane: Allow reseting target airspeed to the parameter value --- ArduPlane/commands_logic.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 4465ca392b..14cd542c4c 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -937,8 +937,12 @@ bool Plane::do_change_speed(const AP_Mission::Mission_Command& cmd) switch (cmd.content.speed.speed_type) { case 0: // Airspeed - if ((cmd.content.speed.target_ms >= aparm.airspeed_min.get()) && (cmd.content.speed.target_ms <= aparm.airspeed_max.get())) { - new_airspeed_cm = cmd.content.speed.target_ms * 100; //new airspeed target for AUTO or GUIDED modes + if (is_equal(cmd.content.speed.target_ms, -2.0f)) { + new_airspeed_cm = -1; // return to default airspeed + return true; + } else if ((cmd.content.speed.target_ms >= aparm.airspeed_min.get()) && + (cmd.content.speed.target_ms <= aparm.airspeed_max.get())) { + new_airspeed_cm = cmd.content.speed.target_ms * 100; //new airspeed target for AUTO or GUIDED modes gcs().send_text(MAV_SEVERITY_INFO, "Set airspeed %u m/s", (unsigned)cmd.content.speed.target_ms); return true; }