From 0b45d2bc0608a8347245a191b982c9f8dc3cceb3 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 27 Feb 2014 08:57:44 +1100
Subject: [PATCH] AP_AHRS: removed the AHRS_GPS_DELAY parameter

the best value has turned out to be 1, and tweaking it has not turned
out to be useful, so this simplifies the code in preparation for
adding the anti-aliasing handling with multiple accelerometers
---
 libraries/AP_AHRS/AP_AHRS.cpp     |  9 ++-----
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 43 ++-----------------------------
 libraries/AP_AHRS/AP_AHRS_DCM.h   |  4 +--
 3 files changed, 5 insertions(+), 51 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
index 317ef864b2..e0a765ec10 100644
--- a/libraries/AP_AHRS/AP_AHRS.cpp
+++ b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -108,13 +108,8 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
     // @User: Advanced
     AP_GROUPINFO("GPS_MINSATS", 11, AP_AHRS, _gps_minsats, 6),
 
-    // @Param: GPS_DELAY
-    // @DisplayName: AHRS GPS delay steps
-    // @Description: number of GPS samples to delay accels for synchronisation with the GPS velocity data
-    // @Range: 0 5
-    // @Increment: 1
-    // @User: Advanced
-    AP_GROUPINFO("GPS_DELAY", 12, AP_AHRS, _gps_delay, 1),
+    // NOTE: index 12 was for GPS_DELAY, but now removed, fixed delay
+    // of 1 was found to be the best choice
 
 #if AP_AHRS_NAVEKF_AVAILABLE
     // @Param: EKF_USE
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index eb3f1401cb..8f37ef5a89 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -461,48 +461,9 @@ AP_AHRS_DCM::drift_correction_yaw(void)
  */
 Vector3f AP_AHRS_DCM::ra_delayed(const Vector3f &ra)
 {
-    if (_ra_delay_length != _gps_delay.get()) {
-        // the AHRS_GPS_DELAY setting has changed
-
-        // constrain it between 0 and 5
-        if (_gps_delay.get() > 5) {
-            _gps_delay.set(5);
-        }
-        if (_gps_delay.get() < 0) {
-            _gps_delay.set(0);
-        }
-        if (_ra_delay_buffer != NULL) {
-            delete[] _ra_delay_buffer;
-            _ra_delay_buffer = NULL;
-        }
-
-        // allocate the new buffer
-        _ra_delay_length = _gps_delay.get();
-        if (_ra_delay_length != 0) {
-            _ra_delay_buffer = new Vector3f[_ra_delay_length];
-        }
-        _ra_delay_next = 0;
-        if (_ra_delay_buffer != NULL) {
-            // on size change prefill the buffer with the current value
-            for (uint8_t i=0; i<_ra_delay_length; i++) {
-                _ra_delay_buffer[i] = ra;
-            }
-        }
-    }
-    if (_ra_delay_buffer == NULL) {
-        // we're not doing any delay
-        return ra;
-    }
-
     // get the old element, and then fill it with the new element
-    Vector3f ret = _ra_delay_buffer[_ra_delay_next];
-    _ra_delay_buffer[_ra_delay_next] = ra;
-
-    // move to the next element
-    _ra_delay_next++;
-    if (_ra_delay_next == _ra_delay_length) {
-        _ra_delay_next = 0;
-    }
+    Vector3f ret = _ra_delay_buffer;
+    _ra_delay_buffer = ra;
     return ret;
 }
 
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index 195222364f..def6909467 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -111,9 +111,7 @@ private:
 
     // variables to cope with delaying the GA sum to match GPS lag
     Vector3f ra_delayed(const Vector3f &ra);
-    uint8_t   _ra_delay_length;
-    uint8_t   _ra_delay_next;
-    Vector3f *_ra_delay_buffer;
+    Vector3f _ra_delay_buffer;
 
     // P term gain based on spin rate
     float           _P_gain(float spin_rate);