diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index e2e9251e5c..c4d1acaa08 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -573,13 +573,17 @@ bool Plane::suppress_throttle(void) 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,5000) && // been flying >5s in any mode - adjusted_relative_altitude_cm() > 500) { // are >5m above AGL/home + 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) { // definate 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 @@ -598,19 +602,18 @@ bool Plane::suppress_throttle(void) 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"), + gcs_send_text_fmt(PSTR("Throttle enabled - 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 (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(PSTR("Throttle unsuppressed - speed %.2f airspeed %.2f"), + gcs_send_text_fmt(PSTR("Throttle enabled - speed %.2f airspeed %.2f"), (double)gps.ground_speed(), (double)airspeed.get_airspeed()); throttle_suppressed = false;