Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: only do_change_speed within FBW min and max

This commit is contained in:
Iampete1 2020-10-04 19:20:47 +01:00 committed by Andrew Tridgell
parent 2acbef5579
commit 2820228ad6
3 changed files with 12 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/GCS_Mavlink.cpp
View File

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

2
ArduPlane/Plane.h
View File

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

10
ArduPlane/commands_logic.cpp
View File

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