[paparazzi-commits] [4188] change ins_init() to use LLA instead of ECEF.

[Pascal Brisset]

Revision: 4188
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4188
Author:   hecto
Date:     2009-09-18 15:37:32 +0000 (Fri, 18 Sep 2009)
Log Message:
-----------
 change ins_init() to use LLA instead of ECEF. Should be called *after* hmsl is 
available

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

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_ins.c       2009-09-18 15:35:52 UTC 
(rev 4187)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.c       2009-09-18 15:37:32 UTC 
(rev 4188)
@@ -84,8 +84,16 @@
 void booz_ins_init() {
 #ifdef USE_INS_NAV_INIT
   booz_ins_ltp_initialised  = TRUE;
+
+  /** FIXME: should use the same code than MOVE_WP in booz2_datalink.c */
+  struct LlaCoor_i llh; /* Height above the ellipsoid */
+  llh.lat = INT32_RAD_OF_DEG(NAV_LAT0);
+  llh.lon = INT32_RAD_OF_DEG(NAV_LON0);
+  llh.alt = NAV_ALT0 - booz_ins_ltp_def.hmsl + booz_ins_ltp_def.lla.alt;
+
   struct EcefCoor_i nav_init;
-  VECT3_ASSIGN(nav_init,NAV_ECEF_X0,NAV_ECEF_Y0,NAV_ECEF_Z0);
+  ecef_of_lla_i(&nav_init, &llh);
+
   ltp_def_from_ecef_i(&booz_ins_ltp_def, &nav_init);
 #else
   booz_ins_ltp_initialised  = FALSE;