diff --git a/Makefile b/Makefile
index 18679bbfcc..eeca7ececf 100644
--- a/Makefile
+++ b/Makefile
@@ -49,9 +49,9 @@ test: static ac1 ac2
 	PAPARAZZI_HOME=$(PAPARAZZI_SRC) PAPARAZZI_SRC=$(PAPARAZZI_SRC) $(SUPERVISION)
 
 ac1 : conf sim_static
-	make AIRCRAFT=Twin1 PAPARAZZI_HOME=$(PAPARAZZI_SRC) sim_ac
+	make AIRCRAFT=Twin1 PAPARAZZI_HOME=$(PAPARAZZI_SRC) sim
 ac2 : conf sim_static
-	make AIRCRAFT=Twin2 PAPARAZZI_HOME=$(PAPARAZZI_SRC) sim_ac
+	make AIRCRAFT=Twin2 PAPARAZZI_HOME=$(PAPARAZZI_SRC) sim
 
 lib:
 	cd $(LIB)/ocaml; $(MAKE)
@@ -69,9 +69,6 @@ configurator: lib
 sim_static :
 	cd $(SIMULATOR); $(MAKE) PAPARAZZI_SRC=$(PAPARAZZI_SRC)
 
-sim_sitl :
-	cd $(SIMULATOR);  $(MAKE) sim_sitl PAPARAZZI_HOME=$(PAPARAZZI_SRC) PAPARAZZI_SRC=$(PAPARAZZI_SRC) AIRCRAFT=$(AIRCRAFT)
-
 fbw fly_by_wire: ac_h
 	cd $(AIRBORNE); $(MAKE) TARGET=fbw all
 
@@ -136,7 +133,6 @@ static_h :
 ac_h : tools static_h
 	PAPARAZZI_HOME=`pwd` PAPARAZZI_SRC=`pwd` $(TOOLS)/gen_aircraft.out $(AIRCRAFT)
 
-sim_ac: ac_h sim_sitl
 hard_ac: ac_h fbw ap
 ac: hard_ac
 
diff --git a/conf/Makefile.sim b/conf/Makefile.sim
index 56f1997cd4..8b54402316 100644
--- a/conf/Makefile.sim
+++ b/conf/Makefile.sim
@@ -31,8 +31,10 @@ SRC_ARCH = $(PAPARAZZI_SRC)/sw/airborne/sim
 
 CC = gcc
 OCAMLC = ocamlc
-CAMLINCLUDES = -I +lablgtk2 -I $(PAPARAZZI_SRC)/sw/lib/ocaml -I $(PAPARAZZI_SRC)/sw/simulator
-SITLCMA = $(PAPARAZZI_SRC)/sw/simulator/sitl.cma
+SIMDIR = $(PAPARAZZI_SRC)/sw/simulator
+CAMLINCLUDES = -I +lablgtk2 -I $(PAPARAZZI_SRC)/sw/lib/ocaml -I $(SIMDIR)
+SIMSITLML = $(OBJDIR)/simsitl.ml
+SITLCMA = $(SIMDIR)/sitl.cma
 
 
 #
@@ -41,7 +43,7 @@ SITLCMA = $(PAPARAZZI_SRC)/sw/simulator/sitl.cma
 
 
 CFLAGS		=							\
-	-W -Wall \
+	-W -Wall -Werror \
 	$(INCLUDES)					  	\
 	-Wstrict-prototypes						\
 	$($(TARGET).CFLAGS)					\
@@ -60,10 +62,15 @@ $(TARGET).objs	= $($(TARGET).objso:%.S=$(OBJDIR)/%.o)
 
 all compile: $(OBJDIR)/simsitl
 
-$(OBJDIR)/simsitl : $($(TARGET).objs) $(SITLCMA)
-	$(OCAMLC) -custom $(CAMLINCLUDES) -o $@ glibivy-ocaml.cma lib-pprz.cma lablgtk.cma gtkInit.cmo $^
+$(OBJDIR)/simsitl : $($(TARGET).objs) $(SITLCMA) $(SIMSITLML)
+	$(OCAMLC) -custom $(CAMLINCLUDES) -o $@ glibivy-ocaml.cma lib-pprz.cma lablgtk.cma $($(TARGET).objs) gtkInit.cmo $(SITLCMA) $(SIMSITLML)
 
 
+# The id of the A/C is hardcoded in the code (to be improved with dynlink ?)
+$(SIMSITLML) : $(SIMDIR)/simsitl.ml
+	@echo "Sim.ac_name := \"$(AIRCRAFT)\"" > $@
+	@cat $< >> $@
+
 %.s: %.c
 	$(CC) $(CFLAGS) -S -o $@ $<
 
diff --git a/conf/airframes/twinstar1.xml b/conf/airframes/twinstar1.xml
index e245603827..99827f0ca0 100644
--- a/conf/airframes/twinstar1.xml
+++ b/conf/airframes/twinstar1.xml
@@ -106,11 +106,6 @@
  <makefile>
 include $(PAPARAZZI_SRC)/conf/autopilot/v1_2_1.makefile
 include $(PAPARAZZI_SRC)/conf/autopilot/twin_mcu_avr.makefile
-
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
-sim.CFLAGS += -Werror
-
-# ap.CFLAGS += -DDATALINK
-# ap.EXTRA_SRCS += traffic_info.c datalink.c
   </makefile>
 </airframe>
diff --git a/conf/airframes/twinstar2.xml b/conf/airframes/twinstar2.xml
index 3cc88226cc..dff18db185 100644
--- a/conf/airframes/twinstar2.xml
+++ b/conf/airframes/twinstar2.xml
@@ -107,40 +107,17 @@
     <define name="PHI0" value="-30" unit="deg"/>
   </section>
 
-  <section name="DOWNLINK" prefix="DOWNLINK_">
-    <define name="FBW_DEVICE" value="uart0"/>
-     <define name="AP_DEVICE" value="uart0"/>
-  </section>
-
   <makefile>
+
 include $(PAPARAZZI_SRC)/conf/autopilot/v1_2_1.makefile
+include $(PAPARAZZI_SRC)/conf/autopilot/twin_mcu_avr.makefile
+include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 
-fbw.srcs += commands.c
-
-fbw.CFLAGS += -DACTUATORS=\"servos_4017.h\" -DSERVOS_4017
-fbw.srcs += $(SRC_ARCH)/servos_4017.c
-
-fbw.CFLAGS += -DRADIO_CONTROL
-fbw.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c 
-
-fbw.CFLAGS += -DDOWNLINK -DUART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=uart0
-fbw.srcs += fbw_downlink.c $(SRC_ARCH)/uart_hw.c
-
-fbw.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK
-fbw.srcs += inter_mcu.c $(SRC_ARCH)/spi_hw.c
-
-fbw.CFLAGS += -Werror
-
-ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_AP_DEVICE=uart0
-ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c
-
-ap.CFLAGS += -DGPS -DUBX -DINFRARED -DRADIO_CONTROL
-ap.CFLAGS += -DDATALINK
-ap.EXTRA_SRCS += traffic_info.c datalink.c
-ap.CFLAGS += -DMOBILE_CAM
-# ap.CFLAGS += -DWAVECARD
-# ap.EXTRA_SRCS += wavecard.c
-ap.CFLAGS += -DMAXSTREAM
-ap.EXTRA_SRCS += maxstream.c
+sim.CFLAGS += -DDATALINK -DMOBILE_CAM
+sim.EXTRA_SRCS += traffic_info.c datalink.c
+#ap.CFLAGS += -DWAVECARD
+#ap.EXTRA_SRCS += wavecard.c
+#ap.CFLAGS += -DMAXSTREAM
+#ap.EXTRA_SRCS += maxstream.c
   </makefile>
 </airframe>
diff --git a/conf/autopilot/sitl.makefile b/conf/autopilot/sitl.makefile
index 6dff78c1a2..a2b1b6cbe6 100644
--- a/conf/autopilot/sitl.makefile
+++ b/conf/autopilot/sitl.makefile
@@ -2,8 +2,8 @@ sim.ARCHDIR = $(ARCHI)
 sim.ARCH = sitl
 sim.TARGET = autopilot
 sim.TARGETDIR = autopilot
-sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU
+sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport
 # ap.CFLAGS += -DGPS -DUBX -DINFRARED -DDOWNLINK
 #ap.CFLAGS += -DACTUATORS=\"servos_4017.h\" -DSERVOS_4017
 #ap.srcs += $(SRC_ARCH)/servos_4017.c
-sim.srcs = radio_control.c downlink.c commands.c gps.c inter_mcu.c link_mcu.c infrared.c pid.c nav.c estimator.c cam.c sys_time.c main_fbw.c main_ap.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c 
+sim.srcs = radio_control.c downlink.c commands.c gps.c inter_mcu.c link_mcu.c infrared.c pid.c nav.c estimator.c cam.c sys_time.c main_fbw.c main_ap.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c  $(SRC_ARCH)/ivy_transport.c 
diff --git a/sw/airborne/downlink.h b/sw/airborne/downlink.h
index 94431a3416..1790107960 100644
--- a/sw/airborne/downlink.h
+++ b/sw/airborne/downlink.h
@@ -26,7 +26,12 @@
 #define DOWNLINK_H
 
 #include <inttypes.h>
+
+#ifdef SITL
+#include "ivy_transport.h"
+#else
 #include "pprz_transport.h"
+#endif
 
 extern uint8_t downlink_nb_ovrn;
 
@@ -50,10 +55,10 @@ extern uint8_t downlink_nb_ovrn;
 
 #define DonwlinkOverrun() downlink_nb_ovrn++;
 
-#define DownlinkStartMessage(msg_id, payload_len) { \
+#define DownlinkStartMessage(_name, msg_id, payload_len) { \
   Transport(Header(payload_len)); \
   Transport(PutUint8(AC_ID)); \
-  Transport(PutUint8(msg_id)); \
+  Transport(PutNamedUint8(_name, msg_id)); \
 }
 
 #define DownlinkEndMessage() Transport(Trailer())
diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c
index d6d5c45b6a..af707d53be 100644
--- a/sw/airborne/main_ap.c
+++ b/sw/airborne/main_ap.c
@@ -40,6 +40,7 @@
 #include "link_mcu.h"
 #include "sys_time.h"
 #include "flight_plan.h"
+#include "datalink.h"
 
 #ifdef LED
 #include "led.h"
diff --git a/sw/airborne/pprz_transport.h b/sw/airborne/pprz_transport.h
index 107b0b7521..01af87a973 100644
--- a/sw/airborne/pprz_transport.h
+++ b/sw/airborne/pprz_transport.h
@@ -60,6 +60,8 @@ extern uint8_t ck_a, ck_b;
     PprzTransportPut1Byte(_byte);		  \
  }
 
+#define PprzTransportPutNamedUint8(_name, _byte) PprzTransportPutUint8(_byte)
+
 #define PprzTransportPut1ByteByAddr(_byte) {	 \
     uint8_t _x = *(_byte);		 \
     PprzTransportPutUint8(_x);	 \
diff --git a/sw/lib/ocaml/extXml.mli b/sw/lib/ocaml/extXml.mli
index ada4ecee14..b5682c580d 100644
--- a/sw/lib/ocaml/extXml.mli
+++ b/sw/lib/ocaml/extXml.mli
@@ -28,11 +28,11 @@ exception Error of string
 val child : Xml.xml -> ?select:(Xml.xml -> bool) -> string -> Xml.xml
 (** [child xml ?p i] If [i] is an integer, returns the [i]'th child of [xml].
 Else returns the child of [xml] with tag [i] (the first one satisfying [p]
-if specified *)
+if specified). Else raises [Not_found]. *)
 
 val get : Xml.xml -> string -> Xml.xml
 (** [get xml path] Returns the son of [xml] specified by [path] (where
-separator is [.] *)
+separator is [.]). May raise [Not_found]. *)
 
 val get_attrib : Xml.xml -> string -> string -> string
 (** [get_attrib xml path attrib_name] *)
diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile
index 5320833450..f8fb5f6f59 100644
--- a/sw/simulator/Makefile
+++ b/sw/simulator/Makefile
@@ -57,8 +57,8 @@ sim_sitl : $(OBJDIR)/simsitl
 simhitl.out : $(SIMHCMO) simhitl.cmo
 	$(OCAMLC) $(INCLUDES) -o $@ str.cma xml-light.cma unix.cma lib.cma lablgtk.cma gtkInit.cmo $^
 
-sitl.cma : $(SIMSCMO)
-	ocamlc -o $@ -a $^ 
+sitl.cma : fg.o $(SIMSCMO)
+	ocamlmklib -o sitl $^ 
 
 fg.o : fg.c
 	$(OCAMLC) -c fg.c
@@ -97,10 +97,6 @@ $(OBJDIR)/simsitl.cmo : $(OBJDIR)/simsitl.ml
 $(OBJDIR)/simsitl.cmx : $(OBJDIR)/simsitl.ml
 	$(OCAMLOPT) $(INCLUDES) -c -o $@ $<
 
-$(OBJDIR)/simsitl.ml : simsitl.ml
-	@echo "Sim.ac_name := \"$(AIRCRAFT)\"" > $@
-	@cat $< >> $@
-
 %.cmo : %.ml
 	$(OCAMLC) $(INCLUDES) -c $<
 
@@ -116,13 +112,13 @@ $(OBJDIR)/simsitl.ml : simsitl.ml
 clean :
 	\rm -f *.cm* *~ *.out .depend *.o *.a *.so gaia
 
-flightModel.cmo: flightModel.cmi 
-hitl.cmo: hitl.cmi 
-sim.cmo: flightModel.cmi sim.cmi 
-simhitl.cmo: hitl.cmi sim.cmi
-simsitl.cmo: sim.cmi sitl.cmi 
-sitl.cmo: sitl.cmi 
-sitl.cmi: sim.cmi 
-data.cmo : data.cmi
-stdlib.cmo : stdlib.cmi
-gps.cmo : gps.cmi
+#
+# Dependencies
+#
+
+.depend:
+	ocamldep *.ml* > .depend
+
+ifneq ($(MAKECMDGOALS),clean) 
+-include .depend
+endif
diff --git a/sw/simulator/data.ml b/sw/simulator/data.ml
index 102c9cdcd0..7641ff10a4 100644
--- a/sw/simulator/data.ml
+++ b/sw/simulator/data.ml
@@ -38,9 +38,9 @@ let messages_ap =
 (*  let xml = Xml.parse_file (pprz_conf_path // "messages.xml") in *)
   let xml = Xml.parse_file (user_conf_path // "messages.xml") in
   try
-    ExtXml.child xml ~select:(fun x -> Xml.attrib x "name" = "telemetry_ap") "class"
+    ExtXml.child xml ~select:(fun x -> Xml.attrib x "name" = "telemetry") "class"
   with
-    Not_found -> failwith "'telemetry_ap' class missing in messages.xml"
+    Not_found -> failwith "'telemetry' class missing in messages.xml"
 
 type aircraft = {
     name : string;
diff --git a/sw/simulator/flightModel.ml b/sw/simulator/flightModel.ml
index ad3daf594a..adc3036de2 100644
--- a/sw/simulator/flightModel.ml
+++ b/sw/simulator/flightModel.ml
@@ -3,7 +3,7 @@
  *
  * Basic flight model for simulation
  *  
- * Copyright (C) 2004-2005 Pascal Brisset, Antoine Drouin
+ * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin
  *
  * This file is part of paparazzi.
  *
@@ -25,6 +25,7 @@
  *)
 
 open Stdlib
+open Printf
 
 let ios = fun x ->
   try int_of_string x with _ -> failwith (Printf.sprintf "int_of_string: '%s'" x)
@@ -123,12 +124,20 @@ module Make(A:Data.MISSION) = struct
 
 
 
-  let servos =
+  let commands =
     try
-      ExtXml.child A.ac.airframe "servos"
+      let l = ExtXml.child A.ac.airframe "commands" in
+      let rec loop i = function
+	  [] -> []
+	| x::xs -> (ExtXml.attrib x "name", i)::loop (i+1) xs in
+      loop 0 (Xml.children l)
     with
       Not_found ->
-	failwith (Printf.sprintf "Child 'servos' expected in '%s'\n" (Xml.to_string A.ac.airframe))
+	failwith (Printf.sprintf "Child 'commands' expected in '%s'\n" (Xml.to_string A.ac.airframe))
+
+  let command = fun n -> 
+    try List.assoc n commands with
+      Not_found -> failwith (sprintf "Unknown command '%s'" n)
 	  
   let misc_section = section "MISC"
 
@@ -139,70 +148,20 @@ module Make(A:Data.MISSION) = struct
   let adc_roll_neutral = ios (defined_value infrared_section "ADC_ROLL_NEUTRAL")
   let roll_neutral_default = rad_of_deg (float_value infrared_section "ROLL_NEUTRAL_DEFAULT")
 
-  let get_servo name =
-    try
-      ExtXml.child servos ~select:(fun x -> ExtXml.attrib x "name" = name) "servo"
-    with
-      Not_found ->
-	failwith (Printf.sprintf "Child 'servo' with name='%s' expected in '%s'\n" name (Xml.to_string servos))
-
-  let us_attrib = fun x a -> int_of_string (ExtXml.attrib x a)
-
-  let gaz = try get_servo "GAZ" with _ -> get_servo "MOTOR_RIGHT"
-  let min_thrust =  us_attrib gaz "min"
-  let max_thrust =  us_attrib gaz "max"
+  let min_thrust =  0
+  let max_thrust =  max_pprz
       
-  type servo_id = int
-  type ppm = int
-
-  let no_thrust = int_of_string (ExtXml.attrib gaz "no")
-
-      
-
-  let some_aileron_left = try Some (get_servo "AILERON_LEFT") with _ -> None
-  let some_ailevon_left = try Some (get_servo "AILEVON_LEFT") with _ -> None
-  let some_ailevon_right = try Some (get_servo "AILEVON_RIGHT") with _ -> None
-
+  let command_throttle = command "THROTTLE"
+  let command_roll = command "ROLL"
+     
   let float_attrib = fun x a -> float_of_string (ExtXml.attrib x a)
   let int_attrib = fun x a -> int_of_string (ExtXml.attrib x a)
 
-  let sign = fun x ->
-    if float_attrib x "min" < float_attrib x "max" then 1 else -1
 
-  let do_thrust = fun state servo ->
-    state.thrust <- (float (servo.(no_thrust) - min_thrust) /. float (max_thrust - min_thrust))
+  let do_commands = fun state commands ->
+    let c_lda = 1e-4 in (* FIXME *)
+    state.delta_a <- c_lda *. float commands.(command_roll);
+    state.thrust <- (float (commands.(command_throttle) - min_thrust) /. float (max_thrust - min_thrust))
 
-  let do_servos =  
-    match some_aileron_left, some_ailevon_left, some_ailevon_right with
-      Some aileron_left, None, None ->
-	let c_lda = 16. *. 9e-5 in (* phi_dot_dot from aileron *)
-
-	let sign_aileron_left = sign aileron_left
-	and n_delta_a = us_attrib aileron_left "neutral"
-	and no_aileron_left = int_attrib aileron_left "no" in
-	fun state servo ->
-	  let left = - sign_aileron_left * (servo.(no_aileron_left) - n_delta_a) in
-	  (** if left <> 0 then Printf.printf "left=%d\n" (servo.(no_aileron_left) - n_delta_a); flush stdout;  **)
-	  state.delta_a <- c_lda *. float left;
-	  do_thrust state servo
-    | None, Some ailevon_left, Some ailevon_right ->
-	let c_lda = 2.5e-4 in (* phi_dot_dot from aileron *)
-
-	let sign_ailevon_left = sign ailevon_left
-	and sign_ailevon_right = sign ailevon_right
-	and left_neutral = us_attrib ailevon_left "neutral"
-	and right_neutral = us_attrib ailevon_right "neutral"
-	and left_travel = float (us_attrib ailevon_left "max" - us_attrib ailevon_left "min") /. 1200.
-	and right_travel = float (us_attrib ailevon_right "max" - us_attrib ailevon_right "min") /. 1200.
-	and no_ailevon_left = int_attrib ailevon_left "no"
-	and no_ailevon_right = int_attrib ailevon_right "no" in
-	fun state servo ->
-	  do_thrust state servo;
-	  let sum = (float (servo.(no_ailevon_left) - left_neutral) /. left_travel +.
-		       float (servo.(no_ailevon_right) - right_neutral) /.  right_travel) /. 2. in
-(*	Printf.printf "%d %f\n" (servo no_ailevon_left - left_neutral) sum; flush stdout; *)
-	  state.delta_a <- c_lda *. (-. sum)
-    | _ -> failwith "Aileron or Ailevon left and right PLEASE"
-
-  let nb_servos = 10 (* 4017 *)
+  let nb_commands = 10 (* FIXME *)
 end
diff --git a/sw/simulator/flightModel.mli b/sw/simulator/flightModel.mli
index 6d97b2bd05..681b45f52a 100644
--- a/sw/simulator/flightModel.mli
+++ b/sw/simulator/flightModel.mli
@@ -3,7 +3,7 @@
  *
  * Basic flight model for simulation
  *  
- * Copyright (C) 2004-2005 Pascal Brisset, Antoine Drouin
+ * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin
  *
  * This file is part of paparazzi.
  *
@@ -42,8 +42,8 @@ val set_air_speed : state -> meter_s -> unit
 module Make :
   functor (A : Data.MISSION) ->
     sig
-      val do_servos : state -> Stdlib.us array -> unit
-      val nb_servos : int
+      val do_commands : state -> Stdlib.pprz_t array -> unit
+      val nb_commands : int
       val nominal_airspeed : float (* m/s *)
       val roll_neutral_default : float (* rad *)
       val state_update : state -> float * float -> float -> unit
diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml
index 17c8bb2301..0fbc0066f4 100644
--- a/sw/simulator/sim.ml
+++ b/sw/simulator/sim.ml
@@ -1,9 +1,9 @@
 (*
  *  $Id$
  *
- * Hardware in the loop basic simulator (handling GPS, infrared and servos)
+ * Hardware in the loop basic simulator (handling GPS, infrared and commands)
  *  
- * Copyright (C) 2004 Pascal Brisset, Antoine Drouin
+ * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin
  *
  * This file is part of paparazzi.
  *
@@ -43,7 +43,7 @@ module type AIRCRAFT =
     val boot : Stdlib.value -> unit
     (** [boot time_acceleration] *)
 
-    val servos : us array -> unit
+    val commands : pprz_t array -> unit
 	(** Called once at init *)
 	
     val infrared : float -> float -> float -> unit
@@ -109,9 +109,9 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
 
     let reset = fun () -> state := initial_state in 
 
-    let servos = Array.create FM.nb_servos 0 in
+    let commands = Array.create FM.nb_commands 0 in
 
-    Aircraft.servos servos;
+    Aircraft.commands commands;
 
     let north_label = GMisc.label ~text:"000" ()
     and east_label = GMisc.label ~text:"000" ()
@@ -140,7 +140,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
 
 
     let fm_task = fun () ->
-      FM.do_servos !state servos;
+      FM.do_commands !state commands;
       FM.state_update !state (!wind_x, !wind_y) fm_period
        
     and ir_task = fun () ->
diff --git a/sw/simulator/sim.mli b/sw/simulator/sim.mli
index 1b4de5b57a..cd3ea3ada6 100644
--- a/sw/simulator/sim.mli
+++ b/sw/simulator/sim.mli
@@ -8,7 +8,7 @@ module type AIRCRAFT =
   sig
     val init : int -> GPack.box -> unit
     val boot : Stdlib.value -> unit
-    val servos : Stdlib.us array -> unit
+    val commands : Stdlib.pprz_t array -> unit
     val infrared : float -> float -> float -> unit
     val gps : Gps.state -> unit
   end
diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml
index 98324abe71..1dc029b6a1 100644
--- a/sw/simulator/sitl.ml
+++ b/sw/simulator/sitl.ml
@@ -1,7 +1,7 @@
 (*
  *  $Id$
  *
- * Software in the loop basic simulator (handling GPS, infrared and servos)
+ * Software in the loop basic simulator (handling GPS, infrared and commands)
  *  
  * Copyright (C) 2004 Pascal Brisset, Antoine Drouin
  *
@@ -41,24 +41,27 @@ module Make(A:Data.MISSION) = struct
     ExtXml.child Data.messages_ap ~select:(fun x -> ExtXml.attrib x "name" = name) "message"
 
 
-(* Servos handling (rservos is the intermediate storage) *)
+(* Commands handling (rcommands is the intermediate storage) *)
   let rc_channels = Array.of_list (Xml.children A.ac.Data.radio)
   let nb_channels = Array.length rc_channels
   let rc_channel_no = fun x -> 
     List.assoc x (Array.to_list (Array.mapi (fun i c -> Xml.attrib c "function", i) rc_channels))
 
-  let rservos = ref [||]
+  let rcommands = ref [||]
   let adj_bat = GData.adjustment ~value:12.5 ~lower:0. ~upper:23. ~step_incr:0.1 ()
 
-  external get_commands : Stdlib.us array -> int = "get_commands"
+  external get_commands : Stdlib.pprz_t array -> int = "get_commands"
 (** Returns gaz servo value (us) *)
 
   let energy = ref 0.
 
+ (** Get the commands from the autopilot, store them
+    (to be used by the flight model)
+     Computes an energy consumption from throttle level *)
   let update_servos =
     let accu = ref 0. in
     fun bat_button () ->
-      let gaz = get_commands !rservos in
+      let gaz = get_commands !rcommands in
       (* 100% = 1W *)
       if bat_button#active then
 	let energy = float (gaz-1000) /. 1000. *. servos_period in
@@ -109,7 +112,6 @@ module Make(A:Data.MISSION) = struct
     window#show ()
 
   external periodic_task : unit -> unit = "sim_periodic_task"
-  external rc_task : unit -> unit = "sim_rc_task"
   external sim_init : int -> unit = "sim_init"
   external update_bat : int -> unit = "update_bat"
 
@@ -182,7 +184,6 @@ module Make(A:Data.MISSION) = struct
   let boot = fun time_scale ->
     Stdlib.timer ~scale:time_scale servos_period (update_servos bat_button);
     Stdlib.timer ~scale:time_scale periodic_period periodic_task;
-    Stdlib.timer ~scale:time_scale rc_period rc_task;
     ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" get_flight_param);
     ignore (Ground_Pprz.message_bind "MOVE_WAYPOINT" get_move_waypoint);
     ignore (Ground_Pprz.message_bind "SEND_EVENT" get_send_event);
@@ -190,7 +191,7 @@ module Make(A:Data.MISSION) = struct
     ignore (Ground_Pprz.message_bind "DL_SETTING" get_dl_setting)
 
 (* Functions called by the simulator *)
-  let servos = fun s -> rservos := s
+  let commands = fun s -> rcommands := s
 
   external set_ir : int -> int -> unit = "set_ir"
   let infrared = fun ir_left ir_front ir_top ->
diff --git a/sw/simulator/stdlib.ml b/sw/simulator/stdlib.ml
index 93975987a4..41bb05317a 100644
--- a/sw/simulator/stdlib.ml
+++ b/sw/simulator/stdlib.ml
@@ -24,7 +24,9 @@
  *
  *)
 
-type us = int
+type pprz_t = int
+
+let max_pprz = 9600
 
 let pi = 4. *. atan 1.
 let rec norm_angle = fun x ->
diff --git a/sw/simulator/stdlib.mli b/sw/simulator/stdlib.mli
index 7a5bd997d4..bbd00a8803 100644
--- a/sw/simulator/stdlib.mli
+++ b/sw/simulator/stdlib.mli
@@ -24,7 +24,8 @@
  *
  *)
 
-type us = int
+type pprz_t = int
+val max_pprz : pprz_t
 val pi : float
 val norm_angle : float -> float
 val deg_of_rad : float -> float
diff --git a/sw/tools/gen_messages.ml b/sw/tools/gen_messages.ml
index 8a169fa0f9..4d6e17eb13 100644
--- a/sw/tools/gen_messages.ml
+++ b/sw/tools/gen_messages.ml
@@ -161,7 +161,7 @@ module Gen_onboard = struct
     fprintf avr_h "){ \\\n";
     let size = (size_fields fields "0") in
     fprintf avr_h "\tif (DownlinkCheckFreeSpace(DownlinkSizeOf(%s))) {\\\n" size;
-    fprintf avr_h "\t  DownlinkStartMessage(DL_%s,%s) \\\n" s size;
+    fprintf avr_h "\t  DownlinkStartMessage(\"%s\", DL_%s, %s) \\\n" s s size;
     List.iter (print_avr_field avr_h) fields;
     fprintf avr_h "\t  DownlinkEndMessage() \\\n";
     fprintf avr_h "\t} else \\\n";