/*
* get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off
*/
int16_t Plane::get_takeoff_pitch_min_cd(void)
{
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF) {
return auto_state.takeoff_pitch_cd;
}
int32_t relative_alt_cm = adjusted_relative_altitude_cm();
int32_t remaining_height_to_target_cm = (auto_state.takeoff_altitude_rel_cm - relative_alt_cm);
// seconds to target alt method
if (g.takeoff_pitch_limit_reduction_sec > 0) {
// if height-below-target has been initialized then use it to create and apply a scaler to the pitch_min
if (auto_state.height_below_takeoff_to_level_off_cm != 0) {
float scalar = remaining_height_to_target_cm / (float)auto_state.height_below_takeoff_to_level_off_cm;
return auto_state.takeoff_pitch_cd * scalar;
}
// are we entering the region where we want to start leveling off before we reach takeoff alt?
if (auto_state.sink_rate < -0.1f) {
float sec_to_target = (remaining_height_to_target_cm * 0.01f) / (-auto_state.sink_rate);
if (sec_to_target > 0 &&
relative_alt_cm >= 1000 &&
sec_to_target <= g.takeoff_pitch_limit_reduction_sec) {
// make a note of that altitude to use it as a start height for scaling
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Takeoff level-off starting at %dm", remaining_height_to_target_cm/100);
auto_state.height_below_takeoff_to_level_off_cm = remaining_height_to_target_cm;
}
}
}
return auto_state.takeoff_pitch_cd;
}
bool Plane::verify_takeoff()
{
if (ahrs.yaw_initialised() && steer_state.hold_course_cd == -1) {
const float min_gps_speed = 5;
if (auto_state.takeoff_speed_time_ms == 0 &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
gps.ground_speed() > min_gps_speed &&
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
auto_state.takeoff_speed_time_ms = millis();
}
if (auto_state.takeoff_speed_time_ms != 0 &&
millis() - auto_state.takeoff_speed_time_ms >= 2000) {
// once we reach sufficient speed for good GPS course
// estimation we save our current GPS ground course
// corrected for summed yaw to set the take off
// course. This keeps wings level until we are ready to
// rotate, and also allows us to cope with arbitrary
// compass errors for auto takeoff
float takeoff_course = wrap_PI(radians(gps.ground_course_cd()*0.01f)) - steer_state.locked_course_err;
takeoff_course = wrap_PI(takeoff_course);
steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100);
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Holding course %ld at %.1fm/s (%.1f)",
steer_state.hold_course_cd,
(double)gps.ground_speed(),
(double)degrees(steer_state.locked_course_err));
}
}
if (steer_state.hold_course_cd != -1) {
// call navigation controller for heading hold
nav_controller->update_heading_hold(steer_state.hold_course_cd);
} else {
nav_controller->update_level_flight();
}
// see if we have reached takeoff altitude
int32_t relative_alt_cm = adjusted_relative_altitude_cm();
if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Takeoff complete at %.2fm",
(double)(relative_alt_cm*0.01f));
steer_state.hold_course_cd = -1;
auto_state.takeoff_complete = true;
next_WP_loc = prev_WP_loc = current_loc;
plane.complete_auto_takeoff();
// don't cross-track on completion of takeoff, as otherwise we
// can end up doing too sharp a turn
auto_state.next_wp_no_crosstrack = true;
return true;
} else {
return false;
}
}
/*
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;
}
}
bool Plane::verify_takeoff()
{
if (ahrs.yaw_initialised() && steer_state.hold_course_cd == -1) {
const float min_gps_speed = 5;
if (auto_state.takeoff_speed_time_ms == 0 &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
gps.ground_speed() > min_gps_speed) {
auto_state.takeoff_speed_time_ms = millis();
}
if (auto_state.takeoff_speed_time_ms != 0 &&
millis() - auto_state.takeoff_speed_time_ms >= 2000) {
// once we reach sufficient speed for good GPS course
// estimation we save our current GPS ground course
// corrected for summed yaw to set the take off
// course. This keeps wings level until we are ready to
// rotate, and also allows us to cope with arbitary
// compass errors for auto takeoff
float takeoff_course = wrap_PI(radians(gps.ground_course_cd()*0.01f)) - steer_state.locked_course_err;
takeoff_course = wrap_PI(takeoff_course);
steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100);
gcs_send_text_fmt(PSTR("Holding course %ld at %.1fm/s (%.1f)"),
steer_state.hold_course_cd,
(double)gps.ground_speed(),
(double)degrees(steer_state.locked_course_err));
}
}
if (steer_state.hold_course_cd != -1) {
// call navigation controller for heading hold
nav_controller->update_heading_hold(steer_state.hold_course_cd);
} else {
nav_controller->update_level_flight();
}
// see if we have reached takeoff altitude
int32_t relative_alt_cm = adjusted_relative_altitude_cm();
if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) {
gcs_send_text_fmt(PSTR("Takeoff complete at %.2fm"),
(double)(relative_alt_cm*0.01f));
steer_state.hold_course_cd = -1;
auto_state.takeoff_complete = true;
next_WP_loc = prev_WP_loc = current_loc;
#if GEOFENCE_ENABLED == ENABLED
if (g.fence_autoenable > 0) {
if (! geofence_set_enabled(true, AUTO_TOGGLED)) {
gcs_send_text_P(SEVERITY_HIGH, PSTR("Enable fence failed (cannot autoenable"));
} else {
gcs_send_text_P(SEVERITY_HIGH, PSTR("Fence enabled. (autoenabled)"));
}
}
#endif
// don't cross-track on completion of takeoff, as otherwise we
// can end up doing too sharp a turn
auto_state.next_wp_no_crosstrack = true;
return true;
} else {
return false;
}
}
/* We want to suppress the throttle if we think we are on the ground and in an autopilot controlled throttle mode.
Disable throttle if following conditions are met:
* 1 - We are in Circle mode (which we use for short term failsafe), or in FBW-B or higher
* AND
* 2 - Our reported altitude is within 10 meters of the home altitude.
* 3 - Our reported speed is under 5 meters per second.
* 4 - We are not performing a takeoff in Auto mode or takeoff speed/accel not yet reached
* OR
* 5 - Home location is not set
* OR
* 6- Landing does not want to allow throttle
*/
bool Plane::suppress_throttle(void)
{
#if PARACHUTE == ENABLED
if (auto_throttle_mode && parachute.release_initiated()) {
// throttle always suppressed in auto-throttle modes after parachute release initiated
throttle_suppressed = true;
return true;
}
#endif
if (landing.is_throttle_suppressed()) {
return true;
}
if (!throttle_suppressed) {
// we've previously met a condition for unsupressing the throttle
return false;
}
if (!auto_throttle_mode) {
// the user controls the throttle
throttle_suppressed = false;
return false;
}
if (control_mode==AUTO && g.auto_fbw_steer == 42) {
// user has throttle control
return false;
}
bool gps_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_2D && gps.ground_speed() >= 5);
if (control_mode==AUTO &&
auto_state.takeoff_complete == false) {
uint32_t launch_duration_ms = ((int32_t)g.takeoff_throttle_delay)*100 + 2000;
if (is_flying() &&
millis() - started_flying_ms > MAX(launch_duration_ms, 5000U) && // been flying >5s in any mode
adjusted_relative_altitude_cm() > 500 && // are >5m above AGL/home
labs(ahrs.pitch_sensor) < 3000 && // not high pitch, which happens when held before launch
gps_movement) { // definite gps movement
// we're already flying, do not suppress the throttle. We can get
// stuck in this condition if we reset a mission and cmd 1 is takeoff
// but we're currently flying around below the takeoff altitude
throttle_suppressed = false;
return false;
}
if (auto_takeoff_check()) {
// we're in auto takeoff
throttle_suppressed = false;
auto_state.baro_takeoff_alt = barometer.get_altitude();
return false;
}
// keep throttle suppressed
return true;
}
if (fabsf(relative_altitude) >= 10.0f) {
// we're more than 10m from the home altitude
throttle_suppressed = false;
return false;
}
if (gps_movement) {
// if we have an airspeed sensor, then check it too, and
// require 5m/s. This prevents throttle up due to spiky GPS
// groundspeed with bad GPS reception
if ((!ahrs.airspeed_sensor_enabled()) || airspeed.get_airspeed() >= 5) {
// we're moving at more than 5 m/s
throttle_suppressed = false;
return false;
}
}
if (quadplane.is_flying()) {
throttle_suppressed = false;
return false;
}
// throttle remains suppressed
return true;
}
/*
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;
}