AHRS: add reset_gyro_drift method

libraries/AP_AHRS/AP_AHRS.h

@@ -181,6 +181,10 @@ public:
     // return the current estimate of the gyro drift
     virtual const Vector3f &get_gyro_drift(void) const = 0;
 
+    // reset the current gyro drift estimate
+    //  should be called if gyro offsets are recalculated
+    virtual void reset_gyro_drift(void) = 0;
+
     // reset the current attitude, used on new IMU calibration
     virtual void reset(bool recover_eulers=false) = 0;
 

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -36,6 +36,15 @@ extern const AP_HAL::HAL& hal;
 // http://gentlenav.googlecode.com/files/fastRotations.pdf
 #define SPIN_RATE_LIMIT 20
 
+// reset the current gyro drift estimate
+//  should be called if gyro offsets are recalculated
+void
+AP_AHRS_DCM::reset_gyro_drift(void)
+{
+    _omega_I.zero();
+    _omega_I_sum.zero();
+    _omega_I_sum_time = 0;
+}
 
 // run a full DCM update round
 void

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -75,6 +75,10 @@ public:
         return _omega_I;
     }
 
+    // reset the current gyro drift estimate
+    //  should be called if gyro offsets are recalculated
+    void reset_gyro_drift(void);
+
     // Methods
     void            update(void);
     void            reset(bool recover_eulers = false);

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -50,6 +50,16 @@ const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const
     return _gyro_bias;
 }
 
+// reset the current gyro drift estimate
+//  should be called if gyro offsets are recalculated
+void AP_AHRS_NavEKF::reset_gyro_drift(void)
+{
+    // update DCM
+    AP_AHRS_DCM::reset_gyro_drift();
+
+    // To-Do: add call to do the same on EKF
+}
+
 void AP_AHRS_NavEKF::update(void)
 {
     // we need to restore the old DCM attitude values as these are

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -49,6 +49,10 @@ public:
     // return the current drift correction integrator value
     const Vector3f &get_gyro_drift(void) const;
 
+    // reset the current gyro drift estimate
+    //  should be called if gyro offsets are recalculated
+    void reset_gyro_drift(void);
+
     void            update(void);
     void            reset(bool recover_eulers = false);