From b3ff3685892838e941fe7ea21c82f9140314afda Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Thu, 20 Sep 2012 15:49:09 +0900 Subject: [PATCH] ArduCopter: position lead filters modified to use lag which varies by gps --- ArduCopter/navigation.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 1d62548978..9d4c85ab60 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -63,8 +63,8 @@ static void calc_XY_velocity(){ filtered_loc.lng = xLeadFilter.get_position(g_gps->longitude, accels_velocity.x); filtered_loc.lat = yLeadFilter.get_position(g_gps->latitude, accels_velocity.y); #else - filtered_loc.lng = xLeadFilter.get_position(g_gps->longitude, x_actual_speed); - filtered_loc.lat = yLeadFilter.get_position(g_gps->latitude, y_actual_speed); + filtered_loc.lng = xLeadFilter.get_position(g_gps->longitude, x_actual_speed, g_gps->get_lag()); + filtered_loc.lat = yLeadFilter.get_position(g_gps->latitude, y_actual_speed, g_gps->get_lag()); #endif }