diff --git a/sw/airborne/arch/sim/led_hw.h b/sw/airborne/arch/sim/led_hw.h
index 58411afdff..f61c3b6a13 100644
--- a/sw/airborne/arch/sim/led_hw.h
+++ b/sw/airborne/arch/sim/led_hw.h
@@ -1,3 +1,6 @@
+#ifndef LED_HW_H
+#define LED_HW_H
+
 #include <stdio.h>
 #include <caml/mlvalues.h>
 #include <caml/memory.h>
@@ -10,3 +13,6 @@ extern value * leds_closure;
 #define LED_OFF(i) { if (leds_closure) callback2(*leds_closure, Val_int(i), Val_int(0)); }
 #define LED_TOGGLE(i) { if (leds_closure) callback2(*leds_closure, Val_int(i), Val_int(2)); }
 
+#define LED_PERIODIC() {}
+
+#endif /* LED_HW_H */
diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c
index be8f4bd1c2..c0ad122a24 100644
--- a/sw/airborne/arch/sim/sim_ap.c
+++ b/sw/airborne/arch/sim/sim_ap.c
@@ -1,6 +1,8 @@
 /* Definitions and declarations required to compile autopilot code on a
    i386 architecture. Bindings for OCaml. */
 
+#define MODULES_C
+
 #include <stdio.h>
 #include <assert.h>
 #include <sys/time.h>
@@ -24,6 +26,8 @@
 #include "subsystems/datalink/datalink.h"
 #include "generated/flight_plan.h"
 
+#include "generated/modules.h"
+
 #include <caml/mlvalues.h>
 #include <caml/memory.h>
 
@@ -46,13 +50,26 @@ uint16_t datalink_time = 0;
 uint8_t ac_id;
 
 value sim_periodic_task(value unit) {
-  periodic_task_ap();
+  sensors_task();
+  attitude_loop();
+  reporting_task();
+  modules_periodic_task();
   periodic_task_fbw();
   event_task_ap();
   event_task_fbw();
   return unit;
 }
 
+value sim_monitor_task(value unit) {
+  monitor_task();
+  return unit;
+}
+
+value sim_nav_task(value unit) {
+  navigation_task();
+  return unit;
+}
+
 
 float ftimeofday(void) {
   struct timeval t;
diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c
index 98b1b7b120..bc9bcb1a70 100644
--- a/sw/airborne/estimator.c
+++ b/sw/airborne/estimator.c
@@ -113,11 +113,6 @@ void estimator_init( void ) {
 }
 
 
-
-void estimator_propagate_state( void ) {
-
-}
-
 bool_t alt_kalman_enabled;
 
 #ifdef ALT_KALMAN
diff --git a/sw/airborne/estimator.h b/sw/airborne/estimator.h
index 8748d43354..75ffcff3f6 100644
--- a/sw/airborne/estimator.h
+++ b/sw/airborne/estimator.h
@@ -74,7 +74,6 @@ extern float estimator_airspeed; /* m/s */
 extern float estimator_AOA; /* radians */
 
 void estimator_init( void );
-void estimator_propagate_state( void );
 
 void estimator_update_state_gps( void );
 
diff --git a/sw/airborne/firmwares/fixedwing/ap_downlink.h b/sw/airborne/firmwares/fixedwing/ap_downlink.h
index d1b25251ed..30dee8aec7 100644
--- a/sw/airborne/firmwares/fixedwing/ap_downlink.h
+++ b/sw/airborne/firmwares/fixedwing/ap_downlink.h
@@ -96,7 +96,7 @@ extern uint8_t telemetry_mode_Ap_DefaultChannel;
       DOWNLINK_SEND_ATTITUDE(_trans, _dev, &estimator_phi, &estimator_psi, &estimator_theta); \
 })
 
-#define PERIODIC_SEND_PPRZ_MODE(_trans, _dev) DOWNLINK_SEND_PPRZ_MODE(_trans, _dev, &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status);
+
 #define PERIODIC_SEND_DESIRED(_trans, _dev) DOWNLINK_SEND_DESIRED(_trans, _dev, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint);
 
 #if USE_INFRARED
@@ -123,9 +123,15 @@ extern uint8_t telemetry_mode_Ap_DefaultChannel;
   DownlinkSendWp(_trans, _dev, i);	    \
 }
 
-#ifdef RADIO_CONTROL_SETTINGS
+#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
+#include "rc_settings.h"
+#define PERIODIC_SEND_PPRZ_MODE(_trans, _dev) DOWNLINK_SEND_PPRZ_MODE(_trans, _dev, &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status);
 #define PERIODIC_SEND_SETTINGS(_trans, _dev) if (!RcSettingsOff()) DOWNLINK_SEND_SETTINGS(_trans, _dev, &slider_1_val, &slider_2_val);
 #else
+#define PERIODIC_SEND_PPRZ_MODE(_trans, _dev) {                         \
+    uint8_t rc_settings_mode_none = 0;                                  \
+    DOWNLINK_SEND_PPRZ_MODE(_trans, _dev, &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode_none, &mcu1_status); \
+  }
 #define PERIODIC_SEND_SETTINGS(_trans, _dev) {}
 #endif
 
diff --git a/sw/airborne/firmwares/fixedwing/autopilot.h b/sw/airborne/firmwares/fixedwing/autopilot.h
index d1d55a814a..4d48171cb4 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot.h
+++ b/sw/airborne/firmwares/fixedwing/autopilot.h
@@ -67,16 +67,12 @@ extern bool_t kill_throttle;
 
 #define FLOAT_OF_PPRZ(pprz, center, travel) ((float)pprz / (float)MAX_PPRZ * travel + center)
 
-extern uint8_t fatal_error_nb;
-
 #define THROTTLE_THRESHOLD_TAKEOFF (pprz_t)(MAX_PPRZ * 0.9)
 
 extern uint8_t lateral_mode;
 extern uint8_t vsupply;
 extern float energy;
 
-extern float slider_1_val, slider_2_val;
-
 extern bool_t launch;
 
 extern uint8_t light_mode;
@@ -91,8 +87,6 @@ extern bool_t sum_err_reset;
   (_mode != new_mode ? _mode = new_mode, TRUE : FALSE); \
 })
 
-void periodic_task( void );
-//void telecommand_task(void);
 
 #ifdef RADIO_CONTROL
 #include "subsystems/radio_control.h"
diff --git a/sw/airborne/firmwares/fixedwing/main.c b/sw/airborne/firmwares/fixedwing/main.c
index 98df36640c..7568bd6627 100644
--- a/sw/airborne/firmwares/fixedwing/main.c
+++ b/sw/airborne/firmwares/fixedwing/main.c
@@ -21,12 +21,12 @@
  * Boston, MA 02111-1307, USA.
  *
  */
-/** \file main.c
- * \brief main loop used both on single and dual MCU configuration */
 
+/**
+ * @file main.c
+ * Main loop used both on single and dual MCU configuration.
+ */
 
-#include "mcu_periph/sys_time.h"
-#include "led.h"
 
 #ifdef FBW
 #include "firmwares/fixedwing/main_fbw.h"
@@ -46,11 +46,8 @@ int main( void ) {
   Fbw(init);
   Ap(init);
   while (1) {
-    if (sys_time_check_and_ack_timer(0)) {
-      Fbw(periodic_task);
-      Ap(periodic_task);
-      LED_PERIODIC();
-    }
+    Fbw(handle_periodic_tasks);
+    Ap(handle_periodic_tasks);
     Fbw(event_task);
     Ap(event_task);
   }
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 7decc8025d..dfdf0f0a8a 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -22,10 +22,11 @@
  *
  */
 
-/** \file main_ap.c
- *  \brief AP ( AutoPilot ) process
+/**
+ * @file main_ap.c
+ * AP ( AutoPilot ) tasks
  *
- *   This process is reponsible for the collecting the different sensors data, fusing them to obtain
+ * This process is reponsible for the collecting the different sensors data, fusing them to obtain
  * aircraft attitude and running the different control loops
  */
 
@@ -35,35 +36,14 @@
 
 #include "firmwares/fixedwing/main_ap.h"
 #include "mcu.h"
-
-#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
-#include "firmwares/fixedwing/guidance/guidance_v.h"
-#include "subsystems/gps.h"
-#include "ap_downlink.h"
-#include "subsystems/nav.h"
-#include "firmwares/fixedwing/autopilot.h"
-#include "estimator.h"
-#include "generated/settings.h"
-#include "link_mcu.h"
 #include "mcu_periph/sys_time.h"
-#include "generated/flight_plan.h"
-#include "subsystems/datalink/datalink.h"
-#include "subsystems/settings.h"
-#include "subsystems/datalink/xbee.h"
 
-#include "gpio.h"
+#include "link_mcu.h"
 
-#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
-#include "rc_settings.h"
+// Sensors
+#if USE_GPS
+#include "subsystems/gps.h"
 #endif
-
-
-
-#ifdef TRAFFIC_INFO
-#include "subsystems/navigation/traffic_info.h"
-#endif
-
-
 #if USE_IMU
 #include "subsystems/imu.h"
 #endif
@@ -74,6 +54,34 @@
 #include "subsystems/ahrs/ahrs_aligner.h"
 #endif
 
+// autopilot & control
+#include "firmwares/fixedwing/autopilot.h"
+#include "estimator.h"
+#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
+#include "firmwares/fixedwing/guidance/guidance_v.h"
+#include "subsystems/nav.h"
+#include "generated/flight_plan.h"
+#ifdef TRAFFIC_INFO
+#include "subsystems/navigation/traffic_info.h"
+#endif
+
+// datalink & telemetry
+#include "subsystems/datalink/datalink.h"
+#include "subsystems/settings.h"
+#include "subsystems/datalink/xbee.h"
+#include "ap_downlink.h"
+
+// modules & settings
+#include "generated/modules.h"
+#include "generated/settings.h"
+#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
+#include "rc_settings.h"
+#endif
+
+#include "gpio.h"
+#include "led.h"
+
+
 #if USE_AHRS
 #if USE_IMU
 static inline void on_gyro_event( void );
@@ -89,23 +97,10 @@ static inline void on_ahrs_event(void);
 static inline void on_gps_solution( void );
 #endif
 
-#if ! defined CATASTROPHIC_BAT_LEVEL && defined LOW_BATTERY
-#warning "LOW_BATTERY deprecated. Renamed into CATASTROPHIC_BAT_LEVEL (in airframe file)"
-#define CATASTROPHIC_BAT_LEVEL LOW_BATTERY
-#endif
-
-#define LOW_BATTERY_DECIVOLT (CATASTROPHIC_BAT_LEVEL*10)
-
-#include "generated/modules.h"
-
-/** FIXME: should be in rc_settings but required by telemetry (ap_downlink.h)*/
-uint8_t rc_settings_mode = 0;
-
-/** Define minimal speed for takeoff in m/s */
-#define MIN_SPEED_FOR_TAKEOFF 5.
 
 bool_t power_switch;
-uint8_t fatal_error_nb = 0;
+
+// what version is this ????
 static const uint16_t version = 1;
 
 uint8_t pprz_mode = PPRZ_MODE_AUTO2;
@@ -119,22 +114,160 @@ static uint8_t  mcu1_ppm_cpt;
 
 bool_t kill_throttle = FALSE;
 
-float slider_1_val, slider_2_val;
-
 bool_t launch = FALSE;
 
-uint8_t vsupply;	// deciVolt
+
+/** Supply voltage in deciVolt.
+ * This the ap copy of the measurement from fbw
+ */
+uint8_t vsupply;
+
+/** Supply current in milliAmpere.
+ * This the ap copy of the measurement from fbw
+ */
 static int32_t current;	// milliAmpere
 
-float energy;       // Fuel consumption (mAh)
+/** Fuel consumption (mAh)
+ * TODO: move to electrical subsystem
+ */
+float energy;
 
 bool_t gps_lost = FALSE;
 
 
-#define Min(x, y) (x < y ? x : y)
-#define Max(x, y) (x > y ? x : y)
+tid_t modules_tid;     ///< id for modules_periodic_task() timer
+tid_t telemetry_tid;   ///< id for telemetry_periodic() timer
+tid_t sensors_tid;     ///< id for sensors_task() timer
+tid_t attitude_tid;    ///< id for attitude_loop() timer
+tid_t navigation_tid;  ///< id for navigation_task() timer
+tid_t monitor_tid;     ///< id for monitor_task() timer
 
-/** \brief Update paparazzi mode
+#ifndef CONTROL_FREQUENCY
+#ifdef  CONTROL_RATE
+#define CONTROL_FREQUENCY CONTROL_RATE
+//#warning "CONTROL_RATE deprecated. Renamed into CONTROL_FREQUENCY (in airframe file)"
+#else
+#define CONTROL_FREQUENCY 20
+#endif
+#endif
+
+#ifndef NAVIGATION_FREQUENCY
+#define NAVIGATION_FREQUENCY 4
+#endif
+
+#ifndef MODULES_FREQUENCY
+#define MODULES_FREQUENCY 60
+#endif
+
+void init_ap( void ) {
+#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
+  mcu_init();
+#endif /* SINGLE_MCU */
+
+  /************* Sensors initialization ***************/
+#if USE_GPS
+  gps_init();
+#endif
+
+#ifdef USE_GPIO
+  GpioInit();
+#endif
+
+#if USE_IMU
+  imu_init();
+#endif
+
+#if USE_AHRS_ALIGNER
+  ahrs_aligner_init();
+#endif
+
+#if USE_AHRS
+  ahrs_init();
+#endif
+
+  /************* Links initialization ***************/
+#if defined MCU_SPI_LINK
+  link_mcu_init();
+#endif
+#if USE_AUDIO_TELEMETRY
+  audio_telemetry_init();
+#endif
+
+  /************ Internal status ***************/
+  h_ctl_init();
+  v_ctl_init();
+  estimator_init();
+#ifdef ALT_KALMAN
+  alt_kalman_init();
+#endif
+  nav_init();
+
+  modules_init();
+
+  settings_init();
+
+  /**** start timers for periodic functions *****/
+  sensors_tid = sys_time_register_timer(1./PERIODIC_FREQUENCY, NULL);
+  navigation_tid = sys_time_register_timer(1./NAVIGATION_FREQUENCY, NULL);
+  attitude_tid = sys_time_register_timer(1./CONTROL_FREQUENCY, NULL);
+  modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL);
+  telemetry_tid = sys_time_register_timer(1./60, NULL);
+  monitor_tid = sys_time_register_timer(1.0, NULL);
+
+  /** - start interrupt task */
+  mcu_int_enable();
+
+#if defined DATALINK
+#if DATALINK == XBEE
+  xbee_init();
+#endif
+#endif /* DATALINK */
+
+#if defined AEROCOMM_DATA_PIN
+  IO0DIR |= _BV(AEROCOMM_DATA_PIN);
+  IO0SET = _BV(AEROCOMM_DATA_PIN);
+#endif
+
+  power_switch = FALSE;
+
+  /************ Multi-uavs status ***************/
+
+#ifdef TRAFFIC_INFO
+  traffic_info_init();
+#endif
+}
+
+
+void handle_periodic_tasks_ap(void) {
+
+  if (sys_time_check_and_ack_timer(sensors_tid))
+    sensors_task();
+
+  if (sys_time_check_and_ack_timer(navigation_tid))
+    navigation_task();
+
+#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
+  if (sys_time_check_and_ack_timer(attitude_tid))
+    attitude_loop();
+#endif
+
+  if (sys_time_check_and_ack_timer(modules_tid))
+    modules_periodic_task();
+
+  if (sys_time_check_and_ack_timer(monitor_tid))
+    monitor_task();
+
+  if (sys_time_check_and_ack_timer(telemetry_tid)) {
+    reporting_task();
+    LED_PERIODIC();
+  }
+
+}
+
+
+/******************** Interaction with FBW *****************************/
+
+/** Update paparazzi mode.
  */
 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
 static inline uint8_t pprz_mode_update( void ) {
@@ -165,7 +298,7 @@ static inline uint8_t mcu1_status_update( void ) {
 }
 
 
-/** \brief Send back uncontrolled channels
+/** Send back uncontrolled channels.
  */
 static inline void copy_from_to_fbw ( void ) {
 #ifdef SetAutoCommandsFromRC
@@ -175,41 +308,14 @@ static inline void copy_from_to_fbw ( void ) {
 #endif
 }
 
-
-
-/*
-   called at 20Hz.
-   sends a serie of initialisation messages followed by a stream of periodic ones
-*/
-
-/** Define number of message at initialisation */
-#define INIT_MSG_NB 2
-
-uint8_t ac_ident = AC_ID;
-
-/** \brief Send a serie of initialisation messages followed by a stream of periodic ones
- *
- * Called at 60Hz.
- */
-static inline void reporting_task( void ) {
-  static uint8_t boot = TRUE;
-
-  /** initialisation phase during boot */
-  if (boot) {
-    DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &version);
-    boot = FALSE;
-  }
-  /** then report periodicly */
-  else {
-    PeriodicSendAp(DefaultChannel, DefaultDevice);
-  }
-}
-
+/** mode to enter when RC is lost in PPRZ_MODE_MANUAL or PPRZ_MODE_AUTO1 */
 #ifndef RC_LOST_MODE
 #define RC_LOST_MODE PPRZ_MODE_HOME
 #endif
 
-/** \brief Function to be called when a message from FBW is available */
+/**
+ * Function to be called when a message from FBW is available
+ */
 static inline void telecommand_task( void ) {
   uint8_t mode_changed = FALSE;
   copy_from_to_fbw();
@@ -274,14 +380,35 @@ static inline void telecommand_task( void ) {
 }
 
 
+/**************************** Periodic tasks ***********************************/
+
+/**
+ * Send a series of initialisation messages followed by a stream of periodic ones.
+ * Called at 60Hz.
+ */
+void reporting_task( void ) {
+  static uint8_t boot = TRUE;
+
+  /** initialisation phase during boot */
+  if (boot) {
+    DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &version);
+    boot = FALSE;
+  }
+  /** then report periodicly */
+  else {
+    PeriodicSendAp(DefaultChannel, DefaultDevice);
+  }
+}
+
+
 #ifdef FAILSAFE_DELAY_WITHOUT_GPS
 #define GpsTimeoutError (cpu_time_sec - gps.last_fix_time > FAILSAFE_DELAY_WITHOUT_GPS)
 #endif
 
-/** \fn void navigation_task( void )
- *  \brief Compute desired_course
+/**
+ *  Compute desired_course
  */
-static void navigation_task( void ) {
+void navigation_task( void ) {
 #if defined FAILSAFE_DELAY_WITHOUT_GPS
   /** This section is used for the failsafe of GPS */
   static uint8_t last_pprz_mode;
@@ -363,33 +490,13 @@ static void navigation_task( void ) {
 }
 
 
-#ifndef KILL_MODE_DISTANCE
-#define KILL_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)
+#if USE_AHRS
+#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
+volatile uint8_t new_ins_attitude = 0;
+#endif
 #endif
 
-
-/** Maximum time allowed for low battery level */
-#define LOW_BATTERY_DELAY 5
-
-/** \fn inline void periodic_task( void )
- *  \brief Do periodic tasks at 60 Hz
- */
-/**There are four @@@@@ boucles @@@@@:
- * - 20 Hz:
- *   - lets use \a reporting_task at 60 Hz
- *   - updates estimator of ir with \a ahrs_update_infrared
- *   - set \a desired_aileron and \a desired_elevator with \a pid_attitude_loop
- *   - sends to \a fbw \a desired_throttle, \a desired_aileron and
- *     \a desired_elevator \note \a desired_throttle is set upon GPS
- *     message reception
- * - 4 Hz:
- *   - calls \a estimator_propagate_state
- *   - do navigation with \a navigation_task
- *
- */
-
-
-static inline void attitude_loop( void ) {
+void attitude_loop( void ) {
 
 #if USE_INFRARED
   ahrs_update_infrared();
@@ -411,192 +518,58 @@ static inline void attitude_loop( void ) {
 
 }
 
-#if USE_AHRS
-#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
-volatile uint8_t new_ins_attitude = 0;
-#endif
-#endif
-
-void periodic_task_ap( void ) {
-
-  static uint8_t _60Hz = 0;
-  static uint8_t _20Hz = 0;
-  static uint8_t _10Hz = 0;
-  static uint8_t _4Hz  = 0;
-  static uint8_t _1Hz  = 0;
 
+/** Run at PERIODIC_FREQUENCY (60Hz if not defined) */
+void sensors_task( void ) {
 #if USE_IMU
-  // Run at PERIODIC_FREQUENCY (60Hz if not defined)
   imu_periodic();
 
 #if USE_AHRS
   if (ahrs_timeout_counter < 255)
     ahrs_timeout_counter ++;
-#endif
-
+#endif // USE_AHRS
 #endif // USE_IMU
-
-#define _check_periodic_freq_ PERIODIC_FREQUENCY % 60
-#if _check_periodic_freq_
-#error Using HighSpeed Periodic: PERIODIC_FREQUENCY has to be a multiple of 60!
-#endif
-  _60Hz++;
-  if (_60Hz >= (PERIODIC_FREQUENCY / 60))
-  {
-    _60Hz = 0;
-  }
-  else
-  {
-    return;
-  }
-
-
-  // Rest of the periodic function still runs at 60Hz like always
-  _20Hz++;
-  if (_20Hz>=3) _20Hz=0;
-  _10Hz++;
-  if (_10Hz>=6) _10Hz=0;
-  _4Hz++;
-  if (_4Hz>=15) _4Hz=0;
-  _1Hz++;
-  if (_1Hz>=60) _1Hz=0;
-
-  reporting_task();
-
-  if (!_1Hz) {
-    if (estimator_flight_time) estimator_flight_time++;
-#if defined DATALINK || defined SITL
-    datalink_time++;
-#endif
-
-    static uint8_t t = 0;
-    if (vsupply < LOW_BATTERY_DECIVOLT) t++; else t = 0;
-    kill_throttle |= (t >= LOW_BATTERY_DELAY);
-    kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE));
-  }
-
-  switch(_4Hz) {
-  case 0:
-    estimator_propagate_state();
-    navigation_task();
-    break;
-  case 1:
-    if (!estimator_flight_time &&
-        estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) {
-      estimator_flight_time = 1;
-      launch = TRUE; /* Not set in non auto launch */
-      DOWNLINK_SEND_TAKEOFF(DefaultChannel, DefaultDevice, &cpu_time_sec);
-  default:
-    break;
-    }
-
-    break;
-
-#ifdef USE_GPIO
-  case 3:
-    GpioUpdate1();
-    break;
-#endif
-
-    /*  default: */
-  }
-
-#ifndef CONTROL_RATE
-#define CONTROL_RATE 20
-#endif
-
-#if CONTROL_RATE != 60 && CONTROL_RATE != 20
-#error "Only 20 and 60 allowed for CONTROL_RATE"
-#endif
-
-#if CONTROL_RATE == 20
-  if (!_20Hz)
-#endif
-    {
-
-#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
-      attitude_loop();
-#endif
-
-    }
-
-
-  modules_periodic_task();
 }
 
 
-void init_ap( void ) {
-#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
-  mcu_init();
-  sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
-#endif /* SINGLE_MCU */
 
-  /************* Sensors initialization ***************/
-#if USE_GPS
-  gps_init();
+
+/** Maximum time allowed for low battery level before going into kill mode */
+#define LOW_BATTERY_DELAY 5
+
+/** Maximum distance from HOME waypoint before going into kill mode */
+#ifndef KILL_MODE_DISTANCE
+#define KILL_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)
 #endif
 
+/** Define minimal speed for takeoff in m/s */
+#define MIN_SPEED_FOR_TAKEOFF 5.
+
+/** monitor stuff run at 1Hz */
+void monitor_task( void ) {
+  if (estimator_flight_time)
+    estimator_flight_time++;
+#if defined DATALINK || defined SITL
+  datalink_time++;
+#endif
+
+  static uint8_t t = 0;
+  if (vsupply < CATASTROPHIC_BAT_LEVEL*10)
+    t++;
+  else
+    t = 0;
+  kill_throttle |= (t >= LOW_BATTERY_DELAY);
+  kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE));
+
+  if (!estimator_flight_time &&
+      estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) {
+    estimator_flight_time = 1;
+    launch = TRUE; /* Not set in non auto launch */
+    DOWNLINK_SEND_TAKEOFF(DefaultChannel, DefaultDevice, &cpu_time_sec);
+  }
+
 #ifdef USE_GPIO
-  GpioInit();
-#endif
-
-#if USE_IMU
-  imu_init();
-#endif
-
-#if USE_AHRS_ALIGNER
-  ahrs_aligner_init();
-#endif
-
-#if USE_AHRS
-  ahrs_init();
-#endif
-
-  /************* Links initialization ***************/
-#if defined MCU_SPI_LINK
-  link_mcu_init();
-#endif
-#if USE_AUDIO_TELEMETRY
-  audio_telemetry_init();
-#endif
-
-  /************ Internal status ***************/
-  h_ctl_init();
-  v_ctl_init();
-  estimator_init();
-#ifdef ALT_KALMAN
-  alt_kalman_init();
-#endif
-  nav_init();
-
-  modules_init();
-
-  settings_init();
-
-  /** - start interrupt task */
-  mcu_int_enable();
-
-  /** wait 0.5s (historical :-) */
-  sys_time_usleep(500000);
-
-#if defined DATALINK
-
-#if DATALINK == XBEE
-  xbee_init();
-#endif
-#endif /* DATALINK */
-
-#if defined AEROCOMM_DATA_PIN
-  IO0DIR |= _BV(AEROCOMM_DATA_PIN);
-  IO0SET = _BV(AEROCOMM_DATA_PIN);
-#endif
-
-  power_switch = FALSE;
-
-  /************ Multi-uavs status ***************/
-
-#ifdef TRAFFIC_INFO
-  traffic_info_init();
+   GpioUpdate1();
 #endif
 }
 
@@ -621,7 +594,7 @@ void event_task_ap( void ) {
 
 
 #ifdef MCU_SPI_LINK
-    link_mcu_event_task();
+  link_mcu_event_task();
 #endif
 
   if (inter_mcu_received_fbw) {
@@ -665,7 +638,7 @@ static inline void on_gyro_event( void ) {
   ahrs_timeout_counter = 0;
 
 #ifdef AHRS_CPU_LED
-    LED_ON(AHRS_CPU_LED);
+  LED_ON(AHRS_CPU_LED);
 #endif
 
 #if USE_AHRS_ALIGNER
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.h b/sw/airborne/firmwares/fixedwing/main_ap.h
index 3ca5cff16b..12c4b8f0b5 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.h
+++ b/sw/airborne/firmwares/fixedwing/main_ap.h
@@ -31,7 +31,13 @@
 #define AP_H
 
 extern void init_ap( void );
-extern void periodic_task_ap( void );
+extern void handle_periodic_tasks_ap( void );
 extern void event_task_ap( void );
 
+extern void sensors_task( void );
+extern void navigation_task( void );
+extern void monitor_task( void );
+extern void reporting_task( void );
+extern void attitude_loop( void );
+
 #endif
diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c
index 23555723af..cce4d39533 100644
--- a/sw/airborne/firmwares/fixedwing/main_fbw.c
+++ b/sw/airborne/firmwares/fixedwing/main_fbw.c
@@ -55,14 +55,14 @@ uint8_t fbw_mode;
 
 volatile uint8_t fbw_new_actuators = 0;
 
+tid_t fbw_periodic_tid; ///< id for periodic_task_fbw() timer
+tid_t electrical_tid;   ///< id for electrical_periodic() timer
 
 /********** INIT *************************************************************/
 void init_fbw( void ) {
 
   mcu_init();
 
-  sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
-
   electrical_init();
 
 #ifdef ACTUATORS
@@ -83,6 +83,10 @@ void init_fbw( void ) {
 
   fbw_mode = FBW_MODE_FAILSAFE;
 
+  /**** start timers for periodic functions *****/
+  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
@@ -164,8 +168,6 @@ void event_task_fbw( void) {
 #endif
 
 
-
-
 #ifdef MCU_SPI_LINK
   if (link_mcu_received) {
     link_mcu_received = FALSE;
@@ -180,9 +182,6 @@ void event_task_fbw( void) {
 
 /************* PERIODIC ******************************************************/
 void periodic_task_fbw( void ) {
-  static uint8_t _10Hz; /* FIXME : sys_time should provide it */
-  _10Hz++;
-  if (_10Hz >= 6) _10Hz = 0;
 
 #ifdef RADIO_CONTROL
   radio_control_periodic_task();
@@ -203,8 +202,14 @@ void periodic_task_fbw( void ) {
   fbw_downlink_periodic_task();
 #endif
 
-  if (!_10Hz) {
+}
+
+void handle_periodic_tasks_fbw(void) {
+
+  if (sys_time_check_and_ack_timer(fbw_periodic_tid))
+    periodic_task_fbw();
+
+  if (sys_time_check_and_ack_timer(electrical_tid))
     electrical_periodic();
-  }
 
 }
diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.h b/sw/airborne/firmwares/fixedwing/main_fbw.h
index 729a38a1ee..75f3d01953 100644
--- a/sw/airborne/firmwares/fixedwing/main_fbw.h
+++ b/sw/airborne/firmwares/fixedwing/main_fbw.h
@@ -42,6 +42,7 @@ extern uint8_t fbw_mode;
 extern bool_t failsafe_mode;
 
 void init_fbw( void );
+void handle_periodic_tasks_fbw( void );
 void periodic_task_fbw( void );
 void event_task_fbw( void );
 
diff --git a/sw/airborne/rc_settings.c b/sw/airborne/rc_settings.c
index d64857a4b4..15573d398a 100644
--- a/sw/airborne/rc_settings.c
+++ b/sw/airborne/rc_settings.c
@@ -34,6 +34,8 @@
 #include "inter_mcu.h"
 #include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
 
+uint8_t rc_settings_mode = 0;
+float slider_1_val, slider_2_val;
 
 #define ParamValInt16(param_init_val, param_travel, cur_pulse, init_pulse) \
 (param_init_val + (int16_t)(((float)(cur_pulse - init_pulse)) * param_travel / (float)MAX_PPRZ))
diff --git a/sw/airborne/rc_settings.h b/sw/airborne/rc_settings.h
index 2fd8677e25..796a881e15 100644
--- a/sw/airborne/rc_settings.h
+++ b/sw/airborne/rc_settings.h
@@ -23,8 +23,9 @@
  */
 
 
-/** \file rc_settings.h
- *  \brief Variable setting though the radio control
+/**
+ * @file rc_settings.h
+ * Variable setting though the radio control
  *
  * The 'rc_control' section of a XML flight plan allows the user to change the
  * value of an autopilot internal variable through the rc transmitter.
@@ -33,6 +34,7 @@
  */
 
 #ifndef RC_SETTINGS_H
+#define RC_SETTINGS_H
 
 #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
 
@@ -43,8 +45,16 @@
 #define RC_SETTINGS_MODE_DOWN      1
 #define RC_SETTINGS_MODE_UP        2
 
+/** rc settings mode
+ *  can be either
+ *  - #RC_SETTINGS_MODE_NONE
+ *  - #RC_SETTINGS_MODE_DOWN
+ *  - #RC_SETTINGS_MODE_UP
+ */
 extern uint8_t rc_settings_mode;
 
+extern float slider_1_val, slider_2_val;
+
 void rc_settings(bool_t mode_changed);
 
 #define RcSettingsOff() (rc_settings_mode==RC_SETTINGS_MODE_NONE)
diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml
index baea8733c8..f001556da9 100644
--- a/sw/simulator/sitl.ml
+++ b/sw/simulator/sitl.ml
@@ -43,6 +43,8 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
 
   let servos_period = 1./.40. (* s *)
   let periodic_period = 1./.60. (* s *)
+  let nav_period = 1./.4. (* s *)
+  let monitor_period = 1. (* s *)
   let rc_period = 1./.40. (* s *)
 
   let msg = fun name ->
@@ -125,6 +127,8 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
     window#show ()
 
   external periodic_task : unit -> unit = "sim_periodic_task"
+  external nav_task : unit -> unit = "sim_nav_task"
+  external monitor_task : unit -> unit = "sim_monitor_task"
   external sim_init : unit -> unit = "sim_init"
   external update_bat : int -> unit = "update_bat"
   external update_adc1 : int -> unit = "update_adc1"
@@ -177,6 +181,8 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
   let boot = fun time_scale ->
     Stdlib.timer ~scale:time_scale servos_period (update_servos bat_button);
     Stdlib.timer ~scale:time_scale periodic_period periodic_task;
+    Stdlib.timer ~scale:time_scale nav_period nav_task;
+    Stdlib.timer ~scale:time_scale monitor_period monitor_task;
 
     (* Forward or broacast messages according to "link" mode *)
     Hashtbl.iter