diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index f644c2de92..5e30e51eee 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/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; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 04d83ea6bc..b698bb6c19 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/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 diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 76160c09e5..8833028d3e 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/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); diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 224e7a9ba7..8853a9dfd6 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/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 diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index d03ffdc173..bc065866fa 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/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);