/* update navigation for landing. Called when on landing approach or final flare */ bool Plane::verify_land() { // 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; auto_state.land_complete = false; auto_state.land_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() > auto_state.takeoff_altitude_rel_cm) { next_WP_loc = current_loc; mission.stop(); bool success = restart_landing_sequence(); mission.resume(); if (!success) { // on a restart failure lets RTL or else the plane may fly away with nowhere to go! set_mode(RTL, MODE_REASON_MISSION_END); } // make sure to return false so it leaves the mission index alone } return false; } float height = height_above_target(); // use rangefinder to correct if possible height -= rangefinder_correction(); /* 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) */ #if RANGEFINDER_ENABLED == ENABLED bool rangefinder_in_range = rangefinder_state.in_range; #else bool rangefinder_in_range = false; #endif // 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 <= g.land_flare_alt); bool below_flare_sec = (aparm.land_flare_sec > 0 && height <= auto_state.sink_rate * aparm.land_flare_sec); bool probably_crashed = (g.crash_detection_enable && fabsf(auto_state.sink_rate) < 0.2f && !is_flying()); if ((on_approach_stage && below_flare_alt) || (on_approach_stage && below_flare_sec && (auto_state.wp_proportion > 0.5)) || (!rangefinder_in_range && auto_state.wp_proportion >= 1) || probably_crashed) { if (!auto_state.land_complete) { auto_state.post_landing_stats = true; if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) { gcs_send_text_fmt(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)gps.ground_speed()); } else { gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f", (double)height, (double)auto_state.sink_rate, (double)gps.ground_speed(), (double)get_distance(current_loc, next_WP_loc)); } auto_state.land_complete = true; update_flight_stage(); } if (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. g.airspeed_cruise_cm.load(); g.min_gndspeed_cm.load(); aparm.throttle_cruise.load(); } } else if (!auto_state.land_complete && !auto_state.land_pre_flare && aparm.land_pre_flare_airspeed > 0) { bool reached_pre_flare_alt = g.land_pre_flare_alt > 0 && (height <= g.land_pre_flare_alt); bool reached_pre_flare_sec = g.land_pre_flare_sec > 0 && (height <= auto_state.sink_rate * g.land_pre_flare_sec); if (reached_pre_flare_alt || reached_pre_flare_sec) { auto_state.land_pre_flare = true; update_flight_stage(); } } /* 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 (auto_state.post_landing_stats && !arming.is_armed()) { auto_state.post_landing_stats = false; gcs_send_text_fmt(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(); /* 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; }
/* update navigation for landing. Called when on landing approach or final flare */ bool Plane::verify_land() { // 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 // If a go around has been commanded, we are done landing. This will send // the mission to the next mission item, which presumably is a mission // segment with operations to perform when a landing is called off. // If there are no commands after the land waypoint mission item then // the plane will proceed to loiter about its home point. if (auto_state.commanded_go_around) { return true; } float height = height_above_target(); // use rangefinder to correct if possible height -= rangefinder_correction(); /* 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) */ #if RANGEFINDER_ENABLED == ENABLED bool rangefinder_in_range = rangefinder_state.in_range; #else bool rangefinder_in_range = false; #endif if (height <= g.land_flare_alt || (aparm.land_flare_sec > 0 && height <= auto_state.sink_rate * aparm.land_flare_sec) || (!rangefinder_in_range && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) || (fabsf(auto_state.sink_rate) < 0.2f && !is_flying())) { if (!auto_state.land_complete) { auto_state.post_landing_stats = true; if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) { gcs_send_text_fmt(PSTR("Flare crash detected: speed=%.1f"), (double)gps.ground_speed()); } else { gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f"), (double)height, (double)auto_state.sink_rate, (double)gps.ground_speed()); } } auto_state.land_complete = true; if (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. g.airspeed_cruise_cm.load(); g.min_gndspeed_cm.load(); aparm.throttle_cruise.load(); } } /* 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 (auto_state.post_landing_stats && !arming.is_armed()) { auto_state.post_landing_stats = false; gcs_send_text_fmt(PSTR("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(); /* we return false as a landing mission item never completes we stay on this waypoint unless the GCS commands us to change mission item or reset the mission, or a go-around is commanded */ return false; }