From 95538d29920a32176d0ae1b7a2b6ce1471e5d1d0 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 28 Oct 2014 20:22:48 +0900
Subject: [PATCH] AHRS: add reset_gyro_drift method
---
libraries/AP_AHRS/AP_AHRS.h | 4 ++++
libraries/AP_AHRS/AP_AHRS_DCM.cpp | 9 +++++++++
libraries/AP_AHRS/AP_AHRS_DCM.h | 4 ++++
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 10 ++++++++++
libraries/AP_AHRS/AP_AHRS_NavEKF.h | 4 ++++
5 files changed, 31 insertions(+)
diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index f644c2de9..5e30e51ee 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 04d83ea6b..b698bb6c1 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 76160c09e..8833028d3 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 224e7a9ba..8853a9dfd 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 d03ffdc17..bc065866f 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);
--
GitLab