diff --git a/sw/airborne/arch/chibios/mcu_arch.h b/sw/airborne/arch/chibios/mcu_arch.h
index d4dafe80b9..6cee6b18aa 100644
--- a/sw/airborne/arch/chibios/mcu_arch.h
+++ b/sw/airborne/arch/chibios/mcu_arch.h
@@ -33,9 +33,6 @@
 
 #include "std.h"
 
-#define mcu_int_enable()  {}
-#define mcu_int_disable() {}
-
 extern void mcu_arch_init(void);
 
 #if USE_HARD_FAULT_RECOVERY
diff --git a/sw/airborne/arch/linux/mcu_arch.h b/sw/airborne/arch/linux/mcu_arch.h
index df1c145f83..9ab25c5291 100644
--- a/sw/airborne/arch/linux/mcu_arch.h
+++ b/sw/airborne/arch/linux/mcu_arch.h
@@ -32,7 +32,4 @@
 
 extern void mcu_arch_init(void);
 
-#define mcu_int_enable() {}
-#define mcu_int_disable() {}
-
 #endif /* MCU_ARCH_H_ */
diff --git a/sw/airborne/arch/sim/mcu_arch.h b/sw/airborne/arch/sim/mcu_arch.h
index 0d2bfe027c..daca2fc7b5 100644
--- a/sw/airborne/arch/sim/mcu_arch.h
+++ b/sw/airborne/arch/sim/mcu_arch.h
@@ -25,8 +25,6 @@
 
 extern void mcu_arch_init(void);
 
-#define mcu_int_enable() {}
-#define mcu_int_disable() {}
 
 #endif /* SIM_MCU_ARCH_H */
 
diff --git a/sw/airborne/arch/stm32/mcu_arch.h b/sw/airborne/arch/stm32/mcu_arch.h
index 9e9740f407..b411dd1d11 100644
--- a/sw/airborne/arch/stm32/mcu_arch.h
+++ b/sw/airborne/arch/stm32/mcu_arch.h
@@ -51,9 +51,6 @@ extern void reset_to_dfu(void);
                  );                            \
   }
 
-#define mcu_int_enable()  {}
-#define mcu_int_disable() {}
-
 uint32_t timer_get_frequency(uint32_t timer_peripheral);
 
 #endif /* STM32_MCU_ARCH_H */
diff --git a/sw/airborne/firmwares/demo/demo_ahrs_actuators.c b/sw/airborne/firmwares/demo/demo_ahrs_actuators.c
index cea42ee29e..fb6922d50c 100644
--- a/sw/airborne/firmwares/demo/demo_ahrs_actuators.c
+++ b/sw/airborne/firmwares/demo/demo_ahrs_actuators.c
@@ -98,7 +98,6 @@ static inline void main_init(void)
 
   settings_init();
 
-  mcu_int_enable();
 
   downlink_init();
 
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 2474c1c109..d9db08f32e 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -47,17 +47,8 @@
 #if USE_IMU
 #include "subsystems/imu.h"
 #endif
-#if USE_AHRS
-#include "subsystems/ahrs.h"
-#endif
-#if USE_BARO_BOARD
-#include "subsystems/sensors/baro.h"
-PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BOARD)
-#endif
-
 
 // autopilot
-#include "state.h"
 #include "autopilot.h"
 #include "firmwares/fixedwing/nav.h"
 #include "generated/flight_plan.h"
@@ -115,13 +106,6 @@ PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
  */
 PRINT_CONFIG_VAR(MODULES_FREQUENCY)
 
-/* BARO_PERIODIC_FREQUENCY is defined in baro_board.makefile
- * defaults to 50Hz or set by BARO_PERIODIC_FREQUENCY configure option in airframe file
- */
-#if USE_BARO_BOARD
-PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
-#endif
-
 
 #if USE_IMU
 #ifdef AHRS_PROPAGATE_FREQUENCY
@@ -141,10 +125,6 @@ tid_t attitude_tid;    ///< id for attitude_loop() timer
 tid_t navigation_tid;  ///< id for navigation_task() timer
 #endif
 tid_t monitor_tid;     ///< id for monitor_task() timer
-#if USE_BARO_BOARD
-tid_t baro_tid;          ///< id for baro_periodic() timer
-#endif
-
 
 void init_ap(void)
 {
@@ -152,27 +132,8 @@ void init_ap(void)
   mcu_init();
 #endif /* SINGLE_MCU */
 
-  /** - start interrupt task */
-  mcu_int_enable();
-
-#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
-  pprz_trig_int_init();
-#endif
-
-  /****** initialize and reset state interface ********/
-
-  stateInit();
-
   /************* Sensors initialization ***************/
 
-#if USE_AHRS
-  ahrs_init();
-#endif
-
-#if USE_BARO_BOARD
-  baro_init();
-#endif
-
   /************* Links initialization ***************/
 #if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
   link_mcu_init();
@@ -202,9 +163,6 @@ void init_ap(void)
   modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
   telemetry_tid = sys_time_register_timer(1. / TELEMETRY_FREQUENCY, NULL);
   monitor_tid = sys_time_register_timer(1.0, NULL);
-#if USE_BARO_BOARD
-  baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
-#endif
 
 #if DOWNLINK
   downlink_init();
@@ -233,12 +191,6 @@ void handle_periodic_tasks_ap(void)
     sensors_task();
   }
 
-#if USE_BARO_BOARD
-  if (sys_time_check_and_ack_timer(baro_tid)) {
-    baro_periodic();
-  }
-#endif
-
 #if USE_GENERATED_AUTOPILOT
   if (sys_time_check_and_ack_timer(attitude_tid)) {
     autopilot_periodic();
@@ -375,10 +327,6 @@ void event_task_ap(void)
   mcu_event();
 #endif /* SINGLE_MCU */
 
-#if USE_BARO_BOARD
-  BaroEvent();
-#endif
-
   modules_event_task();
 
 #if defined MCU_SPI_LINK || defined MCU_UART_LINK
diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c
index 39537cfe34..0b8b582291 100644
--- a/sw/airborne/firmwares/fixedwing/main_fbw.c
+++ b/sw/airborne/firmwares/fixedwing/main_fbw.c
@@ -396,10 +396,6 @@ void init_fbw(void)
   fbw_periodic_tid = sys_time_register_timer((1. / 60.), NULL);
   electrical_tid = sys_time_register_timer(0.1, NULL);
 
-#ifndef SINGLE_MCU
-  mcu_int_enable();
-#endif
-
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_fbw_status);
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_COMMANDS, send_commands);
diff --git a/sw/airborne/firmwares/rotorcraft/main_ap.c b/sw/airborne/firmwares/rotorcraft/main_ap.c
index 6e67861276..135a60c733 100644
--- a/sw/airborne/firmwares/rotorcraft/main_ap.c
+++ b/sw/airborne/firmwares/rotorcraft/main_ap.c
@@ -31,40 +31,10 @@
 #define ABI_C
 
 #include <inttypes.h>
-#include "mcu.h"
-#include "mcu_periph/sys_time.h"
 #include "led.h"
 
-#include "subsystems/datalink/telemetry.h"
-#include "subsystems/datalink/datalink.h"
-#include "subsystems/datalink/downlink.h"
-#include "subsystems/settings.h"
-
-#include "subsystems/commands.h"
-#include "subsystems/actuators.h"
-
-#if USE_IMU
-#include "subsystems/imu.h"
-#endif
-#if USE_GPS
-#include "subsystems/gps.h"
-#endif
-
-#if USE_BARO_BOARD
-#include "subsystems/sensors/baro.h"
-PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BOARD)
-#endif
-
-#include "subsystems/electrical.h"
-
-#include "autopilot.h"
-
 #include "subsystems/radio_control.h"
 
-#include "subsystems/ahrs.h"
-
-#include "state.h"
-
 #include "firmwares/rotorcraft/main_ap.h"
 
 #ifdef SITL
@@ -74,10 +44,6 @@ PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BO
 #include "generated/modules.h"
 #include "subsystems/abi.h"
 
-// needed for stop-gap measure waypoints_localize_all()
-#include "subsystems/navigation/waypoints.h"
-
-
 /* if PRINT_CONFIG is defined, print some config options */
 PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
 /* SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY should be an integer, otherwise the timer will not be correct */
@@ -95,9 +61,6 @@ PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
  */
 PRINT_CONFIG_VAR(MODULES_FREQUENCY)
 
-/* BARO_PERIODIC_FREQUENCY is defined in the shared/baro_board.makefile and defaults to 50Hz */
-PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
-
 #if USE_AHRS && USE_IMU && (defined AHRS_PROPAGATE_FREQUENCY)
 #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
 #warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
@@ -105,49 +68,47 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t
 #endif
 #endif
 
-tid_t main_periodic_tid; ///< id for main_periodic() timer
-tid_t modules_tid;       ///< id for modules_periodic_task() timer
-tid_t failsafe_tid;      ///< id for failsafe_check() timer
-tid_t radio_control_tid; ///< id for radio_control_periodic_task() timer
-tid_t electrical_tid;    ///< id for electrical_periodic() timer
-tid_t telemetry_tid;     ///< id for telemetry_periodic() timer
-#if USE_BARO_BOARD
-tid_t baro_tid;          ///< id for baro_periodic() timer
+/**
+ * IDs for timers
+ */
+tid_t modules_mcu_core_tid; // single step
+tid_t modules_sensors_tid;
+tid_t modules_estimation_tid;
+tid_t modules_radio_control_tid;
+tid_t modules_control_actuators_tid; // single step
+tid_t modules_datalink_tid;
+tid_t modules_default_tid;
+tid_t failsafe_tid;      ///< id for failsafe_check() timer FIXME
+
+#define SYS_PERIOD (1.f / PERIODIC_FREQUENCY)
+#define DATALINK_PERIOD (1.f / TELEMETRY_FREQUENCY)
+
+#ifndef ESTIMATION_OFFSET
+#define ESTIMATION_OFFSET 6e-4f
+#endif
+
+#ifndef CONTROL_OFFSET
+#define CONTROL_OFFSET 7e-4f
+#endif
+
+#ifndef DEFAULT_OFFSET
+#define DEFAULT_OFFSET 8e-4f
 #endif
 
 void main_init(void)
 {
-  mcu_init();
-
-#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
-  pprz_trig_int_init();
-#endif
-
-  electrical_init();
-
-  stateInit();
-
-#ifndef INTER_MCU_AP
-  actuators_init();
-#else
-  intermcu_init();
-#endif
-
+  modules_mcu_init();
+  modules_core_init();
+  modules_sensors_init();
+  modules_estimation_init();
 #ifndef INTER_MCU_AP
   radio_control_init();
+  // modules_radio_control_init(); FIXME
 #endif
-
-#if USE_BARO_BOARD
-  baro_init();
-#endif
-
-#if USE_AHRS
-  ahrs_init();
-#endif
-
-  autopilot_init();
-
-  modules_init();
+  modules_control_init();
+  modules_actuators_init();
+  modules_datalink_init();
+  modules_default_init();
 
   // call autopilot implementation init after guidance modules init
   // it will set startup mode
@@ -157,37 +118,17 @@ void main_init(void)
   autopilot_static_init();
 #endif
 
-  /* temporary hack:
-   * Since INS is now a module, LTP_DEF is not yet initialized when autopilot_init is called
-   * This led to the problem that global waypoints were not "localized",
-   * so as a stop-gap measure we localize them all (again) here..
-   */
-  waypoints_localize_all();
-
-  settings_init();
-
-  mcu_int_enable();
-
-#if DOWNLINK
-  downlink_init();
-#endif
-
-#ifdef INTER_MCU_AP
-  intermcu_init();
-#endif
+  // register timers with temporal dependencies
+  modules_sensors_tid = sys_time_register_timer(SYS_PERIOD, NULL);
+  modules_estimation_tid = sys_time_register_timer_offset(modules_sensors_tid, ESTIMATION_OFFSET, NULL);
+  modules_control_actuators_tid = sys_time_register_timer_offset(modules_sensors_tid, CONTROL_OFFSET, NULL);
+  modules_default_tid = sys_time_register_timer_offset(modules_sensors_tid, DEFAULT_OFFSET, NULL); // should it be an offset ?
 
   // register the timers for the periodic functions
-  main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
-#if PERIODIC_FREQUENCY != MODULES_FREQUENCY
-  modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
-#endif
-  radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
-  failsafe_tid = sys_time_register_timer(0.05, NULL);
-  electrical_tid = sys_time_register_timer(0.1, NULL);
-  telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
-#if USE_BARO_BOARD
-  baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
-#endif
+  modules_mcu_core_tid = sys_time_register_timer(SYS_PERIOD, NULL);
+  modules_radio_control_tid = sys_time_register_timer((1. / 60.), NULL); // FIXME
+  modules_datalink_tid = sys_time_register_timer(DATALINK_PERIOD, NULL);
+  failsafe_tid = sys_time_register_timer(0.05, NULL); // FIXME
 
 #if USE_IMU
   // send body_to_imu from here for now
@@ -201,70 +142,60 @@ void main_init(void)
 
 void handle_periodic_tasks(void)
 {
-  if (sys_time_check_and_ack_timer(main_periodic_tid)) {
-    main_periodic();
-#if PERIODIC_FREQUENCY == MODULES_FREQUENCY
-    /* Use the main periodc freq timer for modules if the freqs are the same
-     * This is mainly useful for logging each step.
-     */
-    modules_periodic_task();
-#else
+  if (sys_time_check_and_ack_timer(modules_sensors_tid)) {
+    modules_sensors_periodic_task();
   }
-  /* separate timer for modules, since it has a different freq than main */
-  if (sys_time_check_and_ack_timer(modules_tid)) {
-    modules_periodic_task();
-#endif
+
+  if (sys_time_check_and_ack_timer(modules_estimation_tid)) {
+    modules_estimation_periodic_task();
   }
-  if (sys_time_check_and_ack_timer(radio_control_tid)) {
+
+  if (sys_time_check_and_ack_timer(modules_radio_control_tid)) {
     radio_control_periodic_task();
+    modules_radio_control_periodic_task(); // FIXME integrate above
   }
-  if (sys_time_check_and_ack_timer(failsafe_tid)) {
-    failsafe_check();
-  }
-  if (sys_time_check_and_ack_timer(electrical_tid)) {
-    electrical_periodic();
-  }
-  if (sys_time_check_and_ack_timer(telemetry_tid)) {
-    telemetry_periodic();
-  }
-#if USE_BARO_BOARD
-  if (sys_time_check_and_ack_timer(baro_tid)) {
-    baro_periodic();
-  }
-#endif
-}
 
-void main_periodic(void)
-{
-#if INTER_MCU_AP
-  /* Inter-MCU watchdog */
-  intermcu_periodic();
-#endif
-
-  /* run control loops */
-  autopilot_periodic();
-  /* set actuators     */
-  //actuators_set(autopilot_get_motors_on());
+  if (sys_time_check_and_ack_timer(modules_control_actuators_tid)) {
+    modules_control_periodic_task();
 
 #if USE_THROTTLE_CURVES
-  throttle_curve_run(commands, autopilot_get_mode());
+    throttle_curve_run(commands, autopilot_get_mode());
 #endif
 
 #ifndef INTER_MCU_AP
-  SetActuatorsFromCommands(commands, autopilot_get_mode());
+    SetActuatorsFromCommands(commands, autopilot_get_mode());
 #else
-  intermcu_set_actuators(commands, autopilot_get_mode());
+    intermcu_set_actuators(commands, autopilot_get_mode());
 #endif
+    modules_actuators_periodic_task(); // FIXME integrate above in actuators periodic
 
-  if (autopilot_in_flight()) {
-    RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++);
+    if (autopilot_in_flight()) {
+      RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++); // TODO make it 1Hz periodic ?
+    }
   }
 
-#if defined DATALINK || defined SITL
-  RunOnceEvery(PERIODIC_FREQUENCY, datalink_time++);
-#endif
+  if (sys_time_check_and_ack_timer(modules_default_tid)) {
+    modules_default_periodic_task();
+  }
+
+  if (sys_time_check_and_ack_timer(modules_mcu_core_tid)) {
+    modules_mcu_periodic_task();
+    modules_core_periodic_task();
+    RunOnceEvery(10, LED_PERIODIC()); // FIXME periodic in led module
+  }
+
+  if (sys_time_check_and_ack_timer(modules_datalink_tid)) {
+    telemetry_periodic();
+    modules_datalink_periodic_task(); // FIXME integrate above
+#if defined DATALINK || defined SITL
+    RunOnceEvery(TELEMETRY_FREQUENCY, datalink_time++);
+#endif
+  }
+
+  if (sys_time_check_and_ack_timer(failsafe_tid)) {
+    failsafe_check(); // FIXME integrate somewhere else
+  }
 
-  RunOnceEvery(10, LED_PERIODIC());
 }
 
 void telemetry_periodic(void)
@@ -338,18 +269,15 @@ void failsafe_check(void)
 
 void main_event(void)
 {
-  /* event functions for mcu peripherals: i2c, usb_serial.. */
-  mcu_event();
-
+  modules_mcu_event_task();
+  modules_core_event_task();
+  modules_sensors_event_task();
+  modules_estimation_event_task();
+  modules_radio_control_event_task(); // FIXME
   if (autopilot.use_rc) {
     RadioControlEvent(autopilot_on_rc_frame);
   }
-
-#if USE_BARO_BOARD
-  BaroEvent();
-#endif
-
-  autopilot_event();
-
-  modules_event_task();
+  modules_actuators_event_task();
+  modules_datalink_event_task();
+  modules_default_event_task();
 }
diff --git a/sw/airborne/firmwares/rotorcraft/main_fbw.c b/sw/airborne/firmwares/rotorcraft/main_fbw.c
index e35b52d1c6..6e609bb4bf 100644
--- a/sw/airborne/firmwares/rotorcraft/main_fbw.c
+++ b/sw/airborne/firmwares/rotorcraft/main_fbw.c
@@ -91,7 +91,6 @@ void main_init(void)
 
   modules_init();
 
-  mcu_int_enable();
 
   intermcu_init();
 
diff --git a/sw/airborne/firmwares/rover/main_ap.c b/sw/airborne/firmwares/rover/main_ap.c
index 47138891ce..0bd1f2f24a 100644
--- a/sw/airborne/firmwares/rover/main_ap.c
+++ b/sw/airborne/firmwares/rover/main_ap.c
@@ -30,49 +30,15 @@
 #define ABI_C
 
 #include <inttypes.h>
-#include "mcu.h"
-#include "mcu_periph/sys_time.h"
 #include "led.h"
 
-#include "subsystems/datalink/telemetry.h"
-#include "subsystems/datalink/datalink.h"
-#include "subsystems/datalink/downlink.h"
-#include "subsystems/settings.h"
-
-#include "subsystems/commands.h"
-#include "subsystems/actuators.h"
-
-#if USE_IMU
-#include "subsystems/imu.h"
-#endif
-#if USE_GPS
-#include "subsystems/gps.h"
-#endif
-
-#if USE_BARO_BOARD
-#include "subsystems/sensors/baro.h"
-PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BOARD)
-#endif
-
-#include "subsystems/electrical.h"
-
-#include "autopilot.h"
-
 #include "subsystems/radio_control.h"
 
-#include "subsystems/ahrs.h"
-
-#include "state.h"
-
 #include "firmwares/rover/main_ap.h"
 
 #include "generated/modules.h"
 #include "subsystems/abi.h"
 
-// needed for stop-gap measure waypoints_localize_all()
-#include "subsystems/navigation/waypoints.h"
-
-
 /* if PRINT_CONFIG is defined, print some config options */
 PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
 /* SYS_TIME_FREQUENCY/PERIODIC_FREQUENCY should be an integer, otherwise the timer will not be correct */
@@ -90,11 +56,6 @@ PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
  */
 PRINT_CONFIG_VAR(MODULES_FREQUENCY)
 
-#ifndef BARO_PERIODIC_FREQUENCY
-#define BARO_PERIODIC_FREQUENCY 50
-#endif
-PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
-
 #if USE_AHRS && USE_IMU && (defined AHRS_PROPAGATE_FREQUENCY)
 #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
 #warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
@@ -102,79 +63,61 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t
 #endif
 #endif
 
-tid_t main_periodic_tid; ///< id for main_periodic() timer
-tid_t modules_tid;       ///< id for modules_periodic_task() timer
-tid_t failsafe_tid;      ///< id for failsafe_check() timer
-tid_t radio_control_tid; ///< id for radio_control_periodic_task() timer
-tid_t electrical_tid;    ///< id for electrical_periodic() timer
-tid_t telemetry_tid;     ///< id for telemetry_periodic() timer
-#if USE_BARO_BOARD
-tid_t baro_tid;          ///< id for baro_periodic() timer
+/**
+ * IDs for timers
+ */
+tid_t modules_mcu_core_tid; // single step
+tid_t modules_sensors_tid;
+tid_t modules_estimation_tid;
+tid_t modules_radio_control_tid;
+tid_t modules_control_actuators_tid; // single step
+tid_t modules_datalink_tid;
+tid_t modules_default_tid;
+tid_t failsafe_tid;      ///< id for failsafe_check() timer FIXME
+
+#define SYS_PERIOD (1.f / PERIODIC_FREQUENCY)
+#define DATALINK_PERIOD (1.f / TELEMETRY_FREQUENCY)
+
+#ifndef ESTIMATION_OFFSET
+#define ESTIMATION_OFFSET 6e-4f
+#endif
+
+#ifndef CONTROL_OFFSET
+#define CONTROL_OFFSET 7e-4f
+#endif
+
+#ifndef DEFAULT_OFFSET
+#define DEFAULT_OFFSET 8e-4f
 #endif
 
 void main_init(void)
 {
-  mcu_init();
-
-#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
-  pprz_trig_int_init();
-#endif
-
-  electrical_init();
-
-  stateInit();
-
-  actuators_init();
-
-#if USE_MOTOR_MIXING
-  motor_mixing_init();
-#endif
-
+  modules_mcu_init();
+  modules_core_init();
+  modules_sensors_init();
+  modules_estimation_init();
   radio_control_init();
-
-#if USE_BARO_BOARD
-  baro_init();
-#endif
-
-#if USE_AHRS
-  ahrs_init();
-#endif
-
-  autopilot_init();
-
-  modules_init();
+  // modules_radio_control_init(); FIXME
+  modules_control_init();
+  modules_actuators_init();
+  modules_datalink_init();
+  modules_default_init();
 
   // call autopilot implementation init after guidance modules init
   // it will set startup mode
   autopilot_generated_init();
 
-  /* temporary hack:
-   * Since INS is now a module, LTP_DEF is not yet initialized when autopilot_init is called
-   * This led to the problem that global waypoints were not "localized",
-   * so as a stop-gap measure we localize them all (again) here..
-   */
-  //waypoints_localize_all();
-
-  settings_init();
-
-  mcu_int_enable();
-
-#if DOWNLINK
-  downlink_init();
-#endif
+  // register timers with temporal dependencies
+  modules_sensors_tid = sys_time_register_timer(SYS_PERIOD, NULL);
+  modules_estimation_tid = sys_time_register_timer_offset(modules_sensors_tid, ESTIMATION_OFFSET, NULL);
+  modules_control_actuators_tid = sys_time_register_timer_offset(modules_sensors_tid, CONTROL_OFFSET, NULL);
+  modules_default_tid = sys_time_register_timer_offset(modules_sensors_tid, DEFAULT_OFFSET, NULL); // should it be an offset ?
 
   // register the timers for the periodic functions
-  main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
-#if PERIODIC_FREQUENCY != MODULES_FREQUENCY
-  modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
-#endif
-  radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
-  failsafe_tid = sys_time_register_timer(0.05, NULL);
-  electrical_tid = sys_time_register_timer(0.1, NULL);
-  telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
-#if USE_BARO_BOARD
-  baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
-#endif
+  modules_mcu_core_tid = sys_time_register_timer(SYS_PERIOD, NULL);
+  modules_radio_control_tid = sys_time_register_timer((1. / 60.), NULL); // FIXME
+  modules_datalink_tid = sys_time_register_timer(DATALINK_PERIOD, NULL);
+  failsafe_tid = sys_time_register_timer(0.05, NULL); // FIXME
 
 #if USE_IMU
   // send body_to_imu from here for now
@@ -188,56 +131,51 @@ void main_init(void)
 
 void handle_periodic_tasks(void)
 {
-  if (sys_time_check_and_ack_timer(main_periodic_tid)) {
-    main_periodic();
-#if PERIODIC_FREQUENCY == MODULES_FREQUENCY
-    /* Use the main periodc freq timer for modules if the freqs are the same
-     * This is mainly useful for logging each step.
-     */
-    modules_periodic_task();
-#else
+  if (sys_time_check_and_ack_timer(modules_sensors_tid)) {
+    modules_sensors_periodic_task();
   }
-  /* separate timer for modules, since it has a different freq than main */
-  if (sys_time_check_and_ack_timer(modules_tid)) {
-    modules_periodic_task();
-#endif
+
+  if (sys_time_check_and_ack_timer(modules_estimation_tid)) {
+    modules_estimation_periodic_task();
   }
-  if (sys_time_check_and_ack_timer(radio_control_tid)) {
+
+  if (sys_time_check_and_ack_timer(modules_radio_control_tid)) {
     radio_control_periodic_task();
+    modules_radio_control_periodic_task(); // FIXME integrate above
   }
-  if (sys_time_check_and_ack_timer(failsafe_tid)) {
-    failsafe_check();
+
+  if (sys_time_check_and_ack_timer(modules_control_actuators_tid)) {
+    modules_control_periodic_task();
+    SetActuatorsFromCommands(commands, autopilot_get_mode());
+    modules_actuators_periodic_task(); // FIXME integrate above in actuators periodic
+
+    if (autopilot_in_flight()) {
+      RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++); // TODO make it 1Hz periodic ?
+    }
   }
-  if (sys_time_check_and_ack_timer(electrical_tid)) {
-    electrical_periodic();
+
+  if (sys_time_check_and_ack_timer(modules_default_tid)) {
+    modules_default_periodic_task();
   }
-  if (sys_time_check_and_ack_timer(telemetry_tid)) {
+
+  if (sys_time_check_and_ack_timer(modules_mcu_core_tid)) {
+    modules_mcu_periodic_task();
+    modules_core_periodic_task();
+    RunOnceEvery(10, LED_PERIODIC()); // FIXME periodic in led module
+  }
+
+  if (sys_time_check_and_ack_timer(modules_datalink_tid)) {
     telemetry_periodic();
-  }
-#if USE_BARO_BOARD
-  if (sys_time_check_and_ack_timer(baro_tid)) {
-    baro_periodic();
-  }
-#endif
-}
-
-void main_periodic(void)
-{
-  /* run control loops */
-  autopilot_periodic();
-  /* set actuators     */
-  //actuators_set(autopilot_get_motors_on());
-  SetActuatorsFromCommands(commands, autopilot_get_mode());
-
-  if (autopilot_in_flight()) {
-    RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++);
-  }
-
+    modules_datalink_periodic_task(); // FIXME integrate above
 #if defined DATALINK || defined SITL
-  RunOnceEvery(PERIODIC_FREQUENCY, datalink_time++);
+    RunOnceEvery(TELEMETRY_FREQUENCY, datalink_time++);
 #endif
+  }
+
+  if (sys_time_check_and_ack_timer(failsafe_tid)) {
+    failsafe_check(); // FIXME integrate somewhere else
+  }
 
-  RunOnceEvery(10, LED_PERIODIC());
 }
 
 void telemetry_periodic(void)
@@ -271,19 +209,16 @@ void failsafe_check(void)
 
 void main_event(void)
 {
-  /* event functions for mcu peripherals: i2c, usb_serial.. */
-  mcu_event();
-
+  modules_mcu_event_task();
+  modules_core_event_task();
+  modules_sensors_event_task();
+  modules_estimation_event_task();
+  modules_radio_control_event_task(); // FIXME
   if (autopilot.use_rc) {
     RadioControlEvent(autopilot_on_rc_frame);
   }
-
-#if USE_BARO_BOARD
-  BaroEvent();
-#endif
-
-  autopilot_event();
-
-  modules_event_task();
+  modules_actuators_event_task();
+  modules_datalink_event_task();
+  modules_default_event_task();
 }
 
diff --git a/sw/airborne/firmwares/setup/setup_actuators.c b/sw/airborne/firmwares/setup/setup_actuators.c
index 5385011d7a..b93e833a84 100644
--- a/sw/airborne/firmwares/setup/setup_actuators.c
+++ b/sw/airborne/firmwares/setup/setup_actuators.c
@@ -83,7 +83,6 @@ static inline void main_init(void)
 
   modules_init();
 
-  mcu_int_enable();
   sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
 }
 
diff --git a/sw/airborne/firmwares/setup/usb_tunnel.c b/sw/airborne/firmwares/setup/usb_tunnel.c
index cc0d152d57..d3d66ec7e8 100644
--- a/sw/airborne/firmwares/setup/usb_tunnel.c
+++ b/sw/airborne/firmwares/setup/usb_tunnel.c
@@ -95,7 +95,6 @@ int main(void)
 
   VCOM_init();
 
-  mcu_int_enable();
 
   while (1) {
     VCOM_event();
diff --git a/sw/airborne/firmwares/tutorial/main_demo3.c b/sw/airborne/firmwares/tutorial/main_demo3.c
index e19e72ef2e..1c2470850c 100644
--- a/sw/airborne/firmwares/tutorial/main_demo3.c
+++ b/sw/airborne/firmwares/tutorial/main_demo3.c
@@ -25,7 +25,6 @@ static inline void main_init(void)
   mcu_init();
   sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
   uart0_init_tx();
-  mcu_int_enable();
 }
 
 static inline void main_periodic_task(void)
diff --git a/sw/airborne/firmwares/tutorial/main_demo4.c b/sw/airborne/firmwares/tutorial/main_demo4.c
index f573fe8893..1f0ca60063 100644
--- a/sw/airborne/firmwares/tutorial/main_demo4.c
+++ b/sw/airborne/firmwares/tutorial/main_demo4.c
@@ -26,7 +26,6 @@ static inline void main_init(void)
   mcu_init();
   sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
   uart0_init_tx();
-  mcu_int_enable();
 }
 
 static inline void main_periodic_task(void)
diff --git a/sw/airborne/firmwares/tutorial/main_demo5.c b/sw/airborne/firmwares/tutorial/main_demo5.c
index 24fa40bc53..a4a23ed26c 100644
--- a/sw/airborne/firmwares/tutorial/main_demo5.c
+++ b/sw/airborne/firmwares/tutorial/main_demo5.c
@@ -30,7 +30,6 @@ static inline void main_init(void)
   mcu_init();
   sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
   uart0_init_tx();
-  mcu_int_enable();
 }
 
 static inline void main_periodic_task(void)
diff --git a/sw/airborne/firmwares/tutorial/main_demo6.c b/sw/airborne/firmwares/tutorial/main_demo6.c
index c9ee9696ff..416075bd19 100644
--- a/sw/airborne/firmwares/tutorial/main_demo6.c
+++ b/sw/airborne/firmwares/tutorial/main_demo6.c
@@ -26,7 +26,6 @@ static inline void main_init(void)
   mcu_init();
   sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
   usb_serial_init();
-  mcu_int_enable();
 }
 
 static inline void main_periodic_task(void)
diff --git a/sw/airborne/mcu_periph/sys_time.c b/sw/airborne/mcu_periph/sys_time.c
index cd13c979bb..0da7685c4d 100644
--- a/sw/airborne/mcu_periph/sys_time.c
+++ b/sw/airborne/mcu_periph/sys_time.c
@@ -56,6 +56,22 @@ tid_t sys_time_register_timer(float duration, sys_time_cb cb)
   return -1;
 }
 
+tid_t sys_time_register_timer_offset(tid_t timer, float offset, sys_time_cb cb)
+{
+  if (timer >=0 && timer < SYS_TIME_NB_TIMER) {
+    for (tid_t i = 0; i < SYS_TIME_NB_TIMER; i++) {
+      if (!sys_time.timer[i].in_use && (timer < i)) { // timer should be already registered
+        sys_time.timer[i].cb         = cb;
+        sys_time.timer[i].elapsed    = false;
+        sys_time.timer[i].end_time   = sys_time.timer[timer].end_time + sys_time_ticks_of_sec(offset); // add offset to end time
+        sys_time.timer[i].duration   = sys_time.timer[timer].duration; // copy duration
+        sys_time.timer[i].in_use     = true;
+        return i;
+      }
+    }
+  }
+  return -1;
+}
 
 void sys_time_cancel_timer(tid_t id)
 {
@@ -69,10 +85,8 @@ void sys_time_cancel_timer(tid_t id)
 // FIXME: race condition ??
 void sys_time_update_timer(tid_t id, float duration)
 {
-  mcu_int_disable();
   sys_time.timer[id].end_time -= (sys_time.timer[id].duration - sys_time_ticks_of_sec(duration));
   sys_time.timer[id].duration = sys_time_ticks_of_sec(duration);
-  mcu_int_enable();
 }
 
 void sys_time_init(void)
diff --git a/sw/airborne/mcu_periph/sys_time.h b/sw/airborne/mcu_periph/sys_time.h
index 5bfbd29799..a1ca4e3fcc 100644
--- a/sw/airborne/mcu_periph/sys_time.h
+++ b/sw/airborne/mcu_periph/sys_time.h
@@ -93,6 +93,15 @@ extern void sys_time_init(void);
  */
 extern tid_t sys_time_register_timer(float duration, sys_time_cb cb);
 
+/**
+ * Register a new system timer with an fixed offset from another one.
+ * @param timer timer providing start time and duration
+ * @param offset offset in seconds beetween the timers (will overlap if longer than duration)
+ * @param cb Callback function that is called from the ISR when timer elapses, or NULL
+ * @return -1 if it failed, the timer id otherwise
+ */
+extern tid_t sys_time_register_timer_offset(tid_t timer, float offset, sys_time_cb cb);
+
 /**
  * Cancel a system timer by id.
  * @param id Timer id.
diff --git a/sw/airborne/test/mcu_periph/test_adc.c b/sw/airborne/test/mcu_periph/test_adc.c
index 774d623488..ad61e374d1 100644
--- a/sw/airborne/test/mcu_periph/test_adc.c
+++ b/sw/airborne/test/mcu_periph/test_adc.c
@@ -72,7 +72,6 @@ static inline void main_init(void)
   adc_buf_channel(ADC_7, &buf_adc[7], ADC_NB_SAMPLES);
 #endif
 
-  mcu_int_enable();
 }
 
 int main(void)
diff --git a/sw/airborne/test/mcu_periph/test_gpio.c b/sw/airborne/test/mcu_periph/test_gpio.c
index b24b1a2424..71b57f946d 100644
--- a/sw/airborne/test/mcu_periph/test_gpio.c
+++ b/sw/airborne/test/mcu_periph/test_gpio.c
@@ -62,7 +62,6 @@ int main(void)
   unsigned int tmr_2 = sys_time_register_timer(2, NULL);
   sys_time_register_timer(1, main_periodic);
 
-  mcu_int_enable();
 
   while (1) {
     if (sys_time_check_and_ack_timer(tmr_2)) {
diff --git a/sw/airborne/test/mcu_periph/test_sys_time_timer.c b/sw/airborne/test/mcu_periph/test_sys_time_timer.c
index 306822b771..f1f45a6385 100644
--- a/sw/airborne/test/mcu_periph/test_sys_time_timer.c
+++ b/sw/airborne/test/mcu_periph/test_sys_time_timer.c
@@ -37,7 +37,6 @@ int main(void)
   tid_t tmr_03 = sys_time_register_timer(0.3, NULL);
   sys_time_register_timer(0.5, main_periodic_05);
 
-  mcu_int_enable();
 
   while (1) {
     if (sys_time_check_and_ack_timer(tmr_02)) {
diff --git a/sw/airborne/test/mcu_periph/test_sys_time_usleep.c b/sw/airborne/test/mcu_periph/test_sys_time_usleep.c
index 17db2e7a71..125be5e2e0 100644
--- a/sw/airborne/test/mcu_periph/test_sys_time_usleep.c
+++ b/sw/airborne/test/mcu_periph/test_sys_time_usleep.c
@@ -34,7 +34,6 @@ int main(void)
   mcu_init();
   sys_time_register_timer(0.5, main_periodic_05);
 
-  mcu_int_enable();
 
   while (1) {
     /* sleep for 1s */
diff --git a/sw/airborne/test/peripherals/test_ami601.c b/sw/airborne/test/peripherals/test_ami601.c
index c34ff344ea..8e84fa231a 100644
--- a/sw/airborne/test/peripherals/test_ami601.c
+++ b/sw/airborne/test/peripherals/test_ami601.c
@@ -60,7 +60,6 @@ static inline void main_init(void)
   LED_ON(4);
   ami601_init();
   downlink_init();
-  mcu_int_enable();
 }
 
 static inline void main_periodic_task(void)
diff --git a/sw/airborne/test/peripherals/test_lis302dl_spi.c b/sw/airborne/test/peripherals/test_lis302dl_spi.c
index 22c2418b63..f100509141 100644
--- a/sw/airborne/test/peripherals/test_lis302dl_spi.c
+++ b/sw/airborne/test/peripherals/test_lis302dl_spi.c
@@ -68,7 +68,6 @@ int main(void)
 static inline void main_init(void)
 {
   mcu_init();
-  mcu_int_enable();
 
   sys_time_register_timer((1. / 50), NULL);
   downlink_init();
diff --git a/sw/airborne/test/peripherals/test_ms2100.c b/sw/airborne/test/peripherals/test_ms2100.c
index 997537efb2..38979184fc 100644
--- a/sw/airborne/test/peripherals/test_ms2100.c
+++ b/sw/airborne/test/peripherals/test_ms2100.c
@@ -56,7 +56,6 @@ static inline void main_init(void)
   ms2100_init(&ms2100, &(MS2100_SPI_DEV), MS2100_SLAVE_IDX);
   downlink_init();
   pprz_dl_init();
-  mcu_int_enable();
 }
 
 static inline void main_periodic_task(void)
diff --git a/sw/airborne/test/subsystems/test_ahrs.c b/sw/airborne/test/subsystems/test_ahrs.c
index dd1f6ec9a5..9fbc704fb5 100644
--- a/sw/airborne/test/subsystems/test_ahrs.c
+++ b/sw/airborne/test/subsystems/test_ahrs.c
@@ -78,7 +78,6 @@ static inline void main_init(void)
   pprz_dl_init();
   downlink_init();
 
-  mcu_int_enable();
 }
 
 static inline void main_periodic_task(void)
diff --git a/sw/airborne/test/subsystems/test_imu.c b/sw/airborne/test/subsystems/test_imu.c
index 60aee95292..78855a1fc5 100644
--- a/sw/airborne/test/subsystems/test_imu.c
+++ b/sw/airborne/test/subsystems/test_imu.c
@@ -80,7 +80,6 @@ static inline void main_init(void)
 
   modules_init();
 
-  mcu_int_enable();
 
   downlink_init();
   pprz_dl_init();
diff --git a/sw/airborne/test/subsystems/test_radio_control.c b/sw/airborne/test/subsystems/test_radio_control.c
index 67fff2eff7..bec80d3c54 100644
--- a/sw/airborne/test/subsystems/test_radio_control.c
+++ b/sw/airborne/test/subsystems/test_radio_control.c
@@ -52,7 +52,6 @@ static inline void main_init(void)
   radio_control_init();
   downlink_init();
   pprz_dl_init();
-  mcu_int_enable();
 }
 
 extern uint32_t debug_len;
diff --git a/sw/airborne/test/subsystems/test_settings.c b/sw/airborne/test/subsystems/test_settings.c
index 34dad7adc8..4ffd900b72 100644
--- a/sw/airborne/test/subsystems/test_settings.c
+++ b/sw/airborne/test/subsystems/test_settings.c
@@ -72,7 +72,6 @@ static inline void main_init(void)
   settings_init();
   pprz_dl_init();
 
-  mcu_int_enable();
 
 #if DOWNLINK
   downlink_init();
diff --git a/sw/airborne/test/test_manual.c b/sw/airborne/test/test_manual.c
index e244b6c82d..e0f7729e1f 100644
--- a/sw/airborne/test/test_manual.c
+++ b/sw/airborne/test/test_manual.c
@@ -86,7 +86,6 @@ static inline void main_init(void)
 
   radio_control_init();
 
-  mcu_int_enable();
   main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
   radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
 
diff --git a/sw/airborne/test/test_math_trig_compressed.c b/sw/airborne/test/test_math_trig_compressed.c
index 485440dbdf..2bd02e634e 100644
--- a/sw/airborne/test/test_math_trig_compressed.c
+++ b/sw/airborne/test/test_math_trig_compressed.c
@@ -204,7 +204,6 @@ static inline void main_init(void)
 {
   mcu_init();
   sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
-  mcu_int_enable();
 
   downlink_init();
 }
diff --git a/sw/airborne/test/test_telemetry.c b/sw/airborne/test/test_telemetry.c
index d83d506334..fbddbcb7fd 100644
--- a/sw/airborne/test/test_telemetry.c
+++ b/sw/airborne/test/test_telemetry.c
@@ -55,7 +55,6 @@ static inline void main_init(void)
 {
   mcu_init();
   sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
-  mcu_int_enable();
 
   downlink_init();
   pprz_dl_init();
diff --git a/sw/lib/ocaml/aircraft.ml b/sw/lib/ocaml/aircraft.ml
index 680cb0b429..a2e563d444 100644
--- a/sw/lib/ocaml/aircraft.ml
+++ b/sw/lib/ocaml/aircraft.ml
@@ -106,7 +106,7 @@ let target_conf_add_module = fun conf target firmware name mtype load_type ->
     { conf with modules = conf.modules @ [(Unloaded, m)] } end
 
 (* topological sort to load modules and their dependencies *)
-let resolve_modules_dep = fun config_by_target firmware fail ->
+let resolve_modules_dep = fun config_by_target firmware user_target ->
   let resolved = ref [] in    (* modules to load (fully resolved) *)
   let unresolved = ref [] in  (* modules no fully resolved (for cycle detection) *)
   let unloaded = ref [] in    (* modules not loaded for this target / firmware *)
@@ -155,8 +155,6 @@ let resolve_modules_dep = fun config_by_target firmware fail ->
           ) m.Module.autoloads;
           (* add conflicts to list *)
           conflicts := !conflicts @ dep.Module.conflicts;
-          (* add required to list (if not present) *)
-          (*required := List.fold_left add_unique !required dep.Module.requires_func;*)
           (* add provides to list (if not present) *)
           provided := List.fold_left add_unique !provided dep.Module.provides;
           (* all dep and autoload resolved, add to list *)
@@ -191,8 +189,15 @@ let resolve_modules_dep = fun config_by_target firmware fail ->
     conflicts := [];
     required := [];
     provided := [];
+    (* check if a board, target or firmware specific module is available *)
+    let target_module_name = Env.paparazzi_conf // "modules" // "targets" // target ^ ".xml" in
+    let target_module = if Sys.file_exists target_module_name then [GC.Var target_module_name] else [] in
+    let firmware_module_name = Env.paparazzi_conf // "modules" // "firmwares" // conf.firmware_name ^ ".xml" in
+    let firmware_module = if Sys.file_exists firmware_module_name then [GC.Var firmware_module_name] else [] in
+    let board_module_name = Env.paparazzi_conf // "modules" // "boards" // conf.board_type ^ ".xml" in
+    let board_module = if Sys.file_exists board_module_name then [GC.Var board_module_name] else [] in
     (* iter on modules of this target from a meta module *)
-    let root_dep = { Module.empty_dep with Module.requires = (List.map (fun (_, m) -> GC.Var m.Module.name) conf.modules) } in
+    let root_dep = { Module.empty_dep with Module.requires = (List.map (fun (_, m) -> GC.Var m.Module.name) conf.modules) @ target_module @ firmware_module @ board_module } in
     let root_module = {
       Module.empty with
       Module.name = "root";
@@ -201,7 +206,7 @@ let resolve_modules_dep = fun config_by_target firmware fail ->
     } in
     dep_resolve root_module target UserLoad;
     (* test for conflicts and required functionalities and option if requested *)
-    if fail then begin
+    if (not (user_target = "")) && (target = user_target) then begin
       (* check conflicts for resolved modules *)
       List.iter (fun c ->
         List.iter (fun (name, _) ->
@@ -440,7 +445,7 @@ let parse_aircraft = fun ?(parse_af=false) ?(parse_ap=false) ?(parse_fp=false) ?
 
   (* resolve modules dep *)
   (* don't fail if no target specified, execpt for cyclic dependencies *)
-  resolve_modules_dep config_by_target firmware (not (target = ""));
+  resolve_modules_dep config_by_target firmware target;
   let loaded_types, loaded_modules = get_loaded_modules config_by_target target in
   let all_modules = get_all_modules config_by_target in
 
diff --git a/sw/lib/ocaml/module.ml b/sw/lib/ocaml/module.ml
index 9cf930d1ed..4b221a5db7 100644
--- a/sw/lib/ocaml/module.ml
+++ b/sw/lib/ocaml/module.ml
@@ -144,7 +144,8 @@ type periodic = {
     delay: float option;
     start: string option;
     stop: string option;
-    autorun: autorun
+    autorun: autorun;
+    cond: string option
   }
 
 let parse_periodic = fun xml ->
@@ -177,25 +178,28 @@ let parse_periodic = fun xml ->
       end
    in
   { call; fname; period_freq; delay = getf "delay";
-    start = get "start"; stop = get "stop";
+    start = get "start"; stop = get "stop"; cond = get "cond";
     autorun = match get "autorun" with
       | None -> Lock
       | Some "TRUE" | Some "true" -> True
       | Some "FALSE" | Some "false" -> False
       | Some "LOCK" | Some "lock" -> Lock
-      | Some _ -> failwith "Module.parse_periodic: unreachable" }
+      | Some a -> failwith ("Module.parse_periodic: unknown autorun: " ^ a) }
 
-type event = { ev: string; handlers: string list }
+type init = { iname: string; cond: string option }
 
-let make_event = fun f handlers ->
+let make_init = fun f cond ->
+  { iname = f;
+    cond = cond
+  }
+
+type event = { ev: string; cond: string option }
+
+let make_event = fun f cond ->
   { ev = f;
-    handlers = List.map
-      (function
-        | Xml.Element ("handler", _, []) as xml -> Xml.attrib xml "fun"
-        | _ -> failwith "Module.make_event: unreachable"
-      ) handlers }
+    cond = cond
+  }
 
-let fprint_event = fun ch e -> Printf.fprintf ch "%s;\n" e.ev
 
 type datalink = { message: string; func: string }
 
@@ -261,7 +265,7 @@ type t = {
   autoloads: autoload list;
   settings: Settings.t list;
   headers: file list;
-  inits: string list;
+  inits: init list;
   periodics: periodic list;
   events: event list;
   datalinks: datalink list;
@@ -282,8 +286,8 @@ let rec parse_xml m = function
     and dir = ExtXml.attrib_opt xml "dir"
     and task = ExtXml.attrib_opt xml "task" in
     List.fold_left parse_xml { m with name; dir; task; xml } children
-  | Xml.Element ("doc", _, _) as xml -> { m with doc = xml }
-  (*| Xml.Element ("settings_file", [("name", name)], files) -> m (* TODO : remove unused *)*)
+  | Xml.Element ("doc", _, _) as xml ->
+    { m with doc = xml }
   | Xml.Element ("settings", _, _) as xml ->
     { m with settings = Settings.from_xml xml :: m.settings }
   | Xml.Element ("dep", _, _) as xml ->
@@ -297,12 +301,15 @@ let rec parse_xml m = function
                List.fold_left (fun acc f -> parse_file f :: acc) m.headers files
     }
   | Xml.Element ("init", _, []) as xml ->
-    { m with inits = Xml.attrib xml "fun" :: m.inits }
+    let f = Xml.attrib xml "fun"
+    and c = ExtXml.attrib_opt xml "cond" in
+    { m with inits = make_init f c :: m.inits }
   | Xml.Element ("periodic", _, []) as xml ->
     { m with periodics = parse_periodic xml :: m.periodics }
-  | Xml.Element ("event", _, handlers) as xml ->
-    let f = Xml.attrib xml "fun" in
-    { m with events = make_event f handlers :: m.events }
+  | Xml.Element ("event", _, []) as xml ->
+    let f = Xml.attrib xml "fun"
+    and c = ExtXml.attrib_opt xml "cond" in
+    { m with events = make_event f c :: m.events }
   | Xml.Element ("datalink", _, []) as xml ->
     let message = Xml.attrib xml "message"
     and func = Xml.attrib xml "fun" in
diff --git a/sw/tools/generators/gen_makefile.ml b/sw/tools/generators/gen_makefile.ml
index 9aa0c87827..24e40a214a 100644
--- a/sw/tools/generators/gen_makefile.ml
+++ b/sw/tools/generators/gen_makefile.ml
@@ -94,13 +94,15 @@ let raw2mk = fun f raw ->
 
 let file2mk = fun f ?(arch = false) dir_name target file ->
   let name = file.Module.filename in
-  let dir_name = match file.Module.directory with Some d -> d
-    | None -> "$(" ^ dir_name ^ ")" in
+  let dir_name = match file.Module.directory with
+    | Some "." -> ""
+    | Some d -> d^"/"
+    | None -> "$(" ^ dir_name ^ ")/" in
   let cond, cond_end = match file.Module.filecond with None -> "", ""
     | Some c -> "\n"^c^"\n", "\nendif" in
   let fmt =
-    if arch then format_of_string "%s%s.srcs += arch/$(ARCH)/%s/%s%s\n"
-    else format_of_string "%s%s.srcs += %s/%s%s\n" in
+    if arch then format_of_string "%s%s.srcs += arch/$(ARCH)/%s%s%s\n"
+    else format_of_string "%s%s.srcs += %s%s%s\n" in
   fprintf f fmt cond target dir_name name cond_end
 
 (* module files and flags except configuration flags *)
diff --git a/sw/tools/generators/gen_modules.ml b/sw/tools/generators/gen_modules.ml
index 5ea03a20f2..2cbbb85e20 100644
--- a/sw/tools/generators/gen_modules.ml
+++ b/sw/tools/generators/gen_modules.ml
@@ -23,9 +23,11 @@
  *)
 
 open Printf
-open Xml2h
 module GC = Gen_common
 
+(** TODO make this a config file ?? *)
+let tasks_order = ["mcu"; "core"; "sensors"; "estimation"; "radio_control"; "control"; "actuators"; "datalink"; "default"]
+
 (** Formatting with a margin *)
 let margin = ref 0
 let step = 2
@@ -37,57 +39,32 @@ let lprintf = fun out f ->
   fprintf out "%s" (String.make !margin ' ');
   fprintf out f
 
+(* Encapsulate a condition attribute for function calls *)
+let lprintf_with_cond out s = function
+  | None -> lprintf out "%s;\n" s
+  | Some cond -> fprintf out "#if %s\n%s%s;\n#endif\n" cond (String.make !margin ' ') s
+
 let print_headers = fun out modules ->
   lprintf out  "#include \"std.h\"\n";
   List.iter (fun m ->
     let dirname = match m.Module.dir with None -> m.Module.name | Some d -> d in
     List.iter (fun h ->
       let dir = match h.Module.directory with None -> dirname | Some d -> d in
-      Printf.fprintf out "#include \"%s/%s\"\n" dir h.Module.filename
+      let inc = sprintf "#include \"%s/%s\"" dir h.Module.filename in
+      match h.Module.filecond with
+      | None -> fprintf out "%s\n" inc
+      | Some cond -> fprintf out "#if %s\n%s\n#endif\n" cond inc
     ) m.Module.headers
   ) modules
 
 let get_status_name = fun f n ->
-  let func = (Xml.attrib f "fun") in
-  n^"_"^String.sub func 0 (try String.index func '(' with _ -> (String.length func))^"_status"
+  n^"_"^String.sub f 0 (try String.index f '(' with _ -> (String.length f))^"_status"
 
-let get_status_shortname = fun f ->
-  let func = (Xml.attrib f "fun") in
-  String.sub func 0 (try String.index func '(' with _ -> (String.length func))
-
-(*let fprint_status = fun ch mod_name p ->
-  match p.autorun with
-  | True | False ->
-    Printf.fprintf ch "EXTERN_MODULES uint8_t %s;\n" (status_name mod_name p)
-  | Lock -> ()
-
-let fprint_periodic_init = fun ch mod_name p ->
-  match p.autorun with
-  | True -> Printf.fprintf ch "%s = %s;" (status_name mod_name p) "MODULES_START"
-  | False -> Printf.fprintf ch "%s = %s;" (status_name mod_name p) "MODULES_IDLE"
-  | Lock -> ()
-
-let fprint_init = fun ch init -> Printf.fprintf ch "%s;\n" init
-*)
-
-let get_period_and_freq = fun f ->
-  let period = try Some (Xml.attrib f "period") with _ -> None
-  and freq = try Some (Xml.attrib f "freq") with _ -> None in
-  match period, freq with
-    | None, None -> ("(1.f / MODULES_FREQUENCY)", "(MODULES_FREQUENCY)")
-    | Some _p, None -> ("("^_p^")", "(1. / ("^_p^"))")
-    | None, Some _f -> ("(1. / ("^_f^"))", "("^_f^")")
-    | Some _p, Some _ ->
-      fprintf stderr "Warning: both period and freq are defined but only period is used for function %s\n" (ExtXml.attrib f "fun");
-      ("("^_p^")", "(1. / ("^_p^"))")
-
-(*let fprint_period_freq = fun ch max_freq p ->
-  let period, freq = match p.period_freq with
-    | Unset -> 1. /. max_freq, max_freq
-    | Set (p, f) -> p, f in
-  let cap_fname = Compat.uppercase_ascii p.fname in
-  Printf.fprintf ch "#define %s_PERIOD %f\n" cap_fname period;
-  Printf.fprintf ch "#define %s_FREQ %f\n" cap_fname freq*)
+let get_period_and_freq = function
+  | Module.Unset -> ("(1.f / MODULES_FREQUENCY)", "(MODULES_FREQUENCY)")
+  | Module.Freq _f -> ("(1.f / ("^_f^"))", "("^_f^")")
+  | Module.Period _p -> ("("^_p^")", "(1.f / ("^_p^"))")
+  | Module.Set (_p, _f) -> (Printf.sprintf "(%ff)" _p, Printf.sprintf "(%ff)" _f)
 
 (* Extract function name and return in capital letters *)
 let get_cap_name = fun f ->
@@ -98,30 +75,27 @@ let get_cap_name = fun f ->
     | [Str.Text t; Str.Delim "("; Str.Text _ ; Str.Delim ")"] -> Compat.uppercase_ascii t
     | _ -> failwith "Gen_modules: not a valid function name"
 
-
 (** Computes the required modulos *)
 let get_functions_modulos = fun modules ->
   let found_modulos = Hashtbl.create 10 in
   let idx = ref 0 in
   let delay = ref 0. in (** Basic balancing: increment with a step of 0.1 *)
   let functions_modulo = List.map (fun m ->
-    let periodic = List.filter (fun i -> (String.compare (Xml.tag i) "periodic") == 0) (Xml.children m.Module.xml) in
     let module_name = m.Module.name in
     List.map (fun x ->
-      let p, _ = get_period_and_freq x in
-      let d = begin try
-        let _d = float_of_string (Xml.attrib x "delay") in
-        if _d > 0.99 then
-          begin
-            fprintf stderr "Warning: 'delay' attribute should be a float value between 0. and 1.\n";
-            _d /. 65536.
-          end
-        else _d (* try to keep some backward compatibility *)
-        with _ ->
-          delay := !delay +. 0.1;
-          if !delay > 0.9 then delay := 0.;
-          !delay
-        end
+      let p, _ = get_period_and_freq x.Module.period_freq in
+      let d = match x.Module.delay with
+        | Some _d ->
+            if _d > 0.99 then
+              begin
+                fprintf stderr "Warning: 'delay' attribute should be a float value between 0. and 1.\n";
+                _d /. 65536.
+              end
+            else _d (* try to keep some backward compatibility *)
+        | None ->
+            delay := !delay +. 0.1;
+            if !delay > 0.9 then delay := 0.;
+            !delay
       in
       try
         ((x, module_name, d), (p, Hashtbl.find found_modulos p))
@@ -129,7 +103,7 @@ let get_functions_modulos = fun modules ->
         incr idx; (* create new modulo *)
         Hashtbl.add found_modulos p !idx;
         ((x, module_name, d), (p, !idx))
-    ) periodic
+    ) m.Module.periodics
   ) modules in
   List.flatten functions_modulo
 
@@ -138,45 +112,43 @@ let print_function_freq = fun out modules ->
   fprintf out "\n";
   List.iter (fun m ->
     List.iter (fun i ->
-      match Xml.tag i with
-          "periodic" ->
-            let fname = get_cap_name (Xml.attrib i "fun") in
-            let p, f = get_period_and_freq i in
-            lprintf out "#define %s_PERIOD %s\n" fname p;
-            lprintf out "#define %s_FREQ %s\n" fname f;
-        | _ -> ())
-      (Xml.children m.Module.xml))
-    modules
+      let fname = Compat.uppercase_ascii i.Module.fname in
+      let p, f = get_period_and_freq i.Module.period_freq in
+      lprintf out "#define %s_PERIOD %s\n" fname p;
+      lprintf out "#define %s_FREQ %s\n" fname f;
+    ) m.Module.periodics
+  ) modules
 
 let print_function_prescalers = fun out functions_modulo ->
   fprintf out "\n";
   let found_modulos = Hashtbl.create 10 in
   List.iter (fun (_, (p, m)) ->
-    if not (Hashtbl.mem found_modulos m) then
-      lprintf out "#define PRESCALER_%d (uint32_t)(MODULES_FREQUENCY * %s)\n" m p
-    else
+    if not (Hashtbl.mem found_modulos m) then begin
+      lprintf out "#define PRESCALER_%d (uint32_t)(MODULES_FREQUENCY * %s)\n" m p;
       Hashtbl.add found_modulos m p
+    end
   ) functions_modulo
 
 
 let is_status_lock = fun p ->
-  let mode = ExtXml.attrib_or_default p "autorun" "LOCK" in
-  mode = "LOCK"
+  match p.Module.autorun with Module.Lock -> true | _ -> false
 
 let print_status = fun out modules ->
   fprintf out "\n";
   List.iter (fun m ->
     let module_name = m.Module.name in
-    List.iter (fun i ->
-      match Xml.tag i with
-          "periodic" ->
-            if not (is_status_lock i) then begin
-              lprintf out "EXTERN_MODULES uint8_t %s;\n" (get_status_name i module_name);
-            end
-        | _ -> ())
-      (Xml.children m.Module.xml))
-    modules
+    List.iter (fun p ->
+      if not (is_status_lock p) then
+        lprintf out "EXTERN_MODULES uint8_t %s;\n" (get_status_name p.Module.fname module_name)
+    ) m.Module.periodics
+  ) modules
 
+(** Create an order list of pairs (task name, list of modules in task)
+  *
+  * FIXME for now it goes with a hashtbl first then a list with hard-coded order
+  * and finally add (and display warning) all extra tasks group
+  * This could be made more configurable
+  *)
 let modules_of_task = fun modules ->
   let h = Hashtbl.create 1 in
   List.iter (fun m ->
@@ -186,39 +158,46 @@ let modules_of_task = fun modules ->
     else
       Hashtbl.add h task [m]
   ) modules;
-  h
+  let mot = List.map (fun task_name ->
+    let task_list = try Hashtbl.find h task_name with Not_found -> [] in
+    Hashtbl.remove h task_name;
+    (task_name, task_list)
+  ) tasks_order in
+  let mot = Hashtbl.fold (fun key tasks l ->
+    Printf.eprintf "Warning: adding an unknown task '%s'\n" key;
+    l @ [(key, tasks)]
+  ) h mot in
+  mot
 
-let print_init = fun out task modules ->
+let print_init = fun out (task, modules) ->
   lprintf out "\nstatic inline void modules_%s_init(void) {\n" task;
   right ();
   List.iter (fun m ->
     let module_name = m.Module.name in
-    List.iter (fun i ->
-      match Xml.tag i with
-          "init" -> lprintf out "%s;\n" (Xml.attrib i "fun")
-        | "periodic" -> if not (is_status_lock i) then
-            lprintf out "%s = %s;\n" (get_status_name i module_name) (try match Xml.attrib i "autorun" with
-                "TRUE" | "true" -> "MODULES_START"
-              | "FALSE" | "false" | "LOCK" | "lock" -> "MODULES_IDLE"
-              | _ -> failwith "Error: Unknown autorun value (possible values are: TRUE, FALSE, LOCK(default))"
-              with _ -> "MODULES_IDLE" (* this should not be possible anyway *))
-        | _ -> ())
-      (Xml.children m.Module.xml))
-    modules;
+    List.iter (fun init ->
+      lprintf_with_cond out init.Module.iname init.Module.cond
+    ) m.Module.inits;
+    List.iter (fun p ->
+      match p.Module.autorun with
+      | Module.True -> lprintf out "%s = MODULES_START;\n" (get_status_name p.Module.fname  module_name)
+      | Module.False -> lprintf out "%s = MODULES_STOP;\n" (get_status_name p.Module.fname  module_name)
+      | _ -> ()
+    ) m.Module.periodics
+  ) modules;
   left ();
   lprintf out "}\n"
 
 let print_init_functions = fun out modules ->
-  let h = modules_of_task modules in
-  Hashtbl.iter (print_init out) h;
+  let mot = modules_of_task modules in
+  List.iter (print_init out) mot;
   lprintf out "\nstatic inline void modules_init(void) {\n";
   right ();
-  Hashtbl.iter (fun t _ -> lprintf out "modules_%s_init();\n" t) h;
+  List.iter (fun (t, _) -> lprintf out "modules_%s_init();\n" t) mot;
   left ();
   lprintf out "}\n"
 
 
-let print_periodic = fun out functions_modulo task modules ->
+let print_periodic = fun out functions_modulo (task, modules) ->
   (* filter for a given task *)
   let functions_modulo = List.filter (fun m ->
     let (_, name, _), _ = m in
@@ -234,63 +213,62 @@ let print_periodic = fun out functions_modulo task modules ->
     lprintf out "static uint32_t %s; %s++; if (%s>=PRESCALER_%d) %s=0;\n" v v v modulo v;)
     modulos;
   (** Print start and stop functions *)
+  let lprint_opt = fun f cond ->
+    match f with Some s -> lprintf_with_cond out s cond | None -> ()
+  in
   List.iter (fun m ->
     let module_name = m.Module.name in
-    let periodic = List.filter (fun i -> (String.compare (Xml.tag i) "periodic") == 0) (Xml.children m.Module.xml) in
-    List.iter (fun f ->
-      if (is_status_lock f) then begin
-        try
-          let start = (Xml.attrib f "start") in
+    List.iter (fun p ->
+      match p.Module.autorun with
+      | Module.Lock ->
+          lprint_opt p.Module.start p.Module.cond;
+          begin match p.Module.stop with
+          | Some stop -> fprintf stderr "Warning: stop %s function will not be called\n" stop
+          | _ -> () end
+      | _ -> (* not locked *)
+          let status = get_status_name p.Module.fname module_name in
           fprintf out "\n";
-          lprintf out "%s;\n" start
-        with _ -> ();
-        try let stop = Xml.attrib f "stop" in fprintf stderr "Warning: stop %s function will not be called\n" stop with _ -> ();
-      end
-      else begin
-        let status = get_status_name f module_name in
-        fprintf out "\n";
-        lprintf out "if (%s == MODULES_START) {\n" status;
-        right ();
-        ignore(try lprintf out "%s;\n" (Xml.attrib f "start") with _ -> ());
-        lprintf out "%s = MODULES_RUN;\n" status;
-        left ();
-        lprintf out "}\n";
-        lprintf out "if (%s == MODULES_STOP) {\n" status;
-        right ();
-        ignore(try lprintf out "%s;\n" (Xml.attrib f "stop") with _ -> ());
-        lprintf out "%s = MODULES_IDLE;\n" status;
-        left ();
-        lprintf out "}\n";
-      end
-    ) periodic
+          lprintf out "if (%s == MODULES_START) {\n" status;
+          right ();
+          lprint_opt p.Module.start p.Module.cond; 
+          lprintf out "%s = MODULES_RUN;\n" status;
+          left ();
+          lprintf out "}\n";
+          lprintf out "if (%s == MODULES_STOP) {\n" status;
+          right ();
+          lprint_opt p.Module.stop p.Module.cond; 
+          lprintf out "%s = MODULES_IDLE;\n" status;
+          left ();
+          lprintf out "}\n"
+    ) m.Module.periodics
   ) modules;
   (** Print periodic functions *)
   let functions = List.sort (fun (_,p) (_,p') -> compare p p') functions_modulo in
   fprintf out "\n";
-  List.iter (fun ((func, name, delay), (p, m)) ->
+  List.iter (fun ((periodic, name, delay), (p, m)) ->
     if (List.exists (fun _module -> _module.Module.name = name) modules) then begin
-      let function_name = ExtXml.attrib func "fun" in
-      let p, f = get_period_and_freq func in
+      let p, f = get_period_and_freq periodic.Module.period_freq in
       if f = "(MODULES_FREQUENCY)" then
         begin
-          if (is_status_lock func) then
-            lprintf out "%s;\n" function_name
-          else begin
-            lprintf out "if (%s == MODULES_RUN) {\n" (get_status_name func name);
-            right ();
-            lprintf out "%s;\n" function_name;
-            left ();
-            lprintf out "}\n";
-          end
+          match periodic.Module.autorun with
+          | Module.Lock ->
+              lprintf_with_cond out periodic.Module.call periodic.Module.cond
+          | _ ->
+              lprintf out "if (%s == MODULES_RUN) {\n" (get_status_name periodic.Module.fname name);
+              right ();
+              lprintf_with_cond out periodic.Module.call periodic.Module.cond;
+              left ();
+              lprintf out "}\n"
         end
       else
         begin
-          let run = if not (is_status_lock func) then sprintf " && %s == MODULES_RUN" (get_status_name func name)
-            else ""
+          let run = match periodic.Module.autorun with
+            | Module.Lock -> ""
+            | _ -> sprintf " && %s == MODULES_RUN" (get_status_name periodic.Module.fname name)
           in
           lprintf out "if (i%d == (uint32_t)(%ff * PRESCALER_%d)%s) {\n" m delay m run;
           right ();
-          lprintf out "%s;\n" function_name;
+          lprintf_with_cond out periodic.Module.call periodic.Module.cond;
           left ();
           lprintf out "}\n"
         end;
@@ -300,34 +278,32 @@ let print_periodic = fun out functions_modulo task modules ->
   lprintf out "}\n"
 
 let print_periodic_functions = fun out functions_modulo modules ->
-  let h = modules_of_task modules in
-  Hashtbl.iter (print_periodic out functions_modulo) h;
+  let mot = modules_of_task modules in
+  List.iter (print_periodic out functions_modulo) mot;
   lprintf out "\nstatic inline void modules_periodic_task(void) {\n";
   right ();
-  Hashtbl.iter (fun t _ -> lprintf out "modules_%s_periodic_task();\n" t) h;
+  List.iter (fun (t, _) -> lprintf out "modules_%s_periodic_task();\n" t) mot;
   left ();
   lprintf out "}\n"
 
 
-let print_event = fun out task modules ->
+let print_event = fun out (task, modules) ->
   lprintf out "\nstatic inline void modules_%s_event_task(void) {\n" task;
   right ();
   List.iter (fun m ->
     List.iter (fun i ->
-      match Xml.tag i with
-          "event" -> lprintf out "%s;\n" (Xml.attrib i "fun")
-        | _ -> ())
-      (Xml.children m.Module.xml))
-    modules;
+      lprintf_with_cond out i.Module.ev i.Module.cond
+    ) m.Module.events
+  ) modules;
   left ();
   lprintf out "}\n"
 
 let print_event_functions = fun out modules ->
-  let h = modules_of_task modules in
-  Hashtbl.iter (print_event out) h;
+  let mot = modules_of_task modules in
+  List.iter (print_event out) mot;
   lprintf out "\nstatic inline void modules_event_task(void) {\n";
   right ();
-  Hashtbl.iter (fun t _ -> lprintf out "modules_%s_event_task();\n" t) h;
+  List.iter (fun (t, _) -> lprintf out "modules_%s_event_task();\n" t) mot;
   left ();
   lprintf out "}\n"
 
@@ -342,14 +318,11 @@ let print_datalink_functions = fun out modules ->
   right ();
   let else_ = ref "" in
   List.iter (fun m ->
-    List.iter (fun i ->
-      match Xml.tag i with
-          "datalink" ->
-            lprintf out "%sif (msg_id == DL_%s) { %s; }\n" !else_ (ExtXml.attrib i "message") (ExtXml.attrib i "fun");
-            else_ := "else "
-        | _ -> ())
-      (Xml.children m.Module.xml))
-    modules;
+    List.iter (fun d ->
+      lprintf out "%sif (msg_id == DL_%s) { %s; }\n" !else_ d.Module.message d.Module.func;
+      else_ := "else "
+    ) m.Module.datalinks
+  ) modules;
   left ();
   lprintf out "}\n"
 
@@ -369,9 +342,6 @@ let parse_modules out modules =
   fprintf out "\n";
   fprintf out "#endif // MODULES_DATALINK_C\n"
 
-let test_section_modules = fun xml ->
-  List.fold_right (fun x r -> ExtXml.tag_is x "modules" || r) (Xml.children xml) false
-
 (** create list of dependencies from string
  * returns a nested list, where the second level consists of OR dependencies
  *)
@@ -388,52 +358,16 @@ let deps_of_string = fun s ->
   with
       _ -> [[]]
 
-let get_pcdata = fun xml tag ->
-  try
-    Xml.pcdata (ExtXml.child (ExtXml.child xml tag) "0")
-  with
-      Not_found -> ""
-
-(** Check dependencies *)
-let check_dependencies = fun modules names ->
-  List.iter (fun m ->
-    try
-      let module_name = m.Module.name in
-      let dep_string = get_pcdata m.Module.xml "depends" in
-      (*fprintf stderr "\n\nWARNING: parsing dep string: %s\n\n" dep_string;
-      fprintf stderr "\n\nWARNING: names: %s" (String.concat "," names);*)
-      let require = deps_of_string dep_string in
-      List.iter (fun deps ->
-        (* iterate over all dependencies, where the second level contains the OR dependencies *)
-        let find_common satisfied d = if List.mem d names then d::satisfied else satisfied in
-        let satisfied = List.fold_left find_common [] deps in
-        if List.length satisfied == 0 then
-          begin
-            fprintf stderr "\nDEPENDENCY Error: Module %s requires %s\n" module_name (String.concat " or " deps);
-            fprintf stderr "Available loaded modules are:\n    %s\n\n" (String.concat "\n    " names);
-            exit 1
-          end)
-        require;
-      let conflict_string = get_pcdata m.Module.xml "conflicts" in
-      let conflict_l = List.flatten (deps_of_string conflict_string) in
-      List.iter (fun con ->
-        if List.exists (fun c -> String.compare c con == 0) names then
-          fprintf stderr "\nDEPENDENCY WARNING: Module %s conflicts with %s\n" module_name con)
-        conflict_l
-    with _ -> ()
-  ) modules
-
-
 let h_name = "MODULES_H"
 
 let generate = fun modules xml_file out_file ->
   let out = open_out out_file in
 
-  begin_out out xml_file h_name;
-  define_out out "MODULES_IDLE " "0";
-  define_out out "MODULES_RUN  " "1";
-  define_out out "MODULES_START" "2";
-  define_out out "MODULES_STOP " "3";
+  Xml2h.begin_out out xml_file h_name;
+  Xml2h.define_out out "MODULES_IDLE " "0";
+  Xml2h.define_out out "MODULES_RUN  " "1";
+  Xml2h.define_out out "MODULES_START" "2";
+  Xml2h.define_out out "MODULES_STOP " "3";
   fprintf out "\n";
 
   fprintf out "#ifndef MODULES_FREQUENCY\n";
@@ -452,6 +386,6 @@ let generate = fun modules xml_file out_file ->
 
   parse_modules out modules;
 
-  finish_out out h_name;
+  Xml2h.finish_out out h_name;
   close_out out