diff --git a/conf/firmwares/lisa_test_progs.makefile b/conf/firmwares/lisa_test_progs.makefile
index bb8f1a53b8..7b0750d5a5 100644
--- a/conf/firmwares/lisa_test_progs.makefile
+++ b/conf/firmwares/lisa_test_progs.makefile
@@ -80,9 +80,11 @@ COMMON_TEST_SRCS   += $(SRC_ARCH)/led_hw.c
 
 COMMON_TELEMETRY_CFLAGS  = -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
 COMMON_TELEMETRY_CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
+COMMON_TELEMETRY_CFLAGS += -DDefaultPeriodic='&telemetry_Main'
 COMMON_TELEMETRY_SRCS    = mcu_periph/uart.c
 COMMON_TELEMETRY_SRCS   += $(SRC_ARCH)/mcu_periph/uart_arch.c
-COMMON_TELEMETRY_SRCS   += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
+COMMON_TELEMETRY_SRCS   += subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c
+COMMON_TELEMETRY_SRCS   += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/rotorcraft_telemetry.c
 
 #COMMON_TEST_SRCS   += math/pprz_trig_int.c
 
diff --git a/conf/firmwares/setup.makefile b/conf/firmwares/setup.makefile
index ca5fb490fb..3eec7d7ae8 100644
--- a/conf/firmwares/setup.makefile
+++ b/conf/firmwares/setup.makefile
@@ -86,7 +86,9 @@ setup_actuators.srcs   += mcu_periph/uart.c $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 setup_actuators.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(MODEM_PORT) -DPPRZ_UART=$(MODEM_PORT)
 setup_actuators.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ
+setup_actuators.CFLAGS += -DDefaultPeriodic='&telemetry_Ap'
 setup_actuators.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
+setup_actuators.srcs += subsystems/datalink/telemetry.c
 ifneq ($(SYS_TIME_LED),none)
 setup_actuators.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
 endif
diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile
index b9984a6ff1..08ccd5f24f 100644
--- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile
+++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile
@@ -93,6 +93,11 @@ $(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_
 $(TARGET).srcs += mcu_periph/i2c.c
 $(TARGET).srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c
 
+#
+# Telemetry
+#
+$(TARGET).srcs += subsystems/datalink/telemetry.c
+
 ######################################################################
 ##
 ## COMMON FOR ALL NON-SIMULATION TARGETS
@@ -115,7 +120,6 @@ endif
 # Main
 #
 ns_srcs	   	+= $(SRC_FIRMWARE)/main.c
-ns_srcs	   	+= subsystems/datalink/telemetry.c
 
 #
 # LEDs
@@ -204,7 +208,7 @@ sim.CFLAGS 		+= -DSITL
 sim.srcs 		+= $(SRC_ARCH)/sim_ap.c
 
 sim.CFLAGS 		+= -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DDefaultPeriodic='&telemetry_Ap'
-sim.srcs 		+= subsystems/datalink/telemetry.c subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c
+sim.srcs 		+= subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c
 sim.srcs		+= $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c
 
 sim.srcs 		+= subsystems/settings.c
@@ -245,8 +249,9 @@ jsbsim.srcs 		+= $(SIMDIR)/sim_ac_jsbsim.c $(SIMDIR)/sim_ac_fw.c $(SIMDIR)/sim_a
 jsbsim.CFLAGS 		+= -I/usr/include $(shell pkg-config glib-2.0 --cflags)
 jsbsim.LDFLAGS		+= $(shell pkg-config glib-2.0 --libs) -lglibivy -lm
 
-jsbsim.CFLAGS 		+= -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport
-jsbsim.srcs 		+= subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_ir.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/jsbsim_ahrs.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/jsbsim_transport.c
+jsbsim.CFLAGS 		+= -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DDefaultPeriodic='&telemetry_Ap'
+jsbsim.srcs 		+= subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c
+jsbsim.srcs 		+= $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_ir.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/jsbsim_ahrs.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/jsbsim_transport.c
 
 jsbsim.srcs 		+= subsystems/settings.c
 jsbsim.srcs 		+= $(SRC_ARCH)/subsystems/settings_arch.c
diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
index 2e5c837a16..5b6698d634 100644
--- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
@@ -38,7 +38,7 @@ else
 endif
 
 
-nps.srcs += $(NPSDIR)/nps_main.c                      \
+nps.srcs += $(NPSDIR)/nps_main.c                 \
        $(NPSDIR)/nps_fdm_jsbsim.c                \
        $(NPSDIR)/nps_random.c                    \
        $(NPSDIR)/nps_sensors.c                   \
@@ -51,7 +51,7 @@ nps.srcs += $(NPSDIR)/nps_main.c                      \
        $(NPSDIR)/nps_radio_control.c             \
        $(NPSDIR)/nps_radio_control_joystick.c    \
        $(NPSDIR)/nps_radio_control_spektrum.c    \
-       $(NPSDIR)/nps_autopilot_rotorcraft.c            \
+       $(NPSDIR)/nps_autopilot_rotorcraft.c      \
        $(NPSDIR)/nps_ivy.c                       \
        $(NPSDIR)/nps_flightgear.c                \
 
@@ -77,15 +77,15 @@ nps.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c
 nps.srcs += subsystems/settings.c
 nps.srcs += $(SRC_ARCH)/subsystems/settings_arch.c
 
-nps.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport
-nps.srcs += $(SRC_FIRMWARE)/rotorcraft_telemetry.c \
-            subsystems/datalink/downlink.c \
-            $(SRC_ARCH)/ivy_transport.c
+nps.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DDefaultPeriodic='&telemetry_Main'
+nps.srcs += $(SRC_ARCH)/ivy_transport.c
+nps.srcs += subsystems/datalink/downlink.c subsystems/datalink/telemetry.c
+nps.srcs += $(SRC_FIRMWARE)/rotorcraft_telemetry.c
+nps.srcs += $(SRC_FIRMWARE)/datalink.c
 
 nps.srcs   += subsystems/actuators.c
 nps.srcs   += subsystems/commands.c
 
-nps.srcs += $(SRC_FIRMWARE)/datalink.c
 
 #
 # Math functions
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index a1ed2a3c64..e63945e6f7 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -31,6 +31,7 @@
 #include <inttypes.h>
 #include "mcu.h"
 #include "mcu_periph/sys_time.h"
+#include "mcu_periph/i2c.h"
 #include "led.h"
 
 #include "subsystems/datalink/telemetry.h"