From 0ac3267d520aa3331c6c003cea9cfc34297c5c30 Mon Sep 17 00:00:00 2001 From: Randy Mackay 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 80f16e89a2..6ef3ad68cf 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1106,6 +1106,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 523a507039..9a161c61f0 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -318,6 +318,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);