diff --git a/conf/modules/telemetry_intermcu.xml b/conf/modules/telemetry_intermcu.xml
index 571c004264..79c9ca565b 100644
--- a/conf/modules/telemetry_intermcu.xml
+++ b/conf/modules/telemetry_intermcu.xml
@@ -14,9 +14,11 @@
   </header>
   <init fun="telemetry_intermcu_init()"/>
   <periodic fun="telemetry_intermcu_periodic()"/>
+  <event fun="telemetry_intermcu_event()"/>
   <makefile target="ap">
     <file name="telemetry_intermcu_ap.c"/>
     <file name="short_transport.c" dir="pprzlink/src"/>
+    <define name="TELEMETRY_INTERMCU"/>
   </makefile>
   <makefile target="fbw">
     <file name="telemetry_intermcu_fbw.c"/>
diff --git a/sw/airborne/firmwares/rotorcraft/main_fbw.c b/sw/airborne/firmwares/rotorcraft/main_fbw.c
index ab77d541c7..d538f9d7d7 100644
--- a/sw/airborne/firmwares/rotorcraft/main_fbw.c
+++ b/sw/airborne/firmwares/rotorcraft/main_fbw.c
@@ -56,7 +56,6 @@
 
 
 /** Fly by wire modes */
-typedef enum {FBW_MODE_MANUAL = 0, FBW_MODE_AUTO = 1, FBW_MODE_FAILSAFE = 2} fbw_mode_enum;
 fbw_mode_enum fbw_mode;
 bool fbw_motors_on = false;
 
diff --git a/sw/airborne/firmwares/rotorcraft/main_fbw.h b/sw/airborne/firmwares/rotorcraft/main_fbw.h
index 9875787115..82bef33c71 100644
--- a/sw/airborne/firmwares/rotorcraft/main_fbw.h
+++ b/sw/airborne/firmwares/rotorcraft/main_fbw.h
@@ -64,6 +64,9 @@
 #define RADIO_FBW_MODE RADIO_MODE
 #endif
 
+typedef enum {FBW_MODE_MANUAL = 0, FBW_MODE_AUTO = 1, FBW_MODE_FAILSAFE = 2} fbw_mode_enum;
+
+
 STATIC_INLINE void main_init(void);
 STATIC_INLINE void main_event(void);
 STATIC_INLINE void handle_periodic_tasks(void);
diff --git a/sw/airborne/modules/telemetry/telemetry_intermcu_ap.c b/sw/airborne/modules/telemetry/telemetry_intermcu_ap.c
index 0e3a9106d8..1ce1de1889 100644
--- a/sw/airborne/modules/telemetry/telemetry_intermcu_ap.c
+++ b/sw/airborne/modules/telemetry/telemetry_intermcu_ap.c
@@ -26,11 +26,13 @@
 
 #define PERIODIC_C_INTERMCU
 #include "telemetry_intermcu.h"
+#include "telemetry_intermcu_ap.h"
 #include "subsystems/intermcu.h"
 #include "pprzlink/intermcu_msg.h"
 #include "pprzlink/short_transport.h"
 #include "generated/periodic_telemetry.h"
 #include "subsystems/datalink/telemetry.h"
+#include "subsystems/datalink/datalink.h"
 
 /* Default maximum telemetry message size */
 #ifndef TELEMERTY_INTERMCU_MSG_SIZE
@@ -74,11 +76,19 @@ void telemetry_intermcu_periodic(void)
   periodic_telemetry_send_InterMCU(DefaultPeriodic, &telemetry_intermcu.trans.trans_tx, &telemetry_intermcu.dev);
 }
 
-void telemetry_intermcu_on_msg(uint8_t msg_id __attribute__((unused)), uint8_t* msg __attribute__((unused)), uint8_t size __attribute__((unused)))
+/* InterMCU event handling of telemetry */
+void telemetry_intermcu_event(void)
 {
 
 }
 
+void telemetry_intermcu_on_msg(uint8_t msg_id __attribute__((unused)), uint8_t* msg, uint8_t size __attribute__((unused)))
+{
+  datalink_time = 0;
+  datalink_nb_msgs++;
+  dl_parse_msg(&telemetry_intermcu.dev, &telemetry_intermcu.trans.trans_tx, msg);
+}
+
 static bool telemetry_intermcu_check_free_space(struct telemetry_intermcu_t *p, long *fd __attribute__((unused)), uint16_t len)
 {
   return ((p->buf_idx + len) < (TELEMERTY_INTERMCU_MSG_SIZE - 1));
diff --git a/sw/airborne/modules/telemetry/telemetry_intermcu_ap.h b/sw/airborne/modules/telemetry/telemetry_intermcu_ap.h
new file mode 100644
index 0000000000..3680ed6a86
--- /dev/null
+++ b/sw/airborne/modules/telemetry/telemetry_intermcu_ap.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright (C) 2016 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file modules/telemetry/telemetry_intermcu_ap.h
+ *  @brief Telemetry through InterMCU
+ */
+
+#ifndef TELEMETRY_INTERMCU_AP_H
+#define TELEMETRY_INTERMCU_AP_H
+
+#include "std.h"
+#include "pprzlink/short_transport.h"
+
+/* Default maximum telemetry message size */
+#ifndef TELEMERTY_INTERMCU_MSG_SIZE
+#define TELEMERTY_INTERMCU_MSG_SIZE 128
+#endif
+
+/* Structure for handling telemetry over InterMCU */
+struct telemetry_intermcu_t {
+  struct link_device dev;                     ///< Device structure for communication
+  struct short_transport trans;               ///< Transport without any extra encoding
+  uint8_t buf[TELEMERTY_INTERMCU_MSG_SIZE];   ///< Buffer for the messages
+  uint8_t buf_idx;                            ///< Index of the buffer
+};
+
+/* Telemetry InterMCU throughput */
+extern struct telemetry_intermcu_t telemetry_intermcu;
+
+#endif /* TELEMETRY_INTERMCU_AP_H */
diff --git a/sw/airborne/modules/telemetry/telemetry_intermcu_fbw.c b/sw/airborne/modules/telemetry/telemetry_intermcu_fbw.c
index 181d644cad..78a86ee29b 100644
--- a/sw/airborne/modules/telemetry/telemetry_intermcu_fbw.c
+++ b/sw/airborne/modules/telemetry/telemetry_intermcu_fbw.c
@@ -29,11 +29,19 @@
 #include "pprzlink/pprz_transport.h"
 #include "pprzlink/intermcu_msg.h"
 #include "subsystems/datalink/telemetry.h"
+#include "firmwares/rotorcraft/main_fbw.h"
+
+#define MSG_SIZE 128
+
+extern fbw_mode_enum fbw_mode;
 
 /* Structure for handling telemetry over InterMCU */
 struct telemetry_intermcu_t {
   struct link_device *dev;      ///< Device structure for communication
   struct pprz_transport trans;  ///< Transport without any extra encoding
+
+  uint8_t rx_buffer[MSG_SIZE];  ///< Received bytes from datalink
+  bool msg_received;            ///< Whenever a datalink message is received
 };
 
 /* Telemetry InterMCU throughput */
@@ -58,6 +66,32 @@ void telemetry_intermcu_periodic(void)
 
 }
 
+/* InterMCU event handling of telemetry */
+void telemetry_intermcu_event(void)
+{
+  pprz_check_and_parse(&(TELEMETRY_INTERMCU_DEV).device, &telemetry_intermcu.trans, telemetry_intermcu.rx_buffer, &telemetry_intermcu.msg_received);
+  if(telemetry_intermcu.msg_received) {
+    /* Switch on MSG ID */
+    switch(telemetry_intermcu.rx_buffer[1]) {
+      case DL_EMERGENCY_CMD:
+        if(DL_EMERGENCY_CMD_ac_id(telemetry_intermcu.rx_buffer) == AC_ID
+            && DL_EMERGENCY_CMD_cmd(telemetry_intermcu.rx_buffer) == 0) {
+          fbw_mode = FBW_MODE_FAILSAFE;
+        }
+        break;
+
+      default:
+        break;
+    }
+
+    /* Forward to AP */
+    pprz_msg_send_IMCU_DATALINK(&(intermcu.transport.trans_tx), intermcu.device,
+                            INTERMCU_FBW, telemetry_intermcu.trans.trans_rx.payload_len, telemetry_intermcu.rx_buffer);
+
+    telemetry_intermcu.msg_received = false;
+  }
+}
+
 void telemetry_intermcu_on_msg(uint8_t msg_id, uint8_t* msg, uint8_t size)
 {
   telemetry_intermcu_repack(&(telemetry_intermcu.trans.trans_tx), telemetry_intermcu.dev, AC_ID, msg_id, msg, size);
diff --git a/sw/airborne/subsystems/intermcu/intermcu_ap.c b/sw/airborne/subsystems/intermcu/intermcu_ap.c
index 50371bda06..612d3ef538 100644
--- a/sw/airborne/subsystems/intermcu/intermcu_ap.c
+++ b/sw/airborne/subsystems/intermcu/intermcu_ap.c
@@ -144,6 +144,16 @@ static inline void intermcu_parse_msg(void (*rc_frame_handler)(void))
       break;
     }
 
+#if TELEMETRY_INTERMCU
+    case DL_IMCU_DATALINK: {
+      uint8_t size = DL_IMCU_DATALINK_msg_length(imcu_msg_buf);
+      uint8_t *msg = DL_IMCU_DATALINK_msg(imcu_msg_buf);
+      telemetry_intermcu_on_msg(0, msg, size);
+      break;
+    }
+
+#endif
+
     default:
       break;
   }