From 2110231ee3bc8e6091faf6328174c44c5b30c2f2 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Thu, 20 Sep 2012 13:17:42 +0900 Subject: [PATCH] AP_LeadFilter: bug fix so velocity * _lag is used (otherwise we are assuming a 1sec lag) --- libraries/AP_LeadFilter/AP_LeadFilter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_LeadFilter/AP_LeadFilter.cpp b/libraries/AP_LeadFilter/AP_LeadFilter.cpp index 19197c00c1..d4d296822f 100644 --- a/libraries/AP_LeadFilter/AP_LeadFilter.cpp +++ b/libraries/AP_LeadFilter/AP_LeadFilter.cpp @@ -40,5 +40,5 @@ AP_LeadFilter::get_position(int32_t pos, int16_t vel) // store velocity for next iteration _last_velocity = vel; - return pos + vel + accel_contribution; + return pos + vel_contribution + accel_contribution; }