diff --git a/Makefile b/Makefile
index c9d41d87e6..00e62fd93a 100644
--- a/Makefile
+++ b/Makefile
@@ -84,7 +84,7 @@ fbw fly_by_wire: ac_h
 ap autopilot: ac_h
 	cd $(AIRBORNE); $(MAKE) TARGET=ap all
 
-sim: ac_h
+sim: ac_h sim_static
 	cd $(AIRBORNE); $(MAKE) TARGET=sim ARCHI=sim all
 
 upload_fbw: fbw
diff --git a/Makefile.gen b/Makefile.gen
index 921cb7d494..f9dc227709 100644
--- a/Makefile.gen
+++ b/Makefile.gen
@@ -22,6 +22,9 @@
 
 # Preprocessing of XML configuration files
 
+# Quiet compilation
+Q=@
+
 include conf/Makefile.local
 
 CONF=conf
@@ -38,19 +41,21 @@ UBX_XML = $(CONF)/ubx.xml
 static:  $(MESSAGES_H) $(UBX_PROTOCOL_H) $(DL_PROTOCOL_H)
 
 $(MESSAGES_H) : $(MESSAGES_XML) $(CONF_XML) $(TOOLS)/gen_messages.out
-	test -d $(STATICINCLUDE) || mkdir -p $(STATICINCLUDE)
-	TMP_FILE=`mktemp`;\
-	$(TOOLS)/gen_messages.out $< telemetry > $$TMP_FILE;\
-	mv  $$TMP_FILE $@
-	chmod a+r $@
+	$(Q)test -d $(STATICINCLUDE) || mkdir -p $(STATICINCLUDE)
+	@echo BUILD $@
+	$(Q)$(TOOLS)/gen_messages.out $< telemetry > /tmp/msg.h
+	$(Q)mv /tmp/msg.h $@
+	$(Q)chmod a+r $@
 
 $(UBX_PROTOCOL_H) : $(UBX_XML)
-	$(TOOLS)/gen_ubx.out $< > /tmp/ubx.h
-	mv /tmp/ubx.h $@
+	@echo BUILD $@
+	$(Q)$(TOOLS)/gen_ubx.out $< > /tmp/ubx.h
+	$(Q)mv /tmp/ubx.h $@
 
 $(DL_PROTOCOL_H) : $(MESSAGES_XML)
-	$(TOOLS)/gen_dl.out $< > /tmp/dl.h
-	mv /tmp/dl.h $@
+	@echo BUILD $@
+	$(Q)$(TOOLS)/gen_messages.out $< datalink > /tmp/dl.h
+	$(Q)mv /tmp/dl.h $@
 
 clean :
 	rm -f $(H_OF_XML)
diff --git a/conf/Makefile.avr b/conf/Makefile.avr
index c8b7ba8e0c..3792781267 100644
--- a/conf/Makefile.avr
+++ b/conf/Makefile.avr
@@ -27,6 +27,8 @@
 # Edit the configuration part to suit your local install
 #
 
+VERIFY=--verify
+
 SRC_ARCH = $(PAPARAZZI_SRC)/sw/airborne/avr
 
 CC		= $(ATMELBIN)/avr-gcc -mmcu=$($(TARGET).ARCH)
@@ -142,7 +144,7 @@ $(OBJDIR)/%.hex: $(OBJDIR)/%.elf
 %.install: $(OBJDIR)/%.hex check_arch
 	# stk200 needs to be erased first
 	$(UISP) $(UISP_FLAGS) --erase
-	$(UISP) $(UISP_FLAGS) --upload --verify if=$<
+	$(UISP) $(UISP_FLAGS) --upload $(VERIFY) if=$<
 
 erase: check_arch
 	$(UISP) $(ISP_FLAGS) --erase
diff --git a/conf/airframes/twinstar1.xml b/conf/airframes/twinstar1.xml
index b85282e026..f724cb5ef6 100644
--- a/conf/airframes/twinstar1.xml
+++ b/conf/airframes/twinstar1.xml
@@ -110,7 +110,17 @@
 # Config for HITL simulation on a V1_2_1 bi-AVR board
 include $(PAPARAZZI_SRC)/conf/autopilot/v1_2_1.makefile
 include $(PAPARAZZI_SRC)/conf/autopilot/twin_mcu_avr.makefile
-ap.CFLAGS += -DUSE_UART0 -DHITL
+
+# Harware In The Loop
+ap.CFLAGS += -DHITL
+
+# Downlink on uart0
+ap.CFLAGS += -DUSE_UART0 -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_AP_DEVICE=Uart0
+ap.EXTRA_SRCS += pprz_transport.c downlink.c
+
+# Datalink (on uart0) used to emulate sensors (infrared and gps)
+ap.EXTRA_SRCS += traffic_info.c datalink.c
+ap.CFLAGS += -DDATALINK -DPPRZ_INPUT
 
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
diff --git a/conf/messages.dtd b/conf/messages.dtd
new file mode 100644
index 0000000000..6b72ff6227
--- /dev/null
+++ b/conf/messages.dtd
@@ -0,0 +1,23 @@
+<!ELEMENT protocol (class+)>
+<!ATTLIST protocol>
+
+<!ELEMENT class (message+)>
+<!ATTLIST class
+  name CDATA #REQUIRED
+>
+
+<!ELEMENT message (field*)>
+<!ATTLIST message
+  name CDATA #REQUIRED
+  id CDATA #REQUIRED
+>
+
+<!ELEMENT field EMPTY>
+<!ATTLIST field
+  name CDATA #REQUIRED
+  type CDATA #REQUIRED
+  format CDATA #IMPLIED
+  unit CDATA #IMPLIED
+  values CDATA #IMPLIED
+>
+
diff --git a/conf/messages.xml b/conf/messages.xml
index ecbb44813b..b127c9f538 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-
+<!DOCTYPE protocol SYSTEM "messages.dtd">
 <protocol>
  <!-- messages from modem or sim to server -->
  <class name="telemetry">
@@ -214,6 +214,11 @@
     <field name="value" type="float"/>
  </message>
 
+ <message name="DEBUG" id="32">
+    <field name="channels" type="int16[]"/>
+ </message>
+
+
   <message name="PPM" ID="100">
     <field name="values" type="uint16[]" unit="ticks"/>
   </message>
@@ -226,7 +231,7 @@
     <field name="values" type="int16[]" unit="ticks"/>
   </message>
 
-  <message name="FBW_STATUS" ID="103" period="1">
+  <message name="FBW_STATUS" ID="103">
     <field name="nb_spi_err" type="uint8"/>
     <field name="rc_status" type="uint8" values="OK|LOST|REALLY_LOST"/>
     <field name="mode" type="uint8" values="MANUAL|AUTO|FAILSAFE"/>
@@ -239,6 +244,8 @@
 
 </class>
 
+
+
 <class name="datalink">
  <message name="ACINFO" ID="1">
   <field name="ac_id" type="uint8"/>
@@ -271,6 +278,16 @@
   <field name="block_id" type="uint8"/>
  </message>
 
+ <message name="HITL_UBX" id="6">
+  <field name="class" type="uint8"/>
+  <field name="id" type="uint8"/>
+  <field name="ubx_payload" type="uint8[]"/>
+ </message>
+
+ <message name="HITL_INFRARED" id="7">
+   <field name="roll" type="int16"/>
+   <field name="pitch" type="int16"/>
+ </message>
 </class>
 
 
@@ -463,6 +480,11 @@
   <field name="values" type="string" format="csv"/>
  </message>
 
+ <message name="RAW_DATALINK" ID="29">
+  <field name="ac_id" type="string"/>
+  <field name="message" type="string" format=";sv"/>
+ </message>
+
 </class>
 
 <class name="alert">
diff --git a/conf/ubx.xml b/conf/ubx.xml
index 1b5d993e75..9c7e5e7afa 100644
--- a/conf/ubx.xml
+++ b/conf/ubx.xml
@@ -145,14 +145,4 @@
 
  </class>
 
- <!-- User class -->
- <class name="USR" ID="0x40">
-
-  <!-- Infrared values from the HITL simulator -->
-  <message name="IRSIM" ID="0x01" length="4">
-   <field name="ROLL" format="I2"/>
-   <field name="PITCH" format="I2"/>
-  </message>
-  </class>
-
 </ubx>
diff --git a/sw/airborne/avr/link_mcu_ap.c b/sw/airborne/avr/link_mcu_ap.c
index f9f834f7c1..f71fb31932 100644
--- a/sw/airborne/avr/link_mcu_ap.c
+++ b/sw/airborne/avr/link_mcu_ap.c
@@ -59,7 +59,7 @@ void link_fbw_send(void) {
   idx_buf = 0;
   crc_in = CRC_INIT;
   crc_out = CRC_INIT;
-  uint8_t byte1 = ((uint8_t*)&from_ap)[0];
+  uint8_t byte1 = ((uint8_t*)&from_ap.from_ap)[0];
   SPDR = byte1;
   crc_out = CrcUpdate(crc_out, byte1);
   from_fbw_receive_valid = FALSE;
diff --git a/sw/airborne/avr/spi_hw.c b/sw/airborne/avr/spi_hw.c
index 52a2d027df..b58d7cf06b 100644
--- a/sw/airborne/avr/spi_hw.c
+++ b/sw/airborne/avr/spi_hw.c
@@ -59,7 +59,6 @@ void spi_reset(void) {
 }
 
 
-/** c.f. autopilot/link_fbw.c */
 SIGNAL(SIG_SPI) {
   static uint8_t tmp, crc_in1;
   
@@ -110,7 +109,9 @@ SIGNAL(SIG_SPI) {
   crc_in = CrcUpdate(crc_in, tmp);
 }
 
-#endif
+#endif /** FBW */
+
+
 
 #ifdef AP
 
diff --git a/sw/airborne/cam.c b/sw/airborne/cam.c
index 310670fc16..d6f1e3405b 100644
--- a/sw/airborne/cam.c
+++ b/sw/airborne/cam.c
@@ -70,8 +70,8 @@ void cam_manual( void ) {
       cam_pitch += FLOAT_OF_PPRZ(pitch, 0, 300.);
       cam_pitch = TRIM_PPRZ(cam_pitch);
     }
-    from_ap.from_ap.channels[COMMAND_CAM_ROLL] = cam_roll;
-    from_ap.from_ap.channels[COMMAND_CAM_PITCH] = cam_pitch;
+    from_ap.from_ap.commands[COMMAND_CAM_ROLL] = cam_roll;
+    from_ap.from_ap.commands[COMMAND_CAM_PITCH] = cam_pitch;
   }
 }
 
diff --git a/sw/airborne/datalink.c b/sw/airborne/datalink.c
index a7aeb64170..d6a58bc88c 100644
--- a/sw/airborne/datalink.c
+++ b/sw/airborne/datalink.c
@@ -38,16 +38,19 @@
 #include "estimator.h"
 #include "pid.h"
 #include "cam.h"
+#include "infrared.h"
+#include "gps.h"
 
 #define MOfCm(_x) (((float)_x)/100.)
 
+#define SenderIdOfMsg(x) (x[0])
 #define IdOfMsg(x) (x[1])
 
 void dl_parse_msg(void) {
   /** Hack: id of the sender is used in uplink to select the receiver */
-  if (IdOfMsg(dl_buffer) == AC_ID) {   
-    uint8_t msg_id = dl_buffer[0];
-    if (msg_id == DL_ACINFO_ID) {
+  if (SenderIdOfMsg(dl_buffer) == AC_ID) {   
+    uint8_t msg_id = IdOfMsg(dl_buffer);
+    if (msg_id == DL_ACINFO) {
       uint8_t id = DL_ACINFO_ac_id(dl_buffer);
       float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer));
       float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer));
@@ -55,24 +58,47 @@ void dl_parse_msg(void) {
       float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.);
       float s = MOfCm(DL_ACINFO_speed(dl_buffer));
       SetAcInfo(id, ux, uy, c, a, s);
-    } else if (msg_id == DL_MOVE_WP_ID) {
+    } else if (msg_id == DL_MOVE_WP) {
       uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
       float ux = MOfCm(DL_MOVE_WP_utm_east(dl_buffer));
       float uy = MOfCm(DL_MOVE_WP_utm_north(dl_buffer));
       float a = MOfCm(DL_MOVE_WP_alt(dl_buffer));
       MoveWaypoint(wp_id, ux, uy, a);
-    } else if (msg_id == DL_EVENT_ID) {
+    } else if (msg_id == DL_EVENT) {
       uint8_t event = DL_EVENT_event(dl_buffer);
       switch (event) {
       case 1 : rc_event_1 = TRUE; break; // FIXME !
       case 2 : rc_event_2 = TRUE; break;
       default: ;
       }
-    } else if (msg_id == DL_BLOCK_ID) {
+    } else if (msg_id == DL_BLOCK) {
       nav_goto_block(DL_BLOCK_block_id(dl_buffer));
     }
+#ifdef HITL
+    /** Infrared and GPS sensors are replaced by messages on the datalink */
+    else if (msg_id == DL_HITL_INFRARED) {
+      /** This code simulates infrared.c:ir_update() */
+      ir_roll = DL_HITL_INFRARED_roll(dl_buffer);
+      ir_pitch = DL_HITL_INFRARED_pitch(dl_buffer);
+    } else if (msg_id == DL_HITL_UBX) {
+      /** This code simulates gps_ubx.c:parse_ubx() */
+      if (gps_msg_received) {
+	gps_nb_ovrn++;
+      } else {
+	ubx_class = DL_HITL_UBX_class(dl_buffer);
+	ubx_id = DL_HITL_UBX_id(dl_buffer);
+	uint8_t l = DL_HITL_UBX_ubx_payload_length(dl_buffer);
+	uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(dl_buffer);
+	uint8_t i;
+	for(i=0; i<l; i++) {
+	  ubx_msg_buf[i] = ubx_payload[i];
+	}
+	gps_msg_received = TRUE;
+      }
+    }
+#endif
 #ifdef DlSetting
-    else if (msg_id == DL_SETTING_ID) {
+    else if (msg_id == DL_SETTING) {
       DlSetting(DL_SETTING_index(dl_buffer), DL_SETTING_value(dl_buffer));
     }
 #endif /** Else there is no dl_settings section in the flight plan */
diff --git a/sw/airborne/datalink.h b/sw/airborne/datalink.h
index e90f9d72df..1a8e012de7 100644
--- a/sw/airborne/datalink.h
+++ b/sw/airborne/datalink.h
@@ -28,6 +28,7 @@
 
 #ifndef DATALINK_H
 #define DATALINK_H
+
 #ifdef DATALINK_C
 #define EXTERN
 #else
@@ -35,11 +36,12 @@
 #endif
 
 EXTERN bool_t dl_msg_available;
+/** Flag provided to control calls to ::dl_parse_msg. NOT used in this module*/
 
 #define MSG_SIZE 128
 EXTERN char dl_buffer[MSG_SIZE];
 
 void dl_parse_msg(void);
-/** Should be called when dl_msg_available is set */
+/** Should be called when chars are available in dl_buffer */
 
 #endif
diff --git a/sw/airborne/downlink.h b/sw/airborne/downlink.h
index fcd9260284..fefc03f6ea 100644
--- a/sw/airborne/downlink.h
+++ b/sw/airborne/downlink.h
@@ -56,6 +56,7 @@ extern uint8_t downlink_nb_ovrn;
 
 #define DownlinkPutInt16Array(_n, _x) Transport(PutInt16Array(_n, _x))
 #define DownlinkPutUint16Array(_n, _x) Transport(PutUint16Array(_n, _x))
+#define DownlinkPutUint8Array(_n, _x) Transport(PutUint8Array(_n, _x))
 
 #define DonwlinkOverrun() downlink_nb_ovrn++;
 
diff --git a/sw/airborne/gps_ubx.c b/sw/airborne/gps_ubx.c
index c9ee3d4831..1899d02f6a 100644
--- a/sw/airborne/gps_ubx.c
+++ b/sw/airborne/gps_ubx.c
@@ -46,20 +46,13 @@ uint8_t gps_utm_zone;
 uint8_t gps_mode;
 volatile bool_t gps_msg_received;
 bool_t gps_pos_available;
+uint8_t ubx_id, ubx_class;
 
 #define UBX_MAX_PAYLOAD 255
-static uint8_t  ubx_msg_buf[UBX_MAX_PAYLOAD];
+uint8_t ubx_msg_buf[UBX_MAX_PAYLOAD];
 
 #define RadianOfDeg(d) ((d)/180.*3.1415927)
 
-#ifdef HITL
-/* With HITL simulation, the GPS uart link is connected to the simulator which
-   sends the UBX frames. This link is also used for the infrared data
-   (replacing the adc, c.f. infrared.c) */
-volatile int16_t hitl_ir_roll;
-volatile int16_t hitl_ir_pitch;
-#endif /* HITL */
-
 #define UNINIT        0
 #define GOT_SYNC1     1
 #define GOT_SYNC2     2
@@ -73,7 +66,7 @@ volatile int16_t hitl_ir_pitch;
 static uint8_t  ubx_status;
 static uint16_t ubx_len;
 static uint8_t  ubx_msg_idx;
-static uint8_t ck_a, ck_b, ubx_id, ubx_class;
+static uint8_t ck_a, ck_b;
 uint8_t send_ck_a, send_ck_b;
 
 void gps_init( void ) {
@@ -154,15 +147,6 @@ void parse_gps_msg( void ) {
       }
     }
   }
-#ifdef HITL
-  if (ubx_class == UBX_USR_ID) {
-    if (ubx_id == UBX_USR_IRSIM_ID) {
-      /** Message from the simalator, containing infrared data */
-      hitl_ir_roll = UBX_USR_IRSIM_ROLL(ubx_msg_buf);
-      hitl_ir_pitch = UBX_USR_IRSIM_PITCH(ubx_msg_buf);
-    }
-  }
-#endif /* HITL */
 }
 
 
diff --git a/sw/airborne/gps_ubx.h b/sw/airborne/gps_ubx.h
index 81f60ee9bd..e81c7f175a 100644
--- a/sw/airborne/gps_ubx.h
+++ b/sw/airborne/gps_ubx.h
@@ -31,6 +31,10 @@
 #ifndef UBX_H
 #define UBX_H
 
+extern uint8_t ubx_id, ubx_class;
+#define UBX_MAX_PAYLOAD 255
+uint8_t ubx_msg_buf[UBX_MAX_PAYLOAD];
+
 extern uint8_t send_ck_a, send_ck_b;
 #define UbxInitCheksum() { send_ck_a = send_ck_b = 0; }
 #define UpdateChecksum(c) { send_ck_a += c; send_ck_b += send_ck_a; }
@@ -52,7 +56,7 @@ extern uint8_t send_ck_a, send_ck_b;
 }
 
 
- 
+/** Includes macros generated from ubx.xml */ 
 #include "ubx_protocol.h"
 
 extern void parse_ubx( uint8_t c );
diff --git a/sw/airborne/infrared.c b/sw/airborne/infrared.c
index 6e066f4ec0..57e793949a 100644
--- a/sw/airborne/infrared.c
+++ b/sw/airborne/infrared.c
@@ -78,21 +78,16 @@ void ir_init(void) {
 }
 
 /** \brief Update \a ir_roll and ir_pitch from ADCs or from simulator
- * message in HITL mode
+ * message in HITL and SITL modes
  */
 void ir_update(void) {
-#if ! defined SITL
-#if defined HITL
-  extern volatile int16_t hitl_ir_roll, hitl_ir_pitch; /* From gps_ubx.c */
-  ir_roll = hitl_ir_roll; 
-  ir_pitch = hitl_ir_pitch;
-#else /* HITL */
+#if ! (defined SITL || defined HITL)
   int16_t x1_mean = buf_ir1.sum/buf_ir1.av_nb_sample;
   int16_t x2_mean = buf_ir2.sum/buf_ir2.av_nb_sample;
   ir_roll = IR_RollOfIrs(x1_mean, x2_mean);
   ir_pitch = IR_PitchOfIrs(x1_mean, x2_mean);
-#endif /* !HITL */
 
+  /** neutrals are not taken into account in SITL and HITL */
   ir_roll -= IR_ADC_ROLL_NEUTRAL;
   ir_pitch -= IR_ADC_PITCH_NEUTRAL;
 #endif /* !SITL */
diff --git a/sw/airborne/inter_mcu.h b/sw/airborne/inter_mcu.h
index 5607bbc612..8ad55a6290 100644
--- a/sw/airborne/inter_mcu.h
+++ b/sw/airborne/inter_mcu.h
@@ -76,7 +76,7 @@ struct from_fbw_msg {
 };
 
 struct from_ap_msg {
-  pprz_t channels[COMMANDS_NB];  
+  pprz_t commands[COMMANDS_NB];  
 };
 
 typedef union  { 
@@ -157,7 +157,6 @@ static inline void to_autopilot_from_rc_values (void) {
 static inline void inter_mcu_event_task( void) {
   if (from_ap_receive_valid) {
     from_ap_receive_valid = FALSE;
-    // printf("inter_mcu valid\n");
     time_since_last_ap = 0;
     ap_ok = TRUE;
     to_autopilot_from_rc_values();
diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c
index 408b79b90e..7cb08fd1c9 100644
--- a/sw/airborne/main_ap.c
+++ b/sw/airborne/main_ap.c
@@ -192,7 +192,7 @@ static inline void events_update( void ) {
  */
 static inline void copy_from_to_fbw ( void ) {
 #ifdef COMMAND_YAW /* FIXME */
-  from_ap.from_ap.channels[COMMAND_YAW] =from_fbw.from_fbw.channels[RADIO_YAW];
+  from_ap.from_ap.commands[COMMAND_YAW] =from_fbw.from_fbw.channels[RADIO_YAW];
 #endif
 }
 
@@ -459,9 +459,9 @@ inline void periodic_task_ap( void ) {
     estimator_update_state_infrared();
 #endif
     roll_pitch_pid_run(); /* Set  desired_aileron & desired_elevator */
-    from_ap.from_ap.channels[COMMAND_THROTTLE] = desired_gaz; /* desired_gaz is set upon GPS message reception */
-    from_ap.from_ap.channels[COMMAND_ROLL] = desired_aileron;
-    from_ap.from_ap.channels[COMMAND_PITCH] = desired_elevator;
+    from_ap.from_ap.commands[COMMAND_THROTTLE] = desired_gaz; /* desired_gaz is set upon GPS message reception */
+    from_ap.from_ap.commands[COMMAND_ROLL] = desired_aileron;
+    from_ap.from_ap.commands[COMMAND_PITCH] = desired_elevator;
     
 #if defined MCU_SPI_LINK
     link_fbw_send();
@@ -599,12 +599,12 @@ void event_task_ap( void ) {
   }
 #endif /** WAVECARD */
 
-#ifdef MAXSTREAM
-  if (maxstream_msg_received) {
-    maxstream_parse_payload();
-    maxstream_msg_received = FALSE;
+#ifdef PPRZ_INPUT
+  if (pprz_msg_received) {
+    pprz_msg_received = FALSE;
+    pprz_parse_payload();
   }
-#endif /** MAXSTREAM */
+#endif /** PPRZ_INPUT */
 
 #ifdef DATALINK
   if (dl_msg_available) {
diff --git a/sw/airborne/main_fbw.c b/sw/airborne/main_fbw.c
index 376e1298f0..f9b5e1d71b 100644
--- a/sw/airborne/main_fbw.c
+++ b/sw/airborne/main_fbw.c
@@ -128,18 +128,11 @@ void event_task_fbw( void) {
     }
     if (fbw_mode == FBW_MODE_MANUAL)
       SetCommandsFromRC(commands);
-  } else if (fbw_mode == FBW_MODE_MANUAL && rc_status == RC_REALLY_LOST) {
+  } else if (rc_status == RC_REALLY_LOST) {
     fbw_mode = FBW_MODE_AUTO;
   }
 #endif
 
-#ifdef MCU_SPI_LINK
-  if ( !SpiIsSelected() && spi_was_interrupted ) {
-    spi_was_interrupted = FALSE;
-    spi_reset();
-  }
-#endif
-
 #ifdef ADC
   fbw_vsupply_decivolt = VoltageOfAdc(vsupply_adc_buf.sum/vsupply_adc_buf.av_nb_sample) * 10;
 #endif
@@ -147,7 +140,14 @@ void event_task_fbw( void) {
 #ifdef INTER_MCU
   inter_mcu_event_task();
   if (ap_ok && fbw_mode == FBW_MODE_AUTO) {
-    SetCommands(from_ap.from_ap.channels);
+    SetCommands(from_ap.from_ap.commands);
+  }
+#endif
+
+#ifdef MCU_SPI_LINK
+  if ( !SpiIsSelected() && spi_was_interrupted ) {
+    spi_was_interrupted = FALSE;
+    spi_reset();
   }
 #endif
 
@@ -158,9 +158,6 @@ void event_task_fbw( void) {
 #endif
 
   if (
-#ifdef RADIO_CONTROL
-      (fbw_mode == FBW_MODE_MANUAL && rc_status == RC_REALLY_LOST) ||
-#endif
 #ifdef INTER_MCU
       (fbw_mode == FBW_MODE_AUTO && !ap_ok) ||
 #endif
diff --git a/sw/airborne/nav.c b/sw/airborne/nav.c
index afc1de2881..c5b842072c 100644
--- a/sw/airborne/nav.c
+++ b/sw/airborne/nav.c
@@ -279,8 +279,7 @@ static void glide_to(uint8_t _last_wp, uint8_t wp) {
   pre_climb = NOMINAL_AIRSPEED * (waypoints[wp].a - last_alt) / leg;
 }
 
-/** static inline void compute_dist2_to_home(void)
- *  \brief Compute square distance to waypoint Home and
+/** \brief Compute square distance to waypoint Home and
  *  verify uav is not too far away from Home.
  */
 static inline void compute_dist2_to_home(void) {
diff --git a/sw/airborne/pprz_transport.c b/sw/airborne/pprz_transport.c
index a22f1c9729..be2b820a36 100644
--- a/sw/airborne/pprz_transport.c
+++ b/sw/airborne/pprz_transport.c
@@ -1,4 +1,40 @@
+/*
+ * Paparazzi mcu0 $Id$
+ *  
+ * Copyright (C) 2006  Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA. 
+ *
+ */
+
+#include <inttypes.h>
 #include "pprz_transport.h"
+#include "uart.h"
 
 uint8_t ck_a, ck_b;
 uint8_t downlink_nb_ovrn;
+volatile bool_t pprz_msg_received = FALSE;
+uint8_t pprz_ovrn, pprz_error;
+volatile uint8_t payload_length;
+uint8_t input_payload[MAX_INPUT_PAYLOAD];
+
+
+#ifdef PPRZ_INPUT
+/** FIXME: Where to put it ??? */
+ReceiveUart0(parse_pprz);
+#endif
diff --git a/sw/airborne/pprz_transport.h b/sw/airborne/pprz_transport.h
index cf0b8c0e8e..34b3dccc05 100644
--- a/sw/airborne/pprz_transport.h
+++ b/sw/airborne/pprz_transport.h
@@ -26,6 +26,8 @@
 #define PPRZ_TRANSPORT_H
 
 #include <inttypes.h>
+#include "std.h"
+#include "datalink.h"
 
 extern uint8_t ck_a, ck_b;
 
@@ -69,22 +71,22 @@ extern uint8_t ck_a, ck_b;
 
 #define PprzTransportPut2ByteByAddr(_byte) { \
     PprzTransportPut1ByteByAddr(_byte);	\
-    PprzTransportPut1ByteByAddr(_byte+1);	\
+    PprzTransportPut1ByteByAddr((uint8_t*)_byte+1);	\
   }
 
 #define PprzTransportPut4ByteByAddr(_byte) { \
     PprzTransportPut2ByteByAddr(_byte);	\
-    PprzTransportPut2ByteByAddr(_byte+2);	\
+    PprzTransportPut2ByteByAddr((uint8_t*)_byte+2);	\
   }
 
 
 #define PprzTransportPutInt8ByAddr(_x) PprzTransportPut1ByteByAddr(_x)
 #define PprzTransportPutUint8ByAddr(_x) PprzTransportPut1ByteByAddr(_x)
-#define PprzTransportPutInt16ByAddr(_x) PprzTransportPut2ByteByAddr(_x)
-#define PprzTransportPutUint16ByAddr(_x) PprzTransportPut2ByteByAddr(_x)
-#define PprzTransportPutInt32ByAddr(_x) PprzTransportPut4ByteByAddr(_x)
-#define PprzTransportPutUint32ByAddr(_x) PprzTransportPut4ByteByAddr(_x)
-#define PprzTransportPutFloatByAddr(_x) PprzTransportPut4ByteByAddr(_x)
+#define PprzTransportPutInt16ByAddr(_x) PprzTransportPut2ByteByAddr((uint8_t*)_x)
+#define PprzTransportPutUint16ByAddr(_x) PprzTransportPut2ByteByAddr((uint8_t*)_x)
+#define PprzTransportPutInt32ByAddr(_x) PprzTransportPut4ByteByAddr((uint8_t*)_x)
+#define PprzTransportPutUint32ByAddr(_x) PprzTransportPut4ByteByAddr((uint8_t*)_x)
+#define PprzTransportPutFloatByAddr(_x) PprzTransportPut4ByteByAddr((uint8_t*)_x)
 
 #define PprzTransportPut(_put, _n, _x) { \
   int i; \
@@ -97,6 +99,76 @@ extern uint8_t ck_a, ck_b;
 #define PprzTransportPutInt16Array(_n, _x) PprzTransportPut(PprzTransportPutInt16ByAddr, _n, _x)
 
 #define PprzTransportPutUint16Array(_n, _x) PprzTransportPut(PprzTransportPutUint16ByAddr, _n, _x)
+#define PprzTransportPutUint8Array(_n, _x) PprzTransportPut(PprzTransportPutUint8ByAddr, _n, _x)
+
+
+/** Receiving pprz messages */
+
+#define UNINIT 0
+#define GOT_STX 1
+#define GOT_LENGTH 2
+#define GOT_PAYLOAD 3
+#define GOT_CRC1 4
+#define GOT_CRC2 5
+
+#define MAX_INPUT_PAYLOAD 256
+extern uint8_t input_payload[MAX_INPUT_PAYLOAD];
+
+extern volatile bool_t pprz_msg_received;
+extern uint8_t pprz_ovrn, pprz_error;
+extern volatile uint8_t payload_length;
+
+static inline void parse_pprz( uint8_t c ) {
+  static uint8_t pprz_status = UNINIT;
+  static uint8_t ck_a, ck_b, payload_idx;
+
+  switch (pprz_status) {
+  case UNINIT:
+    if (c == STX)
+      pprz_status++;
+    break;
+  case GOT_STX:
+    if (pprz_msg_received) {
+      pprz_ovrn++;
+      goto error;
+    }
+    payload_length = c-4; /* Counting STX, LENGTH and CRC1 and CRC2 */
+    ck_a = ck_b = c;
+    pprz_status++;
+    payload_idx = 0;
+    break;
+  case GOT_LENGTH:
+    input_payload[payload_idx] = c;
+    ck_a += c; ck_b += ck_a;
+    payload_idx++;
+    if (payload_idx == payload_length)
+      pprz_status++;
+    break;
+  case GOT_PAYLOAD:
+    if (c != ck_a)
+      goto error;
+    pprz_status++;
+    break;
+  case GOT_CRC1:
+    if (c != ck_b)
+      goto error;
+    pprz_msg_received = TRUE;
+    goto restart;
+  }
+  return;
+ error:
+  pprz_error++;
+ restart:
+  pprz_status = UNINIT;
+  return;
+}
+
+static inline void pprz_parse_payload(void) {
+  uint8_t i;
+  for(i = 0; i < payload_length; i++) 
+    dl_buffer[i] = input_payload[i];
+  dl_msg_available = TRUE;
+}
 
 #endif /* PPRZ_TRANSPORT_H */
 
diff --git a/sw/airborne/radio_control.h b/sw/airborne/radio_control.h
index 32362e45e1..29eab94977 100644
--- a/sw/airborne/radio_control.h
+++ b/sw/airborne/radio_control.h
@@ -38,8 +38,8 @@
 #endif /* DEBUG_RC */
 
 #define RC_AVG_PERIOD 8
-#define RC_LOST_TIME 30  // 500ms with a 60Hz timer
-#define RC_REALLY_LOST_TIME 20
+#define RC_LOST_TIME 30  /* 500ms with a 60Hz timer */
+#define RC_REALLY_LOST_TIME 100 /* ~1.5s */
 
 #define RC_OK          0
 #define RC_LOST        1
@@ -75,10 +75,11 @@ static inline void radio_control_periodic_task ( void ) {
 
   if (time_since_last_ppm >= RC_REALLY_LOST_TIME) {
     rc_status = RC_REALLY_LOST;
-  } else if (time_since_last_ppm >= RC_LOST_TIME) {
-    rc_status = RC_LOST;
-  } else
+  } else {
+    if (time_since_last_ppm >= RC_LOST_TIME)
+      rc_status = RC_LOST;
     time_since_last_ppm++;
+  } 
 }
 
 /********** EVENT ************************************************************/
diff --git a/sw/ground_segment/cockpit/Makefile b/sw/ground_segment/cockpit/Makefile
index 6bf8c6c14a..6dd38688a8 100644
--- a/sw/ground_segment/cockpit/Makefile
+++ b/sw/ground_segment/cockpit/Makefile
@@ -1,3 +1,28 @@
+# 
+#   $Id$
+#   Copyright (C) 2003 Pascal Brisset, Antoine Drouin
+#
+# This file is part of paparazzi.
+#
+# paparazzi is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2, or (at your option)
+# any later version.
+#
+# paparazzi is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with paparazzi; see the file COPYING.  If not, write to
+# the Free Software Foundation, 59 Temple Place - Suite 330,
+# Boston, MA 02111-1307, USA.  
+#
+
+# Quiet compilation
+Q=@
+
 OCAMLC=ocamlc -thread
 OCAMLOPT=ocamlopt -thread
 INCLUDES=-I +lablgtk2 -I +camlimages -I ../../lib/ocaml
@@ -11,20 +36,25 @@ opt : map2d.opt
 
 
 map2d : map2d.ml ../../lib/ocaml/lib-pprz.cma ../../lib/ocaml/xlib-pprz.cma
-	$(OCAMLC) -custom $(INCLUDES) $(LIBS) threads.cma gtkThread.cmo gtkInit.cmo $< -o $@
+	@echo OL $@
+	$(Q)$(OCAMLC) -custom $(INCLUDES) $(LIBS) threads.cma gtkThread.cmo gtkInit.cmo $< -o $@
 
 map2d.opt : map2d.cmx
-	$(OCAMLOPT) $(INCLUDES) str.cmxa unix.cmxa xml-light.cmxa $(LIBS:.cma=.cmxa) threads.cmxa gtkThread.cmx gtkInit.cmx $< -o $@
+	@echo OOL $@
+	$(Q)$(OCAMLOPT) $(INCLUDES) str.cmxa unix.cmxa xml-light.cmxa $(LIBS:.cma=.cmxa) threads.cmxa gtkThread.cmx gtkInit.cmx $< -o $@
 
 
 .SUFFIXES: .ml .mli .cmo .cmi .cmx
 
 %.cmo: %.ml
-	$(OCAMLC) $(INCLUDES) -labels -w s -c $<
+	@echo OC $<
+	$(Q)$(OCAMLC) $(INCLUDES) -labels -w s -c $<
 %.cmi: %.ml
-	$(OCAMLC) $(INCLUDES) -labels -w s -c $<
+	@echo OCI $<
+	$(Q)$(OCAMLC) $(INCLUDES) -labels -w s -c $<
 %.cmx: %.ml
-	$(OCAMLOPT) $(INCLUDES) -labels -w s -c $<
+	@echo OOC $<
+	$(Q)$(OCAMLOPT) $(INCLUDES) -labels -w s -c $<
 
 clean:
 	rm -f *~* *.cm* *.o *.out *.opt map2d
diff --git a/sw/ground_segment/multimon/Makefile b/sw/ground_segment/multimon/Makefile
index 1178ab5b84..3baa0c9143 100644
--- a/sw/ground_segment/multimon/Makefile
+++ b/sw/ground_segment/multimon/Makefile
@@ -1,3 +1,6 @@
+# Quiet compilation
+Q=@
+
 DEBUG		=n
 
 CFLAGS		=-Wall -Wstrict-prototypes -I/usr/X11R6/include
@@ -38,7 +41,8 @@ $(BINDIR)/%.o:	$(BINDIR)/%.s
 		$(AS) -c -o $@ $<
 
 $(BINDIR)/%.o:	%.c
-		$(CC) $(CFLAGS) -c -o $@ $<
+	@echo CC $<
+	$(Q)$(CC) $(CFLAGS) -c -o $@ $<
 
 SRC_L2		=hdlc.c pprz.c
 SRC_L1		= demod_afsk48p.c demod_display.c
@@ -56,7 +60,8 @@ $(BINDIR):
 		$(MKDIR) $(BINDIR)
 
 $(BINDIR)/multimon:	$(OBJ_L2) $(OBJ_L1) $(OBJ_MISC)
-			$(CC) $^ $(LDFLAGS) $(LDFLAGSX) -o $@
+	@echo LD $@
+	$(Q)$(CC) $^ $(LDFLAGS) $(LDFLAGSX) -o $@
 
 $(BINDIR)/gen:		$(OBJ_GEN)
 			$(CC) $^ $(LDFLAGS) -o $@
@@ -68,7 +73,8 @@ costabi.c costabf.c:	$(BINDIR)/mkcostab
 			$(BINDIR)/mkcostab
 
 multimon.cma : pprzlib.o demodml.o demod.ml
-	ocamlmklib -o multimon $^
+	@echo OLD $@
+	$(Q)$ocamlmklib -o multimon $^
 
 libtest: pprzlib.o demodml.c demod.ml test.ml
 	ocamlc -custom -o $@ pprzlib.o demodml.c -I +lablgtk2 unix.cma lablgtk.cma demod.ml test.ml
diff --git a/sw/ground_segment/tmtc/Makefile b/sw/ground_segment/tmtc/Makefile
index 1b652c3fe9..e7138def85 100644
--- a/sw/ground_segment/tmtc/Makefile
+++ b/sw/ground_segment/tmtc/Makefile
@@ -1,6 +1,6 @@
 # 
 #   $Id$
-#   Copyright (C) 2003 Pascal Brisset, Antoine Drouin
+#   Copyright (C) 2003-2006 Pascal Brisset, Antoine Drouin
 #
 # This file is part of paparazzi.
 #
@@ -20,11 +20,14 @@
 # Boston, MA 02111-1307, USA.  
 # 
 
+# Quiet
+Q=@
+
 include ../../../conf/Makefile.local
 CONF = ../../../conf
 VAR = ../../../var
 
-all: hw_modem_listen server messages wavecard $(VAR)/boa.conf maxstream
+all: link server messages $(VAR)/boa.conf
 # stereo_demod
 
 clean:
@@ -48,11 +51,13 @@ messages.opt : messages.ml
 	strip $@
 
 messages : messages.cmo
-	$(OCAMLC) -custom $(INCLUDES) -o $@ lablgtk.cma glibivy-ocaml.cma lib-pprz.cma gtkInit.cmo $^
+	@echo OL $@
+	$(Q)$(OCAMLC) -custom $(INCLUDES) -o $@ lablgtk.cma glibivy-ocaml.cma lib-pprz.cma gtkInit.cmo $^
 
 
 hw_modem_listen : modem.cmo hw_modem_listen.cmo
-	$(OCAMLC) -custom $(INCLUDES) -o $@ lablgtk.cma glibivy-ocaml.cma lib-pprz.cma $^
+	@echo OL $@
+	$(Q)$(OCAMLC) -custom $(INCLUDES) -o $@ lablgtk.cma glibivy-ocaml.cma lib-pprz.cma $^
 
 stereo_demod : ../multimon/multimon.cma stereo_demod.ml ../../lib/ocaml/lib-pprz.cma
 	$(OCAMLC) $(INCLUDES) -I ../multimon -o $@ lablgtk.cma glibivy-ocaml.cma lib-pprz.cma multimon.cma $^
@@ -61,29 +66,21 @@ stereo_demod : ../multimon/multimon.cma stereo_demod.ml ../../lib/ocaml/lib-pprz
 	@chmod a+x $@
 
 server : aircraft.cmo wind.cmo airprox.cmo server.cmo
-	$(OCAMLC) -custom $(INCLUDES) -o $@ lablgtk.cma glibivy-ocaml.cma lib-pprz.cma $^
+	@echo OL $@
+	$(Q)$(OCAMLC) -custom $(INCLUDES) -o $@ lablgtk.cma glibivy-ocaml.cma lib-pprz.cma $^
 
-wavecard : wavecard_connect.cmo
-	$(OCAMLC) -custom $(INCLUDES) -o $@ lablgtk.cma glibivy-ocaml.cma lib-pprz.cma $^
+link : modem.cmo link.cmo
+	@echo OL $@
+	$(Q)$(OCAMLC) -custom $(INCLUDES) -o $@ lablgtk.cma glibivy-ocaml.cma lib-pprz.cma $^
 
-maxstream : maxstream.cmo ../../lib/ocaml/lib-pprz.cma
-	$(OCAMLC) -custom $(INCLUDES) -o $@ lablgtk.cma glibivy-ocaml.cma lib-pprz.cma $<
 
-dl_setting : dl_setting.cmo
-	$(OCAMLC) -custom $(INCLUDES) -o $@ lablgtk.cma glibivy-ocaml.cma lib-pprz.cma gtkInit.cmo $^
+%.cmo : %.ml $(LIBPPRZCMA)
+	@echo OC $<
+	$(Q)$(OCAMLC) $(INCLUDES) -c $<
 
-modem.cmo : modem.cmi
-modem.cmi : $(LIBPPRZCMA)
-hw_modem_listen.cmo : $(LIBPPRZCMA)
-maxstream.cmo : $(LIBPPRZCMA)
-messages.cmo : $(LIBPPRZCMA)
-airprox.cmo server.cmo : $(LIBPPRZCMA)
-
-%.cmo : %.ml
-	$(OCAMLC) $(INCLUDES) -c $<
-
-%.cmi : %.mli
-	$(OCAMLC) $(INCLUDES) $<
+%.cmi : %.mli $(LIBPPRZCMA)
+	@echo OC $<
+	$(Q)$(OCAMLC) $(INCLUDES) $<
 
 airprox.cmi aircraft.cmo : aircraft.cmi
 airprox.cmo : airprox.cmi
diff --git a/sw/ground_segment/tmtc/hw_modem_listen.ml b/sw/ground_segment/tmtc/hw_modem_listen.ml
index 2003cfadb6..2485732f7d 100644
--- a/sw/ground_segment/tmtc/hw_modem_listen.ml
+++ b/sw/ground_segment/tmtc/hw_modem_listen.ml
@@ -85,7 +85,10 @@ let listen_pprz_modem = fun pprz_message_cb tty ->
   in
 
   (** Callback for available chars on the channel *)
-  let scanner = Serial.input (ModemTransport.parse use_modem_message) in
+  let scanner = 
+    match Serial.input (ModemTransport.parse use_modem_message) with
+      Serial.Closure f -> f
+  in
   let cb = fun _ ->
     begin
       try
diff --git a/sw/ground_segment/tmtc/link.ml b/sw/ground_segment/tmtc/link.ml
new file mode 100644
index 0000000000..c8f549e3dc
--- /dev/null
+++ b/sw/ground_segment/tmtc/link.ml
@@ -0,0 +1,198 @@
+(*
+ * $Id$
+ *
+ * Copyright (C) 2004 CENA/ENAC, Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA. 
+ *
+ *)
+
+open Latlong
+open Printf
+module W = Wavecard
+module Tm_Pprz = Pprz.Messages(struct let name = "telemetry" end)
+module Ground_Pprz = Pprz.Messages(struct let name = "ground" end)
+module Dl_Pprz = Pprz.Messages(struct let name = "datalink" end)
+module PprzTransport = Serial.Transport(Pprz.Transport)
+
+let ground_id = 0
+
+type transport =
+    Modem
+  | Pprz
+  | Wavecard
+  | Maxstream
+
+type modem = { fd : Unix.file_descr; transport : transport }
+
+(*let send_ack = fun delay fd () ->
+  ignore (GMain.Timeout.add delay (fun _ -> W.send fd (W.ACK, ""); false)) *)
+
+let sync = Char.chr 0xff
+let stx = Char.chr 0x02
+let etx = Char.chr 0x03
+let wc_received_frame =  Char.chr 0x30
+
+let use_tele_message = fun payload ->
+  Debug.call 'm' (fun f -> let buf = Serial.string_of_payload payload in fprintf f "mm receiving: "; for i = 0 to String.length buf - 1 do fprintf f "%x " (Char.code buf.[i]) done; fprintf f "\n");
+  let (msg_id, ac_id, values) = Tm_Pprz.values_of_payload payload in
+  let msg = Tm_Pprz.message_of_id msg_id in
+  Tm_Pprz.message_send (string_of_int ac_id) msg.Pprz.name values
+
+let parse = fun buf ->
+  PprzTransport.parse use_tele_message buf
+
+let send = fun ac payload ->
+  match ac.transport with
+    Pprz ->
+      let o = Unix.out_channel_of_descr ac.fd in
+      let buf = Pprz.Transport.packet payload in
+      Printf.fprintf o "%s" buf; flush o;
+      Debug.call 'm' (fun f -> fprintf f "mm sending: %s\n" (Debug.xprint buf));
+  | _ -> failwith "send: not yet"
+
+
+let cm_of_m = fun f -> Pprz.Int (truncate (100. *. f))
+
+(** Got a FLIGHT_PARAM message and dispatch a ACINFO *)
+let get_fp = fun ac _sender vs ->
+  let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
+  let f = fun a -> Pprz.float_assoc a vs in
+  let lat = (Deg>>Rad) (f "lat")
+  and long = (Deg>>Rad) (f "long")
+  and course = f "course"
+  and alt = f "alt"
+  and gspeed = f "speed" in
+  let utm = Latlong.utm_of WGS84 {posn_lat=lat; posn_long=long} in
+  let vs = ["ac_id", Pprz.Int ac_id;
+	    "utm_east", cm_of_m utm.utm_x;
+	    "utm_north", cm_of_m utm.utm_y;
+	    "course", Pprz.Int (truncate (10. *. course));
+	    "alt", cm_of_m alt;
+	    "speed", cm_of_m gspeed] in
+  let msg_id, _ = Dl_Pprz.message_of_name "ACINFO" in
+  let s = Dl_Pprz.payload_of_values msg_id ac_id vs in
+  send ac s
+
+(** Got a MOVE_WAYPOINT and send a MOVE_WP *)
+let move_wp = fun ac _sender vs ->
+  Debug.call 'm' (fun f -> fprintf f "mm MOVE WAYPOINT\n");
+  let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
+  let f = fun a -> Pprz.float_assoc a vs in
+  let lat = f "lat"
+  and long = f "long"
+  and alt = f "alt"
+  and wp_id = Pprz.int_assoc "wp_id" vs in
+  let wgs84 = {posn_lat=(Deg>>Rad)lat;posn_long=(Deg>>Rad)long} in
+  let utm = Latlong.utm_of WGS84 wgs84 in
+  let vs = ["wp_id", Pprz.Int wp_id;
+	    "utm_east", cm_of_m utm.utm_x;
+	    "utm_north", cm_of_m utm.utm_y;
+	    "alt", cm_of_m alt] in
+  let msg_id, _ = Dl_Pprz.message_of_name "MOVE_WP" in
+  let s = Dl_Pprz.payload_of_values msg_id ac_id vs in
+  send ac s
+
+(** Got a SEND_EVENT, and send an EVENT *)
+let send_event = fun ac _sender vs ->
+  let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
+  let ev_id = Pprz.int_assoc "event_id" vs in
+  let vs = ["event", Pprz.Int ev_id] in
+  let msg_id, _ = Dl_Pprz.message_of_name "EVENT" in
+  let s = Dl_Pprz.payload_of_values msg_id ac_id vs in
+  send ac s
+    
+
+(** Got a DL_SETTING, and send an SETTING *)
+let setting = fun ac _sender vs ->
+  let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
+  let idx = Pprz.int_assoc "index" vs in
+  let vs = ["event", Pprz.Int idx; "value", List.assoc "value" vs] in
+  let msg_id, _ = Dl_Pprz.message_of_name "SETTING" in
+  let s = Dl_Pprz.payload_of_values msg_id ac_id vs in
+  send ac s
+
+(** Got a JUMP_TO_BLOCK, and send an BLOCK *)
+let jump_block = fun ac _sender vs ->
+  Debug.call 'm' (fun f -> fprintf f "mm JUMP_TO_BLOCK\n");
+  let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
+  let block_id = Pprz.int_assoc "block_id" vs in
+  let vs = ["block_id", Pprz.Int block_id] in
+  let msg_id, _ = Dl_Pprz.message_of_name "BLOCK" in
+  let s = Dl_Pprz.payload_of_values msg_id ac_id vs in
+  send ac s
+
+(** Got a RAW_DATALINK message *)
+let raw_datalink = fun ac _sender vs ->
+  let m = Pprz.string_assoc "message" vs in
+  let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
+  for i = 0 to String.length m - 1 do
+    if m.[i] = ';' then m.[i] <- ' '
+  done;
+  let msg_id, vs = Dl_Pprz.values_of_string m in
+  let s = Dl_Pprz.payload_of_values msg_id ac_id vs in
+  send ac s
+    
+
+let _ =
+  let ivy_bus = ref "127.255.255.255:2010" in
+  let port = ref "/dev/ttyS0" in
+  let baurate = ref "9600" in
+  let transport = ref "pprz" in
+
+  let options =
+    [ "-b", Arg.Set_string ivy_bus, (sprintf "Ivy bus (%s)" !ivy_bus);
+      "-d", Arg.Set_string port, (sprintf "Port (%s)" !port);
+      "-transport", Arg.Set_string port, (sprintf "Transport (%s): modem,pprz,wavecard" !transport);
+      "-s", Arg.Set_string baurate, (sprintf "Baudrate (%s)" !baurate)] in
+  Arg.parse
+    options
+    (fun _x -> ())
+    "Usage: ";
+
+  Ivy.init "Link" "READY" (fun _ _ -> ());
+  Ivy.start !ivy_bus;
+  
+  try
+    let fd = Serial.opendev !port (Serial.speed_of_baudrate !baurate) in
+    assert(!transport="pprz");
+    let ac = { fd=fd; transport=Pprz} in
+
+    (* Listening *)
+    let buffered_input = 
+      match Serial.input (fun buf -> parse buf) with
+	Serial.Closure f -> f in
+    let cb = fun _ -> buffered_input fd; true in
+    ignore (Glib.Io.add_watch [`IN] cb (GMain.Io.channel_of_descr fd));
+
+    (** Listening on Ivy *)
+    ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" (get_fp ac));
+    ignore (Ground_Pprz.message_bind "MOVE_WAYPOINT" (move_wp ac));
+    ignore (Ground_Pprz.message_bind "SEND_EVENT" (send_event ac));
+    ignore (Ground_Pprz.message_bind "DL_SETTING" (setting ac));
+    ignore (Ground_Pprz.message_bind "JUMP_TO_BLOCK" (jump_block ac));
+    ignore (Ground_Pprz.message_bind "RAW_DATALINK" (raw_datalink ac));
+
+
+    (* Main Loop *)
+    let loop = Glib.Main.create true in
+    while Glib.Main.is_running loop do
+      ignore (Glib.Main.iteration true)
+    done
+  with
+    exn -> fprintf stderr "%s\n" (Printexc.to_string exn)
diff --git a/sw/ground_segment/tmtc/maxstream.ml b/sw/ground_segment/tmtc/maxstream.ml
index f3835fa6aa..17d5efc5c8 100644
--- a/sw/ground_segment/tmtc/maxstream.ml
+++ b/sw/ground_segment/tmtc/maxstream.ml
@@ -82,7 +82,9 @@ let maxstream_parse = fun buf ->
   buffer := String.sub b nb_used (String.length b - nb_used);
   String.length buf
 
-let maxstream_receive = Serial.input (fun b -> maxstream_parse b)
+let maxstream_receive = 
+  match Serial.input (fun b -> maxstream_parse b) with
+    Serial.Closure f -> f
 
 
 
diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
index 88dfae7b81..75df606457 100644
--- a/sw/ground_segment/tmtc/server.ml
+++ b/sw/ground_segment/tmtc/server.ml
@@ -156,7 +156,7 @@ let log_and_parse = fun logging ac_name a msg values ->
 	fprintf log "%.2f %s %s %s\n" t ac_name msg.Pprz.name s; flush log
     | None -> ()
   end;
-  let value = fun x -> try List.assoc x values with Not_found -> failwith (sprintf "Error: field '%s' not found\n" x) in
+  let value = fun x -> try Pprz.assoc x values with Not_found -> failwith (sprintf "Error: field '%s' not found\n" x) in
   let fvalue = fun x -> fvalue (value x)
   and ivalue = fun x -> ivalue (value x) in
     match msg.Pprz.name with
diff --git a/sw/lib/ocaml/Makefile b/sw/lib/ocaml/Makefile
index 4b99c9f8f2..ef476c63dd 100644
--- a/sw/lib/ocaml/Makefile
+++ b/sw/lib/ocaml/Makefile
@@ -20,6 +20,8 @@
 # Boston, MA 02111-1307, USA.  
 #
 
+Q=@
+
 INCLUDES=
 XINCLUDES= -I +lablgl -I +camlimages -I +lablgtk2
 OCAMLC=ocamlc $(INCLUDES)
@@ -41,49 +43,55 @@ opt : all lib-pprz.cmxa xlib-pprz.cmxa
 
 
 lib-pprz.cma : $(CMO)
-	ocamlmklib -o lib-pprz str.cma unix.cma xml-light.cma $^
+	@echo OL $@
+	$(Q)ocamlmklib -o lib-pprz str.cma unix.cma xml-light.cma $^
 
 lib-pprz.cmxa : $(CMX)
-	ocamlmklib -o lib-pprz $^
+	@echo OOL $@
+	$(Q)ocamlmklib -o lib-pprz $^
 
 xlib-pprz.cma : $(XCMO)
-	ocamlmklib -o xlib-pprz $^
+	@echo OL $@
+	$(Q)ocamlmklib -o xlib-pprz $^
 
 xlib-pprz.cmxa : $(XCMX)
-	ocamlmklib -o xlib-pprz $^
+	@echo OOL $@
+	$(Q)ocamlmklib -o xlib-pprz $^
 
 xml_get.out : lib-pprz.cma xml_get.cmo
-	$(OCAMLC) -o $@ str.cma xml-light.cma -I . $^
-
-ignutm.opt : latlong.cmx ignutm.ml
-	$(OCAMLOPT) -o $@ -I +camlimages ci_core.cmxa ci_png.cmxa xml-light.cmxa $^
-
-utm_of.opt : latlong.cmx utm_of.ml
-	$(OCAMLOPT) -o $@ $^
+	@echo OL $@
+	$(Q)$(OCAMLC) -o $@ str.cma xml-light.cma -I . $^
 
 GTKCFLAGS := -I /usr/lib/gtk-2.0/include -I/usr/include/gtk-2.0 -I/usr/include/atk-1.0 -I/usr/include/glib-2.0 -I/usr/lib/glib-2.0/include -I/usr/include/pango-1.0 -I /usr/include/cairo
 # GTKCFLAGS := $(shell gtk-config --cflags)
 
 %.o   : %.c
-	$(OCAMLC) -c $<
+	@echo OC $<
+	$(Q)$(OCAMLC) -c $<
 
 ml_gtk_drag.o : ml_gtk_drag.c
-	$(OCAMLC) -c -ccopt "$(GTKCFLAGS)" $<
+	@echo OC $<
+	$(Q)$(OCAMLC) -c -ccopt "$(GTKCFLAGS)" $<
 
 ml_gtkgl_hack.o   : ml_gtkgl_hack.c
-	$(OCAMLC) -c -ccopt "$(GTKCFLAGS)" $<
+	@echo OC $<
+	$(Q)$(OCAMLC) -c -ccopt "$(GTKCFLAGS)" $<
 
 %.cmo : %.ml
-	$(OCAMLC) -c $<
+	@echo OC $<
+	$(Q)$(OCAMLC) -c $<
 
 %.cmx : %.ml
-	$(OCAMLOPT) -c $<
+	@echo OOC $<
+	$(Q)$(OCAMLOPT) -c $<
 
 %.cmi : %.mli
-	$(OCAMLC) $<
+	@echo OC $<
+	$(Q)$(OCAMLC) $<
 
 %.cmi : %.ml
-	$(OCAMLC) $<
+	@echo OC $<
+	$(Q)$(OCAMLC) $<
 
 clean :
 	rm -f *~ *.cm* *.out *.opt .depend *.a *.o *.so
diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml
index 785c8bbd1b..e38d5cbbad 100644
--- a/sw/lib/ocaml/pprz.ml
+++ b/sw/lib/ocaml/pprz.ml
@@ -43,13 +43,14 @@ type field = {
   }
 
 type message = {
-    name : string;
+    name : string; (** Lowercase *)
     fields : (string * field) list
   }
 
 type type_descr = {
     format : string ;
     glib_type : string;
+    inttype : string;
     size : int;
     value : value
   }
@@ -75,14 +76,14 @@ external sprint_float : string -> int -> float -> unit = "c_sprint_float"
 external sprint_int32 : string -> int -> int32 -> unit = "c_sprint_int32"
 
 let types = [
-  ("uint8",  { format = "%u"; glib_type = "guint8";  size = 1; value=Int 42 });
-  ("uint16", { format = "%u";  glib_type = "guint16"; size = 2; value=Int 42 });
-  ("uint32", { format = "%lu" ;  glib_type = "guint32"; size = 4; value=Int 42 });
-  ("int8",   { format = "%d"; glib_type = "gint8";   size = 1; value= Int 42 });
-  ("int16",  { format = "%d";  glib_type = "gint16";  size = 2; value= Int 42 });
-  ("int32",  { format = "%ld" ;  glib_type = "gint32";  size = 4; value=Int 42 });
-  ("float",  { format = "%f" ;  glib_type = "gfloat";  size = 4; value=Float 4.2 });
-  ("string",  { format = "%s" ;  glib_type = "gchar*";  size = max_int; value=String "42" })
+  ("uint8",  { format = "%u"; glib_type = "guint8"; inttype = "uint8_t";  size = 1; value=Int 42 });
+  ("uint16", { format = "%u";  glib_type = "guint16"; inttype = "uint16_t"; size = 2; value=Int 42 });
+  ("uint32", { format = "%lu" ;  glib_type = "guint32"; inttype = "uint32_t"; size = 4; value=Int 42 });
+  ("int8",   { format = "%d"; glib_type = "gint8"; inttype = "int8_t";   size = 1; value= Int 42 });
+  ("int16",  { format = "%d";  glib_type = "gint16"; inttype = "int16_t";  size = 2; value= Int 42 });
+  ("int32",  { format = "%ld" ;  glib_type = "gint32"; inttype = "int32_t";  size = 4; value=Int 42 });
+  ("float",  { format = "%f" ;  glib_type = "gfloat"; inttype = "float";  size = 4; value=Float 4.2 });
+  ("string",  { format = "%s" ;  glib_type = "gchar*"; inttype = "char*";  size = max_int; value=String "42" })
 ]
 
 let is_array_type = fun s -> 
@@ -149,13 +150,13 @@ let field_of_xml = fun xml ->
   let t = ExtXml.attrib xml "type" in
   let t = if is_array_type t then ArrayType (type_of_array_type t) else Scalar t in
   let f = try Xml.attrib xml "format" with _ -> default_format t in
-  (ExtXml.attrib xml "name", { _type = t; fformat = f })
+  (String.lowercase (ExtXml.attrib xml "name"), { _type = t; fformat = f })
 
 let string_of_values = fun vs ->
   String.concat " " (List.map (fun (a,v) -> sprintf "%s=%s" a (string_of_value v)) vs)
 
 let assoc = fun a vs -> 
-  try List.assoc a vs with Not_found -> 
+  try List.assoc (String.lowercase a) vs with Not_found -> 
     failwith (sprintf "Attribute '%s' not found in '%s'" a (string_of_values vs))
 
 let float_assoc = fun (a:string) vs -> 
@@ -319,12 +320,11 @@ module Transport = struct
     m
 end
 
-
+let offset_ac_id = 0
+let offset_msg_id = 1
+let offset_fields = 2
 
 module Messages(Class:CLASS) = struct
-  let offset_ac_id = 0
-  let offset_msg_id = 1
-  let offset_fields = 2
   let max_length = 256
   let messages_by_id, messages_by_name = 
     try
@@ -392,7 +392,7 @@ module Messages(Class:CLASS) = struct
 	end
     | [] -> invalid_arg (sprintf "Pprz.values_of_string: %s" s)
 
-  let string_of_message = fun msg values ->
+  let string_of_message = fun ?(sep=" ") msg values ->
     (** Check that the values are compatible with this message *)
     List.iter 
       (fun (k, _) ->
@@ -400,7 +400,7 @@ module Messages(Class:CLASS) = struct
 	then invalid_arg (sprintf "Pprz.string_of_message: unknown field '%s' in message '%s'" k msg.name))
       values;
 
-    String.concat " "
+    String.concat sep
       (msg.name::
        List.map 
 	 (fun (field_name, field) ->
diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli
index eae7381bcc..8621ea9129 100644
--- a/sw/lib/ocaml/pprz.mli
+++ b/sw/lib/ocaml/pprz.mli
@@ -48,12 +48,16 @@ val string_of_value : value -> string
 type type_descr = {
     format : string ;
     glib_type : string;
+    inttype : string;
     size : int;
     value : value
   }
 val types : (string * type_descr) list
 type values  = (string * value) list
 
+val assoc : string -> values -> value
+(** Safe assoc taking into accound characters case *)
+
 val string_assoc : string -> values -> string
 (** May raise Not_found *)
 
@@ -68,6 +72,8 @@ found in class [class_name]. *)
 
 module Transport : Serial.PROTOCOL
 
+val offset_fields : int
+
 module type CLASS = sig val name : string end
 module Messages : functor (Class : CLASS) -> sig
   val message_of_id : message_id -> message
@@ -82,8 +88,8 @@ module Messages : functor (Class : CLASS) -> sig
   val values_of_string : string -> message_id * values
   (** May raise [(Unknown_msg_name msg_name)] *)
 
-  val string_of_message : message -> values -> string
-  (** [string_of_message msg values] *)
+  val string_of_message : ?sep:string -> message -> values -> string
+  (** [string_of_message ?sep msg values] Default [sep] is space *)
 
   val message_send : string -> string -> values -> unit
   (** [message_send sender msg_name values] *)
diff --git a/sw/lib/ocaml/serial.ml b/sw/lib/ocaml/serial.ml
index 70e7899a83..22065bd497 100644
--- a/sw/lib/ocaml/serial.ml
+++ b/sw/lib/ocaml/serial.ml
@@ -88,6 +88,7 @@ let opendev device speed =
 
 let close = Unix.close
 
+type 'a closure = Closure of 'a
 
 let buffer_len = 256
 let input = fun f ->
@@ -98,7 +99,7 @@ let input = fun f ->
     String.blit buffer start buffer 0 n;
     index := n in
 
-  fun fd ->
+  Closure (fun fd ->
     let n = !index + Unix.read fd buffer !index (buffer_len - !index) in
     Debug.call 'T' (fun f -> fprintf f "input: %d %d\n" !index n);
     let rec parse = fun start n -> 
@@ -109,7 +110,7 @@ let input = fun f ->
 	parse (start + nb_used) (n - nb_used)
       else
 	wait start n in
-    parse 0 n
+    parse 0 n)
 
 
 exception Not_enough
diff --git a/sw/lib/ocaml/serial.mli b/sw/lib/ocaml/serial.mli
index 4e6ba62119..cd05f5c3a0 100644
--- a/sw/lib/ocaml/serial.mli
+++ b/sw/lib/ocaml/serial.mli
@@ -24,6 +24,8 @@
  *
  *)
 
+type 'a closure = Closure of 'a
+
 type speed =
     B0
   | B50
@@ -50,10 +52,11 @@ val speed_of_baudrate : string -> speed
 val opendev : string -> speed -> Unix.file_descr
 val close : Unix.file_descr -> unit
 
-val input : (string -> int) -> Unix.file_descr -> unit
-(** [input f fd] Calls [f] on the buffer of available characters on [fd] each
-time a new character arrives. [f] must return the number of consumed
-characters *)
+val input : (string -> int) -> (Unix.file_descr -> unit) closure
+(** Buffered input. [input f] Returns a closure which must be called when
+characters are available on the stream. These characters are stored in a
+a buffer. [f] is then called on the buffer. [f] must return the number
+of consumed characters. *)
 
 type payload
 
diff --git a/sw/lib/ocaml/ubx.ml b/sw/lib/ocaml/ubx.ml
index e4d79ff2f3..96f9b01d71 100644
--- a/sw/lib/ocaml/ubx.ml
+++ b/sw/lib/ocaml/ubx.ml
@@ -3,7 +3,7 @@
  *
  * UBX protocol handling
  *
- * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec, Pascal Brisset, Antoine Drouin
+ * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin
  *
  * This file is part of paparazzi.
  *
@@ -23,41 +23,70 @@
  * Boston, MA 02111-1307, USA. 
  *
  *)
+
 module Protocol = struct
+  (** SYNC1 SYNC2 CLASS ID LENGTH(2) UBX_PAYLOAD CK_A CK_B
+     LENGTH is the lentgh of UBX_PAYLOAD
+     For us, the 'payload' includes also CLASS, ID and the LENGTH *)
+  let sync1 = Char.chr 0xb5
+  let sync2 = Char.chr 0x62
+  let offset_payload=2
+  let offset_length=4
   let index_start = fun buf ->
     let rec loop = fun i ->
-      let i' = String.index_from buf i (Char.chr 0xb5) in
-      if String.length buf > i'+1 && buf.[i'+1] = Char.chr 0x62 then
+      let i' = String.index_from buf i sync1 in
+      if String.length buf > i'+1 && buf.[i'+1] = sync2 then
 	i'
       else
 	loop (i'+1) in
     loop 0
 
   let payload_length = fun buf start ->
-    Char.code buf.[start+5] lsl 8 + Char.code buf.[start+4]
+    Char.code buf.[start+5] lsl 8 + Char.code buf.[start+4] + 4
 
   let length = fun buf start ->
     let len = String.length buf - start in
-    if len > 6 then
-      payload_length buf start + 8
+    if len >= offset_length+2 then
+      payload_length buf start + 4
     else
       raise Serial.Not_enough
 
-  let payload = fun buf start ->
-    String.sub buf (start+6) (payload_length buf start)
+  let payload = fun buf ->
+    Serial.payload_of_string (String.sub buf offset_payload (payload_length buf 0))
 
   let uint8_t = fun x -> x land 0xff
   let (+=) = fun r x -> r := uint8_t (!r + x)
-  let checksum = fun buf start payload ->
+  let compute_checksum = fun buf ->
     let ck_a = ref 0 and ck_b = ref 0 in
-    let l = String.length payload in
-    for i = 0 to l - 1 do
-      ck_a += Char.code payload.[i];
+    let l = String.length buf in
+    for i = offset_payload to l - 1 - 4 do
+      ck_a += Char.code buf.[i];
       ck_b += !ck_a
     done;
-    !ck_a = Char.code buf.[start+l+6] && !ck_b = Char.code buf.[start+l+7]
+    (!ck_a, !ck_b)
+
+  let checksum = fun buf->
+    let (ck_a, ck_b) = compute_checksum buf in
+    let l = payload_length buf 0 in
+    ck_a = Char.code buf.[offset_payload+l+1] && ck_b = Char.code buf.[offset_payload+l+2]
+
+  let packet = fun payload ->
+    let payload = Serial.string_of_payload payload in
+    let n = String.length payload in
+    let msg_length = n + 4 in
+    let m = String.create msg_length in
+    m.[0] <- sync1;
+    m.[1] <- sync2;
+    String.blit payload 0 m 2 n;
+    let (ck_a, ck_b) = compute_checksum m in
+    m.[msg_length-2] <- Char.chr ck_a;
+    m.[msg_length-1] <- Char.chr ck_b;
+    m
 end
 
+type class_id = int
+type msg_id = int
+
 let (//) = Filename.concat
 
 let ubx_xml =
@@ -85,11 +114,6 @@ let nav_velned () = ubx_nav_id (), ubx_get_nav_msg "VELNED"
 let usr_irsim () = ubx_usr_id (), ubx_get_usr_msg "IRSIM"
 
 
-let send_start_sequence = fun gps ->
-  output_byte gps 0xB5;
-  output_byte gps 0x62
-
-
 let sizeof = function
     "U4" | "I4" -> 4
   | "U2" | "I2" -> 2
@@ -108,7 +132,9 @@ let assoc = fun label fields ->
 
 let byte = fun x -> Char.chr (x land 0xff)
 
-let make_payload = fun msg_xml values ->
+type message_spec = Xml.xml
+
+let ubx_payload = fun msg_xml values ->
   let n = int_of_string (ExtXml.attrib msg_xml "length") in
   let p = String.make n '#' in
   let fields = Xml.children msg_xml in
@@ -124,6 +150,9 @@ let make_payload = fun msg_xml values ->
       |	"U1" ->
 	  assert(value >= 0 && value < 0x100);
 	  p.[pos] <- byte value
+      |	"I1" ->
+	  assert(value >= -0x80 && value <= 0x80);
+	  p.[pos] <- byte value
       |	"I4" | "U4" ->
 	  assert(fmt <> "U4" || value >= 0);
 	  p.[pos+3] <- byte (value asr 24);
@@ -137,29 +166,25 @@ let make_payload = fun msg_xml values ->
     )
     values;
   p
+
+let message = fun class_name msg_name ->
+  let _class = ubx_get_class class_name in
+  let class_id = int_of_string (ExtXml.attrib _class "ID") in
+  let msg = ubx_get_msg _class msg_name in
+  let msg_id = int_of_string (ExtXml.attrib msg "ID") in
+  class_id, msg_id, msg
   
   
+let payload = fun class_name msg_name values ->
+  let class_id, msg_id, msg = message class_name msg_name in
+  let u_payload = ubx_payload msg values in
+  let n = String.length u_payload in
 
-
-
-let send = fun gps (msg_class, msg) values ->
-  let msg_id = int_of_string (Xml.attrib msg "ID") in
-  let payload = make_payload msg values in
-  let n = String.length payload in
-  send_start_sequence gps;
-
-  let ck_a = ref 0 and ck_b = ref 0 in
-  let output_byte_ck = fun c -> 
-    ck_a := (!ck_a+c) land 0xff; ck_b := (!ck_b+ !ck_a) land 0xff;
-    output_byte gps c in
-
-  output_byte_ck msg_class;
-  output_byte_ck msg_id;
-  output_byte_ck (n land 0xff);
-  output_byte_ck ((n land 0xff00) lsr 8);
-  String.iter (fun c -> output_byte_ck (Char.code c)) payload;
-  output_byte gps !ck_a;
-  output_byte gps !ck_b;
-  flush gps
-
-
+  (** Just add CLASS_ID, MSG_ID and LENGTH(2) to the ubx payload *)
+  let m = String.create (n+4) in
+  m.[0] <- Char.chr class_id;
+  m.[1] <- Char.chr msg_id;
+  m.[2] <- Char.chr (n land 0xff);
+  m.[3] <- Char.chr ((n land 0xff00) lsr 8);
+  String.blit u_payload 0 m 4 n;
+  Serial.payload_of_string m
diff --git a/sw/lib/ocaml/ubx.mli b/sw/lib/ocaml/ubx.mli
index dda5e8413a..3cd0029857 100644
--- a/sw/lib/ocaml/ubx.mli
+++ b/sw/lib/ocaml/ubx.mli
@@ -3,7 +3,7 @@
  *
  * UBX protocol handling
  *
- * Copyright (C) 2004 CENA/ENAC, Yann Le Fablec, Pascal Brisset, Antoine Drouin
+ * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin
  *
  * This file is part of paparazzi.
  *
@@ -24,18 +24,13 @@
  *
  *)
 
-module Protocol :
-  sig
-    val index_start : string -> int
-    val payload_length : string -> int -> int
-    val length : string -> int -> int
-    val payload : string -> int -> string
-    val uint8_t : int -> int
-    val checksum : string -> int -> string -> bool
-  end
+module Protocol :  Serial.PROTOCOL
 
-val nav_posutm : unit -> int * Xml.xml
-val nav_status : unit -> int * Xml.xml
-val nav_velned : unit -> int * Xml.xml
-val usr_irsim : unit -> int * Xml.xml
-val send : out_channel -> int * Xml.xml -> (string * int) list -> unit
+type message_spec
+
+type class_id = int
+type msg_id = int
+
+val message : string -> string -> class_id*msg_id*message_spec
+val ubx_payload : message_spec -> (string * int) list -> string
+val payload : string -> string -> (string * int) list -> Serial.payload
diff --git a/sw/lib/ocaml/wavecard.ml b/sw/lib/ocaml/wavecard.ml
index 6cba210102..313a6527ce 100644
--- a/sw/lib/ocaml/wavecard.ml
+++ b/sw/lib/ocaml/wavecard.ml
@@ -172,7 +172,8 @@ let parse = fun buf ?ack f ->
     
 
 let receive = fun ?ack f ->
-  Serial.input (fun b -> parse b ?ack f)
+  match Serial.input (fun b -> parse b ?ack f) with
+    Serial.Closure f -> f
 
 let send = fun fd (cmd, data) ->
   let l = String.length data + 4 in
diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile
index 7769e50c7d..286ea03631 100644
--- a/sw/simulator/Makefile
+++ b/sw/simulator/Makefile
@@ -1,7 +1,5 @@
 # Paparazzi simulator $Id$
 #  
-# Copied from autopilot (autopilot.sf.net) thanx alot Trammell
-#
 # Copyright (C) 2003-2006  Pascal Brisset, Antoine Drouin
 #
 # This file is part of paparazzi.
@@ -21,6 +19,9 @@
 # the Free Software Foundation, 59 Temple Place - Suite 330,
 # Boston, MA 02111-1307, USA. 
 
+# Quiet compilation
+Q=@
+
 include ../../conf/Makefile.local
 
 SIMML = stdlib.ml data.ml flightModel.ml gps.ml
@@ -39,31 +40,38 @@ AIRBORNE = ../airborne
 VARINCLUDE=$(PAPARAZZI_HOME)/var/include
 ACINCLUDE = $(PAPARAZZI_HOME)/var/$(AIRCRAFT)
 
-all : gaia sitl.cma fg.o
+all : gaia sitl.cma simhitl
 
-simhitl.out : $(SIMHCMO) simhitl.cmo
-	$(OCAMLC) $(INCLUDES) -o $@ str.cma xml-light.cma unix.cma lib.cma lablgtk.cma gtkInit.cmo $^
+simhitl : fg.o $(SIMHCMO) simhitl.cmo
+	@echo OL $@
+	$(Q)$(OCAMLC) -custom $(INCLUDES) -o $@ str.cma xml-light.cma unix.cma glibivy-ocaml.cma lib-pprz.cma lablgtk.cma gtkInit.cmo $^
 
 sitl.cma : fg.o $(SIMSCMO)
-	ocamlmklib -o sitl $^ 
+	@echo OL $@
+	$(Q)ocamlmklib -o sitl $^ 
 
 sitl.cmxa : $(SIMSCMX)
 	ocamlopt -o $@ -a $^ 
 
 gaia : gaia.cmo
-	$(OCAMLC) -custom $(INCLUDES) -o $@ glibivy-ocaml.cma lib-pprz.cma lablgtk.cma gtkInit.cmo $<
+	@echo OL $@
+	$(Q)$(OCAMLC) -custom $(INCLUDES) -o $@ glibivy-ocaml.cma lib-pprz.cma lablgtk.cma gtkInit.cmo $<
 
-%.cmo : %.ml
-	$(OCAMLC) $(INCLUDES) -c $<
+%.cmo : %.ml ../lib/ocaml/lib-pprz.cma
+	@echo OC $<
+	$(Q)$(OCAMLC) $(INCLUDES) -c $<
 
 %.o : %.c
-	$(OCAMLC) -c $<
+	@echo OC $<
+	$(Q)$(OCAMLC) -c $<
 
 %.cmx : %.ml
-	$(OCAMLOPT) $(INCLUDES) -c $<
+	@echo OOC $<
+	$(Q)$(OCAMLOPT) $(INCLUDES) -c $<
 
 %.cmi : %.mli
-	$(OCAMLC) $(INCLUDES) -c $<
+	@echo OC $<
+	$(Q)$(OCAMLC) $(INCLUDES) -c $<
 
 clean :
 	\rm -f *.cm* *~ *.out .depend *.o *.a *.so gaia
diff --git a/sw/simulator/data.ml b/sw/simulator/data.ml
index 7641ff10a4..9c8aa86b20 100644
--- a/sw/simulator/data.ml
+++ b/sw/simulator/data.ml
@@ -54,7 +54,7 @@ type aircraft = {
 let aircraft = fun name ->
   let aircraft_xml, id =
     let rec loop = function
-	[] -> failwith ("Aicraft not found : "^name)
+	[] -> failwith ("Aircraft not found : "^name)
       | x::_ when Xml.tag x = "aircraft" && Xml.attrib x "name" = name ->
 	  begin 
 	    try
diff --git a/sw/simulator/hitl.ml b/sw/simulator/hitl.ml
index b13966939e..439fe6a430 100644
--- a/sw/simulator/hitl.ml
+++ b/sw/simulator/hitl.ml
@@ -24,127 +24,76 @@
  *
  *)
 
-
+open Printf
 open Stdlib
-open Latlong
+module LL = Latlong
 
-let get_port = fun n ->
-  Xml.attrib (ExtXml.child ~select:(fun x -> Xml.attrib x "name"=n) Data.ground "link") "port"
-
-let tty0 = ref (get_port "ap")
-let tty1 = ref (get_port "fbw")
-
-let uart_mcu0 = ref Unix.stdout
-let uart_mcu1 = ref Unix.stdin
-
-let open_mcu tty = Serial.opendev tty Serial.B38400
+module TelePprz = Pprz.Messages(struct let name = "telemetry" end)
+module DatalinkPprz = Pprz.Messages(struct let name = "datalink" end)
+module GroundPprz = Pprz.Messages(struct let name = "ground" end)
 
 module Make(A:Data.MISSION) = struct
+  let my_id = ref (-1)
 
-  let init = fun (_:int) (_:GPack.box) ->
-    if !tty0 <> "" then uart_mcu0 := open_mcu !tty0;
-    if !tty1 <> "" then uart_mcu1 := open_mcu !tty1
+  let init = fun id (_:GPack.box) ->
+    my_id := id
 
   let boot = fun time_scale -> ()
 
-    let irs =
-      try
-	ExtXml.child A.ac.Data.airframe
-	  ~select:(fun x -> try Xml.attrib x "prefix" = "IR_" with Xml.No_attribute _ -> false)
-	  "section"
-      with Not_found -> 
-	failwith "Do not find an IR section in airframe description"
-
-    let ir_roll_neutral =
-      try
-	int_of_string (ExtXml.attrib (ExtXml.child irs ~select:(fun x -> try Xml.attrib x "name" = "ADC_ROLL_NEUTRAL" with Xml.No_attribute _ -> false) "define") "value")
-      with
-      Not_found ->
-	failwith "Do not find an ROLL_NEUTRAL_DEFAULT define in IR description" 
-    
-    let ir_pitch_neutral =
-      try
-	int_of_string (ExtXml.attrib (ExtXml.child irs ~select:(fun x -> try Xml.attrib x "name" = "ADC_PITCH_NEUTRAL" with Xml.No_attribute _ -> false) "define") "value")
-      with
-	Not_found ->
-	  failwith "Do not find an PITCH_NEUTRAL_DEFAULT define in IR description"
-	    
-
-
-
   let scale = fun value s -> truncate (value *. s)
 
   open Gps
 
-  let nav_posutm = Ubx.nav_posutm ()
-  let nav_status = Ubx.nav_status ()
-  let nav_velned = Ubx.nav_velned ()
-  let usr_irsim = Ubx.usr_irsim ()
+  let array_of_string = fun s ->
+    Array.init (String.length s) (fun i -> Pprz.Int (Char.code s.[i]))
+
+  let send_raw = fun m ->
+    let vs = ["ac_id",Pprz.Int !my_id; "message", Pprz.String m] in
+    GroundPprz.message_send "hitl" "RAW_DATALINK" vs
+
+  let datalink_message = fun msg_name vs ->
+    let (mid, m) = DatalinkPprz.message_of_name msg_name in
+    DatalinkPprz.string_of_message ~sep:";" m vs
+
+  let send_ubx_message = fun class_name msg_name vs ->
+    let class_id, msg_id, msg_spec = Ubx.message class_name msg_name in
+    let ubx_payload = Ubx.ubx_payload msg_spec vs in
+    Debug.call 'h' (fun f -> fprintf f "ubx_payload: %d:%s\n" msg_id (Debug.xprint ubx_payload));
+    let a = array_of_string ubx_payload in
+    let s = Pprz.string_of_value (Pprz.Array a) in
+    let vs = ["class", Pprz.Int class_id;
+	      "id", Pprz.Int msg_id;
+	      "ubx_payload", Pprz.String s] in
+    let m = datalink_message "HITL_UBX" vs in
+    send_raw m
+	
 
   let gps = fun gps ->
-    let uart = Unix.out_channel_of_descr !uart_mcu0 in
-    let utm = utm_of WGS84 gps.wgs84 in
-    Ubx.send uart nav_posutm
-      ["EAST", scale utm.utm_x 1e2;
-       "NORTH", scale utm.utm_y 1e2;
-       "ALT", scale gps.alt 1e2];
-    Ubx.send uart nav_status ["GPSfix", 3];
-    Ubx.send uart nav_velned
-      ["ITOW",scale gps.time 1e3;
-       "VEL_D", -scale gps.climb 1e2;
-       "GSpeed", scale gps.gspeed 1e2;
-       "Heading", scale (deg_of_rad gps.course) 1e5]
-
-
-
-  let infrared = fun ir_left ir_front ->
-    let uart = Unix.out_channel_of_descr !uart_mcu0 in
-    let ir_left = ir_left + ir_roll_neutral
-    and ir_front = ir_front + ir_pitch_neutral in
-    Ubx.send uart usr_irsim
-      ["ROLL", ir_left;
-       "PITCH", ir_front]
-
-  let size_servos_buf = 256
-  let zero = '\000'
-
-  let get_2bytes = fun buf i ->
-    (Char.code buf.[i] lsl 8) lor (Char.code buf.[i+1])
-
-(* nb_servos 2 bytes values, prefixed by 00 ended by \n
-   Returns optionaly a function associating the read value to the index *)
-  let clock = 16
-  let read_commands = fun servos ->
-    let servos_buf = String.create size_servos_buf
-    and buf_idx = ref 0 in
-    let nb_servos = Array.length servos in
-    let tty = Unix.in_channel_of_descr !uart_mcu1 in
-
-    fun () ->
-      let n = input tty servos_buf !buf_idx (size_servos_buf- !buf_idx) in
-      let rec parse00 = fun i m ->
-	if m >= 2+2*nb_servos+1 then 
-	  (if servos_buf.[i] = zero then parse0 else parse00) (i+1) (m-1)
-	else (* Not enough chars : wait *)
-	  i
-
-      and parse0 = fun i m ->
-	if servos_buf.[i] = zero && servos_buf.[i+2*nb_servos+1] = '\n' then begin
-	  for s = 0 to nb_servos - 1 do
-	    servos.(s) <- get_2bytes servos_buf (i+1+2*s) / clock
-	  done;
-	  i+1+2*nb_servos+1
-	end else (* 0 or \n missing *)
-	  parse00 i m in
-
-      let nb_available_chars = (!buf_idx + n) in
-      let nb_read_chars = parse00 0 nb_available_chars in
-      let rest = nb_available_chars - nb_read_chars in
-      String.blit servos_buf nb_read_chars servos_buf 0 rest;
-      buf_idx := rest
+    let utm = LL.utm_of LL.WGS84 gps.wgs84 in
 
+    send_ubx_message "NAV" "POSUTM" ["EAST", scale utm.LL.utm_x 1e2;
+				     "NORTH", scale utm.LL.utm_y 1e2;
+				     "ALT", scale gps.alt 1e2;
+				     "ZONE", utm.LL.utm_zone];
+    send_ubx_message  "NAV" "STATUS" ["GPSfix", 3];
+    send_ubx_message  "NAV" "VELNED" ["ITOW",scale gps.time 1e3;
+				      "VEL_D", -scale gps.climb 1e2;
+				      "GSpeed", scale gps.gspeed 1e2;
+				      "Heading", scale (deg_of_rad gps.course) 1e5]
+	
+  let infrared = fun ir_left ir_front ir_top ->
+    let m =
+      datalink_message "HITL_INFRARED"
+	["roll", Pprz.Int (truncate ir_left);
+	 "pitch", Pprz.Int (truncate ir_front)] in
+    send_raw m
 
+  let sep_reg = Str.regexp Pprz.separator
+  let read_commands = fun commands _sender values ->
+    let s = Pprz.string_assoc "values" values in
+    let vs = Array.of_list (List.map int_of_string (Str.split sep_reg s)) in
+    Array.blit vs 0 commands 0 (Array.length vs)
 
   let commands = fun commands ->
-    ignore (GMain.Io.add_watch [`IN] (fun _ -> read_commands commands (); true) (GMain.Io.channel_of_descr !uart_mcu1))
+    ignore (TelePprz.message_bind "COMMANDS" (read_commands commands))
 end
diff --git a/sw/simulator/hitl.mli b/sw/simulator/hitl.mli
index 5ac1b8d776..2d4c9a3c45 100644
--- a/sw/simulator/hitl.mli
+++ b/sw/simulator/hitl.mli
@@ -1,5 +1,30 @@
-val tty0 : string ref
-val tty1 : string ref
+(*
+ *  $Id$
+ *
+ * Hardware In The Loop
+ *  
+ * Copyright (C) 2004 Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA. 
+ *
+ *)
 
+(** Listen commands ("COMMANDS" telemetry message) on Ivy. Sends GPS and
+   infrared as ground messages (raw HITL datalink messages) *)
 
 module Make : functor (A : Data.MISSION) -> Sim.AIRCRAFT 
diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml
index 0fbc0066f4..de04497f89 100644
--- a/sw/simulator/sim.ml
+++ b/sw/simulator/sim.ml
@@ -40,6 +40,7 @@ let fg_period = 1./.25.
 module type AIRCRAFT = 
   sig
     val init : int -> GPack.box -> unit
+    (** [init ac_id box] *)
     val boot : Stdlib.value -> unit
     (** [boot time_acceleration] *)
 
@@ -60,7 +61,7 @@ external fg_sizeof : unit -> int = "fg_sizeof"
 external fg_msg : string -> float -> float -> float -> float -> float -> float -> unit = "fg_msg_bytecode" "fg_msg_native"
 
 
-let ac_name = ref ""
+let ac_name = ref "A/C not set"
 
 let ivy_bus = ref "127.255.255.255:2010"
 
diff --git a/sw/simulator/simhitl.ml b/sw/simulator/simhitl.ml
index e50d053882..41270af723 100644
--- a/sw/simulator/simhitl.ml
+++ b/sw/simulator/simhitl.ml
@@ -2,9 +2,7 @@ open Stdlib
 
 let _ = 
   Arg.parse
-    (Sim.common_options@[set_string "-aircraft" Sim.ac_name "aircraft name";
-			 set_string "-fbw" Hitl.tty1 "Fly by wire MCU port";
-		      set_string "-ap" Hitl.tty0 "Autopilot MCU port"])
+    (Sim.common_options@[set_string "-a" Sim.ac_name "aircraft name"])
     (fun x -> Printf.fprintf stderr "Warning: Don't do anythig with %s\n" x)
     "Usage: ";
 
diff --git a/sw/tools/Makefile b/sw/tools/Makefile
index 76b408d629..3ccecc7485 100644
--- a/sw/tools/Makefile
+++ b/sw/tools/Makefile
@@ -1,15 +1,40 @@
+# $Id$
+#  
+# Copyright (C) 2003-2006  Pascal Brisset, Antoine Drouin
+#
+# This file is part of paparazzi.
+#
+# paparazzi is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2, or (at your option)
+# any later version.
+#
+# paparazzi is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with paparazzi; see the file COPYING.  If not, write to
+# the Free Software Foundation, 59 Temple Place - Suite 330,
+# Boston, MA 02111-1307, USA. 
+
+# Quiet compilation
+Q=@
+
 OCAML=ocaml
 OCAMLC=ocamlc -I ../lib/ocaml
 OCAMLLEX=ocamllex
 OCAMLYACC=ocamlyacc
 
-all: gen_aircraft.out gen_airframe.out gen_calib.out gen_messages.out gen_ubx.out gen_flight_plan.out gen_radio.out gen_dl.out extract_makefile.out gen_control.out gen_periodic.out
+all: gen_aircraft.out gen_airframe.out gen_calib.out gen_messages.out gen_ubx.out gen_flight_plan.out gen_radio.out extract_makefile.out gen_control.out gen_periodic.out
 
 FP_CMO = fp_syntax.cmo fp_parser.cmo fp_lexer.cmo fp_proc.cmo gen_flight_plan.ml
 ABS_FP = $(FP_CMO:%=$$PAPARAZZI_SRC/sw/tools/%)
 
 gen_flight_plan.out : $(FP_CMO) ../lib/ocaml/lib-pprz.cma
-	$(OCAMLC) -custom -o $@ ivy-ocaml.cma lib-pprz.cma $^
+	@echo OL $@
+	$(Q)$(OCAMLC) -custom -o $@ ivy-ocaml.cma lib-pprz.cma $^
 
 fp_parser.cmo : fp_parser.cmi fp_syntax.cmi
 fp_parser.cmi : fp_parser.ml fp_syntax.cmi
@@ -20,25 +45,31 @@ fp_syntax.cmo : fp_syntax.cmi
 
 
 %.out : %.ml
-	$(OCAMLC) -o $@ ivy-ocaml.cma lib-pprz.cma $<
+	@echo OC $<
+	$(Q)$(OCAMLC) -o $@ ivy-ocaml.cma lib-pprz.cma $<
 	@cat ../../pprz_src_test.sh > $@
 	@echo '$(OCAML) -I $$PAPARAZZI_SRC/sw/lib/ocaml ivy-ocaml.cma lib-pprz.cma $$PAPARAZZI_SRC/sw/tools/$< $$*' >> $@
 	@chmod a+x $@
 
 %.cmo : %.ml
-	$(OCAMLC) -c $<
+	@echo OC $<
+	$(Q)$(OCAMLC) -c $<
 
 %.cmi : %.mli
-	$(OCAMLC) -c $<
+	@echo OC $<
+	$(Q)$(OCAMLC) -c $<
 
 %.ml : %.mll
-	$(OCAMLLEX) $<
+	@echo OC $<
+	$(Q)$(OCAMLLEX) $<
 
 %.ml : %.mly
-	$(OCAMLYACC) $<
+	@echo OC $<
+	$(Q)$(OCAMLYACC) $<
 
 %.mli : %.mly
-	$(OCAMLYACC) $<
+	@echo OC $<
+	$(Q)$(OCAMLYACC) $<
 
 clean:
 	rm -f *.cm* *.out *~ fp_parser.ml fp_parser.mli
diff --git a/sw/tools/gen_dl.ml b/sw/tools/gen_dl.ml
index 173cfc2810..d22a0133a4 100644
--- a/sw/tools/gen_dl.ml
+++ b/sw/tools/gen_dl.ml
@@ -47,7 +47,7 @@ let get_at = fun offset format ->
     | "int8" -> "int8_t"
     | "float" -> "float"
     | _ -> failwith (sprintf "get_at: unknown format '%s'" format) in
-  sprintf "(*((%s*)(_ubx_payload+%d)))" t offset
+  sprintf "(*((%s*)(_payload+%d)))" t offset
 
 let define = fun x y ->
   fprintf out "#define %s %s\n" x y
@@ -62,18 +62,16 @@ let parse_message = fun m ->
   define (sprintf "DL_%s_ID" msg_name) (Xml.attrib m "ID");
   
   let offset = ref 2 in (** message id + sender id *)
-  let rec parse_field = fun f ->
-    match Xml.tag f with
-      "field" ->
-	let field_name = Xml.attrib f "name"
-	and format = Xml.attrib f "type" in
-	define (sprintf "DL_%s_%s(_ubx_payload)" msg_name field_name) (get_at !offset format);
-	offset += sizeof format
-    | x -> failwith ("Unexpected field: " ^ x)
-    in
 
-    List.iter parse_field (Xml.children m)
-  
+  let rec parse_field = fun f ->
+    let field_name = Xml.attrib f "name"
+    and format = Xml.attrib f "type" in
+    define (sprintf "DL_%s_%s(_payload)" msg_name field_name) (get_at !offset format);
+    offset += sizeof format
+  in
+
+  List.iter parse_field (Xml.children m)
+    
 
 let _ =
   if Array.length Sys.argv <> 2 then begin
diff --git a/sw/tools/gen_messages.ml b/sw/tools/gen_messages.ml
index 67a85b7842..f5521ff7bf 100644
--- a/sw/tools/gen_messages.ml
+++ b/sw/tools/gen_messages.ml
@@ -24,6 +24,8 @@
  *
  *)
 
+(** FIXME: Should use the Pprz Module !!! *)
+
 open Printf
 
 
@@ -116,10 +118,12 @@ module Syntax = struct
       try Xml.parse_file filename with
 	Xml.Error (msg, pos) -> fprintf stderr "%s:%d : %s\n" filename (Xml.line pos) (Xml.error_msg msg); exit 1
     in
-    match List.filter (fun x -> assert(Xml.tag x="class"); Xml.attrib x "name" = class_) (Xml.children xml) with
-      [xml_class] -> List.map of_xml (Xml.children xml_class)
-    | [] -> failwith (sprintf "No class '%s' found" class_)
-    | _ -> failwith (sprintf "Several class '%s' found" class_)
+    try
+      let xml_class =
+	List.find (fun x -> Xml.attrib x "name" = class_) (Xml.children xml) in
+      List.map of_xml (Xml.children xml_class)
+    with
+      Not_found -> failwith (sprintf "No class '%s' found" class_)
 end
 
 module Gen_onboard = struct
@@ -197,6 +201,29 @@ module Gen_onboard = struct
   let print_null_avr_macros = fun avr_h messages ->
     List.iter (print_null_avr_macro avr_h) messages
 
+  let print_get_macros = fun avr_h message ->
+    let msg_name = message.name in
+    let offset = ref Pprz.offset_fields in
+    
+    let parse_field = fun (_type, field_name, _format) ->
+      if !offset < 0 then
+	failwith "FIXME: No field allowed after an array field (print_gen_macro)x";
+      let typed = fun o t -> 
+	sprintf "(%s*)(_payload+%d)" (assoc_types t).Pprz.inttype o in
+      match _type with 
+	Basic t ->
+	  fprintf avr_h "#define DL_%s_%s(_payload) (*%s)\n" msg_name field_name (typed !offset t);
+	  offset := !offset + int_of_string (sizeof _type)
+      | Array (t, varname) ->
+	  fprintf avr_h "#define DL_%s_%s_length(_payload) (*%s)\n" msg_name field_name (typed !offset "uint8");
+	  incr offset;
+	  fprintf avr_h "#define DL_%s_%s(_payload) %s\n" msg_name field_name (typed !offset t);
+	  offset := -1 (** Mark for no more fields *)
+    in
+    
+    fprintf avr_h "\n";
+    List.iter parse_field message.fields
+
 end
 
 let _ =
@@ -209,10 +236,22 @@ let _ =
   let messages = Syntax.read filename class_name in
 
   let avr_h = stdout in
+
   Printf.fprintf avr_h "/* Automatically generated from %s */\n" filename;
   Printf.fprintf avr_h "/* Please DO NOT EDIT */\n";
-  Printf.fprintf avr_h "#ifdef DOWNLINK\n";
+
+  Printf.fprintf avr_h "/* Macros to send and receive messages of class %s */\n" class_name;
+
+  (** Macros for airborne downlink (sending) *)
+  if class_name = "telemetry" then (** FIXME *)
+    Printf.fprintf avr_h "#ifdef DOWNLINK\n";
   Gen_onboard.print_avr_macros filename avr_h messages;
-  Printf.fprintf avr_h "#else // DOWNLINK\n";
-  Gen_onboard.print_null_avr_macros avr_h messages;
-  Printf.fprintf avr_h "#endif // DOWNLINK\n"
+  if class_name = "telemetry" then begin
+    Printf.fprintf avr_h "#else // DOWNLINK\n";
+    Gen_onboard.print_null_avr_macros avr_h messages;
+    Printf.fprintf avr_h "#endif // DOWNLINK\n"
+  end;
+
+  (** Macros for airborne datalink (receiving) *)
+  List.iter (Gen_onboard.print_get_macros avr_h) messages
+