From 53aced1defb1e63588ffe84a0af22e4e12b4d889 Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Thu, 20 Sep 2012 15:27:27 +0900
Subject: [PATCH] AP_LeadFilter: moved lag to be a parameter passed into the
 get_position function.

This saves 4 bytes of memory and will work better with the way we initialise the gps.
---
 libraries/AP_LeadFilter/AP_LeadFilter.cpp      | 18 +++---------------
 libraries/AP_LeadFilter/AP_LeadFilter.h        |  6 ++----
 .../examples/AP_LeadFilter/AP_LeadFilter.pde   | 12 +++++++-----
 3 files changed, 12 insertions(+), 24 deletions(-)

diff --git a/libraries/AP_LeadFilter/AP_LeadFilter.cpp b/libraries/AP_LeadFilter/AP_LeadFilter.cpp
index d4d296822f..83ff6a2c2c 100644
--- a/libraries/AP_LeadFilter/AP_LeadFilter.cpp
+++ b/libraries/AP_LeadFilter/AP_LeadFilter.cpp
@@ -17,25 +17,13 @@
 
 #include "AP_LeadFilter.h"
 
-/*
- *  int32_t
- *  AP_LeadFilter::get_position(int32_t pos, int16_t vel)
- *  {
- *       vel = (_last_velocity + vel) / 2;
- *       pos += vel;
- *       pos += (vel - _last_velocity);
- *       _last_velocity = vel;
- *       return pos;
- *  }
- */
-
 // setup the control preferences
 int32_t
-AP_LeadFilter::get_position(int32_t pos, int16_t vel)
+AP_LeadFilter::get_position(int32_t pos, int16_t vel, float lag_in_seconds)
 {
     // assumes a 1 second delay in the GPS
-    int16_t accel_contribution = (vel - _last_velocity) * _lag * _lag;
-    int16_t vel_contribution = vel * _lag;
+    int16_t accel_contribution = (vel - _last_velocity) * lag_in_seconds * lag_in_seconds;
+    int16_t vel_contribution = vel * lag_in_seconds;
 
     // store velocity for next iteration
     _last_velocity = vel;
diff --git a/libraries/AP_LeadFilter/AP_LeadFilter.h b/libraries/AP_LeadFilter/AP_LeadFilter.h
index c4424aeee0..86c3b0f5fb 100644
--- a/libraries/AP_LeadFilter/AP_LeadFilter.h
+++ b/libraries/AP_LeadFilter/AP_LeadFilter.h
@@ -16,17 +16,15 @@ public:
     ///
     ///
     AP_LeadFilter() :
-        _last_velocity(0), _lag(1.0) {
+        _last_velocity(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; }
+    int32_t         get_position(int32_t pos, int16_t vel, float lag_in_seconds = 1.0);
     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 8ab4c6bf99..52c4cd942d 100644
--- a/libraries/AP_LeadFilter/examples/AP_LeadFilter/AP_LeadFilter.pde
+++ b/libraries/AP_LeadFilter/examples/AP_LeadFilter/AP_LeadFilter.pde
@@ -38,14 +38,17 @@ void setup()
 
 void loop()
 {
-    int16_t velocity = 0;
-    int32_t position = 0;
+    int16_t velocity;
+    int32_t position;
     int32_t new_position;
     int16_t i;
 
+    position = 0;
+    velocity = 0;
+    xLeadFilter.clear();
+
     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
@@ -57,13 +60,12 @@ void loop()
     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
+        new_position = xLeadFilter.get_position(position, velocity, 0.200);     // 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;