/* 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 */ bool Plane::suppress_throttle(void) { 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) { // user has throttle control return false; } if (control_mode==AUTO && auto_state.takeoff_complete == false) { if (auto_takeoff_check()) { // we're in auto takeoff throttle_suppressed = false; return false; } // keep throttle suppressed return true; } if (relative_altitude_abs_cm() >= 1000) { // we're more than 10m from the home altitude throttle_suppressed = false; gcs_send_text_fmt(PSTR("Throttle unsuppressed - altitude %.2f"), (double)(relative_altitude_abs_cm()*0.01f)); return false; } if (gps.status() >= AP_GPS::GPS_OK_FIX_2D && gps.ground_speed() >= 5) { // 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 gcs_send_text_fmt(PSTR("Throttle unsuppressed - speed %.2f airspeed %.2f"), (double)gps.ground_speed(), (double)airspeed.get_airspeed()); throttle_suppressed = false; return false; } } // throttle remains suppressed return true; }
/* main stabilization function for all 3 axes */ void Plane::stabilize() { if (control_mode == MANUAL) { // nothing to do return; } float speed_scaler = get_speed_scaler(); if (control_mode == TRAINING) { stabilize_training(speed_scaler); } else if (control_mode == ACRO) { stabilize_acro(speed_scaler); } else if (control_mode == QSTABILIZE || control_mode == QHOVER || control_mode == QLOITER || control_mode == QLAND || control_mode == QRTL) { quadplane.control_run(); } else { if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) { stabilize_stick_mixing_fbw(); } stabilize_roll(speed_scaler); stabilize_pitch(speed_scaler); if (g.stick_mixing == STICK_MIXING_DIRECT || control_mode == STABILIZE) { stabilize_stick_mixing_direct(); } stabilize_yaw(speed_scaler); } /* see if we should zero the attitude controller integrators. */ if (channel_throttle->get_control_in() == 0 && relative_altitude_abs_cm() < 500 && fabsf(barometer.get_climb_rate()) < 0.5f && gps.ground_speed() < 3) { // we are low, with no climb rate, and zero throttle, and very // low ground speed. Zero the attitude controller // integrators. This prevents integrator buildup pre-takeoff. rollController.reset_I(); pitchController.reset_I(); yawController.reset_I(); // if moving very slowly also zero the steering integrator if (gps.ground_speed() < 1) { steerController.reset_I(); } } }
/* 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 */ 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 (!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 (relative_altitude_abs_cm() >= 1000) { // we're more than 10m from the home altitude throttle_suppressed = false; gcs_send_text_fmt(MAV_SEVERITY_INFO, "Throttle enabled. Altitude %.2f", (double)(relative_altitude_abs_cm()*0.01f)); 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 gcs_send_text_fmt(MAV_SEVERITY_INFO, "Throttle enabled. Speed %.2f airspeed %.2f", (double)gps.ground_speed(), (double)airspeed.get_airspeed()); throttle_suppressed = false; return false; } } if (quadplane.is_flying()) { gcs_send_text_fmt(MAV_SEVERITY_INFO, "Throttle enabled VTOL"); throttle_suppressed = false; } // throttle remains suppressed return true; }