diff --git a/conf/airframes/TUDelft/airframes/test.xml b/conf/airframes/TUDelft/airframes/test.xml
index 75b3329df7..6723728b54 100644
--- a/conf/airframes/TUDelft/airframes/test.xml
+++ b/conf/airframes/TUDelft/airframes/test.xml
@@ -18,14 +18,17 @@
       <subsystem name="fdm" type="jsbsim"/>
     </target>
 
-    <!--subsystem name="telemetry"     type="xbee_api"/-->
+    <subsystem name="telemetry"     type="transparent"/>
     <subsystem name="imu"           type="lisa_m_v2.1"/>
-    <subsystem name="gps"           type="ublox"/>
+    <subsystem name="gps"           type="ublox">
+      <configure name="GPS_PORT" value="UART4"/>
+    </subsystem>
     <subsystem name="stabilization" type="int_quat"/>
     <subsystem name="ahrs"          type="int_cmpl_quat"/>
     <subsystem name="ins"           type="hff"/>
     <subsystem name="guidance"      type="indi"/>
     <subsystem name="intermcu"      type="uart"/>
+    <define name="REMAP_UART3" value="TRUE"/>
   </firmware>
 
   <firmware name="rotorcraft">
@@ -38,11 +41,11 @@
     <subsystem name="radio_control" type="spektrum">
       <define name="RADIO_CONTROL_NB_CHANNEL" value="7"/>
       <define name="RADIO_CONTROL_SPEKTRUM_NO_SIGN" value="1"/>
-      <define name="RADIO_KILL_SWITCH" value="RADIO_AUX2"/>
     </subsystem>
 
     <subsystem name="actuators"     type="pwm"/>
     <subsystem name="intermcu"      type="uart"/>
+    <define name="REMAP_UART3" value="TRUE"/>
   </firmware>
 
   <modules>
@@ -96,6 +99,7 @@
     <define name="NAV_DESCEND_VSPEED" value="-1.0"/>
     <define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
     <define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING" value="TRUE"/>
+    <define name="RADIO_KILL_SWITCH" value="RADIO_AUX2"/>
     <define name="VoltageOfAdc(adc)" value="(0.014355*adc)"/>
   </section>
 
diff --git a/conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile b/conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile
index 298b661fa3..494411cd62 100644
--- a/conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile
@@ -3,14 +3,14 @@
 # InterMCU type UART
 
 ifeq ($(TARGET),fbw)
-  INTERMCU_PORT ?= UART2
+  INTERMCU_PORT ?= UART3
   INTERMCU_PORT_LOWER = $(shell echo $(INTERMCU_PORT) | tr A-Z a-z)
   fbw.CFLAGS += -DINTERMCU_LINK=$(INTERMCU_PORT_LOWER) -DUSE_$(INTERMCU_PORT) -D$(INTERMCU_PORT)_BAUD=B230400
   fbw.CFLAGS += -DINTER_MCU_FBW
   fbw.srcs += subsystems/datalink/pprz_transport.c
   fbw.srcs += subsystems/intermcu/intermcu_fbw.c
 else
-  INTERMCU_PORT ?= UART2
+  INTERMCU_PORT ?= UART3
   INTERMCU_PORT_LOWER = $(shell echo $(INTERMCU_PORT) | tr A-Z a-z)
   ap.CFLAGS += -DINTERMCU_LINK=$(INTERMCU_PORT_LOWER) -DUSE_$(INTERMCU_PORT) -D$(INTERMCU_PORT)_BAUD=B230400
   ap.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/intermcu/intermcu_ap.h\" -DRADIO_CONTROL
diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c
index 1919b31fea..861a942323 100644
--- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c
@@ -386,7 +386,7 @@ void usart3_isr(void) { usart_isr(&uart3); }
 #endif /* USE_UART3 */
 
 
-#if USE_UART4 && defined STM32F4
+#if USE_UART4
 
 /* by default enable UART Tx and Rx */
 #ifndef USE_UART4_TX
diff --git a/sw/airborne/boards/lisa_m_common.h b/sw/airborne/boards/lisa_m_common.h
index 12f9cca3f6..47b8c72699 100644
--- a/sw/airborne/boards/lisa_m_common.h
+++ b/sw/airborne/boards/lisa_m_common.h
@@ -39,11 +39,26 @@
 #define UART2_GPIO_PORT_TX GPIO_BANK_USART2_TX
 #define UART2_GPIO_TX GPIO_USART2_TX
 
+#if REMAP_UART3 // For UART4 we need to remap UART 3
+#define UART3_GPIO_AF 0
+#define UART3_GPIO_PORT_RX GPIO_BANK_USART3_RX
+#define UART3_GPIO_RX GPIO_USART3_RX
+#define UART3_GPIO_PORT_TX GPIO_BANK_USART3_TX
+#define UART3_GPIO_TX GPIO_USART3_TX
+
+#define UART4_GPIO_AF 0
+#define UART4_GPIO_PORT_RX GPIOC
+#define UART4_GPIO_RX GPIO10
+#define UART4_GPIO_PORT_TX GPIOC
+#define UART4_GPIO_TX GPIO11
+
+#else
 #define UART3_GPIO_AF AFIO_MAPR_USART3_REMAP_PARTIAL_REMAP
 #define UART3_GPIO_PORT_RX GPIO_BANK_USART3_PR_RX
 #define UART3_GPIO_RX GPIO_USART3_PR_RX
 #define UART3_GPIO_PORT_TX GPIO_BANK_USART3_PR_TX
 #define UART3_GPIO_TX GPIO_USART3_PR_TX
+#endif
 
 #define UART5_GPIO_AF 0
 #define UART5_GPIO_PORT_RX GPIO_BANK_UART5_RX
diff --git a/sw/airborne/subsystems/intermcu/intermcu_ap.c b/sw/airborne/subsystems/intermcu/intermcu_ap.c
index 2dd12a1cbb..6d9bba33d6 100644
--- a/sw/airborne/subsystems/intermcu/intermcu_ap.c
+++ b/sw/airborne/subsystems/intermcu/intermcu_ap.c
@@ -54,6 +54,7 @@ void intermcu_periodic(void)
 
 void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode __attribute__((unused)))
 {
+  command_values[1] = 7000;
   pprz_msg_send_IMCU_COMMANDS(&(intermcu_transport.trans_tx), intermcu_device,
     INTERMCU_AP, 0, COMMANDS_NB, command_values); //TODO: Fix status
 }
diff --git a/sw/airborne/subsystems/intermcu/intermcu_ap.h b/sw/airborne/subsystems/intermcu/intermcu_ap.h
index 8644e083f6..f10837560c 100644
--- a/sw/airborne/subsystems/intermcu/intermcu_ap.h
+++ b/sw/airborne/subsystems/intermcu/intermcu_ap.h
@@ -24,6 +24,7 @@
 #define INTERMCU_AP_ROTORCRAFT_H
 
 #include "subsystems/intermcu.h"
+#include "generated/airframe.h"
 
 void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode);
 void RadioControlEvent(void (*frame_handler)(void));
diff --git a/sw/airborne/subsystems/intermcu/intermcu_fbw.c b/sw/airborne/subsystems/intermcu/intermcu_fbw.c
index bc60eb478f..fc2320940f 100644
--- a/sw/airborne/subsystems/intermcu/intermcu_fbw.c
+++ b/sw/airborne/subsystems/intermcu/intermcu_fbw.c
@@ -81,6 +81,8 @@ static inline void intermcu_parse_msg(struct transport_rx * trans, void (*comman
         for(i = 0; i < size; i++)
           intermcu_commands[i] = new_commands[i];
 
+        inter_mcu.status = INTERMCU_OK;
+        inter_mcu.time_since_last_frame = 0;
         commands_frame_handler();
       break;
     }