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

Plane: added an arming check for VTOL land too short 

this is meant to catch bad mission setup, especially for UGCS, which
planes waypoints right on top of the landing point
This commit is contained in:
Andrew Tridgell 2022-07-30 12:44:18 +10:00 committed by Randy Mackay
parent c52b64a78a
commit d4300a8202
1 changed files with 24 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ArduPlane/AP_Arming.cpp
View File

@ -391,5 +391,29 @@ bool AP_Arming_Plane::mission_checks(bool report)
ret = false;
check_failed(ARMING_CHECK_MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled");
}
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.available()) {
const uint16_t num_commands = plane.mission.num_commands();
AP_Mission::Mission_Command prev_cmd;
for (uint16_t i=1; i<num_commands; i++) {
AP_Mission::Mission_Command cmd;
if (!plane.mission.read_cmd_from_storage(i, cmd)) {
break;
}
if ((cmd.id == MAV_CMD_NAV_VTOL_LAND || cmd.id == MAV_CMD_NAV_LAND) &&
prev_cmd.id == MAV_CMD_NAV_WAYPOINT) {
const float dist = cmd.content.location.get_distance(prev_cmd.content.location);
const float tecs_land_speed = plane.TECS_controller.get_land_airspeed();
const float landing_speed = is_positive(tecs_land_speed)?tecs_land_speed:plane.aparm.airspeed_cruise_cm*0.01;
const float min_dist = 0.75 * plane.quadplane.stopping_distance(sq(landing_speed));
if (dist < min_dist) {
ret = false;
check_failed(ARMING_CHECK_MISSION, report, "VTOL land too short, min %.0fm", min_dist);
}
}
prev_cmd = cmd;
}
}
#endif
return ret;
}