/* * 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; }