From 330be5c8bfcae80cfa2f75e9f48c128b0120911a Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Wed, 3 Mar 2010 13:49:58 +0000
Subject: [PATCH] move start delay from booz main to actuators

---
 .../booz/actuators/booz_actuators_asctec.c    | 19 +++++++++++++++++++
 .../booz/actuators/booz_actuators_mkk.c       | 16 ++++++++++++++++
 sw/airborne/booz/booz2_main.c                 | 12 ------------
 3 files changed, 35 insertions(+), 12 deletions(-)

diff --git a/sw/airborne/booz/actuators/booz_actuators_asctec.c b/sw/airborne/booz/actuators/booz_actuators_asctec.c
index d4858f8f43..1280ec7449 100644
--- a/sw/airborne/booz/actuators/booz_actuators_asctec.c
+++ b/sw/airborne/booz/actuators/booz_actuators_asctec.c
@@ -3,18 +3,37 @@
 
 #include "booz2_commands.h"
 #include "i2c.h"
+#include "sys_time.h"
 
 struct ActuatorsAsctec actuators_asctec; 
 
+uint32_t actuators_delay_time;
+bool_t   actuators_delay_done;
+
 void actuators_init(void) {
   actuators_asctec.cmd = NONE;
   actuators_asctec.cur_addr = FRONT;
   actuators_asctec.new_addr = FRONT;
   actuators_asctec.i2c_done = TRUE;
   actuators_asctec.nb_err = 0;
+
+#if defined BOOZ_START_DELAY && ! defined SITL
+  actuators_delay_done = FALSE;
+  SysTimeTimerStart(actuators_delay_time);
+#else
+  actuators_delay_done = TRUE;
+  actuators_delay_time = 0;
+#endif
 }
 
 void actuators_set(bool_t motors_on) {
+#if defined BOOZ_START_DELAY && ! defined SITL
+  if (!actuators_delay_done) {
+    if (SysTimeTimer(actuators_delay_time) < SYS_TICS_OF_SEC(BOOZ_START_DELAY)) return;
+    else actuators_delay_done = TRUE;
+  }
+#endif
+
   if (!actuators_asctec.i2c_done)
     actuators_asctec.nb_err++;
 
diff --git a/sw/airborne/booz/actuators/booz_actuators_mkk.c b/sw/airborne/booz/actuators/booz_actuators_mkk.c
index eacf138df5..5b08ebf5aa 100644
--- a/sw/airborne/booz/actuators/booz_actuators_mkk.c
+++ b/sw/airborne/booz/actuators/booz_actuators_mkk.c
@@ -31,6 +31,9 @@ struct ActuatorsMkk actuators_mkk;
 
 const uint8_t actuators_addr[ACTUATORS_MKK_NB] = ACTUATORS_MKK_ADDR;
 
+uint32_t actuators_delay_time;
+bool_t   actuators_delay_done;
+
 void actuators_init(void) {
 
   supervision_init();
@@ -38,10 +41,23 @@ void actuators_init(void) {
   actuators_mkk.i2c_done = TRUE;
   actuators_mkk.idx = 0;
 
+#if defined BOOZ_START_DELAY && ! defined SITL
+  actuators_delay_done = FALSE;
+  SysTimeTimerStart(actuators_delay_time);
+#else
+  actuators_delay_done = TRUE;
+  actuators_delay_time = 0;
+#endif
 }
 
 
 void actuators_set(bool_t motors_on) {
+#if defined BOOZ_START_DELAY && ! defined SITL
+  if (!actuators_delay_done) {
+    if (SysTimeTimer(actuators_delay_time) < SYS_TICS_OF_SEC(BOOZ_START_DELAY)) return;
+    else actuators_delay_done = TRUE;
+  }
+#endif
 
   supervision_run(motors_on, FALSE, booz2_commands);
   actuators_mkk.status = BUSY;
diff --git a/sw/airborne/booz/booz2_main.c b/sw/airborne/booz/booz2_main.c
index 9866b69603..84001f5dbf 100644
--- a/sw/airborne/booz/booz2_main.c
+++ b/sw/airborne/booz/booz2_main.c
@@ -129,24 +129,12 @@ STATIC_INLINE void booz2_main_init( void ) {
 
   int_enable();
 
-#if defined BOOZ_START_DELAY && ! defined SITL
-  delay_done = FALSE;
-  init_done_time = T0TC;
-#endif
-
 }
 
 
 STATIC_INLINE void booz2_main_periodic( void ) {
   booz_imu_periodic();
   
-#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;
-  }
-#endif
-  
   /* run control loops */
   booz2_autopilot_periodic();
   /* set actuators     */