[paparazzi-commits] [4040] gps lag compensation more robust to fluctuating GPS frequency.

[Felix Ruess]

Revision: 4040
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4040
Author:   flixr
Date:     2009-09-01 11:31:49 +0000 (Tue, 01 Sep 2009)
Log Message:
-----------
gps lag compensation more robust to fluctuating GPS frequency.
change GPS_LAG_TOLERANCE to adjust the tolerance

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c

Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c      2009-09-01 
08:46:54 UTC (rev 4039)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c      2009-09-01 
11:31:49 UTC (rev 4040)
@@ -92,6 +92,9 @@
 #define GPS_LAG_N ((int) (GPS_LAG * HFF_FREQ + 0.5))
 /* number of propagation steps between two GPS updates */
 #define GPS_DT_N ((int) (HFF_FREQ / 4))
+/* tolerance of the GPS lag accuracy is +- GPS_LAG_TOLERANCE seconds */
+#define GPS_LAG_TOLERANCE 0.08
+#define GPS_LAG_TOL_N ((int) (GPS_LAG_TOLERANCE * HFF_FREQ + 0.5))
 
 /* maximum number of past propagation steps to rerun per ap cycle
  * make sure GPS_LAG_N/MAX_PP_STEPS < 128
@@ -175,6 +178,7 @@
   printf("GPS_LAG_N: %d\n", GPS_LAG_N);
   printf("GPS_DT_N: %d\n", GPS_DT_N);
   printf("DT_HFILTER: %f\n", DT_HFILTER);
+  printf("GPS_LAG_TOL_N: %f\n", GPS_LAG_TOL_N);
 #endif
 #else
   b2_hff_rb_last = &b2_hff_state;
@@ -402,7 +406,7 @@
        /* roll back if state was saved approx when GPS was valid */
        lag_counter_err = b2_hff_rb_last->lag_counter - GPS_LAG_N;
        PRINT_DBG(2, ("update. rb_n: %d  lag_counter: %d  lag_cnt_err: %d\n", 
b2_hff_rb_n, b2_hff_rb_last->lag_counter, lag_counter_err));
-       if (abs(lag_counter_err) < 3) {
+       if (abs(lag_counter_err) <= GPS_LAG_TOL_N) {
          b2_hff_rb_last->rollback = TRUE;
          b2_hff_update_x(b2_hff_rb_last, booz_ins_gps_pos_m_ned.x);
          b2_hff_update_y(b2_hff_rb_last, booz_ins_gps_pos_m_ned.y);
@@ -413,7 +417,7 @@
          past_save_counter = GPS_DT_N-1 + lag_counter_err;
          PRINT_DBG(2, ("gps updated. past_save_counter: %d\n", 
past_save_counter));
          b2_hff_propagate_past(b2_hff_rb_last);
-       } else if (lag_counter_err >= GPS_DT_N) {
+       } else if (lag_counter_err >= GPS_DT_N - 2*GPS_LAG_TOL_N) {
          /* apparently missed a GPS update, try next saved state */
          PRINT_DBG(2, ("try next saved state\n"));
          b2_hff_rb_drop_last();