bool AP_Landing::verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc,
const int32_t auto_state_takeoff_altitude_rel_cm, bool &throttle_suppressed)
{
switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE:
type_slope_verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed);
break;
case TYPE_DEEPSTALL:
deepstall.verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed);
break;
default:
break;
}
// see if we have reached abort altitude
if (adjusted_relative_altitude_cm_fn() > auto_state_takeoff_altitude_rel_cm) {
next_WP_loc = current_loc;
mission.stop();
if (restart_landing_sequence()) {
mission.resume();
}
// else we're in AUTO with a stopped mission and handle_auto_mode() will set RTL
}
log();
// make sure to always return false so it leaves the mission index alone
return false;
}
/*
update navigation for landing. Called when on landing approach or
final flare
*/
bool AP_Landing::type_slope_verify_land(const AP_SpdHgtControl::FlightStage flight_stage, const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc,
const int32_t auto_state_takeoff_altitude_rel_cm, const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms, const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range, bool &throttle_suppressed)
{
// we don't 'verify' landing in the sense that it never completes,
// so we don't verify command completion. Instead we use this to
// adjust final landing parameters
// when aborting a landing, mimic the verify_takeoff with steering hold. Once
// the altitude has been reached, restart the landing sequence
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
throttle_suppressed = false;
complete = false;
pre_flare = false;
nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc));
// see if we have reached abort altitude
if (adjusted_relative_altitude_cm_fn() > auto_state_takeoff_altitude_rel_cm) {
next_WP_loc = current_loc;
mission.stop();
if (restart_landing_sequence()) {
mission.resume();
}
// else we're in AUTO with a stopped mission and handle_auto_mode() will set RTL
}
// make sure to return false so it leaves the mission index alone
return false;
}
/* Set land_complete (which starts the flare) under 3 conditions:
1) we are within LAND_FLARE_ALT meters of the landing altitude
2) we are within LAND_FLARE_SEC of the landing point vertically
by the calculated sink rate (if LAND_FLARE_SEC != 0)
3) we have gone past the landing point and don't have
rangefinder data (to prevent us keeping throttle on
after landing if we've had positive baro drift)
*/
// flare check:
// 1) below flare alt/sec requires approach stage check because if sec/alt are set too
// large, and we're on a hard turn to line up for approach, we'll prematurely flare by
// skipping approach phase and the extreme roll limits will make it hard to line up with runway
// 2) passed land point and don't have an accurate AGL
// 3) probably crashed (ensures motor gets turned off)
bool on_approach_stage = (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE);
bool below_flare_alt = (height <= flare_alt);
bool below_flare_sec = (flare_sec > 0 && height <= sink_rate * flare_sec);
bool probably_crashed = (aparm.crash_detection_enable && fabsf(sink_rate) < 0.2f && !is_flying);
if ((on_approach_stage && below_flare_alt) ||
(on_approach_stage && below_flare_sec && (wp_proportion > 0.5)) ||
(!rangefinder_state_in_range && wp_proportion >= 1) ||
probably_crashed) {
if (!complete) {
post_stats = true;
if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)ahrs.get_gps().ground_speed());
} else {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f",
(double)height, (double)sink_rate,
(double)ahrs.get_gps().ground_speed(),
(double)get_distance(current_loc, next_WP_loc));
}
complete = true;
update_flight_stage_fn();
}
if (ahrs.get_gps().ground_speed() < 3) {
// reload any airspeed or groundspeed parameters that may have
// been set for landing. We don't do this till ground
// speed drops below 3.0 m/s as otherwise we will change
// target speeds too early.
aparm.airspeed_cruise_cm.load();
aparm.min_gndspeed_cm.load();
aparm.throttle_cruise.load();
}
} else if (!complete && !pre_flare && pre_flare_airspeed > 0) {
bool reached_pre_flare_alt = pre_flare_alt > 0 && (height <= pre_flare_alt);
bool reached_pre_flare_sec = pre_flare_sec > 0 && (height <= sink_rate * pre_flare_sec);
if (reached_pre_flare_alt || reached_pre_flare_sec) {
pre_flare = true;
update_flight_stage_fn();
}
}
/*
when landing we keep the L1 navigation waypoint 200m ahead. This
prevents sudden turns if we overshoot the landing point
*/
struct Location land_WP_loc = next_WP_loc;
int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
location_update(land_WP_loc,
land_bearing_cd*0.01f,
get_distance(prev_WP_loc, current_loc) + 200);
nav_controller->update_waypoint(prev_WP_loc, land_WP_loc);
// once landed and stationary, post some statistics
// this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm
if (post_stats && !is_armed) {
post_stats = false;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)get_distance(current_loc, next_WP_loc));
}
// check if we should auto-disarm after a confirmed landing
disarm_if_autoland_complete_fn();
/*
we return false as a landing mission item never completes
we stay on this waypoint unless the GCS commands us to change
mission item, reset the mission, command a go-around or finish
a land_abort procedure.
*/
return false;
}
