From cae4a2be8ada3d71b679764be2803decbe1f17ce Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Wed, 24 Feb 2010 09:40:44 +0000
Subject: [PATCH] add modules in booz

---
 sw/airborne/booz/booz2_datalink.c |  9 +++++++++
 sw/airborne/booz/booz2_main.c     | 23 +++++++++++++++++++++++
 2 files changed, 32 insertions(+)

diff --git a/sw/airborne/booz/booz2_datalink.c b/sw/airborne/booz/booz2_datalink.c
index e6e9965e37..a4faf3a1a2 100644
--- a/sw/airborne/booz/booz2_datalink.c
+++ b/sw/airborne/booz/booz2_datalink.c
@@ -22,7 +22,12 @@
  */
 
 #define DATALINK_C
+#define MODULES_DATALINK_C
+
 #include "datalink.h"
+#ifdef USE_MODULES
+#include "modules.h"
+#endif
 
 #include "settings.h"
 #include "downlink.h"
@@ -107,6 +112,10 @@ void dl_parse_msg(void) {
     }
     break;
 #endif /* USE_NAVIGATION */
+#ifdef USE_MODULES
+    /* Parse modules datalink */
+    modules_parse_datalink(msg_id);
+#endif
 
   }
 }
diff --git a/sw/airborne/booz/booz2_main.c b/sw/airborne/booz/booz2_main.c
index 45254336c1..e3a4177f60 100644
--- a/sw/airborne/booz/booz2_main.c
+++ b/sw/airborne/booz/booz2_main.c
@@ -21,6 +21,8 @@
  * Boston, MA 02111-1307, USA.
  */
 
+#define MODULES_C
+
 #include <inttypes.h>
 
 #include "init_hw.h"
@@ -73,6 +75,11 @@
 #include "nps_autopilot_booz.h"
 #endif
 
+#ifdef USE_MODULES
+#include "modules.h"
+#endif
+
+
 static inline void on_gyro_accel_event( void );
 static inline void on_baro_event( void );
 static inline void on_gps_event( void );
@@ -144,6 +151,10 @@ STATIC_INLINE void booz2_main_init( void ) {
 
   int_enable();
 
+#ifdef USE_MODULES
+  modules_init();
+#endif
+
 #if defined BOOZ_START_DELAY && ! defined SITL
   delay_done = FALSE;
   init_done_time = T0TC;
@@ -210,6 +221,10 @@ STATIC_INLINE void booz2_main_periodic( void ) {
   booz2_analog_periodic();
 #endif
 
+#ifdef USE_MODULES
+  modules_periodic_task();
+#endif
+
   if (booz2_autopilot_in_flight) {
     RunOnceEvery(512, { booz2_autopilot_flight_time++; datalink_time++; });
   }
@@ -233,6 +248,10 @@ STATIC_INLINE void booz2_main_event( void ) {
 
   Booz2AnalogBaroEvent(on_baro_event);
 
+#ifdef BOOZ2_SONAR
+  Booz2SonarEvent(booz_ins_update_sonar);
+#endif
+
 #ifdef USE_GPS
   Booz2GpsEvent(on_gps_event);
 #endif
@@ -241,6 +260,10 @@ STATIC_INLINE void booz2_main_event( void ) {
   BoozDetectGroundEvent();
 #endif
 
+#ifdef USE_MODULES
+  modules_event_task();
+#endif
+
 }
 
 static inline void on_gyro_accel_event( void ) {