[paparazzi-commits] [4350] fixes for simulation

[Gautier Hattenberger]

Revision: 4350
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4350
Author:   gautier
Date:     2009-11-27 13:37:48 +0000 (Fri, 27 Nov 2009)
Log Message:
-----------
fixes for simulation

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/booz2_gps.h
    paparazzi3/trunk/sw/airborne/booz/booz2_main.c

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_gps.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_gps.h       2009-11-27 13:36:28 UTC 
(rev 4349)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_gps.h       2009-11-27 13:37:48 UTC 
(rev 4350)
@@ -58,6 +58,7 @@
 #define GPS_LINKChAvailable() (FALSE)
 #define GPS_LINKGetch() (TRUE)
 #include "nps_sensors.h"
+#include "flight_plan.h"
 
 static inline void  booz_gps_feed_value() {
   booz_gps_state.ecef_pos.x = sensors.gps.ecef_pos.x * 100.;
@@ -66,6 +67,10 @@
   booz_gps_state.ecef_vel.x = sensors.gps.ecef_vel.x * 100.;
   booz_gps_state.ecef_vel.y = sensors.gps.ecef_vel.y * 100.;
   booz_gps_state.ecef_vel.z = sensors.gps.ecef_vel.z * 100.;
+  booz_gps_state.lla_pos.lat = DegOfRad(sensors.gps.lla_pos.lat) * 1e7;
+  booz_gps_state.lla_pos.lon = DegOfRad(sensors.gps.lla_pos.lon) * 1e7;
+  booz_gps_state.lla_pos.alt = sensors.gps.lla_pos.alt * 100. + NAV_HMSL0;
+  booz_gps_state.hmsl        = sensors.gps.lla_pos.alt * 100.;
   booz_gps_state.fix = BOOZ2_GPS_FIX_3D;
   booz_gps_available = TRUE;
 }

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_main.c      2009-11-27 13:36:28 UTC 
(rev 4349)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_main.c      2009-11-27 13:37:48 UTC 
(rev 4350)
@@ -145,7 +145,7 @@
 
   int_enable();
 
-#ifdef BOOZ_START_DELAY
+#if defined BOOZ_START_DELAY && ! defined SITL
   delay_done = FALSE;
   init_done_time = T0TC;
 #endif
@@ -157,7 +157,8 @@
   //  t0 = T0TC;
 
   booz_imu_periodic();
-#ifdef BOOZ_START_DELAY
+//#ifdef BOOZ_START_DELAY
+#if defined BOOZ_START_DELAY && ! defined SITL
   if (!delay_done) {
     if ((uint32_t)(T0TC-init_done_time) < 
SYS_TICS_OF_USEC((uint32_t)(BOOZ_START_DELAY*1e6))) return;
     else delay_done = TRUE;