diff --git a/conf/airframes/microjet4.xml b/conf/airframes/microjet4.xml
index 1acbd2ae04..bfbe4d55e1 100644
--- a/conf/airframes/microjet4.xml
+++ b/conf/airframes/microjet4.xml
@@ -139,6 +139,8 @@ ap.srcs += nav.c pid.c
 # Harware In The Loop
 ap.CFLAGS += -DHITL
 
+test.CFLAGS += -DFBW -DCONFIG=\"tiny.h\" -DLED -DTIME_LED=1 -DACTUATORS=\"servos_4015_hw.h\" -DSERVOS_4015 -DUSE_UART0 -DUART0_BAUD=B9600 -DDATALINK=PPRZ -DPPRZ_UART=Uart0
+test.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_transport.c setup_actuators.c $(SRC_ARCH)/uart_hw.c $(SRC_ARCH)/servos_4015_hw.c main.c
 
   </makefile>
 </airframe>
diff --git a/conf/autopilot/tiny.makefile b/conf/autopilot/tiny.makefile
index 661889bd8c..6c94b9c0f6 100644
--- a/conf/autopilot/tiny.makefile
+++ b/conf/autopilot/tiny.makefile
@@ -7,5 +7,10 @@ ap.ARCH = arm7tdmi
 ap.TARGET = autopilot
 ap.TARGETDIR = autopilot
 
+test.ARCHDIR = $(ARCHI)
+test.ARCH = arm7tdmi
+test.TARGET = autopilot
+test.TARGETDIR = autopilot
+
 LPC21ISP_BAUD = 115200
 LPC21ISP_XTAL = 14746
diff --git a/sw/airborne/arm7/servos_4015_hw.c b/sw/airborne/arm7/servos_4015_hw.c
index 2979af53f8..1ad91f9598 100644
--- a/sw/airborne/arm7/servos_4015_hw.c
+++ b/sw/airborne/arm7/servos_4015_hw.c
@@ -5,10 +5,6 @@
 #include "sys_time.h"
 
 
-
-#define COMMAND(i) servos_values[i]
-#define SERVOS_TICS_OF_USEC(s) SYS_TICS_OF_USEC(s)
-
 void actuators_init ( void ) {
   /* PWM selected as IRQ */
   VICIntSelect &= ~VIC_BIT(VIC_PWM);   
@@ -32,6 +28,10 @@ void actuators_init ( void ) {
   PWMMCR = PWMMCR_MR0R | PWMMCR_MR5I;
   /* enable PWM5 ouptput        */
   PWMPCR = PWMPCR_ENA5;
+
+  /* Prescaler */
+  PWMPR = PWM_PRESCALER-1;
+
   /* enable PWM timer counter and PWM mode  */
   PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE; 
   /* Load failsafe values              */
@@ -52,12 +52,11 @@ uint16_t servos_values[_4015_NB_CHANNELS];
 #define SERV7_START_POS 1600
 
 
-#define SERVO_REFRESH_US 25000
-uint32_t servos_delay = CLOCK_OF_US(SERVO_REFRESH_US - SERV4_START_POS - SERV5_START_POS - SERV6_START_POS - SERV7_START_POS) / 2; 
-uint8_t servos_idx = 0;
-
+#define SERVO_REFRESH_TICS SERVOS_TICS_OF_USEC(25000)
 
 
+static uint8_t servos_idx = 0;
+static uint32_t servos_delay;
 
 void PWM_ISR ( void ) {
   ISR_ENTRY();
@@ -66,12 +65,14 @@ void PWM_ISR ( void ) {
     IO1CLR = _BV(SERV0_RESET_PIN);
     IO1SET = _BV(SERV0_DATA_PIN);
     PWMMR0 = servos_values[servos_idx];
+    servos_delay = SERVO_REFRESH_TICS - servos_values[servos_idx];
     PWMLER = PWMLER_LATCH0;
     servos_idx++;
   }
   else if (servos_idx < _4015_NB_CHANNELS) {
     IO1CLR = _BV(SERV0_DATA_PIN);
     PWMMR0 = servos_values[servos_idx];
+    servos_delay -= servos_values[servos_idx];
     PWMLER = PWMLER_LATCH0;
     servos_idx++;
   }
diff --git a/sw/airborne/arm7/servos_4015_hw.h b/sw/airborne/arm7/servos_4015_hw.h
index 3950439dc3..5f7a64a113 100644
--- a/sw/airborne/arm7/servos_4015_hw.h
+++ b/sw/airborne/arm7/servos_4015_hw.h
@@ -9,7 +9,11 @@
 
 #include CONFIG
 
-#define SERVOS_TICS_OF_USEC(s) SYS_TICS_OF_USEC(s)
+#define PWM_PRESCALER 1
+
+#define PWM_TICS_OF_USEC(us)   (uint32_t)((us) *1e-6 * PCLK / PWM_PRESCALER + 0.5)
+
+#define SERVOS_TICS_OF_USEC(s) PWM_TICS_OF_USEC(s)
 #define ChopServo(x,a,b) Chop(x, a, b)
 
 #define _4015_NB_CHANNELS 8
diff --git a/sw/airborne/arm7/uart_hw.c b/sw/airborne/arm7/uart_hw.c
index 57d40f87c8..9c6bcb971d 100644
--- a/sw/airborne/arm7/uart_hw.c
+++ b/sw/airborne/arm7/uart_hw.c
@@ -203,30 +203,34 @@ void uart0_ISR(void)
  * for both uarts
  *
  */
-#define UART1_TX_INT_MODE 1
-#define UART1_RX_INT_MODE 1
 
-#if defined(UART1_TX_INT_MODE) || defined(UART1_RX_INT_MODE)
+#ifdef USE_UART1
+
 void uart1_ISR(void) __attribute__((naked));
-#endif
 
-#ifdef UART1_RX_INT_MODE
 uint8_t  uart1_rx_buffer[UART1_RX_BUFFER_SIZE];
 uint16_t uart1_rx_insert_idx, uart1_rx_extract_idx;
-#endif
 
-#ifdef UART1_TX_INT_MODE
+
+static void uart1_init_param( uint16_t baud, uint8_t mode, uint8_t fmode);
+
 uint8_t  uart1_tx_buffer[UART1_TX_BUFFER_SIZE];
 uint16_t uart1_tx_insert_idx, uart1_tx_extract_idx;
 uint8_t  uart1_tx_running;
-#endif
-
-static void uart1_init_param( uint16_t baud, uint8_t mode, uint8_t fmode);
 
 void uart1_init_tx( void ) {
   uart1_init_param(UART1_BAUD, UART_8N1, UART_FIFO_8);
 }
 
+bool_t uart1_check_free_space( uint8_t len) {
+  int16_t space;
+  if ((space = (uart1_tx_extract_idx - uart1_tx_insert_idx)) <= 0)
+    space += UART1_TX_BUFFER_SIZE;
+  
+  return (uint16_t)(space - 1) >= len;
+}
+
+
 void uart1_init_rx( void ) {}
 
 static void uart1_init_param( uint16_t baud, uint8_t mode, uint8_t fmode) {
@@ -247,41 +251,28 @@ static void uart1_init_param( uint16_t baud, uint8_t mode, uint8_t fmode) {
   // user specified operating parameters
   U1LCR = (mode & ~ULCR_DLAB_ENABLE);
   U1FCR = fmode;
-#if defined(UART1_TX_INT_MODE) || defined(UART1_RX_INT_MODE)
+
   // initialize the interrupt vector
   VICIntSelect &= ~VIC_BIT(VIC_UART1);  // UART0 selected as IRQ
   VICIntEnable = VIC_BIT(VIC_UART1);    // UART0 interrupt enabled
   VICVectCntl6 = VIC_ENABLE | VIC_UART1;
   VICVectAddr6 = (uint32_t)uart1_ISR;    // address of the ISR
 
-#ifdef UART1_TX_INT_MODE
   // initialize the transmit data queue
   uart1_tx_extract_idx = 0;
   uart1_tx_insert_idx = 0;
   uart1_tx_running = 0;
-#endif
 
-#ifdef UART1_RX_INT_MODE
   // initialize the receive data queue
   uart1_rx_extract_idx = 0;
   uart1_rx_insert_idx = 0;
 
   // enable receiver interrupts
   U1IER = UIER_ERBFI;
-#endif
-#endif
 }
 
-bool_t uart1_check_free_space( uint8_t len) {
-  int16_t space;
-  if ((space = (uart1_tx_extract_idx - uart1_tx_insert_idx)) <= 0)
-    space += UART1_TX_BUFFER_SIZE;
-  
-  return (uint16_t)(space - 1) >= len;
-}
 
 void uart1_transmit( unsigned char data ) {
-#ifdef UART1_TX_INT_MODE
   uint16_t temp;
   unsigned cpsr;
 
@@ -312,13 +303,6 @@ void uart1_transmit( unsigned char data ) {
   cpsr = disableIRQ();                  // disable global interrupts
   U1IER |= UIER_ETBEI;                  // enable TX interrupts
   restoreIRQ(cpsr);                     // restore global interrupts
-#else
-  while (!(U1LSR & ULSR_THRE))          // wait for TX buffer to empty
-    continue;                           // also either WDOG() or swap()
-
-  U1THR = (uint8_t)ch;
-#endif
-  //  return (uint8_t)ch;
 }
 
 
@@ -339,7 +323,6 @@ void uart1_ISR(void)
         U1LSR;                          // read LSR to clear
         break;
 
-#ifdef UART1_RX_INT_MODE
       case UIIR_CTI_INT:                // Character Timeout Indicator
       case UIIR_RDA_INT:                // Receive Data Available
         do
@@ -356,9 +339,7 @@ void uart1_ISR(void)
         while (U1LSR & ULSR_RDR);
 
         break;
-#endif
 
-#ifdef UART1_TX_INT_MODE
       case UIIR_THRE_INT:               // Transmit Holding Register Empty
         while (U1LSR & ULSR_THRE)
           {
@@ -377,7 +358,6 @@ void uart1_ISR(void)
           }
 
         break;
-#endif // UART1_TX_INT_MODE
 
       case UIIR_MS_INT:                 // MODEM Status
         U1MSR;                          // read MSR to clear
@@ -394,3 +374,5 @@ void uart1_ISR(void)
   VICVectAddr = 0x00000000;             // clear this interrupt from the VIC
   ISR_EXIT();                           // recover registers and return
 }
+
+#endif /* USE_UART1 */
diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml
index 4ad84c7e65..64ba5688a8 100644
--- a/sw/lib/ocaml/pprz.ml
+++ b/sw/lib/ocaml/pprz.ml
@@ -246,8 +246,8 @@ let rec sprint_value = fun buf i _type v ->
       buf.[i+1] <- byte (value lsr 8);
       buf.[i+0] <- byte value;
       sizeof _type
-  | Scalar ("int16"|"uint16"), Int value ->
-      assert (_type <> Scalar "uint16" || value >= 0);
+  | Scalar "uint16", Int value ->
+      assert (value >= 0);
       buf.[i+1] <- byte (value lsr 8);
       buf.[i+0] <- byte value;
       sizeof _type