From 148da2e4a04f610c07d2e0751432068ea3347df0 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Wed, 18 Jul 2012 11:09:05 -0700 Subject: [PATCH] Lead Filter: simplified calculation --- libraries/AP_LeadFilter/AP_LeadFilter.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/libraries/AP_LeadFilter/AP_LeadFilter.cpp b/libraries/AP_LeadFilter/AP_LeadFilter.cpp index 30e4efcc54..59d9b47423 100644 --- a/libraries/AP_LeadFilter/AP_LeadFilter.cpp +++ b/libraries/AP_LeadFilter/AP_LeadFilter.cpp @@ -17,7 +17,7 @@ #include "AP_LeadFilter.h" -// setup the control preferences +/* int32_t AP_LeadFilter::get_position(int32_t pos, int16_t vel) { @@ -27,3 +27,14 @@ AP_LeadFilter::get_position(int32_t pos, int16_t vel) _last_velocity = vel; return pos; } +*/ + +// setup the control preferences +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; + _last_velocity = vel; + return pos + vel + acc; +}