From a0969905ce4414f249eedef84b055ae17af3cb14 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 21 Apr 2015 21:42:17 +1000
Subject: [PATCH] AP_AHRS: make get_error_rp() and get_error_yaw() const

this makes them usable by multiple consumers, and allows use by EKF
---
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 36 ++-----------------------------
 libraries/AP_AHRS/AP_AHRS_DCM.h   | 20 ++++++-----------
 2 files changed, 8 insertions(+), 48 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index b13d57c541..a15a9345af 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -476,8 +476,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
         _omega_I_sum.z += error_z * _ki_yaw * yaw_deltat;
     }
 
-    _error_yaw_sum += fabsf(yaw_error);
-    _error_yaw_count++;
+    _error_yaw = 0.9f * _error_yaw + 0.1f * fabsf(yaw_error);
 }
 
 
@@ -737,8 +736,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
         return;
     }
 
-    _error_rp_sum += best_error;
-    _error_rp_count++;
+    _error_rp = 0.9f * _error_rp + 0.1f * best_error;
 
     // base the P gain on the spin rate
     float spin_rate = _omega.length();
@@ -873,36 +871,6 @@ AP_AHRS_DCM::euler_angles(void)
     update_cd_values();
 }
 
-/* reporting of DCM state for MAVLink */
-
-// average error_roll_pitch since last call
-float AP_AHRS_DCM::get_error_rp(void)
-{
-    if (_error_rp_count == 0) {
-        // this happens when telemetry is setup on two
-        // serial ports
-        return _error_rp_last;
-    }
-    _error_rp_last = _error_rp_sum / _error_rp_count;
-    _error_rp_sum = 0;
-    _error_rp_count = 0;
-    return _error_rp_last;
-}
-
-// average error_yaw since last call
-float AP_AHRS_DCM::get_error_yaw(void)
-{
-    if (_error_yaw_count == 0) {
-        // this happens when telemetry is setup on two
-        // serial ports
-        return _error_yaw_last;
-    }
-    _error_yaw_last = _error_yaw_sum / _error_yaw_count;
-    _error_yaw_sum = 0;
-    _error_yaw_count = 0;
-    return _error_yaw_last;
-}
-
 // return our current position estimate using
 // dead-reckoning or GPS
 bool AP_AHRS_DCM::get_position(struct Location &loc) const
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index 0fa498f1d0..9ee94aee9c 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -30,12 +30,8 @@ public:
         _omega_I_sum_time(0.0f),
         _renorm_val_sum(0.0f),
         _renorm_val_count(0),
-        _error_rp_sum(0.0f),
-        _error_rp_count(0),
-        _error_rp_last(0.0f),
-        _error_yaw_sum(0.0f),
-        _error_yaw_count(0),
-        _error_yaw_last(0.0f),
+        _error_rp(1.0f),
+        _error_yaw(1.0f),
         _gps_last_update(0),
         _ra_deltat(0.0f),
         _ra_sum_start(0),
@@ -93,8 +89,8 @@ public:
     virtual bool get_position(struct Location &loc) const;
 
     // status reporting
-    float           get_error_rp(void);
-    float           get_error_yaw(void);
+    float           get_error_rp(void) const { return _error_rp; } 
+    float           get_error_yaw(void) const { return _error_yaw; }
 
     // return a wind estimation vector, in m/s
     Vector3f wind_estimate(void) {
@@ -154,12 +150,8 @@ private:
     // state to support status reporting
     float _renorm_val_sum;
     uint16_t _renorm_val_count;
-    float _error_rp_sum;
-    uint16_t _error_rp_count;
-    float _error_rp_last;
-    float _error_yaw_sum;
-    uint16_t _error_yaw_count;
-    float _error_yaw_last;
+    float _error_rp;
+    float _error_yaw;
 
     // time in millis when we last got a GPS heading
     uint32_t _gps_last_update;