From ecda56006c3045e6da9812cd27070d181a664fc8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 21 Sep 2015 12:10:19 +0900 Subject: [PATCH] Tracker: calibrate gyros on accel cal and trim --- AntennaTracker/GCS_Mavlink.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 62037c6958..99b8c7551e 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -613,11 +613,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } else if (is_equal(packet.param5,1.0f)) { float trim_roll, trim_pitch; AP_InertialSensor_UserInteract_MAVLink interact(this); + // start with gyro calibration + tracker.ins.init_gyro(); + // reset ahrs gyro bias + if (tracker.ins.gyro_calibrated_ok_all()) { + tracker.ahrs.reset_gyro_drift(); + } if(tracker.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); } } else if (is_equal(packet.param5,2.0f)) { + // start with gyro calibration + tracker.ins.init_gyro(); // accel trim float trim_roll, trim_pitch; if(tracker.ins.calibrate_trim(trim_roll, trim_pitch)) {