diff --git a/conf/airframes/gorrazoptere_esc_3DMG_test.xml b/conf/airframes/gorrazoptere_esc_3DMG_test.xml
index 17fb6abe7e..c6a00ed99f 100644
--- a/conf/airframes/gorrazoptere_esc_3DMG_test.xml
+++ b/conf/airframes/gorrazoptere_esc_3DMG_test.xml
@@ -36,17 +36,35 @@
     <loop name="roll_dot" type="P" pgain="100" measure="estimator_roll_dot" setpoint="setpoint_rolldot" output="command_roll_dot"/>
     <loop name="pitch_dot" type="PD" pgain="100" dgain="200" measure="estimator_pitch_dot" setpoint="setpoint_picthdot" output="command_pitch_dot"/>
   </level>
+
+  <level name="attitudes">
+    <loop name="roll" type="P" pgain="100" measure="estimator_roll" setpoint="setpoint_roll" output="setpoint_rolldot"/>
+    <loop name="pitch" type="PD" pgain="100" dgain="200" measure="estimator_pitch" setpoint="setpoint_picth" output="setpoint_pitchdot"/>
+  </level>
+
+
+
   <mode name="MANUAL">
-    <control type="direct" input="RADIO_THROTTLE" output="COMMAND_THROTTLE"/>
-    <control type="P" setpoint="RADIO_ROLL" measure="estimator_roll_dot" output="COMMAND_ROLL_DOT"/>
-    <control type="P" setpoint="RADIO_PITCH" measure="estimator_pitch_dot" output="COMMAND_PITCH_DOT"/>
-    <control type="P" setpoint="RADIO_YAW" measure="estimator_yaw_dot" output="COMMAND_YAW_DOT"/>
+    <input input="rc_values[COMMAND_THROTTLE]" output="control_commands[RADIO_THROTTLE]"/>
+    <input input="rc_values[RADIO_ROLL]" output="control_setpoint_rolldot"/>
+    <input input="rc_values[RADIO_PITCH]" output="control_setpoint_pitchdot"/>
+    <input input="rc_values[RADIO_YAW]" output="control_setpoint_yawdot"/>
   </mode>
 
   <mode name="AUTO1">
-    <control type="direct" input="RADIO_THROTTLE" output="COMMAND_THROTTLE"/>
-    <control type="P" setpoint="RADIO_ROLL" measure="estimator_roll" output="setpoint_roll_dot"/>
-   </mode>
+    <input input="rc_values[COMMAND_THROTTLE]" output="control_commands[RADIO_THROTTLE]"/>
+    <input input="rc_values[RADIO_ROLL]" output="control_setpoint_roll"/>
+    <input input="rc_values[RADIO_PITCH]" output="control_setpoint_pitch"/>
+    <input input="rc_values[RADIO_YAW]" output="control_setpoint_yawdot"/>
+  </mode>
+
+  <mode name="AUTO2">
+    <input input="rc_values[COMMAND_THROTTLE]" output="control_commands[RADIO_THROTTLE]"/>
+    <input input="rc_values[RADIO_ROLL]" output="control_setpoint_roll"/>
+    <input input="rc_values[RADIO_PITCH]" output="control_setpoint_pitch"/>
+    <input input="rc_values[RADIO_YAW]" output="control_setpoint_yawdot"/>
+  </mode>
+
 </control>
 
 
diff --git a/sw/tools/gen_control.ml b/sw/tools/gen_control.ml
new file mode 100644
index 0000000000..b3bd6dd607
--- /dev/null
+++ b/sw/tools/gen_control.ml
@@ -0,0 +1,60 @@
+open Printf
+open Xml2h
+
+let parse_input = fun s ->
+  match Xml.tag s with
+    "input" ->
+      let input = ExtXml.attrib s "input"
+      and output = ExtXml.attrib s "output" in
+      printf "  %s = %s;\n" output input 
+  | _ -> ignore ()
+
+
+
+let parse_loop = fun s ->
+  match Xml.tag s with
+    "loop" ->
+      let name = ExtXml.attrib s "name"
+      and loop_type = ExtXml.attrib s "type"
+      and meas = ExtXml.attrib s "type" in
+      printf "name %s\n" name
+  | _ -> ignore ()
+
+
+
+let parse_control = fun s ->
+  match Xml.tag s with
+    "level" ->
+      let level_name = ExtXml.attrib s "name" in
+      printf "static inline void control_run_%s_loops ( void ) {\n" level_name;
+      List.iter parse_loop (Xml.children s);
+      printf "}\n";
+      nl();
+  | "mode" ->
+      let mode_name = ExtXml.attrib s "name" in
+      printf "static inline void control_process_radio_control_%s ( void ) {\n" mode_name;
+      List.iter parse_input (Xml.children s);
+      printf "}\n";
+      nl();
+  | _ -> ignore ()
+
+let parse_section = fun s ->
+  match Xml.tag s with
+    "control" ->
+      List.iter parse_control (Xml.children s)
+  | _ -> ignore ()
+
+
+let h_name = "CONTROL_H"
+ 
+let _ =
+  if Array.length Sys.argv <> 2 then
+    failwith (Printf.sprintf "Usage: %s xml_file" Sys.argv.(0));
+  let xml_file = Sys.argv.(1) in
+  try
+    let xml = start_and_begin xml_file h_name in
+    List.iter parse_section (Xml.children xml);
+    finish h_name
+  with
+    Xml.Error e -> prerr_endline (Xml.error e)
+	  
diff --git a/sw/tools/gen_control.out b/sw/tools/gen_control.out
deleted file mode 100755
index 3c2c2536fd..0000000000
--- a/sw/tools/gen_control.out
+++ /dev/null
@@ -1,5 +0,0 @@
-#! /bin/sh
-if test -z "$PAPARAZZI_SRC"; then
-    PAPARAZZI_SRC=/usr/share/paparazzi
-fi
-ocaml -I $PAPARAZZI_SRC/sw/lib/ocaml ivy-ocaml.cma lib-pprz.cma $PAPARAZZI_SRC/sw/tools/gen_control.ml $*