diff --git a/sw/airborne/subsystems/gps/gps_mtk.c b/sw/airborne/subsystems/gps/gps_mtk.c
index 826e1ce127..7287b745b6 100644
--- a/sw/airborne/subsystems/gps/gps_mtk.c
+++ b/sw/airborne/subsystems/gps/gps_mtk.c
@@ -393,9 +393,12 @@ restart:
  */
 #ifdef GPS_CONFIGURE
 
+#include "mcu_periph/link_device.h"
+
 static void MtkSend_CFG(char *dat)
 {
-  while (*dat != 0) { GpsLink(Transmit(*dat++)); }
+  struct link_device *dev = &((GPS_LINK).device);
+  while (*dat != 0) { dev->transmit(dev->periph, *dat++); }
 }
 
 void gps_configure_uart(void)
diff --git a/sw/airborne/subsystems/gps/gps_mtk.h b/sw/airborne/subsystems/gps/gps_mtk.h
index fe6c400cab..d2221e65f6 100644
--- a/sw/airborne/subsystems/gps/gps_mtk.h
+++ b/sw/airborne/subsystems/gps/gps_mtk.h
@@ -58,7 +58,6 @@ extern struct GpsMtk gps_mtk;
 #define _GpsLink(dev, _x)  __GpsLink(dev, _x)
 #define GpsLink(_x) _GpsLink(GPS_LINK, _x)
 
-#define GpsBuffer() GpsLink(ChAvailable())
 
 #ifdef GPS_CONFIGURE
 extern bool_t gps_configuring;
@@ -70,44 +69,43 @@ extern bool_t gps_configuring;
 #define GpsConfigure() {}
 #endif
 
-#define GpsEvent(_sol_available_callback) {             \
-    if (GpsBuffer()) {                                  \
-      ReadGpsBuffer();                                  \
-      GpsConfigure();                                   \
-    }                                                   \
-    if (gps_mtk.msg_available) {                        \
-      gps.last_msg_ticks = sys_time.nb_sec_rem;         \
-      gps.last_msg_time = sys_time.nb_sec;              \
-      gps_mtk_read_message();                           \
-      if (gps_mtk.msg_class == MTK_DIY14_ID &&          \
-          gps_mtk.msg_id == MTK_DIY14_NAV_ID) {         \
-        if (gps.fix == GPS_FIX_3D) {                    \
-          gps.last_3dfix_ticks = sys_time.nb_sec_rem;   \
-          gps.last_3dfix_time = sys_time.nb_sec;        \
-        }                                               \
-        _sol_available_callback();                      \
-      }                                                 \
-      if (gps_mtk.msg_class == MTK_DIY16_ID &&          \
-          gps_mtk.msg_id == MTK_DIY16_NAV_ID) {         \
-        if (gps.fix == GPS_FIX_3D) {                    \
-          gps.last_3dfix_ticks = sys_time.nb_sec_rem;   \
-          gps.last_3dfix_time = sys_time.nb_sec;        \
-        }                                               \
-        _sol_available_callback();                      \
-      }                                                 \
-      gps_mtk.msg_available = FALSE;                    \
-    }                                                   \
-  }
-
-#define ReadGpsBuffer() {         \
-    while (GpsLink(ChAvailable())&&!gps_mtk.msg_available)  \
-      gps_mtk_parse(GpsLink(Getch()));      \
-  }
-
-
 extern void gps_mtk_read_message(void);
 extern void gps_mtk_parse(uint8_t c);
 
+static inline void GpsEvent(void (* _sol_available_callback)(void))
+{
+  struct link_device *dev = &((GPS_LINK).device);
+
+  if (dev->char_available(dev->periph)) {
+    while (dev->char_available(dev->periph) && !gps_ubx.msg_available) {
+      gps_mtk_parse(dev->getchar(dev->periph));
+    }
+    GpsConfigure();
+  }
+  if (gps_mtk.msg_available) {
+    gps.last_msg_ticks = sys_time.nb_sec_rem;
+    gps.last_msg_time = sys_time.nb_sec;
+    gps_mtk_read_message();
+    if (gps_mtk.msg_class == MTK_DIY14_ID &&
+        gps_mtk.msg_id == MTK_DIY14_NAV_ID) {
+      if (gps.fix == GPS_FIX_3D) {
+        gps.last_3dfix_ticks = sys_time.nb_sec_rem;
+        gps.last_3dfix_time = sys_time.nb_sec;
+      }
+      _sol_available_callback();
+    }
+    if (gps_mtk.msg_class == MTK_DIY16_ID &&
+        gps_mtk.msg_id == MTK_DIY16_NAV_ID) {
+      if (gps.fix == GPS_FIX_3D) {
+        gps.last_3dfix_ticks = sys_time.nb_sec_rem;
+        gps.last_3dfix_time = sys_time.nb_sec;
+      }
+      _sol_available_callback();
+    }
+    gps_mtk.msg_available = FALSE;
+  }
+}
+
 /*
  * dynamic GPS configuration
  */
diff --git a/sw/tools/generators/gen_mtk.ml b/sw/tools/generators/gen_mtk.ml
index fd0885fa72..835d9292ff 100644
--- a/sw/tools/generators/gen_mtk.ml
+++ b/sw/tools/generators/gen_mtk.ml
@@ -105,33 +105,7 @@ let parse_message = fun class_name m ->
       if l <> !offset then raise (Length_error (m, l, !offset))
     with
         Xml.No_attribute("length") -> () (** Undefined length authorized *)
-  end;
-
-  (** Generating send function *)
-  let param_name = fun f -> String.lowercase (field_name f) in
-  let rec param_names = fun f r ->
-    if Xml.tag f = "field" then
-      param_name f :: r
-    else
-      List.fold_right param_names (Xml.children f) r in
-  let param_type = fun f -> c_type (format f) in
-  fprintf out "\n#define MtkSend_%s_%s(" class_name msg_name;
-  fprintf out "%s" (String.concat "," (param_names m []));
-  fprintf out ") { \\\n";
-  fprintf out "  MtkHeader(MTK_%s_ID, %s, %d);\\\n" class_name msg_id !offset;
-  let rec send_one_field = fun f ->
-    match Xml.tag f with
-        "field" ->
-          let s = sizeof (format f) in
-          let p = param_name f in
-          let t = param_type f in
-          fprintf out "  %s _%s = %s; MtkSend%dByAddr((uint8_t*)&_%s);\\\n" t p p s p
-      | "block" ->
-        List.iter send_one_field (Xml.children f)
-      | _ -> assert (false) in
-  List.iter send_one_field (Xml.children m);
-  fprintf out "  MtkTrailer();\\\n";
-  fprintf out "}\n\n#define MTK_%s_%s_LENGTH %d\n" class_name msg_name !offset
+  end
 
 
 let parse_class = fun c ->