diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index ccc20a4fb6..7e8a16b1a2 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -302,6 +302,17 @@ static void set_servos(void) #else // convert 0 to 100% into PWM g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get()); + if (g.airspeed_enabled == true) { + // We want to supress the throttle if we think we are on the ground and in an autopilot controlled throttle mode. + if((control_mode==Circle || control_mode>=FBW_B) && current_loc.alt-home.alt < 500 && airspeed< 500) { + if(!(control_mode==Auto && takeoff_complete == false) g.channel_throttle.servo_out = 0; + } + } else { + if((control_mode==Circle || control_mode>=FBW_B) && current_loc.alt-home.alt < 500 && g_gps->ground_speed < 500) { + if(!(control_mode==Auto && takeoff_complete == false) g.channel_throttle.servo_out = 0; + } + } + #endif g.channel_throttle.calc_pwm(); diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 296b238c19..3c4693d140 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -222,6 +222,7 @@ static void do_takeoff() next_WP.lat = home.lat + 1000; // so we don't have bad calcs next_WP.lng = home.lng + 1000; // so we don't have bad calcs takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction + // Flag also used to override "on the ground" throttle disable } static void do_nav_wp()