diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile
index 9defff09be..e243295ad8 100644
--- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile
+++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile
@@ -143,7 +143,6 @@ fbw_srcs 		+= $(SRC_FIRMWARE)/main_fbw.c
 fbw_srcs 		+= subsystems/electrical.c
 fbw_srcs 		+= subsystems/commands.c
 fbw_srcs 		+= subsystems/actuators.c
-fbw_srcs		+= $(SRC_FIRMWARE)/fbw_downlink.c
 
 ######################################################################
 ##
@@ -151,11 +150,8 @@ fbw_srcs		+= $(SRC_FIRMWARE)/fbw_downlink.c
 ##
 
 ap_CFLAGS 		+= -DAP
-ap_CFLAGS 		+= -DDefaultPeriodic='&telemetry_Ap'
 ap_srcs 		+= $(SRC_FIRMWARE)/main_ap.c
 ap_srcs 		+= $(SRC_FIRMWARE)/autopilot.c
-ap_srcs			+= $(SRC_FIRMWARE)/ap_downlink.c
-ap_srcs         += subsystems/datalink/telemetry.c
 ap_srcs 		+= state.c
 ap_srcs 		+= subsystems/settings.c
 ap_srcs 		+= $(SRC_ARCH)/subsystems/settings_arch.c
@@ -182,8 +178,8 @@ sim.srcs 		+= $(fbw_srcs) $(ap_srcs)
 sim.CFLAGS 		+= -DSITL
 sim.srcs 		+= $(SRC_ARCH)/sim_ap.c
 
-sim.CFLAGS 		+= -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=ivy_tp -DDOWNLINK_DEVICE=ivy_tp
-sim.srcs 		+= subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c subsystems/datalink/ivy_transport.c
+sim.CFLAGS 		+= -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=ivy_tp -DDOWNLINK_DEVICE=ivy_tp -DDefaultPeriodic='&telemetry_Ap'
+sim.srcs 		+= subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c subsystems/datalink/ivy_transport.c subsystems/datalink/telemetry.c $(SRC_FIRMWARE)/ap_downlink.c $(SRC_FIRMWARE)/fbw_downlink.c
 
 sim.srcs 		+= $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_adc_generic.c
 
@@ -243,9 +239,6 @@ else
   ifeq ($(SEPARATE_FBW),)
     ap.CFLAGS 		+= $(fbw_CFLAGS)
     ap.srcs 		+= $(fbw_srcs)
-  else
-   # avoid fbw_telemetry_mode error
-   ap_srcs		+= $(SRC_FIRMWARE)/fbw_downlink.c
   endif
 endif
 
diff --git a/conf/firmwares/subsystems/fixedwing/telemetry_superbitrf.makefile b/conf/firmwares/subsystems/fixedwing/telemetry_superbitrf.makefile
index 721c2cfb58..0f7de97bb3 100644
--- a/conf/firmwares/subsystems/fixedwing/telemetry_superbitrf.makefile
+++ b/conf/firmwares/subsystems/fixedwing/telemetry_superbitrf.makefile
@@ -6,9 +6,14 @@ ap.CFLAGS += -DUSE_$(MODEM_PORT)
 ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
 
 ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_FBW_DEVICE=superbitrf -DDOWNLINK_AP_DEVICE=superbitrf
-ap.CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=SUPERBITRF
+ap.CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=SUPERBITRF -DDefaultPeriodic='&telemetry_Ap'
 #ap.CFLAGS += -DUSE_SUPERBITRF -DUSE_SPI2 -DUSE_SPI_SLAVE2
 
 ap.srcs += peripherals/cyrf6936.c
 ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/superbitrf.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c
-ap.srcs += $(SRC_FIRMWARE)/datalink.c
+ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/ap_downlink.c
+
+# avoid fbw_telemetry_mode error
+ap.srcs += $(SRC_FIRMWARE)/fbw_downlink.c
+
+fbw.srcs += $(SRC_FIRMWARE)/fbw_downlink.c
diff --git a/conf/firmwares/subsystems/fixedwing/telemetry_transparent.makefile b/conf/firmwares/subsystems/fixedwing/telemetry_transparent.makefile
index 69f6e6d3d8..68ea55bbb1 100644
--- a/conf/firmwares/subsystems/fixedwing/telemetry_transparent.makefile
+++ b/conf/firmwares/subsystems/fixedwing/telemetry_transparent.makefile
@@ -5,9 +5,14 @@ PPRZ_MODEM_PORT_LOWER=$(shell echo $(MODEM_PORT) | tr A-Z a-z)
 telemetry_CFLAGS = -DUSE_$(MODEM_PORT)
 telemetry_CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
 telemetry_CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=$(PPRZ_MODEM_PORT_LOWER) -DPPRZ_UART=$(MODEM_PORT)
-telemetry_CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=PPRZ
-telemetry_srcs = subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
+telemetry_CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=PPRZ -DDefaultPeriodic='&telemetry_Ap'
+telemetry_srcs = subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c
 
 ap.CFLAGS += $(telemetry_CFLAGS)
-ap.srcs += $(telemetry_srcs) $(SRC_FIRMWARE)/datalink.c
+ap.srcs += $(telemetry_srcs) $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/ap_downlink.c
+
+# avoid fbw_telemetry_mode error
+ap.srcs += $(SRC_FIRMWARE)/fbw_downlink.c
+
+fbw.srcs += $(SRC_FIRMWARE)/fbw_downlink.c
 
diff --git a/conf/firmwares/subsystems/fixedwing/telemetry_transparent_usb.makefile b/conf/firmwares/subsystems/fixedwing/telemetry_transparent_usb.makefile
index b792904363..194f6bf507 100644
--- a/conf/firmwares/subsystems/fixedwing/telemetry_transparent_usb.makefile
+++ b/conf/firmwares/subsystems/fixedwing/telemetry_transparent_usb.makefile
@@ -1,21 +1,26 @@
 # Hey Emacs, this is a -*- makefile -*-
 
-
 #serial USB (e.g. /dev/ttyACM0)
 
+telemetry_CFLAGS = -DDOWNLINK -DDOWNLINK_DEVICE=usb_serial -DPPRZ_UART=UsbS
+telemetry_CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=PPRZ -DUSE_USB_SERIAL
+telemetry_CFLAGS += -DDefaultPeriodic='&telemetry_Ap'
+telemetry_srcs = subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c
+telemetry_srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/ap_downlink.c $(SRC_FIRMWARE)/fbw_downlink.c
+# avoid fbw_telemetry_mode error
+telemetry_srcs += $(SRC_FIRMWARE)/fbw_downlink.c
+
+ap.CFLAGS += $(telemetry_CFLAGS)
+ap.srcs += $(telemetry_srcs)
+
+fbw.srcs += $(SRC_FIRMWARE)/fbw_downlink.c
+
 ifeq ($(ARCH), lpc21)
-ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=usb_serial -DPPRZ_UART=UsbS
-ap.CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=PPRZ -DUSE_USB_SERIAL -DUSE_USB_HIGH_PCLK
-ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
-ap.srcs += $(SRC_FIRMWARE)/datalink.c
+ap.CFLAGS += -DUSE_USB_HIGH_PCLK
 ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c
 ap.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c
 else
 ifeq ($(ARCH), stm32)
-ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=usb_serial -DPPRZ_UART=UsbS
-ap.CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=PPRZ -DUSE_USB_SERIAL
-ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
-ap.srcs += $(SRC_FIRMWARE)/datalink.c
 ap.srcs += $(SRC_ARCH)/usb_ser_hw.c
 else
 ifneq ($(ARCH), sim)
diff --git a/conf/firmwares/subsystems/fixedwing/telemetry_w5100.makefile b/conf/firmwares/subsystems/fixedwing/telemetry_w5100.makefile
index b7dcaa187d..a2244fb54e 100644
--- a/conf/firmwares/subsystems/fixedwing/telemetry_w5100.makefile
+++ b/conf/firmwares/subsystems/fixedwing/telemetry_w5100.makefile
@@ -8,10 +8,15 @@ W5100_MULTICAST_IP ?= "224,1,1,11"
 W5100_MULTICAST_PORT ?= "1234"
 
 ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=chip0
-ap.CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=W5100
+ap.CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=W5100 -DDefaultPeriodic='&telemetry_Ap'
 ap.CFLAGS += -DW5100_IP=$(W5100_IP) -DW5100_SUBNET=$(W5100_SUBNET) -DW5100_MULTICAST_IP=$(W5100_MULTICAST_IP) -DW5100_MULTICAST_PORT=$(W5100_MULTICAST_PORT)
-ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/w5100.c subsystems/datalink/pprz_transport.c
-ap.srcs += $(SRC_FIRMWARE)/datalink.c
+ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/w5100.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c
+ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/ap_downlink.c
+
+# avoid fbw_telemetry_mode error
+ap.srcs += $(SRC_FIRMWARE)/fbw_downlink.c
+
+fbw.srcs += $(SRC_FIRMWARE)/fbw_downlink.c
 
 ifeq ($(ARCH), lpc21)
 # only an issue of setting the DRDY pin in w5100.c, which is stm32 specific
diff --git a/conf/firmwares/subsystems/fixedwing/telemetry_xbee_api.makefile b/conf/firmwares/subsystems/fixedwing/telemetry_xbee_api.makefile
index 796f710b0d..0aed956964 100644
--- a/conf/firmwares/subsystems/fixedwing/telemetry_xbee_api.makefile
+++ b/conf/firmwares/subsystems/fixedwing/telemetry_xbee_api.makefile
@@ -8,6 +8,11 @@ ap.CFLAGS += -DUSE_$(MODEM_PORT)
 ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -DXBEE_BAUD=$(MODEM_BAUD)
 
 ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=$(XBEE_MODEM_PORT_LOWER) -DXBEE_UART=$(MODEM_PORT)
-ap.CFLAGS += -DDOWNLINK_TRANSPORT=xbee_tp -DDATALINK=XBEE
-ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/xbee.c
-ap.srcs += $(SRC_FIRMWARE)/datalink.c
+ap.CFLAGS += -DDOWNLINK_TRANSPORT=xbee_tp -DDATALINK=XBEE -DDefaultPeriodic='&telemetry_Ap'
+ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/xbee.c subsystems/datalink/telemetry.c
+ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/ap_downlink.c
+
+# avoid fbw_telemetry_mode error
+ap.srcs += $(SRC_FIRMWARE)/fbw_downlink.c
+
+fbw.srcs += $(SRC_FIRMWARE)/fbw_downlink.c
diff --git a/sw/airborne/firmwares/fixedwing/autopilot.c b/sw/airborne/firmwares/fixedwing/autopilot.c
index 74894ec9c9..5c4c437dc6 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot.c
+++ b/sw/airborne/firmwares/fixedwing/autopilot.c
@@ -77,7 +77,9 @@ static void send_mode(struct transport_tx *trans, struct link_device *dev) {
 
 void autopilot_send_mode(void) {
   // use default telemetry here
+#if DOWNLINK
   send_mode(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
+#endif
 }
 
 static void send_attitude(struct transport_tx *trans, struct link_device *dev) {
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index de81a12cfb..18415ce48b 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -241,7 +241,9 @@ void init_ap( void ) {
   /** - start interrupt task */
   mcu_int_enable();
 
+#if DOWNLINK
   downlink_init();
+#endif
 
 #if defined AEROCOMM_DATA_PIN
   IO0DIR |= _BV(AEROCOMM_DATA_PIN);
@@ -449,7 +451,9 @@ void reporting_task( void ) {
   /** then report periodicly */
   else {
     //PeriodicSendAp(DefaultChannel, DefaultDevice);
+#if PERIODIC_TELEMETRY
     periodic_telemetry_send_Ap(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
+#endif
   }
 }
 
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index 6f807317f8..3a292ff228 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -176,7 +176,9 @@ STATIC_INLINE void main_init( void ) {
 
   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);
@@ -220,14 +222,20 @@ STATIC_INLINE void main_periodic( void ) {
   SetActuatorsFromCommands(commands, autopilot_mode);
 
   if (autopilot_in_flight) {
-    RunOnceEvery(PERIODIC_FREQUENCY, { autopilot_flight_time++; datalink_time++; });
+    RunOnceEvery(PERIODIC_FREQUENCY, { autopilot_flight_time++;
+#if defined DATALINK || defined SITL
+        datalink_time++;
+#endif
+        });
   }
 
   RunOnceEvery(10, LED_PERIODIC());
 }
 
 STATIC_INLINE void telemetry_periodic(void) {
+#if PERIODIC_TELEMETRY
   periodic_telemetry_send_Main(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
+#endif
 }
 
 /** mode to enter when RC is lost while using a mode with RC input (not AP_MODE_NAV) */
diff --git a/sw/airborne/subsystems/datalink/telemetry.c b/sw/airborne/subsystems/datalink/telemetry.c
index 1bc2d62f71..9f28188370 100644
--- a/sw/airborne/subsystems/datalink/telemetry.c
+++ b/sw/airborne/subsystems/datalink/telemetry.c
@@ -36,6 +36,8 @@
  * @return TRUE if message registered with success, FALSE otherwise
  */
 bool_t register_periodic_telemetry(struct pprz_telemetry * _pt, const char * _msg, telemetry_cb _cb) {
+  // return FALSE if NULL is passed as pprz_telemetry
+  if (_pt == NULL) return FALSE;
   // look for message name
   uint8_t i;
   for (i = 0; i < _pt->nb; i++) {
diff --git a/sw/airborne/subsystems/datalink/telemetry_common.h b/sw/airborne/subsystems/datalink/telemetry_common.h
index ab107ddbc0..30227b7768 100644
--- a/sw/airborne/subsystems/datalink/telemetry_common.h
+++ b/sw/airborne/subsystems/datalink/telemetry_common.h
@@ -34,6 +34,12 @@
 #include "mcu_periph/link_device.h"
 #include "subsystems/datalink/transport.h"
 
+/** Set default periodic telemetry to NULL
+ */
+#ifndef DefaultPeriodic
+#define DefaultPeriodic NULL
+#endif
+
 /** Telemetry callback definition
  */
 typedef void (*telemetry_cb)(struct transport_tx *trans, struct link_device *dev);
@@ -55,12 +61,17 @@ struct pprz_telemetry {
 };
 
 /** Register a telemetry callback function.
+ * empty implementation is provided if PERIODIC_TELEMETRY is not set or set to FALSE
  * @param _pt periodic telemetry structure to register
  * @param _msg message name (string) as defined in telemetry xml file
  * @param _cb callback function, called according to telemetry mode and specified period
  * @return TRUE if message registered with success, FALSE otherwise
  */
+#if PERIODIC_TELEMETRY
 extern bool_t register_periodic_telemetry(struct pprz_telemetry * _pt, const char * _msg, telemetry_cb _cb);
+#else
+static inline bool_t register_periodic_telemetry(struct pprz_telemetry * _pt __attribute__((unused)), const char * _msg __attribute__((unused)), telemetry_cb _cb __attribute__((unused))) { return FALSE; }
+#endif
 
 #if USE_PERIODIC_TELEMETRY_REPORT
 /** Send an error report when trying to send message that as not been register
diff --git a/sw/tools/generators/gen_messages.ml b/sw/tools/generators/gen_messages.ml
index 6fa169a6ec..7b75fc6597 100644
--- a/sw/tools/generators/gen_messages.ml
+++ b/sw/tools/generators/gen_messages.ml
@@ -166,17 +166,20 @@ module Gen_onboard = struct
       print_macro_param h f;
       List.iter (fun f -> fprintf h ", "; print_macro_param h f) fields
 
-  let print_fun_param h = function
-      (Array (t, _), s, _) -> fprintf h "uint8_t %s, %s *_%s" (Syntax.length_name s) (c_type (Syntax.nameof (Basic t))) s
-    | (FixedArray (t, _, _), s, _) -> fprintf h "%s *_%s" (c_type (Syntax.nameof (Basic t))) s
-    | (t, s, _) -> fprintf h "%s *_%s" (c_type (Syntax.nameof t)) s
+  let print_unused_param = fun unused ->
+    if unused then " __attribute__((unused))" else ""
 
-  let print_function_parameters h = function
+  let print_fun_param ?(unused=false) h = function
+      (Array (t, _), s, _) -> fprintf h "uint8_t %s%s, %s *_%s%s" (Syntax.length_name s) (print_unused_param unused) (c_type (Syntax.nameof (Basic t))) s (print_unused_param unused)
+    | (FixedArray (t, _, _), s, _) -> fprintf h "%s *_%s%s" (c_type (Syntax.nameof (Basic t))) s (print_unused_param unused)
+    | (t, s, _) -> fprintf h "%s *_%s%s" (c_type (Syntax.nameof t)) s (print_unused_param unused)
+
+  let print_function_parameters ?(unused=false) h = function
       [] -> ()
     | f::fields ->
       fprintf h ", ";
-      print_fun_param h f;
-      List.iter (fun f -> fprintf h ", "; print_fun_param h f) fields
+      print_fun_param ~unused h f;
+      List.iter (fun f -> fprintf h ", "; print_fun_param ~unused h f) fields
 
   let rec size_fields = fun fields size ->
     match fields with
@@ -222,8 +225,8 @@ module Gen_onboard = struct
     fprintf h "#define DOWNLINK_SEND_%s(_trans, _dev" s;
     print_macro_parameters h fields;
     fprintf h ") {}\n";
-    fprintf h "void pprz_msg_send_%s(struct transport_tx *trans, struct link_device *dev, uint8_t ac_id" s;
-    print_function_parameters h fields;
+    fprintf h "static inline void pprz_msg_send_%s(struct transport_tx *trans __attribute__((unused)), struct link_device *dev __attribute__((unused)), uint8_t ac_id __attribute__((unused))" s;
+    print_function_parameters ~unused:true h fields;
     fprintf h ") {}\n"
 
   (** Prints the messages ids *)
@@ -370,7 +373,7 @@ let () =
 
     (** Macros for airborne downlink (sending) *)
     if class_name = "telemetry" then begin (** FIXME *)
-      Printf.fprintf h "#ifdef DOWNLINK\n"
+      Printf.fprintf h "#if DOWNLINK\n"
     end;
     Gen_onboard.print_downlink_macros h class_name messages;
     if class_name = "telemetry" then begin