void Plane::set_guided_WP(void)
{
if (g.loiter_radius < 0 || guided_WP_loc.flags.loiter_ccw) {
loiter.direction = -1;
} else {
loiter.direction = 1;
}
// copy the current location into the OldWP slot
// ---------------------------------------
prev_WP_loc = current_loc;
// Load the next_WP slot
// ---------------------
next_WP_loc = guided_WP_loc;
// used to control FBW and limit the rate of climb
// -----------------------------------------------
set_target_altitude_current();
update_flight_stage();
setup_glide_slope();
setup_turn_angle();
// reset loiter start time.
loiter.start_time_ms = 0;
// start in non-VTOL mode
auto_state.vtol_loiter = false;
loiter_angle_reset();
}
void Plane::update_alt()
{
barometer.update();
if (should_log(MASK_LOG_IMU)) {
Log_Write_Baro();
}
// calculate the sink rate.
float sink_rate;
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
sink_rate = vel.z;
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
sink_rate = gps.velocity().z;
} else {
sink_rate = -barometer.get_climb_rate();
}
// low pass the sink rate to take some of the noise out
auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
geofence_check(true);
update_flight_stage();
}
void Plane::set_guided_WP(void)
{
if (g.loiter_radius < 0) {
loiter.direction = -1;
} else {
loiter.direction = 1;
}
// copy the current location into the OldWP slot
// ---------------------------------------
prev_WP_loc = current_loc;
// Load the next_WP slot
// ---------------------
next_WP_loc = guided_WP_loc;
// used to control FBW and limit the rate of climb
// -----------------------------------------------
set_target_altitude_current();
update_flight_stage();
setup_glide_slope();
setup_turn_angle();
loiter_angle_reset();
}
void Plane::update_alt()
{
barometer.update();
if (should_log(MASK_LOG_IMU)) {
Log_Write_Baro();
}
// calculate the sink rate.
float sink_rate;
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
sink_rate = vel.z;
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
sink_rate = gps.velocity().z;
} else {
sink_rate = -barometer.get_climb_rate();
}
// low pass the sink rate to take some of the noise out
auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
geofence_check(true);
update_flight_stage();
if (auto_throttle_mode && !throttle_suppressed) {
bool is_doing_auto_land = false;
float distance_beyond_land_wp = 0;
switch (flight_stage) {
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
is_doing_auto_land = true;
if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
distance_beyond_land_wp = get_distance(current_loc, next_WP_loc);
}
break;
default:
break;
}
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
target_airspeed_cm,
flight_stage,
is_doing_auto_land,
distance_beyond_land_wp,
get_takeoff_pitch_min_cd(),
throttle_nudge,
tecs_hgt_afe(),
aerodynamic_load_factor);
if (should_log(MASK_LOG_TECS)) {
Log_Write_TECS_Tuning();
}
}
}
// exit_mission_callback - callback function called from ap-mission when the mission has completed
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
void Plane::exit_mission_callback()
{
if (control_mode == AUTO) {
gcs_send_text_fmt(PSTR("Returning to Home"));
memset(&auto_rtl_command, 0, sizeof(auto_rtl_command));
auto_rtl_command.content.location =
rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
auto_rtl_command.id = MAV_CMD_NAV_LOITER_UNLIM;
setup_terrain_target_alt(auto_rtl_command.content.location);
update_flight_stage();
setup_glide_slope();
setup_turn_angle();
start_command(auto_rtl_command);
}
}
void Plane::update_alt()
{
barometer.update();
// calculate the sink rate.
float sink_rate;
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
sink_rate = vel.z;
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
sink_rate = gps.velocity().z;
} else {
sink_rate = -barometer.get_climb_rate();
}
// low pass the sink rate to take some of the noise out
auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
geofence_check(true);
update_flight_stage();
if (auto_throttle_mode && !throttle_suppressed) {
float distance_beyond_land_wp = 0;
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
distance_beyond_land_wp = get_distance(current_loc, next_WP_loc);
}
bool soaring_active = false;
#if SOARING_ENABLED == ENABLED
if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) {
soaring_active = true;
}
#endif
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
target_airspeed_cm,
flight_stage,
distance_beyond_land_wp,
get_takeoff_pitch_min_cd(),
throttle_nudge,
tecs_hgt_afe(),
aerodynamic_load_factor,
soaring_active);
}
}
/*
setup for a gradual glide slope to the next waypoint, if appropriate
*/
void Plane::setup_glide_slope(void)
{
// establish the distance we are travelling to the next waypoint,
// for calculating out rate of change of altitude
auto_state.wp_distance = current_loc.get_distance(next_WP_loc);
auto_state.wp_proportion = location_path_proportion(current_loc,
prev_WP_loc, next_WP_loc);
SpdHgt_Controller->set_path_proportion(auto_state.wp_proportion);
update_flight_stage();
/*
work out if we will gradually change altitude, or try to get to
the new altitude as quickly as possible.
*/
switch (control_mode) {
case RTL:
case AVOID_ADSB:
case GUIDED:
/* glide down slowly if above target altitude, but ascend more
rapidly if below it. See
https://github.com/ArduPilot/ardupilot/issues/39
*/
if (above_location_current(next_WP_loc)) {
set_offset_altitude_location(next_WP_loc);
} else {
reset_offset_altitude();
}
break;
case AUTO:
// we only do glide slide handling in AUTO when above 20m or
// when descending. The 20 meter threshold is arbitrary, and
// is basically to prevent situations where we try to slowly
// gain height at low altitudes, potentially hitting
// obstacles.
if (adjusted_relative_altitude_cm() > 2000 || above_location_current(next_WP_loc)) {
set_offset_altitude_location(next_WP_loc);
} else {
reset_offset_altitude();
}
break;
default:
reset_offset_altitude();
break;
}
}
/*
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 Plane::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_send_text_fmt(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_send_text_fmt(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_send_text_fmt(MAV_SEVERITY_NOTICE, "Restarted landing sequence at waypoint %d", prev_cmd_with_wp_index);
success = true;
} else {
gcs_send_text_fmt(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();
}
return success;
}
void Plane::update_alt()
{
barometer.update();
if (should_log(MASK_LOG_IMU)) {
Log_Write_Baro();
}
// calculate the sink rate.
float sink_rate;
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
sink_rate = vel.z;
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
sink_rate = gps.velocity().z;
} else {
sink_rate = -barometer.get_climb_rate();
}
// low pass the sink rate to take some of the noise out
auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
geofence_check(true);
update_flight_stage();
if (auto_throttle_mode && !throttle_suppressed) {
float distance_beyond_land_wp = 0;
if (auto_state.land_in_progress && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
distance_beyond_land_wp = get_distance(current_loc, next_WP_loc);
}
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
target_airspeed_cm,
flight_stage,
auto_state.land_in_progress,
distance_beyond_land_wp,
get_takeoff_pitch_min_cd(),
throttle_nudge,
tecs_hgt_afe(),
aerodynamic_load_factor);
}
}
void Plane::do_RTL(void)
{
auto_state.next_wp_no_crosstrack = true;
auto_state.no_crosstrack = true;
prev_WP_loc = current_loc;
next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
setup_terrain_target_alt(next_WP_loc);
set_target_altitude_location(next_WP_loc);
if (g.loiter_radius < 0) {
loiter.direction = -1;
} else {
loiter.direction = 1;
}
update_flight_stage();
setup_glide_slope();
setup_turn_angle();
if (should_log(MASK_LOG_MODE))
DataFlash.Log_Write_Mode(control_mode);
}
/*
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;
}
