diff --git a/sw/airborne/arch/chibios/mcu_periph/uart_arch.c b/sw/airborne/arch/chibios/mcu_periph/uart_arch.c
index 8e5ff608b35..abeb0e3117f 100644
--- a/sw/airborne/arch/chibios/mcu_periph/uart_arch.c
+++ b/sw/airborne/arch/chibios/mcu_periph/uart_arch.c
@@ -46,6 +46,7 @@ static const SerialConfig usart1_config = {
 };
 void uart1_init(void)
 {
+  uart_periph_init(&uart1);
   sdStart(&SD1, &usart1_config);
   uart1.reg_addr = &SD1;
 }
@@ -64,6 +65,7 @@ static const SerialConfig usart2_config = {
 };
 void uart2_init(void)
 {
+  uart_periph_init(&uart2);
   sdStart(&SD2, &usart2_config);
   uart2.reg_addr = &SD2;
 }
@@ -81,6 +83,7 @@ static const SerialConfig usart3_config = {
 };
 void uart3_init(void)
 {
+  uart_periph_init(&uart3);
   sdStart(&SD3, &usart3_config);
   uart3.reg_addr = &SD3;
 }
@@ -98,6 +101,7 @@ static const SerialConfig usart4_config = {
 };
 void uart4_init(void)
 {
+  uart_periph_init(&uart4);
   sdStart(&SD4, &usart4_config);
   uart4.reg_addr = &SD4;
 }
@@ -115,6 +119,7 @@ static const SerialConfig usart5_config = {
 };
 void uart5_init(void)
 {
+  uart_periph_init(&uart5);
   sdStart(&SD5, &usart5_config);
   uart5.reg_addr = &SD5;
 }
diff --git a/sw/airborne/test/chibios_test_telemetry.c b/sw/airborne/test/chibios_test_telemetry.c
index bc42fbcb8f2..4bb40a2aa51 100644
--- a/sw/airborne/test/chibios_test_telemetry.c
+++ b/sw/airborne/test/chibios_test_telemetry.c
@@ -57,6 +57,18 @@ static msg_t ThdBlinker(void *arg) {
   return 0;
 }
 
+static WORKING_AREA(waThdTx, 1024);
+static msg_t ThdTx(void *arg) {
+
+  (void)arg;
+  chRegSetThreadName("sender");
+  while (TRUE) {
+    DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
+    chThdSleepMilliseconds(100);
+  }
+  return 0;
+}
+
 
 int main(void)
 {
@@ -68,14 +80,10 @@ int main(void)
    * Creates the blinker thread.
    */
   chThdCreateStatic(waThdBlinker, sizeof(waThdBlinker), NORMALPRIO, ThdBlinker, NULL);
+  chThdCreateStatic(waThdTx, sizeof(waThdTx), NORMALPRIO, ThdTx, NULL);
 
   while (TRUE) {
-    DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
-
-#ifdef LED_GREEN
-    LED_TOGGLE(LED_GREEN);
-#endif
-    chThdSleep(MS2ST(100));
+    chThdSleep(S2ST(1));
   }
 
   return 0;