diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml
index 4ed45b1ef9..1f3e132b66 100644
--- a/conf/conf_tests.xml
+++ b/conf/conf_tests.xml
@@ -400,7 +400,7 @@
    ac_id="1"
    airframe="airframes/examples/quadrotor_lisa_mx_mavlink.xml"
    radio="radios/dummy.xml"
-   telemetry="telemetry/default_rotorcraft.xml"
+   telemetry="telemetry/default_rotorcraft_mavlink.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/rotorcraft_basic.xml settings/estimation/ahrs_float_mlkf.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int_quat.xml settings/estimation/ahrs_secondary.xml settings/estimation/ahrs_int_cmpl_quat.xml"
    settings_modules="modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml"
diff --git a/conf/telemetry/telemetry.dtd b/conf/telemetry/telemetry.dtd
index e20c46fe1a..09389a37e0 100644
--- a/conf/telemetry/telemetry.dtd
+++ b/conf/telemetry/telemetry.dtd
@@ -6,6 +6,7 @@
 <!ATTLIST telemetry>
 <!ATTLIST process
   name CDATA #REQUIRED
+  type CDATA #IMPLIED
 >
 <!ATTLIST mode
   name CDATA #REQUIRED
diff --git a/sw/airborne/modules/datalink/mavlink.c b/sw/airborne/modules/datalink/mavlink.c
index 2db1cf3cb4..9686fd2d5f 100644
--- a/sw/airborne/modules/datalink/mavlink.c
+++ b/sw/airborne/modules/datalink/mavlink.c
@@ -32,6 +32,12 @@
 #include "mavlink/paparazzi/mavlink.h"
 #pragma GCC diagnostic pop
 
+#if PERIODIC_TELEMETRY
+#define PERIODIC_C_MAVLINK
+#include "subsystems/datalink/telemetry_common.h"
+#include "generated/periodic_telemetry.h"
+#endif
+
 #include "generated/airframe.h"
 #include "generated/modules.h"
 #include "generated/settings.h"
@@ -91,6 +97,22 @@ static inline void mavlink_send_vfr_hud(void);
 #define MAVLINK_SYSID AC_ID
 #endif
 
+#if PERIODIC_TELEMETRY
+struct telemetry_cb_slots mavlink_cbs[TELEMETRY_MAVLINK_NB_MSG] = TELEMETRY_MAVLINK_CBS;
+struct periodic_telemetry mavlink_telemetry = { TELEMETRY_MAVLINK_NB_MSG, mavlink_cbs };
+
+static void send_heartbeat(struct transport_tx *trans __attribute__((unused)),
+                          struct link_device *dev __attribute__((unused)))
+{
+  mavlink_send_heartbeat();
+}
+static void send_sys_status(struct transport_tx *trans __attribute__((unused)),
+                            struct link_device *dev __attribute__((unused)))
+{
+  mavlink_send_sys_status();
+}
+#endif
+
 void mavlink_init(void)
 {
   mavlink_system.sysid = MAVLINK_SYSID; // System ID, 1-255
@@ -99,6 +121,11 @@ void mavlink_init(void)
   get_pprz_git_version(custom_version);
 
   mavlink_mission_init(&mission_mgr);
+
+#if PERIODIC_TELEMETRY
+  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_HEARTBEAT, send_heartbeat);
+  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_SYS_STATUS, send_sys_status);
+#endif
 }
 
 /**
@@ -107,8 +134,11 @@ void mavlink_init(void)
  */
 void mavlink_periodic(void)
 {
-  RunOnceEvery(2, mavlink_send_heartbeat());
-  RunOnceEvery(5, mavlink_send_sys_status());
+  // transport and device not used here yet...
+  periodic_telemetry_send_Mavlink(&mavlink_telemetry, NULL, NULL);
+
+  //RunOnceEvery(2, mavlink_send_heartbeat());
+  //RunOnceEvery(5, mavlink_send_sys_status());
   RunOnceEvery(20, mavlink_send_system_time());
   RunOnceEvery(10, mavlink_send_attitude());
   RunOnceEvery(5, mavlink_send_attitude_quaternion());
diff --git a/sw/airborne/subsystems/datalink/telemetry.c b/sw/airborne/subsystems/datalink/telemetry.c
index e0166f0dd3..157e7b5cb4 100644
--- a/sw/airborne/subsystems/datalink/telemetry.c
+++ b/sw/airborne/subsystems/datalink/telemetry.c
@@ -33,8 +33,8 @@
 /* Implement global structures from generated header.
  * Can register up to #TELEMETRY_NB_CBS callbacks per periodic message.
  */
-struct telemetry_cb_slots telemetry_cbs[TELEMETRY_NB_MSG] = TELEMETRY_CBS;
-struct periodic_telemetry pprz_telemetry = { TELEMETRY_NB_MSG, telemetry_cbs };
+struct telemetry_cb_slots telemetry_cbs[TELEMETRY_PPRZ_NB_MSG] = TELEMETRY_PPRZ_CBS;
+struct periodic_telemetry pprz_telemetry = { TELEMETRY_PPRZ_NB_MSG, telemetry_cbs };
 
 
 /** Register a telemetry callback function.
diff --git a/sw/tools/generators/gen_messages.ml b/sw/tools/generators/gen_messages.ml
index b84421469f..954e5cf90e 100644
--- a/sw/tools/generators/gen_messages.ml
+++ b/sw/tools/generators/gen_messages.ml
@@ -235,7 +235,10 @@ module Gen_onboard = struct
       if m.id < 0 || m.id > 255 then begin
         fprintf stderr "Error: message %s has id %d but should be between 0 and 255\n" m.name m.id; exit 1;
       end
-      else fprintf h "#define DL_%s %d\n" m.name m.id
+      else begin
+        fprintf h "#define DL_%s %d\n" m.name m.id;
+        fprintf h "#define PPRZ_MSG_ID_%s %d\n" m.name m.id;
+      end
     ) messages;
     fprintf h "#define DL_MSG_%s_NB %d\n\n" class_ (List.length messages)
 
diff --git a/sw/tools/generators/gen_periodic.ml b/sw/tools/generators/gen_periodic.ml
index 6cf66015d6..aac3c145a6 100644
--- a/sw/tools/generators/gen_periodic.ml
+++ b/sw/tools/generators/gen_periodic.ml
@@ -37,7 +37,7 @@ let lprintf = fun c f ->
   fprintf c "%s" (String.make !margin ' ');
   fprintf c f
 
-let output_modes = fun out_h process_name modes freq modules ->
+let output_modes = fun out_h process_name telem_type modes freq modules ->
   let min_period = 1./.float freq in
   let max_period = 65536. /. float freq in
 
@@ -87,15 +87,15 @@ let output_modes = fun out_h process_name modes freq modules ->
           right ();
           lprintf out_h "for (j = 0; j < TELEMETRY_NB_CBS; j++) {\n";
           right ();
-          lprintf out_h "if (telemetry->cbs[TELEMETRY_MSG_%s_ID].slots[j] != NULL)\n" message_name;
+          lprintf out_h "if (telemetry->cbs[TELEMETRY_%s_MSG_%s_IDX].slots[j] != NULL)\n" telem_type message_name;
           right ();
-          lprintf out_h "telemetry->cbs[TELEMETRY_MSG_%s_ID].slots[j](trans, dev);\n" message_name;
+          lprintf out_h "telemetry->cbs[TELEMETRY_%s_MSG_%s_IDX].slots[j](trans, dev);\n" telem_type message_name;
           left ();
           lprintf out_h "else break;\n";
           left ();
           lprintf out_h "}\n";
           fprintf out_h "#if USE_PERIODIC_TELEMETRY_REPORT\n";
-          lprintf out_h "if (j == 0) periodic_telemetry_err_report(TELEMETRY_PROCESS_%s, telemetry_mode_%s, DL_%s);\n" process_name process_name message_name;
+          lprintf out_h "if (j == 0) periodic_telemetry_err_report(TELEMETRY_PROCESS_%s, telemetry_mode_%s, %s_MSG_ID_%s);\n" process_name process_name telem_type message_name;
           fprintf out_h "#endif\n";
           left ();
           lprintf out_h "}\n"
@@ -138,10 +138,12 @@ let write_settings = fun xml_file out_set telemetry_xml ->
   fprintf out_set "</settings>\n"
 
 let print_message_table = fun out_h xml ->
-  let messages = Hashtbl.create 15 in
-  fprintf out_h "/* Periodic telemetry messages */\n";
+  let telemetry_types = Hashtbl.create 2 in
   (* For each process *)
   List.iter (fun process ->
+    let telem_type = String.uppercase (ExtXml.attrib_or_default process "type" "pprz") in
+    if not (Hashtbl.mem telemetry_types telem_type) then Hashtbl.add telemetry_types telem_type (Hashtbl.create 15);
+    let messages = Hashtbl.find telemetry_types telem_type in
     (** For each mode of this process *)
     List.iter (fun mode ->
       (** For each message in this mode *)
@@ -152,28 +154,33 @@ let print_message_table = fun out_h xml ->
       ) (Xml.children mode)
     ) (Xml.children process)
   ) (Xml.children xml);
-  (* Print ID *)
-  let nb = Hashtbl.fold (fun n _ i ->
-    Xml2h.define (sprintf "TELEMETRY_MSG_%s_ID" n) (sprintf "%d" i);
-    i+1
-  ) messages 0 in
-  Xml2h.define "TELEMETRY_NB_MSG" (sprintf "%d" nb);
-  (* Structure initialization *)
-  fprintf out_h "\n#define TELEMETRY_MSG_NAMES { \\\n";
-  Hashtbl.iter (fun n _ -> fprintf out_h "  \"%s\", \\\n" n) messages;
-  fprintf out_h "}\n\n";
-  fprintf out_h "#define TELEMETRY_CBS { \\\n";
-  Hashtbl.iter (fun n _ -> fprintf out_h "  {.id=DL_%s, .slots={ NULL }}, \\\n" n) messages;
-  fprintf out_h "}\n\n"
+  (* for each telemetry type, print ID and other defines *)
+  Hashtbl.iter (fun telem_type messages ->
+    (* Print ID *)
+    fprintf out_h "/* Periodic telemetry messages of type %s */\n" telem_type;
+    let nb = Hashtbl.fold (fun n _ i ->
+      Xml2h.define (sprintf "TELEMETRY_%s_MSG_%s_IDX" telem_type n) (sprintf "%d" i);
+      i+1
+    ) messages 0 in
+    fprintf out_h "#define TELEMETRY_%s_NB_MSG %d\n" telem_type nb;
+    (* Structure initialization *)
+    fprintf out_h "\n#define TELEMETRY_%s_MSG_NAMES { \\\n" telem_type;
+    Hashtbl.iter (fun n _ -> fprintf out_h "  \"%s\", \\\n" n) messages;
+    fprintf out_h "}\n\n";
+    fprintf out_h "#define TELEMETRY_%s_CBS { \\\n" telem_type;
+    Hashtbl.iter (fun n _ -> fprintf out_h "  {.id=%s_MSG_ID_%s, .slots={ NULL }}, \\\n" telem_type n) messages;
+    fprintf out_h "}\n\n"
+  ) telemetry_types
 
 let print_process_send = fun out_h xml freq modules ->
   (** For each process *)
   List.iter
     (fun process ->
       let process_name = ExtXml.attrib process "name" in
+      let telem_type = String.uppercase (ExtXml.attrib_or_default process "type" "pprz") in
       let modes = Xml.children process in
 
-      fprintf out_h "\n/* Periodic telemetry: %s process */\n" process_name;
+      fprintf out_h "\n/* Periodic telemetry (type %s): %s process */\n" telem_type process_name;
       let p_id = ref 0 in
       Xml2h.define (sprintf "TELEMETRY_PROCESS_%s" process_name) (string_of_int !p_id);
       incr p_id;
@@ -204,7 +211,7 @@ let print_process_send = fun out_h xml freq modules ->
 
       lprintf out_h "static inline void periodic_telemetry_send_%s(struct periodic_telemetry *telemetry, struct transport_tx *trans, struct link_device *dev) {  /* %dHz */\n" process_name freq;
       right ();
-      output_modes out_h process_name modes freq modules;
+      output_modes out_h process_name telem_type modes freq modules;
       left ();
       lprintf out_h "}\n"
     )