Browse Source

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
apm_2208
Andrew Tridgell 3 years ago
parent
commit
3201ecd381
  1. 24
      ArduPlane/AP_Arming.cpp

24
ArduPlane/AP_Arming.cpp

@ -392,5 +392,29 @@ bool AP_Arming_Plane::mission_checks(bool report) @@ -392,5 +392,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;
}

Loading…
Cancel
Save