diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index c4a428176cd9a0b3694f74e2ac45e1b1e87eb0f9..d7930ccbb99c3c687c7f524addc1ab1ec1b26da1 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 4ab9ff348725daafcaa9ce61f28c80e3fe30d1e3..4e34d8cb4ef76cced3c7a532d1aa7d4ecac4d5d5 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);