2015-05-13 03:09:36 -03:00
|
|
|
#include "Plane.h"
|
|
|
|
|
2014-08-11 22:21:35 -03:00
|
|
|
/*
|
|
|
|
landing logic
|
|
|
|
*/
|
|
|
|
|
|
|
|
/*
|
|
|
|
update navigation for landing. Called when on landing approach or
|
|
|
|
final flare
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
bool Plane::verify_land()
|
2014-08-11 22:21:35 -03:00
|
|
|
{
|
|
|
|
// 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
|
|
|
|
|
2015-05-09 13:36:40 -03:00
|
|
|
// 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;
|
2016-01-30 03:22:21 -04:00
|
|
|
auto_state.land_pre_flare = false;
|
2015-05-09 13:36:40 -03:00
|
|
|
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!
|
2016-08-13 04:54:37 -03:00
|
|
|
set_mode(RTL, MODE_REASON_MISSION_END);
|
2015-05-09 13:36:40 -03:00
|
|
|
}
|
|
|
|
// make sure to return false so it leaves the mission index alone
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2014-08-11 22:21:35 -03:00
|
|
|
float height = height_above_target();
|
|
|
|
|
2014-08-26 08:18:24 -03:00
|
|
|
// use rangefinder to correct if possible
|
|
|
|
height -= rangefinder_correction();
|
|
|
|
|
2014-08-26 08:15:43 -03:00
|
|
|
/* Set land_complete (which starts the flare) under 3 conditions:
|
2014-08-11 22:21:35 -03:00
|
|
|
1) we are within LAND_FLARE_ALT meters of the landing altitude
|
|
|
|
2) we are within LAND_FLARE_SEC of the landing point vertically
|
2015-04-06 12:56:29 -03:00
|
|
|
by the calculated sink rate (if LAND_FLARE_SEC != 0)
|
2014-08-27 01:33:57 -03:00
|
|
|
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)
|
2014-08-11 22:21:35 -03:00
|
|
|
*/
|
2015-05-13 20:00:11 -03:00
|
|
|
#if RANGEFINDER_ENABLED == ENABLED
|
|
|
|
bool rangefinder_in_range = rangefinder_state.in_range;
|
|
|
|
#else
|
|
|
|
bool rangefinder_in_range = false;
|
|
|
|
#endif
|
2016-04-14 23:44:17 -03:00
|
|
|
|
2016-04-21 20:57:12 -03:00
|
|
|
// 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)
|
2016-04-21 20:56:50 -03:00
|
|
|
|
|
|
|
bool on_approach_stage = (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
|
|
|
|
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE);
|
2016-04-21 20:46:27 -03:00
|
|
|
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);
|
2016-08-22 23:19:03 -03:00
|
|
|
bool probably_crashed = (g.crash_detection_enable && fabsf(auto_state.sink_rate) < 0.2f && !is_flying());
|
2016-04-21 20:46:27 -03:00
|
|
|
|
2016-04-21 20:56:50 -03:00
|
|
|
if ((on_approach_stage && below_flare_alt) ||
|
|
|
|
(on_approach_stage && below_flare_sec && (auto_state.wp_proportion > 0.5)) ||
|
2016-04-21 20:46:27 -03:00
|
|
|
(!rangefinder_in_range && auto_state.wp_proportion >= 1) ||
|
|
|
|
probably_crashed) {
|
2014-08-11 22:21:35 -03:00
|
|
|
|
|
|
|
if (!auto_state.land_complete) {
|
2015-08-17 18:14:10 -03:00
|
|
|
auto_state.post_landing_stats = true;
|
2015-05-13 19:05:32 -03:00
|
|
|
if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) {
|
2015-11-03 23:52:54 -04:00
|
|
|
gcs_send_text_fmt(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)gps.ground_speed());
|
2015-03-15 20:59:40 -03:00
|
|
|
} else {
|
2015-11-03 23:52:54 -04:00
|
|
|
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f",
|
2015-09-15 22:14:26 -03:00
|
|
|
(double)height, (double)auto_state.sink_rate,
|
|
|
|
(double)gps.ground_speed(),
|
|
|
|
(double)get_distance(current_loc, next_WP_loc));
|
2015-03-15 20:59:40 -03:00
|
|
|
}
|
2016-01-30 03:22:21 -04:00
|
|
|
auto_state.land_complete = true;
|
|
|
|
update_flight_stage();
|
2014-08-11 22:21:35 -03:00
|
|
|
}
|
2016-01-30 03:22:21 -04:00
|
|
|
|
2014-08-11 22:21:35 -03:00
|
|
|
|
|
|
|
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();
|
|
|
|
}
|
2016-01-30 03:22:21 -04:00
|
|
|
} 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();
|
|
|
|
}
|
2014-08-11 22:21:35 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
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);
|
|
|
|
|
2015-08-17 18:14:10 -03:00
|
|
|
// 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;
|
2015-11-03 23:52:54 -04:00
|
|
|
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)get_distance(current_loc, next_WP_loc));
|
2015-08-17 18:14:10 -03:00
|
|
|
}
|
|
|
|
|
2015-03-11 20:44:10 -03:00
|
|
|
// check if we should auto-disarm after a confirmed landing
|
|
|
|
disarm_if_autoland_complete();
|
|
|
|
|
2014-08-11 22:21:35 -03:00
|
|
|
/*
|
2014-10-24 08:04:59 -03:00
|
|
|
we return false as a landing mission item never completes
|
|
|
|
|
|
|
|
we stay on this waypoint unless the GCS commands us to change
|
2015-05-09 13:36:40 -03:00
|
|
|
mission item, reset the mission, command a go-around or finish
|
|
|
|
a land_abort procedure.
|
2014-08-11 22:21:35 -03:00
|
|
|
*/
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2015-03-11 20:44:10 -03:00
|
|
|
/*
|
|
|
|
If land_DisarmDelay is enabled (non-zero), check for a landing then auto-disarm after time expires
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::disarm_if_autoland_complete()
|
2015-03-11 20:44:10 -03:00
|
|
|
{
|
|
|
|
if (g.land_disarm_delay > 0 &&
|
|
|
|
auto_state.land_complete &&
|
|
|
|
!is_flying() &&
|
|
|
|
arming.arming_required() != AP_Arming::NO &&
|
|
|
|
arming.is_armed()) {
|
|
|
|
/* we have auto disarm enabled. See if enough time has passed */
|
2015-05-13 19:05:32 -03:00
|
|
|
if (millis() - auto_state.last_flying_ms >= g.land_disarm_delay*1000UL) {
|
2015-03-11 20:44:10 -03:00
|
|
|
if (disarm_motors()) {
|
2015-11-18 15:17:50 -04:00
|
|
|
gcs_send_text(MAV_SEVERITY_INFO,"Auto disarmed");
|
2015-03-11 20:44:10 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-01-06 22:30:41 -04:00
|
|
|
void Plane::adjust_landing_slope_for_rangefinder_bump(void)
|
|
|
|
{
|
|
|
|
#if RANGEFINDER_ENABLED == ENABLED
|
|
|
|
// check the rangefinder correction for a large change. When found, recalculate the glide slope. This is done by
|
|
|
|
// determining the slope from your current location to the land point then following that back up to the approach
|
|
|
|
// altitude and moving the prev_wp to that location. From there
|
|
|
|
float correction_delta = fabsf(rangefinder_state.last_stable_correction) - fabsf(rangefinder_state.correction);
|
|
|
|
|
|
|
|
if (g.land_slope_recalc_shallow_threshold <= 0 ||
|
|
|
|
fabsf(correction_delta) < g.land_slope_recalc_shallow_threshold) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
rangefinder_state.last_stable_correction = rangefinder_state.correction;
|
2015-03-11 20:44:10 -03:00
|
|
|
|
2016-01-06 22:30:41 -04:00
|
|
|
float corrected_alt_m = (adjusted_altitude_cm() - next_WP_loc.alt)*0.01f - rangefinder_state.correction;
|
|
|
|
float total_distance_m = get_distance(prev_WP_loc, next_WP_loc);
|
|
|
|
float top_of_glide_slope_alt_m = total_distance_m * corrected_alt_m / auto_state.wp_distance;
|
|
|
|
prev_WP_loc.alt = top_of_glide_slope_alt_m*100 + next_WP_loc.alt;
|
|
|
|
|
2016-05-14 03:04:38 -03:00
|
|
|
// re-calculate auto_state.land_slope with updated prev_WP_loc
|
2016-01-06 22:30:41 -04:00
|
|
|
setup_landing_glide_slope();
|
2016-07-18 22:56:59 -03:00
|
|
|
|
2016-05-14 03:09:58 -03:00
|
|
|
if (rangefinder_state.correction >= 0) { // we're too low or object is below us
|
|
|
|
// correction positive means we're too low so we should continue on with
|
|
|
|
// the newly computed shallower slope instead of pitching/throttling up
|
|
|
|
|
|
|
|
} else if (g.land_slope_recalc_steep_threshold_to_abort > 0) {
|
|
|
|
// correction negative means we're too high and need to point down (and speed up) to re-align
|
|
|
|
// to land on target. A large negative correction means we would have to dive down a lot and will
|
|
|
|
// generating way too much speed that we can not bleed off in time. It is better to remember
|
|
|
|
// the large baro altitude offset and abort the landing to come around again with the correct altitude
|
|
|
|
// offset and "perfect" slope.
|
|
|
|
|
|
|
|
// calculate projected slope with projected alt
|
2016-07-18 22:56:59 -03:00
|
|
|
float new_slope_deg = degrees(atan(auto_state.land_slope));
|
2016-05-14 03:09:58 -03:00
|
|
|
float initial_slope_deg = degrees(atan(auto_state.initial_land_slope));
|
|
|
|
|
|
|
|
// is projected slope too steep?
|
|
|
|
if (new_slope_deg - initial_slope_deg > g.land_slope_recalc_steep_threshold_to_abort) {
|
2016-07-19 20:09:21 -03:00
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Steep landing slope (%.0fm %.1fdeg)",
|
|
|
|
(double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg));
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "aborting landing!");
|
|
|
|
auto_state.land_alt_offset = rangefinder_state.correction;
|
2016-05-14 03:09:58 -03:00
|
|
|
auto_state.commanded_go_around = 1;
|
|
|
|
g.land_slope_recalc_steep_threshold_to_abort = 0; // disable this feature so we only perform it once
|
|
|
|
}
|
|
|
|
}
|
2016-01-06 22:30:41 -04:00
|
|
|
#endif
|
|
|
|
}
|
2014-08-27 01:33:57 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
a special glide slope calculation for the landing approach
|
|
|
|
|
|
|
|
During the land approach use a linear glide slope to a point
|
|
|
|
projected through the landing point. We don't use the landing point
|
|
|
|
itself as that leads to discontinuities close to the landing point,
|
|
|
|
which can lead to erratic pitch control
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::setup_landing_glide_slope(void)
|
2014-08-27 01:33:57 -03:00
|
|
|
{
|
2014-12-15 06:44:59 -04:00
|
|
|
float total_distance = get_distance(prev_WP_loc, next_WP_loc);
|
|
|
|
|
2015-08-02 20:27:59 -03:00
|
|
|
// If someone mistakenly puts all 0's in their LAND command then total_distance
|
|
|
|
// will be calculated as 0 and cause a divide by 0 error below. Lets avoid that.
|
|
|
|
if (total_distance < 1) {
|
|
|
|
total_distance = 1;
|
|
|
|
}
|
|
|
|
|
2014-12-15 06:44:59 -04:00
|
|
|
// height we need to sink for this WP
|
|
|
|
float sink_height = (prev_WP_loc.alt - next_WP_loc.alt)*0.01f;
|
|
|
|
|
|
|
|
// current ground speed
|
|
|
|
float groundspeed = ahrs.groundspeed();
|
|
|
|
if (groundspeed < 0.5f) {
|
|
|
|
groundspeed = 0.5f;
|
|
|
|
}
|
|
|
|
|
|
|
|
// calculate time to lose the needed altitude
|
|
|
|
float sink_time = total_distance / groundspeed;
|
|
|
|
if (sink_time < 0.5f) {
|
|
|
|
sink_time = 0.5f;
|
|
|
|
}
|
|
|
|
|
|
|
|
// find the sink rate needed for the target location
|
|
|
|
float sink_rate = sink_height / sink_time;
|
|
|
|
|
|
|
|
// the height we aim for is the one to give us the right flare point
|
2015-03-14 23:53:08 -03:00
|
|
|
float aim_height = aparm.land_flare_sec * sink_rate;
|
2015-04-29 12:56:20 -03:00
|
|
|
if (aim_height <= 0) {
|
|
|
|
aim_height = g.land_flare_alt;
|
|
|
|
}
|
|
|
|
|
2015-02-03 01:44:16 -04:00
|
|
|
// don't allow the aim height to be too far above LAND_FLARE_ALT
|
|
|
|
if (g.land_flare_alt > 0 && aim_height > g.land_flare_alt*2) {
|
|
|
|
aim_height = g.land_flare_alt*2;
|
|
|
|
}
|
|
|
|
|
2016-05-14 03:03:22 -03:00
|
|
|
// calculate slope to landing point
|
|
|
|
bool is_first_calc = is_zero(auto_state.land_slope);
|
|
|
|
auto_state.land_slope = (sink_height - aim_height) / total_distance;
|
|
|
|
if (is_first_calc) {
|
|
|
|
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing glide slope %.1f degrees", (double)degrees(atanf(auto_state.land_slope)));
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2014-12-15 06:44:59 -04:00
|
|
|
// time before landing that we will flare
|
|
|
|
float flare_time = aim_height / SpdHgt_Controller->get_land_sinkrate();
|
|
|
|
|
|
|
|
// distance to flare is based on ground speed, adjusted as we
|
|
|
|
// get closer. This takes into account the wind
|
|
|
|
float flare_distance = groundspeed * flare_time;
|
2015-02-03 01:44:16 -04:00
|
|
|
|
|
|
|
// don't allow the flare before half way along the final leg
|
|
|
|
if (flare_distance > total_distance/2) {
|
|
|
|
flare_distance = total_distance/2;
|
|
|
|
}
|
2014-12-15 06:44:59 -04:00
|
|
|
|
2016-05-14 03:03:22 -03:00
|
|
|
// project a point 500 meters past the landing point, passing
|
|
|
|
// through the landing point
|
|
|
|
const float land_projection = 500;
|
|
|
|
int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
|
|
|
|
|
2014-12-15 06:44:59 -04:00
|
|
|
// now calculate our aim point, which is before the landing
|
|
|
|
// point and above it
|
2016-05-14 03:03:22 -03:00
|
|
|
Location loc = next_WP_loc;
|
2014-12-15 06:44:59 -04:00
|
|
|
location_update(loc, land_bearing_cd*0.01f, -flare_distance);
|
|
|
|
loc.alt += aim_height*100;
|
|
|
|
|
|
|
|
// calculate point along that slope 500m ahead
|
2014-08-27 01:33:57 -03:00
|
|
|
location_update(loc, land_bearing_cd*0.01f, land_projection);
|
2016-05-14 03:03:22 -03:00
|
|
|
loc.alt -= auto_state.land_slope * land_projection * 100;
|
2014-08-27 01:33:57 -03:00
|
|
|
|
|
|
|
// setup the offset_cm for set_target_altitude_proportion()
|
|
|
|
target_altitude.offset_cm = loc.alt - prev_WP_loc.alt;
|
|
|
|
|
|
|
|
// calculate the proportion we are to the target
|
2015-01-01 00:17:45 -04:00
|
|
|
float land_proportion = location_path_proportion(current_loc, prev_WP_loc, loc);
|
2014-08-27 01:33:57 -03:00
|
|
|
|
|
|
|
// now setup the glide slope for landing
|
2015-01-01 00:17:45 -04:00
|
|
|
set_target_altitude_proportion(loc, 1.0f - land_proportion);
|
2014-08-27 01:33:57 -03:00
|
|
|
|
|
|
|
// stay within the range of the start and end locations in altitude
|
|
|
|
constrain_target_altitude_location(loc, prev_WP_loc);
|
|
|
|
}
|
2014-10-24 00:08:20 -03:00
|
|
|
|
2015-05-09 13:36:40 -03:00
|
|
|
/*
|
|
|
|
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.
|
|
|
|
*/
|
2015-05-09 13:36:40 -03:00
|
|
|
bool Plane::restart_landing_sequence()
|
2015-05-09 13:36:40 -03:00
|
|
|
{
|
|
|
|
if (mission.get_current_nav_cmd().id != MAV_CMD_NAV_LAND) {
|
2015-05-09 13:36:40 -03:00
|
|
|
return false;
|
2015-05-09 13:36:40 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
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();
|
2015-05-09 13:36:40 -03:00
|
|
|
bool success = false;
|
2015-08-24 07:15:42 -03:00
|
|
|
uint16_t current_index = mission.get_current_nav_index();
|
|
|
|
AP_Mission::Mission_Command cmd;
|
2015-05-09 13:36:40 -03:00
|
|
|
|
2015-08-24 07:15:42 -03:00
|
|
|
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
|
2015-11-18 15:17:50 -04:00
|
|
|
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Restarted landing sequence. Climbing to %dm", cmd.content.location.alt/100);
|
2015-08-24 07:15:42 -03:00
|
|
|
success = true;
|
|
|
|
}
|
|
|
|
else if (do_land_start_index != 0 &&
|
|
|
|
mission.set_current_cmd(do_land_start_index))
|
2015-05-09 13:36:40 -03:00
|
|
|
{
|
|
|
|
// look for a DO_LAND_START and use that index
|
2015-11-03 23:52:54 -04:00
|
|
|
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Restarted landing via DO_LAND_START: %d",do_land_start_index);
|
2015-05-09 13:36:40 -03:00
|
|
|
success = true;
|
2015-08-24 07:15:42 -03:00
|
|
|
}
|
|
|
|
else if (prev_cmd_with_wp_index != AP_MISSION_CMD_INDEX_NONE &&
|
2015-05-09 13:36:40 -03:00
|
|
|
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
|
2015-11-03 23:52:54 -04:00
|
|
|
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Restarted landing sequence at waypoint %d", prev_cmd_with_wp_index);
|
2015-05-09 13:36:40 -03:00
|
|
|
success = true;
|
|
|
|
} else {
|
2015-11-18 15:17:50 -04:00
|
|
|
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Unable to restart landing sequence");
|
2015-05-09 13:36:40 -03:00
|
|
|
success = false;
|
2015-05-09 13:36:40 -03:00
|
|
|
}
|
2016-04-22 02:53:55 -03:00
|
|
|
|
|
|
|
if (success) {
|
|
|
|
// exit landing stages if we're no longer executing NAV_LAND
|
|
|
|
update_flight_stage();
|
|
|
|
}
|
2015-05-09 13:36:40 -03:00
|
|
|
return success;
|
2015-05-09 13:36:40 -03:00
|
|
|
}
|
|
|
|
|
2014-10-24 07:50:18 -03:00
|
|
|
/*
|
|
|
|
find the nearest landing sequence starting point (DO_LAND_START) and
|
|
|
|
switch to that mission item. Returns false if no DO_LAND_START
|
|
|
|
available.
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
bool Plane::jump_to_landing_sequence(void)
|
2014-10-24 07:50:18 -03:00
|
|
|
{
|
2014-10-24 00:08:20 -03:00
|
|
|
uint16_t land_idx = mission.get_landing_sequence_start();
|
|
|
|
if (land_idx != 0) {
|
|
|
|
if (mission.set_current_cmd(land_idx)) {
|
2016-08-13 04:54:37 -03:00
|
|
|
|
|
|
|
// in case we're in RTL
|
|
|
|
set_mode(AUTO, MODE_REASON_UNKNOWN);
|
2014-10-24 07:51:27 -03:00
|
|
|
|
2014-10-24 00:08:20 -03:00
|
|
|
//if the mission has ended it has to be restarted
|
|
|
|
if (mission.state() == AP_Mission::MISSION_STOPPED) {
|
|
|
|
mission.resume();
|
|
|
|
}
|
|
|
|
|
2015-11-18 15:17:50 -04:00
|
|
|
gcs_send_text(MAV_SEVERITY_INFO, "Landing sequence start");
|
2014-10-24 00:08:20 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-11-18 15:17:50 -04:00
|
|
|
gcs_send_text(MAV_SEVERITY_WARNING, "Unable to start landing sequence");
|
2014-10-24 00:08:20 -03:00
|
|
|
return false;
|
|
|
|
}
|
2015-03-14 23:53:08 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
the height above field elevation that we pass to TECS
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
float Plane::tecs_hgt_afe(void)
|
2015-03-14 23:53:08 -03:00
|
|
|
{
|
|
|
|
/*
|
|
|
|
pass the height above field elevation as the height above
|
|
|
|
the ground when in landing, which means that TECS gets the
|
|
|
|
rangefinder information and thus can know when the flare is
|
|
|
|
coming.
|
|
|
|
*/
|
|
|
|
float hgt_afe;
|
|
|
|
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL ||
|
2016-01-30 02:32:42 -04:00
|
|
|
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
|
2015-03-14 23:53:08 -03:00
|
|
|
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
|
|
|
|
hgt_afe = height_above_target();
|
|
|
|
hgt_afe -= rangefinder_correction();
|
|
|
|
} else {
|
|
|
|
// when in normal flight we pass the hgt_afe as relative
|
|
|
|
// altitude to home
|
|
|
|
hgt_afe = relative_altitude();
|
|
|
|
}
|
|
|
|
return hgt_afe;
|
|
|
|
}
|