From 2c36693a5043ac512bd23648f9c7d663d1df68cd Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Thu, 20 Sep 2012 13:10:24 +0900
Subject: [PATCH] AP_LeadFilter: added set_lag feature to allow us to adjust it
 for different GPSs

---
 libraries/AP_LeadFilter/AP_LeadFilter.cpp     |  8 ++-
 libraries/AP_LeadFilter/AP_LeadFilter.h       |  5 +-
 .../examples/AP_LeadFilter/AP_LeadFilter.pde  | 52 ++++++++++++-------
 3 files changed, 43 insertions(+), 22 deletions(-)

diff --git a/libraries/AP_LeadFilter/AP_LeadFilter.cpp b/libraries/AP_LeadFilter/AP_LeadFilter.cpp
index c35d3caabf..19197c00c1 100644
--- a/libraries/AP_LeadFilter/AP_LeadFilter.cpp
+++ b/libraries/AP_LeadFilter/AP_LeadFilter.cpp
@@ -34,7 +34,11 @@ int32_t
 AP_LeadFilter::get_position(int32_t pos, int16_t vel)
 {
     // assumes a 1 second delay in the GPS
-    int16_t acc = vel - _last_velocity;
+    int16_t accel_contribution = (vel - _last_velocity) * _lag * _lag;
+    int16_t vel_contribution = vel * _lag;
+
+    // store velocity for next iteration
     _last_velocity = vel;
-    return pos + vel + acc;
+
+    return pos + vel + accel_contribution;
 }
diff --git a/libraries/AP_LeadFilter/AP_LeadFilter.h b/libraries/AP_LeadFilter/AP_LeadFilter.h
index 008c2eb5c3..c4424aeee0 100644
--- a/libraries/AP_LeadFilter/AP_LeadFilter.h
+++ b/libraries/AP_LeadFilter/AP_LeadFilter.h
@@ -16,14 +16,17 @@ public:
     ///
     ///
     AP_LeadFilter() :
-        _last_velocity(0) {
+        _last_velocity(0), _lag(1.0) {
     }
 
     // setup min and max radio values in CLI
     int32_t         get_position(int32_t pos, int16_t vel);
+    void            set_lag(float delay_in_seconds) { _lag = delay_in_seconds; }
+    void            clear() { _last_velocity = 0; }
 
 private:
     int16_t         _last_velocity;
+    float           _lag;
 
 };
 
diff --git a/libraries/AP_LeadFilter/examples/AP_LeadFilter/AP_LeadFilter.pde b/libraries/AP_LeadFilter/examples/AP_LeadFilter/AP_LeadFilter.pde
index 0cbefae992..8ab4c6bf99 100644
--- a/libraries/AP_LeadFilter/examples/AP_LeadFilter/AP_LeadFilter.pde
+++ b/libraries/AP_LeadFilter/examples/AP_LeadFilter/AP_LeadFilter.pde
@@ -11,10 +11,6 @@
 #include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
 #include <AP_LeadFilter.h>      // GPS Lead filter
 
-// define APM1 or APM2
-#define APM_HARDWARE_APM1       1
-#define APM_HARDWARE_APM2       2
-
 // set your hardware type here
 #define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
 
@@ -29,33 +25,51 @@ Arduino_Mega_ISR_Registry isr_registry;
 // use.
 //
 FastSerialPort0(Serial);        // FTDI/console
-FastSerialPort1(Serial1);       // GPS port
-FastSerialPort3(Serial3);       // Telemetry port
-
-////////////////////////////////////////////
-// RC Hardware
-////////////////////////////////////////////
-#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
-APM_RC_APM2 APM_RC;
-#else
-APM_RC_APM1 APM_RC;
-#endif
 
 AP_LeadFilter xLeadFilter;      // GPS lag filter
 
 void setup()
 {
     Serial.begin(115200);
-    Serial.println("ArduPilot RC Channel test");
+    Serial.println("AP_LeadFilter test ver 1.0");
 
     delay(500);
-
-    int32_t temp = xLeadFilter.get_position(0, 100);
-    Serial.printf("temp %ld \n", temp);
 }
 
 void loop()
 {
+    int16_t velocity = 0;
+    int32_t position = 0;
+    int32_t new_position;
+    int16_t i;
+
+    Serial.printf("------------------\n");
+    Serial.printf("start position = 0, lag of 1sec.\n");
+    xLeadFilter.set_lag(1.0);
+    for( i = 0; i < 10; i++ ) {
+        // get updated position
+        new_position = xLeadFilter.get_position(position, velocity);     // new position with velocity of 1 m/s
+        Serial.printf("start pos: %ld, start vel: %d, end pos: %ld\n", (long int)position, (int)velocity, (long int)new_position);
+        position = new_position;
+        velocity += 100;
+    }
+
+    position = 0;
+    velocity = 0;
+    xLeadFilter.clear();
+    xLeadFilter.set_lag(0.200);
+
+    Serial.printf("------------------\n");
+    Serial.printf("start position = 0, lag of 200ms\n");
+    for( i = 0; i < 10; i++ ) {
+        // get updated position
+        new_position = xLeadFilter.get_position(position, velocity);     // new position with velocity of 1 m/s
+        Serial.printf("start pos: %ld, start vel: %d, end pos: %ld\n", (long int)position, (int)velocity, (long int)new_position);
+        position = new_position;
+        velocity += 100;
+    }
+
+    delay(10000);
 }