/* Restart a landing by first checking for a DO_LAND_START and jump there. Otherwise decrement waypoint so we would re-start from the top with same glide slope. Return true if successful. */ bool AP_Landing::restart_landing_sequence() { if (mission.get_current_nav_cmd().id != MAV_CMD_NAV_LAND) { return false; } uint16_t do_land_start_index = mission.get_landing_sequence_start(); uint16_t prev_cmd_with_wp_index = mission.get_prev_nav_cmd_with_wp_index(); bool success = false; uint16_t current_index = mission.get_current_nav_index(); AP_Mission::Mission_Command cmd; if (mission.read_cmd_from_storage(current_index+1,cmd) && cmd.id == MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT && (cmd.p1 == 0 || cmd.p1 == 1) && mission.set_current_cmd(current_index+1)) { // if the next immediate command is MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT to climb, do it GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_NOTICE, "Restarted landing sequence. Climbing to %dm", cmd.content.location.alt/100); success = true; } else if (do_land_start_index != 0 && mission.set_current_cmd(do_land_start_index)) { // look for a DO_LAND_START and use that index GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_NOTICE, "Restarted landing via DO_LAND_START: %d",do_land_start_index); success = true; } else if (prev_cmd_with_wp_index != AP_MISSION_CMD_INDEX_NONE && mission.set_current_cmd(prev_cmd_with_wp_index)) { // if a suitable navigation waypoint was just executed, one that contains lat/lng/alt, then // repeat that cmd to restart the landing from the top of approach to repeat intended glide slope GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_NOTICE, "Restarted landing sequence at waypoint %d", prev_cmd_with_wp_index); success = true; } else { GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "Unable to restart landing sequence"); success = false; } if (success) { // exit landing stages if we're no longer executing NAV_LAND update_flight_stage_fn(); } log(); return success; }
/* 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; }