From 02d6f012ce6b970d89c42f5600763eccf906478f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 4 Nov 2013 15:50:33 +1100
Subject: [PATCH] AP_AHRS: added accel sum delay buffer to account for GPS lag

---
 libraries/AP_AHRS/AP_AHRS.cpp     |  8 +++++
 libraries/AP_AHRS/AP_AHRS.h       |  1 +
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 60 ++++++++++++++++++++++++++++++-
 libraries/AP_AHRS/AP_AHRS_DCM.h   |  6 ++++
 4 files changed, 74 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
index 8d0ebfecba..b51496621a 100644
--- a/libraries/AP_AHRS/AP_AHRS.cpp
+++ b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -105,6 +105,14 @@ 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, 2),
+
     AP_GROUPEND
 };
 
diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index de4c850017..380128e180 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -242,6 +242,7 @@ protected:
     AP_Int8 _wind_max;
     AP_Int8 _board_orientation;
     AP_Int8 _gps_minsats;
+    AP_Int8 _gps_delay;
 
     // flags structure
     struct ahrs_flags {
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index 5a950b0315..2dcd3cdf3a 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -431,7 +431,55 @@ AP_AHRS_DCM::drift_correction_yaw(void)
 }
 
 
+/**
+   return an accel vector delayed by AHRS_ACCEL_DELAY samples
+ */
+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;
+    }
+    return ret;
+}
 
 // perform drift correction. This function aims to update _omega_P and
 // _omega_I with our best estimate of the short term and long term
@@ -544,6 +592,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
     Vector3f GA_e;
     GA_e = Vector3f(0, 0, -1.0f);
 
+    bool using_gps_corrections = false;
     if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) {
         float v_scale = gps_gain.get()/(_ra_deltat*GRAVITY_MSS);
         Vector3f vdelta = (velocity - _last_velocity) * v_scale;
@@ -553,10 +602,19 @@ AP_AHRS_DCM::drift_correction(float deltat)
             // wait for some non-zero acceleration information
             return;
         }
+        using_gps_corrections = true;
     }
 
     // calculate the error term in earth frame.
-    Vector3f GA_b = _ra_sum / (_ra_deltat * GRAVITY_MSS);
+    _ra_sum /= (_ra_deltat * GRAVITY_MSS);
+
+    // get the delayed ra_sum to match the GPS lag
+    Vector3f GA_b;
+    if (using_gps_corrections) {
+        GA_b = ra_delayed(_ra_sum);
+    } else {
+        GA_b = _ra_sum;
+    }
     GA_b.normalize();
     if (GA_b.is_inf()) {
         // wait for some non-zero acceleration information
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index 4b4340108a..b3372d9bec 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -99,6 +99,12 @@ private:
     float _omega_I_sum_time;
     Vector3f _omega;                            // Corrected Gyro_Vector data
 
+    // 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;
+
     // P term gain based on spin rate
     float           _P_gain(float spin_rate);