[paparazzi-commits] [4102] start delay

[Gautier Hattenberger]

Revision: 4102
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4102
Author:   gautier
Date:     2009-09-08 11:40:50 +0000 (Tue, 08 Sep 2009)
Log Message:
-----------
start delay

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

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_main.c      2009-09-08 11:10:14 UTC 
(rev 4101)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_main.c      2009-09-08 11:40:50 UTC 
(rev 4102)
@@ -85,11 +85,13 @@
 }
 #endif /* SITL */
 
+uint32_t init_done_time;
 
 STATIC_INLINE void booz2_main_init( void ) {
 
   hw_init();
   sys_time_init();
+
   actuators_init();
   radio_control_init();
 
@@ -125,9 +127,7 @@
 
   int_enable();
 
-#ifdef BOOZ_START_DELAY
-  sys_time_usleep((int)(BOOZ_START_DELAY*1e6));
-#endif
+  init_done_time = T0TC;
 
 }
 
@@ -136,6 +136,10 @@
   //  t0 = T0TC;
 
   booz_imu_periodic();
+#ifdef BOOZ_START_DELAY
+  if ((uint32_t)(T0TC-init_done_time) < 
SYS_TICS_OF_USEC((uint32_t)(BOOZ_START_DELAY*1e6))) return;
+#endif
+
   /* run control loops */
   booz2_autopilot_periodic();
   /* set actuators     */