Plane: Allow reseting target airspeed to the parameter value

This commit is contained in:
Michael du Breuil 2022-04-15 16:07:11 -07:00 committed by Andrew Tridgell
parent ad6bf648e9
commit b7a8852747
1 changed files with 6 additions and 2 deletions

View File

@ -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;
}