[paparazzi-commits] [4616] move start delay from booz main to actuators

[Gautier Hattenberger]

Revision: 4616
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4616
Author:   gautier
Date:     2010-03-03 13:49:58 +0000 (Wed, 03 Mar 2010)
Log Message:
-----------
move start delay from booz main to actuators

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

Modified: paparazzi3/trunk/sw/airborne/booz/actuators/booz_actuators_asctec.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/actuators/booz_actuators_asctec.c 
2010-03-03 13:48:36 UTC (rev 4615)
+++ paparazzi3/trunk/sw/airborne/booz/actuators/booz_actuators_asctec.c 
2010-03-03 13:49:58 UTC (rev 4616)
@@ -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++;
 

Modified: paparazzi3/trunk/sw/airborne/booz/actuators/booz_actuators_mkk.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/actuators/booz_actuators_mkk.c    
2010-03-03 13:48:36 UTC (rev 4615)
+++ paparazzi3/trunk/sw/airborne/booz/actuators/booz_actuators_mkk.c    
2010-03-03 13:49:58 UTC (rev 4616)
@@ -31,6 +31,9 @@
 
 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 @@
   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;

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_main.c      2010-03-03 13:48:36 UTC 
(rev 4615)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_main.c      2010-03-03 13:49:58 UTC 
(rev 4616)
@@ -129,24 +129,12 @@
 
   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     */