diff --git a/conf/autopilot/setup.makefile b/conf/autopilot/setup.makefile
index 4d063bcb3e..33a14d5998 100644
--- a/conf/autopilot/setup.makefile
+++ b/conf/autopilot/setup.makefile
@@ -97,7 +97,7 @@ endif
 
 # a test program to setup actuators
 ifeq ($(ARCH), lpc21)
-setup_actuators.CFLAGS += -DFBW -DUSE_LED
+setup_actuators.CFLAGS += -DFBW -DUSE_LED -DPERIPHERALS_AUTO_INIT
 setup_actuators.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DDOWNLINK_DEVICE=Uart1 -DPPRZ_UART=Uart1
 setup_actuators.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ
 setup_actuators.CFLAGS += -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1
diff --git a/sw/airborne/firmwares/setup/setup_actuators.c b/sw/airborne/firmwares/setup/setup_actuators.c
index a852e928a7..f758a1d1dd 100644
--- a/sw/airborne/firmwares/setup/setup_actuators.c
+++ b/sw/airborne/firmwares/setup/setup_actuators.c
@@ -8,17 +8,12 @@
 #define DATALINK_C
 #include "subsystems/datalink/datalink.h"
 #include "subsystems/datalink/pprz_transport.h"
+#include "mcu_periph/uart.h"
+#include "subsystems/datalink/downlink.h"
 #include "firmwares/fixedwing/main_fbw.h"
 
 #include "generated/settings.h"
 
-#ifndef DOWNLINK_DEVICE
-#define DOWNLINK_DEVICE DOWNLINK_FBW_DEVICE
-#endif
-#include "mcu_periph/uart.h"
-#include "subsystems/datalink/downlink.h"
-#include "ap_downlink.h"
-
 #define IdOfMsg(x) (x[1])
 
 
@@ -49,14 +44,9 @@ void dl_parse_msg( void ) {
 #endif
 }
 
-#define PprzUartInit() Link(Init())
 
 void init_fbw( void ) {
   mcu_init();
-  sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
-  led_init();
-
-  PprzUartInit();
 
   actuators_init();
 
@@ -67,9 +57,18 @@ void init_fbw( void ) {
 
   //  SetServo(SERVO_GAZ, SERVO_GAZ_MIN);
 
+  sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
+
   mcu_int_enable();
 }
 
+void handle_periodic_tasks_fbw(void) {
+
+  if (sys_time_check_and_ack_timer(0))
+    periodic_task_fbw();
+
+}
+
 void periodic_task_fbw(void) {
    /* static float t; */
    /* t += 1./60.; */