From 7b4cd9ee371a2abf7943d7eaaf1ca7a768060877 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 28 Oct 2014 20:23:18 +0900
Subject: [PATCH] Copter: reset ahrs gyro drift after gyro calibration

---
 ArduCopter/GCS_Mavlink.pde | 2 ++
 ArduCopter/system.pde      | 5 +++++
 2 files changed, 7 insertions(+)

diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index c4a428176..d7930ccbb 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -1113,6 +1113,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             if (packet.param1 == 1) {
                 // gyro offset calibration
                 ins.init_gyro();
+                // reset ahrs gyro bias
+                ahrs.reset_gyro_drift();
                 result = MAV_RESULT_ACCEPTED;
             }
             if (packet.param3 == 1) {
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index 4ab9ff348..4e34d8cb4 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -320,6 +320,11 @@ static void startup_ground(bool force_gyro_cal)
     report_ins();
  #endif
 
+    // reset ahrs gyro bias
+    if (force_gyro_cal) {
+        ahrs.reset_gyro_drift();
+    }
+
     // setup fast AHRS gains to get right attitude
     ahrs.set_fast_gains(true);
 
-- 
GitLab