From 2820228ad6f101063c640232f4e1d3bbaed1f945 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 4 Oct 2020 19:20:47 +0100 Subject: [PATCH] Plane: only do_change_speed within FBW min and max --- ArduPlane/GCS_Mavlink.cpp | 6 ++++-- ArduPlane/Plane.h | 2 +- ArduPlane/commands_logic.cpp | 10 +++++++--- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 93b1204a0e..73631cce6d 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -974,8 +974,10 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l } AP_Mission::Mission_Command cmd; - if (AP_Mission::mavlink_cmd_long_to_mission_cmd(packet, cmd) == MAV_MISSION_ACCEPTED) { - plane.do_change_speed(cmd); + if (AP_Mission::mavlink_cmd_long_to_mission_cmd(packet, cmd) != MAV_MISSION_ACCEPTED) { + return MAV_RESULT_DENIED; + } + if (plane.do_change_speed(cmd)) { return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 3587799722..dae31e674e 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -898,7 +898,7 @@ private: bool verify_landing_vtol_approach(const AP_Mission::Mission_Command& cmd); void do_wait_delay(const AP_Mission::Mission_Command& cmd); void do_within_distance(const AP_Mission::Mission_Command& cmd); - void do_change_speed(const AP_Mission::Mission_Command& cmd); + bool do_change_speed(const AP_Mission::Mission_Command& cmd); void do_set_home(const AP_Mission::Mission_Command& cmd); bool start_command_callback(const AP_Mission::Mission_Command &cmd); bool verify_command_callback(const AP_Mission::Mission_Command& cmd); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index abf2eeec6e..04a20ec9bb 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -886,26 +886,30 @@ void Plane::do_loiter_at_location() next_WP_loc = current_loc; } -void Plane::do_change_speed(const AP_Mission::Mission_Command& cmd) +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 > 0) { + if ((cmd.content.speed.target_ms >= aparm.airspeed_min.get()) && (cmd.content.speed.target_ms <= aparm.airspeed_max.get())) { aparm.airspeed_cruise_cm.set(cmd.content.speed.target_ms * 100); gcs().send_text(MAV_SEVERITY_INFO, "Set airspeed %u m/s", (unsigned)cmd.content.speed.target_ms); + return true; } break; case 1: // Ground speed gcs().send_text(MAV_SEVERITY_INFO, "Set groundspeed %u", (unsigned)cmd.content.speed.target_ms); aparm.min_gndspeed_cm.set(cmd.content.speed.target_ms * 100); - break; + return true; } if (cmd.content.speed.throttle_pct > 0 && cmd.content.speed.throttle_pct <= 100) { gcs().send_text(MAV_SEVERITY_INFO, "Set throttle %u", (unsigned)cmd.content.speed.throttle_pct); aparm.throttle_cruise.set(cmd.content.speed.throttle_pct); + return true; } + + return false; } void Plane::do_set_home(const AP_Mission::Mission_Command& cmd)