[paparazzi-commits] [4655] move ins state update into hff

[Felix Ruess]

Revision: 4655
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4655
Author:   flixr
Date:     2010-03-10 00:42:54 +0000 (Wed, 10 Mar 2010)
Log Message:
-----------
move 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-10 00:42:42 UTC 
(rev 4654)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.c       2010-03-10 00:42:54 UTC 
(rev 4655)
@@ -100,11 +100,11 @@
 #else
   booz_ins_ltp_initialised  = FALSE;
 #endif
-  booz_ins_vf_realign = FALSE;
 #ifdef USE_VFF
   booz_ins_baro_initialised = FALSE;
   b2_vff_init(0., 0., 0.);
 #endif
+  booz_ins_vf_realign = FALSE;
   booz_ins_hf_realign = FALSE;
 #ifdef USE_HFF
   b2_hff_init(0., 0., 0., 0.);
@@ -229,12 +229,6 @@
 #endif
     }
     b2_hff_update_gps();
-    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);
 
 #ifndef USE_VFF /* vff not used */
     booz_ins_ltp_pos.z =  (booz_ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / 
INT32_POS_OF_CM_DEN;

Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c      2010-03-10 
00:42:42 UTC (rev 4654)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c      2010-03-10 
00:42:54 UTC (rev 4655)
@@ -500,6 +500,8 @@
 #ifdef GPS_LAG
   if (GPS_LAG_N == 0) {
 #endif
+
+    /* update filter state with measurement */
     b2_hff_update_x(&b2_hff_state, booz_ins_gps_pos_m_ned.x, Rgps_pos);
     b2_hff_update_y(&b2_hff_state, booz_ins_gps_pos_m_ned.y, Rgps_pos);
 #ifdef B2_HFF_UPDATE_SPEED
@@ -507,6 +509,14 @@
     b2_hff_update_ydot(&b2_hff_state, booz_ins_gps_speed_m_s_ned.y, Rgps_vel);
 #endif
 
+    /* update ins state */
+    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
   } else if (b2_hff_rb_n > 0) {
     /* roll back if state was saved approx when GPS was valid */