[autopilot] reorganize autopilot code

- better structure for the autopilot tasks
- convergence between firmwares
- remove unused int_enable functions
- update generators

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

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_ */

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 */
 

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 */

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();
 

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

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);

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
+ * IDs for timers
-tid_t failsafe_tid;      ///< id for failsafe_check() timer
+ */
-tid_t radio_control_tid; ///< id for radio_control_periodic_task() timer
+tid_t modules_mcu_core_tid; // single step
-tid_t electrical_tid;    ///< id for electrical_periodic() timer
+tid_t modules_sensors_tid;
-tid_t telemetry_tid;     ///< id for telemetry_periodic() timer
+tid_t modules_estimation_tid;
-#if USE_BARO_BOARD
+tid_t modules_radio_control_tid;
-tid_t baro_tid;          ///< id for baro_periodic() timer
+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();
+  modules_mcu_init();
-
+  modules_core_init();
-#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
+  modules_sensors_init();
-  pprz_trig_int_init();
+  modules_estimation_init();
-#endif
-
-  electrical_init();
-
-  stateInit();
-
-#ifndef INTER_MCU_AP
-  actuators_init();
-#else
-  intermcu_init();
-#endif
-
 #ifndef INTER_MCU_AP
   radio_control_init();
+  // modules_radio_control_init(); FIXME
 #endif
-
+  modules_control_init();
-#if USE_BARO_BOARD
+  modules_actuators_init();
-  baro_init();
+  modules_datalink_init();
-#endif
+  modules_default_init();
-
-#if USE_AHRS
-  ahrs_init();
-#endif
-
-  autopilot_init();
-
-  modules_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:
+  // register timers with temporal dependencies
-   * Since INS is now a module, LTP_DEF is not yet initialized when autopilot_init is called
+  modules_sensors_tid = sys_time_register_timer(SYS_PERIOD, NULL);
-   * This led to the problem that global waypoints were not "localized",
+  modules_estimation_tid = sys_time_register_timer_offset(modules_sensors_tid, ESTIMATION_OFFSET, NULL);
-   * so as a stop-gap measure we localize them all (again) here..
+  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 ?
-  waypoints_localize_all();
-
-  settings_init();
-
-  mcu_int_enable();
-
-#if DOWNLINK
-  downlink_init();
-#endif
-
-#ifdef INTER_MCU_AP
-  intermcu_init();
-#endif
 
   // register the timers for the periodic functions
-  main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
+  modules_mcu_core_tid = sys_time_register_timer(SYS_PERIOD, NULL);
-#if PERIODIC_FREQUENCY != MODULES_FREQUENCY
+  modules_radio_control_tid = sys_time_register_timer((1. / 60.), NULL); // FIXME
-  modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
+  modules_datalink_tid = sys_time_register_timer(DATALINK_PERIOD, NULL);
-#endif
+  failsafe_tid = sys_time_register_timer(0.05, NULL); // FIXME
-  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
 
 #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)) {
+  if (sys_time_check_and_ack_timer(modules_sensors_tid)) {
-    main_periodic();
+    modules_sensors_periodic_task();
-#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
   }
-  /* separate timer for modules, since it has a different freq than main */
+
-  if (sys_time_check_and_ack_timer(modules_tid)) {
+  if (sys_time_check_and_ack_timer(modules_estimation_tid)) {
-    modules_periodic_task();
+    modules_estimation_periodic_task();
-#endif
   }
-  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 (sys_time_check_and_ack_timer(modules_control_actuators_tid)) {
-{
+    modules_control_periodic_task();
-#if INTER_MCU_AP
-  /* Inter-MCU watchdog */
-  intermcu_periodic();
-#endif
-
-  /* run control loops */
-  autopilot_periodic();
-  /* set actuators     */
-  //actuators_set(autopilot_get_motors_on());
 
 #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()) {
+    if (autopilot_in_flight()) {
-    RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++);
+      RunOnceEvery(PERIODIC_FREQUENCY, autopilot.flight_time++); // TODO make it 1Hz periodic ?
+    }
   }
 
-#if defined DATALINK || defined SITL
+  if (sys_time_check_and_ack_timer(modules_default_tid)) {
-  RunOnceEvery(PERIODIC_FREQUENCY, datalink_time++);
+    modules_default_periodic_task();
-#endif
+  }
+
+  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.. */
+  modules_mcu_event_task();
-  mcu_event();
+  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);
   }
-
+  modules_actuators_event_task();
-#if USE_BARO_BOARD
+  modules_datalink_event_task();
-  BaroEvent();
+  modules_default_event_task();
-#endif
-
-  autopilot_event();
-
-  modules_event_task();
 }

sw/airborne/firmwares/rotorcraft/main_fbw.c

@@ -91,7 +91,6 @@ void main_init(void)
 
   modules_init();
 
-  mcu_int_enable();
 
   intermcu_init();
 

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
+ * IDs for timers
-tid_t failsafe_tid;      ///< id for failsafe_check() timer
+ */
-tid_t radio_control_tid; ///< id for radio_control_periodic_task() timer
+tid_t modules_mcu_core_tid; // single step
-tid_t electrical_tid;    ///< id for electrical_periodic() timer
+tid_t modules_sensors_tid;
-tid_t telemetry_tid;     ///< id for telemetry_periodic() timer
+tid_t modules_estimation_tid;
-#if USE_BARO_BOARD
+tid_t modules_radio_control_tid;
-tid_t baro_tid;          ///< id for baro_periodic() timer
+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();
+  modules_mcu_init();
-
+  modules_core_init();
-#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
+  modules_sensors_init();
-  pprz_trig_int_init();
+  modules_estimation_init();
-#endif
-
-  electrical_init();
-
-  stateInit();
-
-  actuators_init();
-
-#if USE_MOTOR_MIXING
-  motor_mixing_init();
-#endif
-
   radio_control_init();
-
+  // modules_radio_control_init(); FIXME
-#if USE_BARO_BOARD
+  modules_control_init();
-  baro_init();
+  modules_actuators_init();
-#endif
+  modules_datalink_init();
-
+  modules_default_init();
-#if USE_AHRS
-  ahrs_init();
-#endif
-
-  autopilot_init();
-
-  modules_init();
 
   // call autopilot implementation init after guidance modules init
   // it will set startup mode
   autopilot_generated_init();
 
-  /* temporary hack:
+  // register timers with temporal dependencies
-   * Since INS is now a module, LTP_DEF is not yet initialized when autopilot_init is called
+  modules_sensors_tid = sys_time_register_timer(SYS_PERIOD, NULL);
-   * This led to the problem that global waypoints were not "localized",
+  modules_estimation_tid = sys_time_register_timer_offset(modules_sensors_tid, ESTIMATION_OFFSET, NULL);
-   * so as a stop-gap measure we localize them all (again) here..
+  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 ?
-  //waypoints_localize_all();
-
-  settings_init();
-
-  mcu_int_enable();
-
-#if DOWNLINK
-  downlink_init();
-#endif
 
   // register the timers for the periodic functions
-  main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
+  modules_mcu_core_tid = sys_time_register_timer(SYS_PERIOD, NULL);
-#if PERIODIC_FREQUENCY != MODULES_FREQUENCY
+  modules_radio_control_tid = sys_time_register_timer((1. / 60.), NULL); // FIXME
-  modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
+  modules_datalink_tid = sys_time_register_timer(DATALINK_PERIOD, NULL);
-#endif
+  failsafe_tid = sys_time_register_timer(0.05, NULL); // FIXME
-  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
 
 #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)) {
+  if (sys_time_check_and_ack_timer(modules_sensors_tid)) {
-    main_periodic();
+    modules_sensors_periodic_task();
-#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
   }
-  /* separate timer for modules, since it has a different freq than main */
+
-  if (sys_time_check_and_ack_timer(modules_tid)) {
+  if (sys_time_check_and_ack_timer(modules_estimation_tid)) {
-    modules_periodic_task();
+    modules_estimation_periodic_task();
-#endif
   }
-  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();
-  }
+    modules_datalink_periodic_task(); // FIXME integrate above
-#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++);
-  }
-
 #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.. */
+  modules_mcu_event_task();
-  mcu_event();
+  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);
   }
-
+  modules_actuators_event_task();
-#if USE_BARO_BOARD
+  modules_datalink_event_task();
-  BaroEvent();
+  modules_default_event_task();
-#endif
-
-  autopilot_event();
-
-  modules_event_task();
 }
 

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);
 }
 

sw/airborne/firmwares/setup/usb_tunnel.c

@@ -95,7 +95,6 @@ int main(void)
 
   VCOM_init();
 
-  mcu_int_enable();
 
   while (1) {
     VCOM_event();

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)

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)

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)

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)

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)

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.

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)

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)) {

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)) {

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 */

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)

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();

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)

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)

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();

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;

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();

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);
 

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();
 }

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();

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
 

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
+    cond = cond
-      (function
+  }
-        | Xml.Element ("handler", _, []) as xml -> Xml.attrib xml "fun"
-        | _ -> failwith "Module.make_event: unreachable"
-      ) handlers }
 
-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 ("doc", _, _) as xml ->
-  (*| Xml.Element ("settings_file", [("name", name)], files) -> m (* TODO : remove unused *)*)
+    { 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 ->
+  | Xml.Element ("event", _, []) as xml ->
-    let f = Xml.attrib xml "fun" in
+    let f = Xml.attrib xml "fun"
-    { m with events = make_event f handlers :: m.events }
+    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

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
+  let dir_name = match file.Module.directory with
-    | None -> "$(" ^ dir_name ^ ")" in
+    | 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"
+    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
+    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 *)