diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 501abc5052..e567629711 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -161,6 +161,12 @@ static void auto_trim() //g.rc_2.dead_zone = 60; auto_level_counter--; + + if( motors.armed() ) { + // set high AHRS gains to force accelerometers to impact attitude estimate + ahrs.set_fast_gains(true); + } + trim_accel(); led_mode = AUTO_TRIM_LEDS; do_simple = false; @@ -174,6 +180,11 @@ static void auto_trim() reset_control_switch(); + // go back to normal AHRS gains + if( motors.armed() ) { + ahrs.set_fast_gains(false); + } + //Serial.println("Done"); auto_level_counter = 0; }