[paparazzi-commits] [4652] update ins realignment stuff

[Felix Ruess]

Revision: 4652
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4652
Author:   flixr
Date:     2010-03-09 22:56:44 +0000 (Tue, 09 Mar 2010)
Log Message:
-----------
update ins realignment stuff

Modified Paths:
--------------
    paparazzi3/trunk/conf/settings/settings_booz2.xml
    paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
    paparazzi3/trunk/sw/airborne/booz/booz2_ins.h
    paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
    paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c
    paparazzi3/trunk/sw/airborne/modules/vision/cam_track.c

Modified: paparazzi3/trunk/conf/settings/settings_booz2.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_booz2.xml   2010-03-09 22:56:36 UTC 
(rev 4651)
+++ paparazzi3/trunk/conf/settings/settings_booz2.xml   2010-03-09 22:56:44 UTC 
(rev 4652)
@@ -48,7 +48,7 @@
       <dl_setting var="booz2_guidance_v_kd" min="-600" step="1" max="0"   
module="guidance/booz2_guidance_v" shortname="kd"/>
       <dl_setting var="booz2_guidance_v_ki" min="-300" step="1" max="0"   
module="guidance/booz2_guidance_v" shortname="ki" handler="SetKi" />
       <dl_setting var="booz2_guidance_v_z_sp" min="-5" step="0.5" max="3" 
module="guidance/booz2_guidance_v" shortname="sp" unit="2e-8m" alt_unit="m" 
alt_unit_coef="0.00390625"/>
-      <dl_setting var="booz_ins_vff_realign" min="0" step="1" max="1" 
module="booz2_ins" shortname="vff_realign" values="OFF|ON"/>
+      <dl_setting var="booz_ins_vf_realign" min="0" step="1" max="1" 
module="booz2_ins" shortname="vf_realign" values="OFF|ON"/>
    </dl_settings>
 
    <dl_settings NAME="Horiz Loop">
@@ -60,7 +60,7 @@
       <dl_setting var="booz2_guidance_h_igain" min="-400" step="1" max="0" 
module="guidance/booz2_guidance_h" shortname="ki" handler="SetKi"/>
       <dl_setting var="booz2_guidance_h_ngain" min="-400" step="1" max="0" 
module="guidance/booz2_guidance_h" shortname="kn"/>
       <dl_setting var="booz2_guidance_h_again" min="-400" step="1" max="0" 
module="guidance/booz2_guidance_h" shortname="ka"/>
-      <dl_setting var="booz_ins_hff_realign" min="0" step="1" max="1" 
module="booz2_ins" shortname="hff_realign" values="OFF|ON"/>
+      <dl_setting var="booz_ins_hf_realign" min="0" step="1" max="1" 
module="booz2_ins" shortname="hf_realign" values="OFF|ON"/>
    </dl_settings>
 
    <dl_settings NAME="NAV">

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_ins.c       2010-03-09 22:56:36 UTC 
(rev 4651)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.c       2010-03-09 22:56:44 UTC 
(rev 4652)
@@ -62,15 +62,15 @@
 struct FloatVect2 booz_ins_gps_pos_m_ned;
 struct FloatVect2 booz_ins_gps_speed_m_s_ned;
 #endif
-bool_t booz_ins_hff_realign;
+bool_t booz_ins_hf_realign;
 
 /* barometer                   */
 #ifdef USE_VFF
 int32_t booz_ins_qfe;
 bool_t  booz_ins_baro_initialised;
 int32_t booz_ins_baro_alt;
-bool_t  booz_ins_vff_realign;
 #endif
+bool_t  booz_ins_vf_realign;
 
 /* output                      */
 struct NedCoor_i booz_ins_ltp_pos;
@@ -100,14 +100,14 @@
 #else
   booz_ins_ltp_initialised  = FALSE;
 #endif
+  booz_ins_vf_realign = FALSE;
 #ifdef USE_VFF
   booz_ins_baro_initialised = FALSE;
-  booz_ins_vff_realign = FALSE;
   b2_vff_init(0., 0., 0.);
 #endif
+  booz_ins_hf_realign = FALSE;
 #ifdef USE_HFF
   b2_hff_init(0., 0., 0., 0.);
-  booz_ins_hff_realign = FALSE;
 #endif
   INT32_VECT3_ZERO(booz_ins_ltp_pos);
   INT32_VECT3_ZERO(booz_ins_ltp_speed);
@@ -126,6 +126,18 @@
 #endif
 }
 
+void booz_ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) {
+#ifdef USE_HFF
+  b2_hff_realign(pos, speed);
+#endif
+}
+
+void booz_ins_realign_v(float z) {
+#ifdef USE_VFF
+  b2_vff_realign(z);
+#endif
+}
+
 void booz_ins_propagate() {
   /* untilt accels */
   struct Int32Vect3 accel_body;
@@ -143,8 +155,9 @@
   }
   else { // feed accel from the sensors
     booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
-    booz_ins_enu_accel.z = -booz_ins_ltp_accel.z;
   }
+#else
+  booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
 #endif /* USE_VFF */
 
 #ifdef USE_HFF
@@ -160,6 +173,10 @@
     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;
@@ -179,8 +196,8 @@
     }
     booz_ins_baro_alt = (((int32_t)booz2_analog_baro_value - booz_ins_qfe) * 
BOOZ_INS_BARO_SENS_NUM)/BOOZ_INS_BARO_SENS_DEN;
     float alt_float = POS_FLOAT_OF_BFP(booz_ins_baro_alt);
-    if (booz_ins_vff_realign) {
-      booz_ins_vff_realign = FALSE;
+    if (booz_ins_vf_realign) {
+      booz_ins_vf_realign = FALSE;
       booz_ins_qfe = booz2_analog_baro_value;
       b2_vff_realign(0.);
       booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
@@ -213,15 +230,16 @@
     VECT2_SDIV(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_m_ned, 100.);
     VECT2_ASSIGN(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_cm_s_ned.x, 
booz_ins_gps_speed_cm_s_ned.y);
     VECT2_SDIV(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_m_s_ned, 100.);
-    if (booz_ins_hff_realign) {
-      booz_ins_hff_realign = FALSE;
+    if (booz_ins_hf_realign) {
+      booz_ins_hf_realign = FALSE;
 #ifdef SITL
       struct FloatVect2 true_pos, true_speed;
       VECT2_COPY(true_pos, fdm.ltpprz_pos);
       VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
       b2_hff_realign(true_pos, true_speed);
 #else
-      b2_hff_realign(booz_ins_gps_pos_m_ned, booz_ins_gps_speed_m_s_ned);
+      const struct FloatVect2 zero = {0.0, 0.0};
+      b2_hff_realign(booz_ins_gps_pos_m_ned, zero);
 #endif
     }
     b2_hff_update_gps();
@@ -231,23 +249,29 @@
     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 /* only hf */
+
+#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;
     booz_ins_ltp_speed.z =  (booz_ins_gps_speed_cm_s_ned.z * 
INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
-#endif /* only hf */
-#else /* hf not used */
+#endif /* vff not used */
+#endif /* hff used */
+
+
+#ifndef USE_HFF /* hff not used */
+#ifndef USE_VFF /* neither hf nor vf used */
+    INT32_VECT3_SCALE_3(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned, 
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
+    INT32_VECT3_SCALE_3(booz_ins_ltp_speed, booz_ins_gps_speed_cm_s_ned, 
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
+#else /* only vff used */
     INT32_VECT2_SCALE_2(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned, 
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
     INT32_VECT2_SCALE_2(booz_ins_ltp_speed, booz_ins_gps_speed_cm_s_ned, 
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
+#endif
+
 #ifdef USE_GPS_LAG_HACK
     VECT2_COPY(d_pos, booz_ins_ltp_speed);
     INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
     VECT2_ADD(booz_ins_ltp_pos, d_pos);
 #endif
-#ifndef USE_VFF /* neither hf nor vf used */
-    INT32_VECT3_SCALE_2(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned, 
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
-    INT32_VECT3_SCALE_2(booz_ins_ltp_speed, booz_ins_gps_speed_cm_s_ned, 
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
-#endif /* neither hf nor vf used */
-#endif /* USE_HFF */
+#endif /* hff not used */
 
     INT32_VECT3_ENU_OF_NED(booz_ins_enu_pos, booz_ins_ltp_pos);
     INT32_VECT3_ENU_OF_NED(booz_ins_enu_speed, booz_ins_ltp_speed);

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_ins.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_ins.h       2010-03-09 22:56:36 UTC 
(rev 4651)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.h       2010-03-09 22:56:44 UTC 
(rev 4652)
@@ -39,7 +39,6 @@
 extern int32_t booz_ins_baro_alt;
 extern int32_t booz_ins_qfe;
 extern bool_t  booz_ins_baro_initialised;
-extern bool_t  booz_ins_vff_realign;
 #endif
 
 /* output LTP NED               */
@@ -55,10 +54,14 @@
 extern struct FloatVect2 booz_ins_gps_pos_m_ned;
 extern struct FloatVect2 booz_ins_gps_speed_m_s_ned;
 #endif
-extern bool_t  booz_ins_hff_realign;
 
+extern bool_t booz_ins_hf_realign;
+extern bool_t booz_ins_vf_realign;
+
 extern void booz_ins_init( void );
 extern void booz_ins_periodic( void );
+extern void booz_ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed);
+extern void booz_ins_realign_v(float z);
 extern void booz_ins_propagate( void );
 extern void booz_ins_update_baro( void );
 extern void booz_ins_update_gps( void );

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c        2010-03-09 
22:56:36 UTC (rev 4651)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c        2010-03-09 
22:56:44 UTC (rev 4652)
@@ -240,15 +240,13 @@
 /** Reset the geographic reference to the current GPS fix */
 unit_t nav_reset_reference( void ) {
   booz_ins_ltp_initialised = FALSE;
-  booz_ins_vff_realign = TRUE;
-#ifdef USE_HFF
-  booz_ins_hff_realign = TRUE;
-#endif
+  booz_ins_hf_realign = TRUE;
+  booz_ins_vf_realign = TRUE;
   return 0;
 }
 
 unit_t nav_reset_alt( void ) {
-  booz_ins_vff_realign = TRUE;
+  booz_ins_vf_realign = TRUE;
 
 #ifdef USE_GPS
   booz_ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt;

Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c       
2010-03-09 22:56:36 UTC (rev 4651)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c       
2010-03-09 22:56:44 UTC (rev 4652)
@@ -152,7 +152,7 @@
   }
   else {
     // reset vertical filter until takeoff
-    //booz_ins_vff_realign = TRUE;
+    //booz_ins_vf_realign = TRUE;
   }
 
   switch (booz2_guidance_v_mode) {

Modified: paparazzi3/trunk/sw/airborne/modules/vision/cam_track.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vision/cam_track.c     2010-03-09 
22:56:36 UTC (rev 4651)
+++ paparazzi3/trunk/sw/airborne/modules/vision/cam_track.c     2010-03-09 
22:56:44 UTC (rev 4652)
@@ -122,18 +122,16 @@
 void track_event(void) {
   if (!booz_ins_ltp_initialised) {
     booz_ins_ltp_initialised = TRUE;
-#ifdef USE_HFF
-    booz_ins_hff_realign = TRUE;
-#endif
+    booz_ins_hf_realign = TRUE;
   }
 
 #ifdef USE_HFF
-  if (booz_ins_hff_realign) {
-    booz_ins_hff_realign = FALSE;
+  if (booz_ins_hf_realign) {
+    booz_ins_hf_realign = FALSE;
     struct FloatVect2 pos, zero;
     pos.x = -target_pos_ned.x;
     pos.y = -target_pos_ned.y;
-    b2_hff_realign(pos, zero);
+    booz_ins_realign_h(pos, zero);
   }
   b2_hff_update_pos(-target_pos_ned.x, -target_pos_ned.y);
   booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);