diff --git a/conf/firmwares/subsystems/fixedwing/fbw_datalink.makefile b/conf/firmwares/subsystems/fixedwing/fbw_datalink.makefile
index 4983c1fab2..ecb94029fe 100644
--- a/conf/firmwares/subsystems/fixedwing/fbw_datalink.makefile
+++ b/conf/firmwares/subsystems/fixedwing/fbw_datalink.makefile
@@ -1,5 +1,8 @@
 
+FBW_MODEM_PORT_LOWER=$(shell echo $(MODEM_PORT) | tr A-Z a-z)
+FBW_AP_PORT_LOWER=$(shell echo $(AUTOPILOT_PORT) | tr A-Z a-z)
+
 fbw.srcs += firmwares/fixedwing/fbw_datalink.c
 fbw.CFLAGS += -DFBW_DATALINK
-fbw.CFLAGS += -DMODEM_LINK=$(MODEM_PORT) -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
-fbw.CFLAGS += -DAUTOPILOT_LINK=$(AUTOPILOT_PORT) -DUSE_$(AUTOPILOT_PORT) -D$(AUTOPILOT_PORT)_BAUD=$(MODEM_BAUD)
+fbw.CFLAGS += -DMODEM_LINK=$(FBW_MODEM_PORT_LOWER) -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+fbw.CFLAGS += -DAUTOPILOT_LINK=$(FBW_AP_PORT_LOWER) -DUSE_$(AUTOPILOT_PORT) -D$(AUTOPILOT_PORT)_BAUD=$(MODEM_BAUD)
diff --git a/sw/airborne/firmwares/fixedwing/fbw_datalink.c b/sw/airborne/firmwares/fixedwing/fbw_datalink.c
index d09a3fa017..d1ab3c9734 100644
--- a/sw/airborne/firmwares/fixedwing/fbw_datalink.c
+++ b/sw/airborne/firmwares/fixedwing/fbw_datalink.c
@@ -31,38 +31,19 @@
 #include "led.h"
 
 
-#define __ModemLink(dev, _x) dev##_x
-#define _ModemLink(dev, _x)  __ModemLink(dev, _x)
-#define ModemLink(_x) _ModemLink(MODEM_LINK, _x)
-#define ModemBuffer() ModemLink(ChAvailable())
-
-
-#define __AutopilotLink(dev, _x) dev##_x
-#define _AutopilotLink(dev, _x)  __AutopilotLink(dev, _x)
-#define AutopilotLink(_x) _AutopilotLink(AUTOPILOT_LINK, _x)
-
-#define AutopilotBuffer() AutopilotLink(ChAvailable())
+#define ModemLinkDevice (&(MODEM_LINK).device)
+#define AutopilotLinkDevice (&(AUTOPILOT_LINK).device)
 
 static inline void autopilot_parse(char c)
 {
-  ModemLink(Transmit(c));
+  ModemLinkDevice->transmit(ModemLinkDevice->periph, c);
 }
 
 static inline void modem_parse(char c)
 {
-  AutopilotLink(Transmit(c));
+  AutopilotLinkDevice->transmit(AutopilotLinkDevice->periph, c);
 }
 
-#define ReadAutopilotBuffer() {                 \
-    while (AutopilotLink(ChAvailable()))        \
-      autopilot_parse(AutopilotLink(Getch()));  \
-  }
-
-#define ReadModemBuffer() {                     \
-    while (ModemLink(ChAvailable()))            \
-      modem_parse(ModemLink(Getch()));          \
-  }
-
 void fbw_datalink_periodic(void)
 {
 #ifdef MODEM_LINK_LED
@@ -76,16 +57,19 @@ void fbw_datalink_periodic(void)
 void fbw_datalink_event(void)
 {
 #ifdef MODEM_LINK_LED
-  if (ModemLink(ChAvailable())) {
+  if (ModemLinkDevice->char_available(ModemLinkDevice->periph)) {
     LED_ON(MODEM_LINK_LED);
   }
 #endif
 #ifdef AUTOPILOT_LINK_LED
-  if (AutopilotLink(ChAvailable())) {
+  if (AutopilotLinkDevice->char_available(AutopilotLinkDevice->periph)) {
     LED_ON(AUTOPILOT_LINK_LED);
   }
 #endif
 
-  ReadModemBuffer();
-  ReadAutopilotBuffer();
+  while (ModemLinkDevice->char_available(ModemLinkDevice->periph))
+    modem_parse(ModemLinkDevice->getchar(ModemLinkDevice->periph));
+
+  while (AutopilotLinkDevice->char_available(AutopilotLinkDevice->periph))
+    autopilot_parse(AutopilotLinkDevice->getchar(AutopilotLinkDevice->periph));
 }