/*
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
*/
void Plane::setup_landing_glide_slope(void)
{
Location loc = next_WP_loc;
// 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);
float total_distance = get_distance(prev_WP_loc, next_WP_loc);
// 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;
}
// 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
float aim_height = aparm.land_flare_sec * sink_rate;
if (aim_height <= 0) {
aim_height = g.land_flare_alt;
}
// 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;
}
// 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;
// don't allow the flare before half way along the final leg
if (flare_distance > total_distance/2) {
flare_distance = total_distance/2;
}
// now calculate our aim point, which is before the landing
// point and above it
location_update(loc, land_bearing_cd*0.01f, -flare_distance);
loc.alt += aim_height*100;
// calculate slope to landing point
float land_slope = (sink_height - aim_height) / total_distance;
// calculate point along that slope 500m ahead
location_update(loc, land_bearing_cd*0.01f, land_projection);
loc.alt -= land_slope * land_projection * 100;
// 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
float land_proportion = location_path_proportion(current_loc, prev_WP_loc, loc);
// now setup the glide slope for landing
set_target_altitude_proportion(loc, 1.0f - land_proportion);
// stay within the range of the start and end locations in altitude
constrain_target_altitude_location(loc, prev_WP_loc);
}
/*
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;
}
/*
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;
}
/*
check that we have fetched all mission terrain data
*/
void AP_Terrain::update_mission_data(void)
{
if (last_mission_change_ms != mission.last_change_time_ms() ||
last_mission_spacing != grid_spacing) {
// the mission has changed - start again
next_mission_index = 1;
next_mission_pos = 0;
last_mission_change_ms = mission.last_change_time_ms();
last_mission_spacing = grid_spacing;
}
if (next_mission_index == 0) {
// nothing to do
return;
}
uint16_t pending, loaded;
get_statistics(pending, loaded);
if (pending && ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
// wait till we have fully filled the current set of grids
return;
}
// don't do more than 20 waypoints at a time, to prevent too much
// CPU usage
for (uint8_t i=0; i<20; i++) {
// get next mission command
AP_Mission::Mission_Command cmd;
if (!mission.read_cmd_from_storage(next_mission_index, cmd)) {
// nothing more to do
next_mission_index = 0;
return;
}
// we only want nav waypoint commands. That should be enough to
// prefill the terrain data and makes many things much simpler
while ((cmd.id != MAV_CMD_NAV_WAYPOINT &&
cmd.id != MAV_CMD_NAV_SPLINE_WAYPOINT) ||
(cmd.content.location.lat == 0 && cmd.content.location.lng == 0)) {
next_mission_index++;
if (!mission.read_cmd_from_storage(next_mission_index, cmd)) {
// nothing more to do
next_mission_index = 0;
next_mission_pos = 0;
return;
}
}
// we will fetch 5 points around the waypoint. Four at 10 grid
// spacings away at 45, 135, 225 and 315 degrees, and the
// point itself
if (next_mission_pos != 4) {
location_update(cmd.content.location, 45+90*next_mission_pos, grid_spacing.get() * 10);
}
// we have a mission command to check
float height;
if (!height_amsl(cmd.content.location, height)) {
// if we can't get data for a mission item then return and
// check again next time
return;
}
next_mission_pos++;
if (next_mission_pos == 5) {
#if TERRAIN_DEBUG
hal.console->printf("checked waypoint %u\n", (unsigned)next_mission_index);
#endif
// move to next waypoint
next_mission_index++;
next_mission_pos = 0;
}
}
}
/*
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;
}
/*
update navigation for landing
*/
bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc,
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)
{
switch (stage) {
case DEEPSTALL_STAGE_FLY_TO_LANDING:
if (get_distance(current_loc, landing_point) > 2 * landing.aparm.loiter_radius) {
landing.nav_controller->update_waypoint(current_loc, landing_point);
return false;
}
stage = DEEPSTALL_STAGE_ESTIMATE_WIND;
loiter_sum_cd = 0; // reset the loiter counter
// no break
case DEEPSTALL_STAGE_ESTIMATE_WIND:
{
landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, 1);
if (!landing.nav_controller->reached_loiter_target() || (fabsf(height) > DEEPSTALL_LOITER_ALT_TOLERANCE)) {
// wait until the altitude is correct before considering a breakout
return false;
}
// only count loiter progress when within the target altitude
int32_t target_bearing = landing.nav_controller->target_bearing_cd();
int32_t delta = wrap_180_cd(target_bearing - last_target_bearing);
if (delta > 0) { // only accumulate turns in the correct direction
loiter_sum_cd += delta;
}
last_target_bearing = target_bearing;
if (loiter_sum_cd < 36000) {
// wait until we've done at least one complete loiter at the correct altitude
landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, 1);
return false;
}
stage = DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT;
loiter_sum_cd = 0; // reset the loiter counter
// no break
}
case DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT:
// rebuild the approach path if we have done less then a full circle to allow it to be
// more into the wind as the estimator continues to refine itself, and allow better
// compensation on windy days. This is limited to a single full circle though, as on
// a no wind day you could be in this loop forever otherwise.
if (loiter_sum_cd < 36000) {
build_approach_path();
}
if (!verify_breakout(current_loc, arc_entry, height)) {
int32_t target_bearing = landing.nav_controller->target_bearing_cd();
int32_t delta = wrap_180_cd(target_bearing - last_target_bearing);
if (delta > 0) { // only accumulate turns in the correct direction
loiter_sum_cd += delta;
}
last_target_bearing = target_bearing;
landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, 1);
return false;
}
stage = DEEPSTALL_STAGE_FLY_TO_ARC;
memcpy(&breakout_location, ¤t_loc, sizeof(Location));
// no break
case DEEPSTALL_STAGE_FLY_TO_ARC:
if (get_distance(current_loc, arc_entry) > 2 * landing.aparm.loiter_radius) {
landing.nav_controller->update_waypoint(breakout_location, arc_entry);
return false;
}
stage = DEEPSTALL_STAGE_ARC;
// no break
case DEEPSTALL_STAGE_ARC:
{
Vector2f groundspeed = landing.ahrs.groundspeed_vector();
if (!landing.nav_controller->reached_loiter_target() ||
(fabsf(wrap_180(target_heading_deg -
degrees(atan2f(-groundspeed.y, -groundspeed.x) + M_PI))) >= 10.0f)) {
landing.nav_controller->update_loiter(arc, landing.aparm.loiter_radius, 1);
return false;
}
stage = DEEPSTALL_STAGE_APPROACH;
}
// no break
case DEEPSTALL_STAGE_APPROACH:
{
Location entry_point;
landing.nav_controller->update_waypoint(arc_exit, extended_approach);
float relative_alt_D;
landing.ahrs.get_relative_position_D_home(relative_alt_D);
const float travel_distance = predict_travel_distance(landing.ahrs.wind_estimate(), -relative_alt_D, false);
memcpy(&entry_point, &landing_point, sizeof(Location));
location_update(entry_point, target_heading_deg + 180.0, travel_distance);
if (!location_passed_point(current_loc, arc_exit, entry_point)) {
if (location_passed_point(current_loc, arc_exit, extended_approach)) {
// this should never happen, but prevent against an indefinite fly away
stage = DEEPSTALL_STAGE_FLY_TO_LANDING;
}
return false;
}
predict_travel_distance(landing.ahrs.wind_estimate(), -relative_alt_D, true);
stage = DEEPSTALL_STAGE_LAND;
stall_entry_time = AP_HAL::millis();
const SRV_Channel* elevator = SRV_Channels::get_channel_for(SRV_Channel::k_elevator);
if (elevator != nullptr) {
// take the last used elevator angle as the starting deflection
// don't worry about bailing here if the elevator channel can't be found
// that will be handled within override_servos
initial_elevator_pwm = elevator->get_output_pwm();
}
L1_xtrack_i = 0; // reset the integrators
}
// no break
case DEEPSTALL_STAGE_LAND:
// while in deepstall the only thing verify needs to keep the extended approach point sufficently far away
landing.nav_controller->update_waypoint(current_loc, extended_approach);
return false;
default:
return true;
}
}
