From 1b0f5ac84f5938dabccfb46879c0f294ab9f34a9 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 21 Aug 2012 15:38:31 +1000
Subject: [PATCH] ACM: use ahrs.set_fast_gains() on arm/disarm

---
 ArduCopter/motors.pde | 6 ++++++
 ArduCopter/system.pde | 8 ++++++--
 2 files changed, 12 insertions(+), 2 deletions(-)

diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde
index 937c14b5c5..728542228c 100644
--- a/ArduCopter/motors.pde
+++ b/ArduCopter/motors.pde
@@ -158,6 +158,9 @@ static void init_arm_motors()
 	// temp hack
 	motor_light = true;
 	digitalWrite(A_LED_PIN, LED_ON);
+
+    // go back to normal AHRS gains
+    ahrs.set_fast_gains(false);
 }
 
 
@@ -181,6 +184,9 @@ static void init_disarm_motors()
 		piezo_beep();
 	}
 	#endif
+
+    // setup fast AHRS gains to get right attitude
+    ahrs.set_fast_gains(true);
 }
 
 /*****************************************
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index 68657e44c8..7ac9b8b69d 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -385,10 +385,14 @@ static void startup_ground(void)
 		#if CLI_ENABLED == ENABLED
 			report_imu();
 		#endif
-		// initialise ahrs (may push imu calibration into the mpu6000 if using that device).
-		ahrs.init();
 	#endif
 
+    // initialise ahrs (may push imu calibration into the mpu6000 if using that device).
+    ahrs.init();
+
+    // setup fast AHRS gains to get right attitude
+    ahrs.set_fast_gains(true);
+
 	// reset the leds
 	// ---------------------------
 	clear_leds();