diff --git a/sw/airborne/fly_by_wire/telemetry.c b/sw/airborne/fly_by_wire/telemetry.c
index 7b66c741e0..2b45e27fab 100644
--- a/sw/airborne/fly_by_wire/telemetry.c
+++ b/sw/airborne/fly_by_wire/telemetry.c
@@ -1,2 +1,14 @@
 #include "telemetry.h"
+#include <avr/io.h>
 uint8_t ck_a, ck_b;
+
+#define DL_FBW_STATUS 0x04
+
+void telemetry_send_fbw_status(uint8_t* nb_spi_err, uint8_t* rc_status, uint8_t* mode) {  if (TELEMETRY_CHECK_FREE_SPACE(7)) {
+    TELEMETRY_START_MESSAGE(DL_FBW_STATUS);			    
+    TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR((uint8_t*)(nb_spi_err));
+    TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR((uint8_t*)(rc_status));
+    TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR((uint8_t*)(mode));    
+    TELEMETRY_END_MESSAGE();
+  }
+}
diff --git a/sw/airborne/fly_by_wire/telemetry.h b/sw/airborne/fly_by_wire/telemetry.h
index c41248a39d..fc8fa3633d 100644
--- a/sw/airborne/fly_by_wire/telemetry.h
+++ b/sw/airborne/fly_by_wire/telemetry.h
@@ -33,13 +33,15 @@
 extern uint8_t ck_a, ck_b;
 #include "uart.h"
 
+void telemetry_send_fbw_status(uint8_t* nb_spi_err, uint8_t* rc_status, uint8_t* mode);
+
 #define TELEMETRY_START_MESSAGE(id) {					\
     UART_PUT_1_BYTE(STX); UART_PUT_1_BYTE(id); ck_a = id; ck_b = id;	\
-}
+  }
 
 #define TELEMETRY_END_MESSAGE() {					\
-    UART_PUT_1_BYTE(ck_a); UART_PUT_1_BYTE(ck_b); UART_CHECK_RUNNING(); \
-}
+    UART_PUT_1_BYTE(ck_a); UART_PUT_1_BYTE(ck_b);			\
+  }
 
 #define TELEMETRY_CHECK_FREE_SPACE(_space) UART_CHECK_FREE_SPACE(_space)
 
@@ -47,18 +49,18 @@ extern uint8_t ck_a, ck_b;
     UART_PUT_1_BYTE_BY_ADDR(_byte);		   \
     ck_a += *(_byte);				   \
     ck_b += ck_a;				   \
-}
+  }
 
 #define TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(_byte) { \
     TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(_byte);	   \
     TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(_byte+1);	   \
-}
+  }
 
 #define TELEMETRY_PUT_4_DATA_BYTE_BY_ADDR(_byte) { \
-  TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(_byte); \
-  TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(_byte+1); \
-  TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(_byte+2); \
-  TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(_byte+3); \
-}
+    TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(_byte);	   \
+    TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(_byte+1);	   \
+    TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(_byte+2);	   \
+    TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(_byte+3);	   \
+  }
 
 #endif /* TELEMETRY_H */
diff --git a/sw/airborne/fly_by_wire/test/new_rc_transmitter.c b/sw/airborne/fly_by_wire/test/new_rc_transmitter.c
index 93d99a1c12..3c3abe5f8b 100644
--- a/sw/airborne/fly_by_wire/test/new_rc_transmitter.c
+++ b/sw/airborne/fly_by_wire/test/new_rc_transmitter.c
@@ -6,7 +6,7 @@
 #include "uart.h"
 #include "ppm.h"
 
-#include "fbw_messages.h"
+#include "telemetry.h"
 
 uint8_t nb_spi_err = 0;
 uint8_t rc_status = 0;
@@ -14,7 +14,6 @@ uint8_t mode = 0;
 
 int main( void ) {
   uart_init_tx();
-  uart_print_string("Calib_radio Booting $Id$\n");
   timer_init();
   ppm_init();
   sei(); 
@@ -27,7 +26,9 @@ int main( void ) {
       n++;
       if (n == 60) {
 	n = 0;
-	TELEMETRY_SEND_FBW_STATUS(&nb_spi_err, &rc_status, &mode);
+	//	uart_transmit('A');
+	//	uart_transmit('\n');
+	telemetry_send_fbw_status(&nb_spi_err, &rc_status, &mode);
       }
     }
   }
diff --git a/sw/airborne/fly_by_wire/uart.c b/sw/airborne/fly_by_wire/uart.c
index 12d311a503..77b9fa58d5 100644
--- a/sw/airborne/fly_by_wire/uart.c
+++ b/sw/airborne/fly_by_wire/uart.c
@@ -30,10 +30,10 @@
 #include "std.h"
 #include "uart.h"
 
-uint8_t           tx_head; /* next free in buf */
-volatile uint8_t  tx_tail; /* next char to send */
+uint8_t           tx_head = 0; /* next free in buf */
+volatile uint8_t  tx_tail = 0; /* next char to send */
 uint8_t           tx_buf[ TX_BUF_SIZE ];
-
+uint8_t           uart_nb_overrun = 0;
 /*
  * UART Baud rate generation settings:
  *
@@ -100,10 +100,10 @@ void uart_print_string(const uint8_t* s) {
 
 SIGNAL(SIG_UART_TRANS) {
   if (tx_head == tx_tail) {
-    /* Nothing more to send */
+    /* buffer ring is empty */
     cbi(UCSRB, TXCIE); /* disable interrupt */
   } else {
     UDR = tx_buf[tx_tail];
-    tx_tail++; /* warning tx_buf_len is 256 */
+    tx_tail++; /* WARNING : only works with TX_BUF_LEN = 256 */
   }
 }
diff --git a/sw/airborne/fly_by_wire/uart.h b/sw/airborne/fly_by_wire/uart.h
index 711985b7a6..743445c272 100644
--- a/sw/airborne/fly_by_wire/uart.h
+++ b/sw/airborne/fly_by_wire/uart.h
@@ -36,17 +36,28 @@ void uart_print_hex16 ( uint16_t c );
 void uart_print_string(const uint8_t* s);
 void uart_print_float( const float * f);
 
-#define TX_BUF_SIZE     256
+#define TX_BUF_SIZE      256
 extern uint8_t           tx_head;
 extern volatile uint8_t  tx_tail;
 extern uint8_t           tx_buf[ TX_BUF_SIZE ];
+extern uint8_t           uart_nb_overrun;
 
-#define UART_CHECK_FREE_SPACE(_space) (tx_head>=tx_tail? _space < (TX_BUF_SIZE - (tx_head - tx_tail)) : _space < (tx_tail - tx_head))
+#define UART_CHECK_FREE_SPACE(_space) (tx_head>=tx_tail ? _space < (TX_BUF_SIZE - (tx_head - tx_tail)) : _space < (tx_tail - tx_head))
 
-#define UART_PUT_1_BYTE(_byte) { \
-  tx_buf[tx_head] = _byte; \
-  tx_head++; \
-  if (tx_head >= TX_BUF_SIZE) tx_head = 0; \
+#define UART_PUT_1_BYTE(_byte) {					\
+    if (UCSRB & _BV(TXCIE)) {						\
+      /* something is already beeing transmitted so we're buffering */	\
+      if (tx_tail == tx_head + 1) { /* BUF_SIZE = 256 */		\
+	/* Buffer is full (well almost... but tx_head = tx_tail means "empty" */ \
+	uart_nb_overrun++;						\
+      }	else {								\
+	tx_buf[tx_head] = _byte;					\
+	tx_head++; /* WARNING : only works with BUF_SIZE = 256 */	\
+      }									\
+    } else {     /* uart is available: just send */			\
+      UDR = _byte;							\
+      sbi(UCSRB, TXCIE);						\
+    }									\
 }
 
 #define UART_PUT_1_BYTE_BY_ADDR(_byte) { \
diff --git a/sw/ground_segment/tmtc/listen.ml b/sw/ground_segment/tmtc/listen.ml
index d7cdd4a7f6..61d9c85b0c 100644
--- a/sw/ground_segment/tmtc/listen.ml
+++ b/sw/ground_segment/tmtc/listen.ml
@@ -28,17 +28,22 @@ open Printf
 
 
 let _ =
-  if Array.length Sys.argv <> 3 then
-    failwith (sprintf "Usage: %s <message class> <port>" Sys.argv.(0));
+  let bus = ref "127.255.255.255:2010" in
+  let class_name = ref "telemetry_fbw" in
+  let serial_dev = ref "/dev/ttyUSB0" in
+  Arg.parse
+    [ "-b", Arg.String (fun x -> bus := x), "Bus\tDefault is 127.255.255.25:2010";
+      "-c",  Arg.String (fun x -> class_name := x), "class name";
+      "-d",  Arg.String (fun x -> serial_dev := x), "serial device"]
+    (fun x -> prerr_endline ("WARNING: don't do anything with "))
+    "Usage: ";
 
-  let class_name = Sys.argv.(1) in
-
-  let module Tele_Class = struct let name = class_name end in
+  let module Tele_Class = struct let name = !class_name end in
   let module Tele_Pprz = Pprz.Protocol(Tele_Class) in
   let module PprzTransport = Serial.Transport(Tele_Pprz) in
 
   let listen_tty = fun use_pprz_message tty ->
-    let fd = Serial.opendev tty Serial.B4800 in
+    let fd = Serial.opendev tty Serial.B38400 in
 
     let use_pprz_buf = fun buf ->
       use_pprz_message (Tele_Pprz.values_of_bin buf) in
@@ -60,10 +65,9 @@ let _ =
     let s = String.concat " " (List.map snd values) in
     Ivy.send (sprintf "%s %s" msg.Pprz.name s) in
 
-  let port = Sys.argv.(2) in
-
-  listen_tty handle_pprz_message port;
-
+  listen_tty handle_pprz_message !serial_dev;
+  Ivy.init "Paparazzi listen" "READY" (fun _ _ -> ());
+  Ivy.start "127.255.255.255:2010";
   let loop = Glib.Main.create true in
   while Glib.Main.is_running loop do ignore (Glib.Main.iteration true) done