From 589f8bc557ed08fc1e90e1ac8e5e8d601eb58a28 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 13 Jan 2012 12:48:02 -0800 Subject: [PATCH] Disabled Ryans filter until we get real-world testing in. --- ArduCopter/navigation.pde | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index d763360880..76760a0619 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -46,25 +46,26 @@ static void calc_XY_velocity(){ static int32_t last_longitude = 0; static int32_t last_latitude = 0; - static int16_t x_speed_old = 0; - static int16_t y_speed_old = 0; + //static int16_t x_speed_old = 0; + //static int16_t y_speed_old = 0; // y_GPS_speed positve = Up // x_GPS_speed positve = Right // this speed is ~ in cm because we are using 10^7 numbers from GPS float tmp = 1.0/dTnav; - //float tmp = 5; // straightforward approach: - /* + ///* int16_t x_estimate = (float)(g_gps->longitude - last_longitude) * tmp; int16_t y_estimate = (float)(g_gps->latitude - last_latitude) * tmp; + // slight averaging filter x_GPS_speed = (x_GPS_speed + x_estimate) >> 1; y_GPS_speed = (y_GPS_speed + y_estimate) >> 1; - */ + //*/ + /* // Ryan Beall's forward estimator: int16_t x_speed_new = (float)(g_gps->longitude - last_longitude) * tmp; int16_t y_speed_new = (float)(g_gps->latitude - last_latitude) * tmp; @@ -74,6 +75,7 @@ static void calc_XY_velocity(){ x_speed_old = x_speed_new; y_speed_old = y_speed_new; + */ last_longitude = g_gps->longitude; last_latitude = g_gps->latitude;