[paparazzi-commits] [4654] move store_accel_body and ins state update into hff

[Felix Ruess]

Revision: 4654
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4654
Author:   flixr
Date:     2010-03-10 00:42:42 +0000 (Wed, 10 Mar 2010)
Log Message:
-----------
move store_accel_body and ins state update into hff

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

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_ins.c       2010-03-09 23:38:15 UTC 
(rev 4653)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.c       2010-03-10 00:42:42 UTC 
(rev 4654)
@@ -161,22 +161,8 @@
 #endif /* USE_VFF */
 
 #ifdef USE_HFF
-  b2_hff_store_accel_body();
   /* propagate horizontal filter */
   b2_hff_propagate();
-  if ( booz_ins_ltp_initialised ) {
-    /* update ins state from horizontal filter */
-    booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
-    booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
-    booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
-    booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
-    booz_ins_ltp_pos.x   = POS_BFP_OF_REAL(b2_hff_state.x);
-    booz_ins_ltp_pos.y   = POS_BFP_OF_REAL(b2_hff_state.y);
-  }
-  else {
-    booz_ins_ltp_accel.x = accel_ltp.x;
-    booz_ins_ltp_accel.y = accel_ltp.y;
-  }
 #else
   booz_ins_ltp_accel.x = accel_ltp.x;
   booz_ins_ltp_accel.y = accel_ltp.y;

Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c      2010-03-09 
23:38:15 UTC (rev 4653)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c      2010-03-10 
00:42:42 UTC (rev 4654)
@@ -429,6 +429,9 @@
   }
 #endif
 
+  /* store body accelerations for mean computation */
+  b2_hff_store_accel_body();
+
   /* propagate current state if it is time */
   if (b2_hff_ps_counter == HFF_PRESCALER) {
        b2_hff_ps_counter = 1;
@@ -447,11 +450,17 @@
       /*
        * propagate current state
        */
-      if ( booz_ins_ltp_initialised ) {
-        b2_hff_propagate_x(&b2_hff_state);
-        b2_hff_propagate_y(&b2_hff_state);
-      }
+      b2_hff_propagate_x(&b2_hff_state);
+      b2_hff_propagate_y(&b2_hff_state);
 
+      /* update ins state from horizontal filter */
+      booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
+      booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
+      booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
+      booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
+      booz_ins_ltp_pos.x   = POS_BFP_OF_REAL(b2_hff_state.x);
+      booz_ins_ltp_pos.y   = POS_BFP_OF_REAL(b2_hff_state.y);
+
 #ifdef GPS_LAG
       /* increase lag counter on last saved state */
       if (b2_hff_rb_n > 0)