diff --git a/sw/airborne/subsystems/datalink/telemetry.c b/sw/airborne/subsystems/datalink/telemetry.c
index 115ffa4ba6..556dfa9b33 100644
--- a/sw/airborne/subsystems/datalink/telemetry.c
+++ b/sw/airborne/subsystems/datalink/telemetry.c
@@ -30,10 +30,11 @@
 #include "subsystems/datalink/telemetry_common.h"
 #include "generated/periodic_telemetry.h"
 
-/* Implement global structures from generated header
+/* Implement global structures from generated header.
+ * Can register up to #TELEMETRY_NB_CBS callbacks per periodic message.
  */
 telemetry_msg telemetry_msgs[TELEMETRY_NB_MSG] = TELEMETRY_MSG_NAMES;
-telemetry_cb telemetry_cbs[TELEMETRY_NB_MSG] = TELEMETRY_CBS_NULL;
+struct telemetry_cb_slots telemetry_cbs[TELEMETRY_NB_MSG] = TELEMETRY_CBS_NULL;
 struct periodic_telemetry pprz_telemetry = { TELEMETRY_NB_MSG, telemetry_msgs, telemetry_cbs };
 
 
@@ -48,14 +49,18 @@ bool_t register_periodic_telemetry(struct periodic_telemetry *_pt, const char *_
   // return FALSE if NULL is passed as periodic_telemetry
   if (_pt == NULL) { return FALSE; }
   // look for message name
-  uint8_t i;
+  uint8_t i, j;
   for (i = 0; i < _pt->nb; i++) {
     if (str_equal(_pt->msgs[i], _msg)) {
-      // register callback if not already done
-      if (_pt->cbs[i] == NULL) {
-        _pt->cbs[i] = _cb;
-        return TRUE;
-      } else { return FALSE; }
+      // register another callback if not all TELEMETRY_NB_CBS slots taken
+      for (j = 0; j < TELEMETRY_NB_CBS; j++) {
+        if (_pt->cbs[i].slots[j] == NULL) {
+          _pt->cbs[i].slots[j] = _cb;
+          return TRUE;
+        }
+      }
+      // message matched but no more empty slots available
+      return FALSE;
     }
   }
   // message name is not in telemetry file
diff --git a/sw/airborne/subsystems/datalink/telemetry_common.h b/sw/airborne/subsystems/datalink/telemetry_common.h
index 61c3457c3e..d9b4ad91e1 100644
--- a/sw/airborne/subsystems/datalink/telemetry_common.h
+++ b/sw/airborne/subsystems/datalink/telemetry_common.h
@@ -42,15 +42,21 @@ typedef void (*telemetry_cb)(struct transport_tx *trans, struct link_device *dev
  */
 typedef const char telemetry_msg[64];
 
+/** number of callbacks that can be registered per msg */
+#define TELEMETRY_NB_CBS 4
+
+struct telemetry_cb_slots {
+  telemetry_cb slots[TELEMETRY_NB_CBS];
+};
 
 /** Periodic telemetry structure.
  *  Contains the total number of messages (from generated telemetry file)
  *  and the list of registered callbacks
  */
 struct periodic_telemetry {
-  uint8_t nb;           ///< number of messages
-  telemetry_msg *msgs;  ///< the array of msg names
-  telemetry_cb *cbs;    ///< array of associated callbacks
+  uint8_t nb;                ///< number of messages
+  telemetry_msg *msgs;       ///< the array of msg names
+  struct telemetry_cb_slots *cbs; ///< array of associated callbacks
 };
 
 /** Register a telemetry callback function.
diff --git a/sw/tools/generators/gen_periodic.ml b/sw/tools/generators/gen_periodic.ml
index 74199daa8a..205c02292b 100644
--- a/sw/tools/generators/gen_periodic.ml
+++ b/sw/tools/generators/gen_periodic.ml
@@ -65,6 +65,8 @@ let output_modes = fun out_h process_name modes freq modules ->
         lprintf out_h "static %s %s = 0; %s++; if (%s>=%d) %s=0;\n" _type v v v m v;
       ) modulos;
 
+      lprintf out_h "uint8_t j;\n";
+
       (** For each message in this mode *)
       let messages = List.sort (fun (_,p) (_,p') -> compare p p') messages in
       let i = ref 0 in (** Basic balancing:1 message every 10Hz *)
@@ -82,12 +84,17 @@ let output_modes = fun out_h process_name modes freq modules ->
           l := (p, !phase) :: !l;
           i := !i + freq/10;
           right ();
-          lprintf out_h "if (telemetry->cbs[TELEMETRY_MSG_%s_ID] != NULL)\n" message_name;
+          lprintf out_h "for (j = 0; j < TELEMETRY_NB_CBS; j++) {\n";
           right ();
-          lprintf out_h "telemetry->cbs[TELEMETRY_MSG_%s_ID](trans, dev);\n" message_name;
+          lprintf out_h "if (telemetry->cbs[TELEMETRY_MSG_%s_ID].slots[j] != NULL)\n" message_name;
+          right ();
+          lprintf out_h "telemetry->cbs[TELEMETRY_MSG_%s_ID].slots[j](trans, dev);\n" 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 "else periodic_telemetry_err_report(TELEMETRY_PROCESS_%s, telemetry_mode_%s, TELEMETRY_MSG_%s_ID);\n" process_name process_name message_name;
+          lprintf out_h "if (j == 0) periodic_telemetry_err_report(TELEMETRY_PROCESS_%s, telemetry_mode_%s, TELEMETRY_MSG_%s_ID);\n" process_name process_name message_name;
           fprintf out_h "#endif\n";
           left ();
           lprintf out_h "}\n"
@@ -155,8 +162,15 @@ let print_message_table = fun out_h xml ->
   Hashtbl.iter (fun n _ -> fprintf out_h "  \"%s\", \\\n" n) messages;
   fprintf out_h "};\n\n";
   fprintf out_h "#define TELEMETRY_CBS_NULL { \\\n";
-  for i = 1 to (Hashtbl.length messages) do fprintf out_h "  NULL, \\\n" done;
-  fprintf out_h "};\n\n"
+  for i = 1 to (Hashtbl.length messages) do
+    fprintf out_h "  {{";
+    (* TODO: fix hardcoded value of 4 *)
+    for j = 1 to 4 do
+      fprintf out_h "NULL, "
+    done;
+    fprintf out_h "}}, \\\n"
+  done;
+  fprintf out_h "}\n\n"
 
 let print_process_send = fun out_h xml freq modules ->
   (** For each process *)