From a8f36ce808579e0e43a9e0a9a6ec1e7843cacedf Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Thu, 24 Mar 2011 14:30:12 +0100
Subject: [PATCH 01/99] start reworking the joystick improvements from mantauav

---
 .../joystick/attack3_booz_nav.xml             |   0
 conf/joystick/aurora_skate_controller.xml     | 144 +++++++++
 conf/joystick/logitech_dual_action.xml        |  87 ++++++
 conf/joystick/logitech_f710.xml               | 153 ++++++++++
 .../joystick/ngs_set_actuators.xml            |   0
 .../joystick/xbox_booz_nav.xml                |   0
 conf/messages.xml                             |   9 +
 sw/airborne/firmwares/fixedwing/datalink.c    |   9 +
 sw/ground_segment/joystick/README             |   4 +-
 sw/ground_segment/joystick/input2ivy.ml       | 225 ++++++++++++--
 sw/ground_segment/joystick/usb_stick.c        | 282 +++++++++++++-----
 sw/ground_segment/joystick/usb_stick.h        |  15 +-
 12 files changed, 823 insertions(+), 105 deletions(-)
 rename {sw/ground_segment => conf}/joystick/attack3_booz_nav.xml (100%)
 create mode 100644 conf/joystick/aurora_skate_controller.xml
 create mode 100644 conf/joystick/logitech_dual_action.xml
 create mode 100644 conf/joystick/logitech_f710.xml
 rename {sw/ground_segment => conf}/joystick/ngs_set_actuators.xml (100%)
 rename {sw/ground_segment => conf}/joystick/xbox_booz_nav.xml (100%)

diff --git a/sw/ground_segment/joystick/attack3_booz_nav.xml b/conf/joystick/attack3_booz_nav.xml
similarity index 100%
rename from sw/ground_segment/joystick/attack3_booz_nav.xml
rename to conf/joystick/attack3_booz_nav.xml
diff --git a/conf/joystick/aurora_skate_controller.xml b/conf/joystick/aurora_skate_controller.xml
new file mode 100644
index 00000000000..9b3e579bd21
--- /dev/null
+++ b/conf/joystick/aurora_skate_controller.xml
@@ -0,0 +1,144 @@
+<!-- Custom Aurora Skate Controller
+  Joystick has four axes:
+  axis 0: X on right stick
+  axis 1: Y on right stick
+  axis 2: Y on left stick
+  axis 3: X on left stick
+
+It has 9 buttons.
+ b0  - right side/left bumper
+ b1  - right side/right bumper
+ b2  - right side/down bumper
+ b3  - right side/up bumper
+ b7 - yellow
+ b8 - red
+ b9 - green
+ b10 - left side/left bumper
+ b11 - left side/right bumper
+
+-->
+
+<joystick>
+ <input>
+  <axis index="0" name="rx" limit="1.00" exponent="0.7" trim="0"/>
+  <axis index="1" name="ry" limit="1.00" exponent="0.7" trim="0"/>
+  <axis index="2" name="ly" limit="1.00" exponent="0.0" trim="0"/>
+  <axis index="3" name="lx" limit="1.00" exponent="0.7" trim="0"/>
+  <button index="0" name="b0"/>
+  <button index="1" name="b1"/>
+  <button index="2" name="b2"/>
+  <button index="3" name="b3"/>  
+  <button index="4" name="b4"/>
+  <button index="5" name="b5"/>
+  <button index="6" name="b6"/>
+  <button index="7" name="b7"/>
+  <button index="8" name="b8"/>
+  <button index="9" name="b9"/>
+  <button index="10" name="b10"/>
+  <button index="11" name="b11"/>
+</input>
+
+<messages period="0.0333333333">
+
+
+  <message class="datalink" name="RC_4CH" send_always="1">
+   <field name="joystick_id" value="JoystickID()"/>
+   <field name="throttle"    value="Scale(ly,-10,127)"/>
+   <field name="roll"        value="rx"/>
+   <field name="yaw"         value="lx"/>
+   <field name="pitch"       value="0-ry"/>
+  </message>
+
+
+  <!-- trim commands -->
+  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="b1">
+    <field name="index" value="IndexOfSetting('rc_roll_trim')"/>
+    <field name="mod" value="10"/>
+  </message>
+  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="b0">
+    <field name="index" value="IndexOfSetting('rc_roll_trim')"/>
+    <field name="mod" value="0-10"/>
+  </message>
+
+  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="b3">
+    <field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
+    <field name="mod" value="10"/>
+  </message>
+  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="b2">
+    <field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
+    <field name="mod" value="0-10"/>
+  </message>
+
+  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="b11">
+    <field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
+    <field name="mod" value="10"/>
+  </message>
+  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="b10">
+    <field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
+    <field name="mod" value="0-10"/>
+  </message>
+
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b10 && b11">
+    <field name="index" value="IndexOfSetting('rc_roll_trim')"/>
+    <field name="value" value="0"/>
+  </message>
+
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b10 && b11">
+    <field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
+    <field name="value" value="0"/>
+  </message>
+
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b10 && b11">
+    <field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
+    <field name="value" value="0-10"/>
+  </message>
+<!--
+  <message class="ground" name="lx" on_event="b8"/>
+  <message class="trim_plus" name="ly" on_event="b10"/>
+  <message class="trim_minus" name="ly" on_event="b11"/>
+  <message class="trim_plus" name="rx" on_event="b2"/>
+  <message class="trim_minus" name="rx" on_event="b0"/>
+  <message class="trim_minus" name="ry" on_event="b3"/> 
+  <message class="trim_plus" name="ry" on_event="b1"/>
+  <message class="trim_save" name="" on_event="b5"/>
+-->
+
+<!-- Camera switch. Currently uses same buttons as yaw trim -->
+
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b1">
+    <field name="index" value="IndexOfSetting('cam_switch')"/>
+    <field name="value" value="1"/> 
+  </message>
+
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b0">
+    <field name="index" value="IndexOfSetting('cam_switch')"/>
+    <field name="value" value="0"/> 
+  </message>
+
+
+  <!-- Red Button -->  <!-- AUTO2 mode -->
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b8">
+    <field name="index" value="IndexOfSetting('pprz_mode')"/>
+    <field name="value" value="2"/> 
+  </message>
+
+  <!-- Yellow Button -->   <!-- AUTO1 mode -->
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b7">
+    <field name="index" value="IndexOfSetting('pprz_mode')"/>
+    <field name="value" value="1"/>
+  </message>
+
+  <!-- Green Button -->  <!-- MANUAL mode -->
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b9">
+    <field name="index" value="IndexOfSetting('pprz_mode')"/>
+    <field name="value" value="0"/> 
+  </message>
+
+ <message class="ground" name="DL_SETTING" send_always="0" on_event="b7 || b8 || b9">
+    <field name="index" value="IndexOfSetting('rc_dl_active_joystick')"/>
+    <field name="value" value="JoystickID()"/> 
+  </message>
+
+</messages>
+
+</joystick>
diff --git a/conf/joystick/logitech_dual_action.xml b/conf/joystick/logitech_dual_action.xml
new file mode 100644
index 00000000000..ecbe18f3ebc
--- /dev/null
+++ b/conf/joystick/logitech_dual_action.xml
@@ -0,0 +1,87 @@
+<!-- Logitech dual action controller profile
+
+  Joystick has six axes:
+  axis 1: X on left stick
+  axis 2: Y on left stick
+  axis 3: X on right stick
+  axis 4: Y on right stick
+  axis 5: X on D pad
+  axis 6: Y on D pad
+It has 12 buttons, 10 of which are labeled on the joystick
+Buttons 11 and 12 are not to be found.
+
+The "mode" button swaps the axes on the left stick and the d pad.
+-->
+
+<joystick>
+ <input>
+  <axis index="0" name="lx" limit="1.00" exponent="0.7" trim="0"/>
+  <axis index="1" name="ly" limit="1.00" exponent="0.7" trim="0"/>
+  <axis index="2" name="rx" limit="1.00" exponent="0.7" trim="0"/>
+  <axis index="3" name="ry" limit="1.00" exponent="0.7" trim="0"/>
+  <axis index="4" name="dpadx"/>
+  <axis index="5" name="dpady"/>
+  <button index="0" name="b0"/>
+  <button index="1" name="b1"/>
+  <button index="2" name="b2"/>
+  <button index="3" name="b3"/>  
+  <button index="4" name="b4"/>
+  <button index="5" name="b5"/>
+  <button index="6" name="b6"/>
+  <button index="7" name="b7"/>
+  <button index="8" name="b8"/>
+  <button index="9" name="b9"/>
+  <button index="10" name="b10"/>
+  <button index="11" name="b11"/>
+</input>
+
+<messages period="0.017">
+
+
+  <message class="datalink" name="RC_4CH" send_always="1">
+   <field name="throttle" value="Scale(0-ly,0,255)"/>
+   <field name="roll" value="rx"/>
+   <field name="yaw" value="lx"/>
+   <field name="pitch" value="ry"/>
+  </message>
+
+
+  <!-- trim commands -->
+<!--  not needed
+  <message class="trim_plus" name="lx" on_event="b9"/>
+  <message class="trim_minus" name="lx" on_event="b8"/>
+  <message class="trim_plus" name="ly" on_event="b10"/>
+  <message class="trim_minus" name="ly" on_event="b11"/>
+  <message class="trim_plus" name="rx" on_event="b2"/>
+  <message class="trim_minus" name="rx" on_event="b0"/>
+  <message class="trim_minus" name="ry" on_event="b3"/> 
+  <message class="trim_plus" name="ry" on_event="b1"/>
+  <message class="trim_save" name="" on_event="b5"/>
+ -->
+
+  <!-- Button 5 top left bumper) -->  <!-- AUTO1 mode -->
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b4">
+    <field name="index" value="5"/>
+    <field name="value" value="1"/> 
+  </message>
+ 
+  <!-- Button 7 bottom left bumper -->   <!-- AUTO2 mode -->
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b6">
+    <field name="index" value="5"/>
+    <field name="value" value="2"/>
+  </message>
+
+  <!-- Buttons 6+8 (right pad) -->  <!-- KILL mode -->
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b5">
+    <field name="index" value="5"/>
+    <field name="value" value="5"/> 
+  </message>
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b7">
+    <field name="index" value="5"/>
+    <field name="value" value="5"/> 
+  </message>
+
+
+</messages>
+
+</joystick>
diff --git a/conf/joystick/logitech_f710.xml b/conf/joystick/logitech_f710.xml
new file mode 100644
index 00000000000..66bd8f9d21f
--- /dev/null
+++ b/conf/joystick/logitech_f710.xml
@@ -0,0 +1,153 @@
+<!-- Logitech F710
+  This config is for Xinput mode. Make sure slider switch on back of controller is on X (not D)
+  Also make sure controller not in sports mode (mode light should be off)
+
+  Joystick has eight axes:
+  axis 0: X on left stick
+  axis 1: Y on left stick
+  axis 2: Left trigger
+  axis 3: X on right stick
+  axis 4: Y on right stick
+  axis 5: Right trigger
+  axis 6: X on D-Pad (not analog)
+  axis 7: Y on D-Pad (not analog)
+
+It has 8 buttons.
+ b_green  - green button (A)
+ b_red  - red button (B)
+ b_blue  - blue button (X)
+ b_yellow  - yellow button (Y)
+ b_ltb - left trigger button
+ b_rtb - right trigger button
+ b_start - start button
+ b_back - back button
+
+-->
+
+<joystick>
+ <input>
+  <axis index="0" name="lx" limit="1.00" exponent="0.7" trim="0"/>
+  <axis index="1" name="ly" limit="1.00" exponent="0.0" trim="0"/>
+  <axis index="2" name="lt" limit="1.00" exponent="0.0" trim="0"/>
+  <axis index="3" name="rx" limit="1.00" exponent="0.7" trim="0"/>
+  <axis index="4" name="ry" limit="1.00" exponent="0.7" trim="0"/>
+  <axis index="5" name="rt" limit="1.00" exponent="0.0" trim="0"/>
+  <axis index="6" name="dx" limit="1.00" exponent="0.0" trim="0"/>
+  <axis index="7" name="dy" limit="1.00" exponent="0.0" trim="0"/>
+  <button index="0" name="b_green"/>
+  <button index="1" name="b_red"/>
+  <button index="2" name="b_blue"/>
+  <button index="3" name="b_yellow"/>  
+  <button index="4" name="b_ltb"/>
+  <button index="5" name="b_rtb"/>
+  <button index="6" name="b_start"/>
+  <button index="7" name="b7"/>
+  <button index="8" name="b8"/>
+  <button index="9" name="b9"/>
+  <button index="10" name="b_back"/>
+</input>
+
+<messages period="0.0333333333">
+
+
+  <message class="datalink" name="RC_4CH" send_always="1">
+   <field name="joystick_id" value="JoystickID()"/>
+   <field name="throttle"    value="Bound(0-ly,0,127)"/>
+   <field name="roll"        value="rx"/>
+   <field name="yaw"         value="lx"/>
+   <field name="pitch"       value="ry"/>
+  </message>
+
+
+  <!-- trim commands -->
+  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="dx > 100">
+    <field name="index" value="IndexOfSetting('rc_roll_trim')"/>
+    <field name="mod" value="10"/>
+  </message>
+  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="0-dx > 100"	>
+    <field name="index" value="IndexOfSetting('rc_roll_trim')"/>
+    <field name="mod" value="0-10"/>	
+  </message>
+
+  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="dy > 100">
+    <field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
+    <field name="mod" value="10"/>
+  </message>
+  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="0-dy > 100">
+    <field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
+    <field name="mod" value="0-10"/>
+  </message>
+
+  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="b_rtb">
+    <field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
+    <field name="mod" value="10"/>
+  </message>
+  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="b_ltb">
+    <field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
+    <field name="mod" value="0-10"/>
+  </message>
+
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b_ltb && b_rtb">
+    <field name="index" value="IndexOfSetting('rc_roll_trim')"/>
+    <field name="value" value="0"/>
+  </message>
+
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b_ltb && b_rtb">
+    <field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
+    <field name="value" value="0"/>
+  </message>
+
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b_ltb && b_rtb">
+    <field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
+    <field name="value" value="0-10"/>
+  </message>
+<!--
+  <message class="ground" name="lx" on_event="b8"/>
+  <message class="trim_plus" name="ly" on_event="b10"/>
+  <message class="trim_minus" name="ly" on_event="b11"/>
+  <message class="trim_plus" name="rx" on_event="b2"/>
+  <message class="trim_minus" name="rx" on_event="b0"/>
+  <message class="trim_minus" name="ry" on_event="b3"/> 
+  <message class="trim_plus" name="ry" on_event="b1"/>
+  <message class="trim_save" name="" on_event="b5"/>
+-->
+
+<!-- Camera switch. Currently uses same buttons as yaw trim -->
+
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b_start">
+    <field name="index" value="IndexOfSetting('cam_switch')"/>
+    <field name="value" value="1"/> 
+  </message>
+
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b_back">
+    <field name="index" value="IndexOfSetting('cam_switch')"/>
+    <field name="value" value="0"/> 
+  </message>
+
+
+  <!-- Red Button -->  <!-- AUTO2 mode -->
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b_red">
+    <field name="index" value="IndexOfSetting('pprz_mode')"/>
+    <field name="value" value="2"/> 
+  </message>
+
+  <!-- Yellow Button -->   <!-- AUTO1 mode -->
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b_yellow">
+    <field name="index" value="IndexOfSetting('pprz_mode')"/>
+    <field name="value" value="1"/>
+  </message>
+
+  <!-- Green Button -->  <!-- MANUAL mode -->
+  <message class="ground" name="DL_SETTING" send_always="0" on_event="b_green">
+    <field name="index" value="IndexOfSetting('pprz_mode')"/>
+    <field name="value" value="0"/> 
+  </message>
+
+ <message class="ground" name="DL_SETTING" send_always="0" on_event="b_green || b_yellow || b_red">
+    <field name="index" value="IndexOfSetting('rc_dl_active_joystick')"/>
+    <field name="value" value="JoystickID()"/> 
+  </message>
+
+</messages>
+
+</joystick>
diff --git a/sw/ground_segment/joystick/ngs_set_actuators.xml b/conf/joystick/ngs_set_actuators.xml
similarity index 100%
rename from sw/ground_segment/joystick/ngs_set_actuators.xml
rename to conf/joystick/ngs_set_actuators.xml
diff --git a/sw/ground_segment/joystick/xbox_booz_nav.xml b/conf/joystick/xbox_booz_nav.xml
similarity index 100%
rename from sw/ground_segment/joystick/xbox_booz_nav.xml
rename to conf/joystick/xbox_booz_nav.xml
diff --git a/conf/messages.xml b/conf/messages.xml
index 7357d2027de..896f8cae8d9 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1943,6 +1943,15 @@
     <field name="pitch" type="int8"/>
   </message>
 
+  <message name="RC_4CH" id="52" link="broadcasted">
+    <field name="ac_id"       type="uint8"/>
+    <field name="mode"        type="uint8"/>
+    <field name="throttle"    type="uint8"/>
+    <field name="roll"        type="int8"/>
+    <field name="yaw"         type="int8"/>
+    <field name="pitch"       type="int8"/>
+  </message>
+
   <message name="KITE_COMMAND" id="96">
     <field name="POWER"     type="uint16"/>
     <field name="TURN"      type="uint16"/>
diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c
index 89e9c735b7a..cc0320864cc 100644
--- a/sw/airborne/firmwares/fixedwing/datalink.c
+++ b/sw/airborne/firmwares/fixedwing/datalink.c
@@ -194,6 +194,15 @@ LED_TOGGLE(3);
           DL_RC_3CH_roll(dl_buffer),
           DL_RC_3CH_pitch(dl_buffer));
     } else
+    if (msg_id == DL_RC_4CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) {
+LED_TOGGLE(3);
+      parse_rc_4ch_datalink(
+          DL_RC_4CH_joystick_id(dl_buffer),
+          DL_RC_4CH_throttle(dl_buffer),
+          DL_RC_4CH_roll(dl_buffer),
+          DL_RC_4CH_pitch(dl_buffer),
+          DL_RC_4CH_yaw(dl_buffer));
+    } else
 #endif // RC_DATALINK
   { /* Last else */
     /* Parse modules datalink */
diff --git a/sw/ground_segment/joystick/README b/sw/ground_segment/joystick/README
index 6c6fb0bde19..abc7bc77b5b 100644
--- a/sw/ground_segment/joystick/README
+++ b/sw/ground_segment/joystick/README
@@ -21,7 +21,9 @@
 # 
 
 
-Tools for controling an aircraft with a ground joystick
+Tools for controlling an aircraft with a ground joystick
+
+Also used as general purpose joystick to ivybus connector.  Can generate arbitrary ivybus messages based on control input from buttons or axes.  Supports limiting, exponential and scaling on axes.
 
 
 
diff --git a/sw/ground_segment/joystick/input2ivy.ml b/sw/ground_segment/joystick/input2ivy.ml
index df2a3b8fcb3..a4d665aeb4a 100644
--- a/sw/ground_segment/joystick/input2ivy.ml
+++ b/sw/ground_segment/joystick/input2ivy.ml
@@ -24,12 +24,32 @@
  *
  *)
 
+(*  1/26/2011 - Additional functionality added by jpeverill:
+      Joystick xml config files now loaded from PAPARAZZI_HOME/conf/joystick/
+      Exponential output setting (per channel)
+      Limit output setting (per channel)
+      Per channel trim, controlled through joystick buttons
+        Trim can also be saved into auxilliary file, based on aircraft, and loaded at runtime if it exists
+        File will be called <xml joystick profile name>.<A/C name>.trim and is saved in the conf/joystick directory as well
+      Division in channel mapping
+      Interactive keyboard trim control (primitive)
+*)
+
 open Printf
+open Unix
+open Random
+
 
 let (//) = Filename.concat
 let conf_dir = Env.paparazzi_home // "conf"
 let verbose = ref false (* Command line option *)
 
+(** global trim file name *)
+let trim_file_name = ref ""
+
+(** global joystick id *)
+let joystick_id = ref (Random.int 255)
+
 (** Messages libraries *)
 module DL = Pprz.Messages(struct let name = "datalink" end)
 module G = Pprz.Messages(struct let name = "ground" end)
@@ -55,10 +75,11 @@ external stick_read : unit -> int * int * int array = "ml_stick_read"
 (** Range for the input axis *)
 let max_input = 127
 let min_input = -127
+let trim_step = 0.2
 
 (** Representation of an input value *)
 type input =
-    Axis of int * int (* (index, deadband) *)
+    Axis of int * int * float * float * float ref (* (index, deadband, limit, exponent, trim ) *)
   | Button of int
 
 (** Description of a message *)
@@ -66,7 +87,8 @@ type msg = {
     msg_name : string;
     msg_class : string;
     fields : (string * Syntax.expression) list;
-    on_event : Syntax.expression option
+    on_event : Syntax.expression option;
+    send_always : int
   }
 
 (** Represenation of an input device and of the messages to send *)
@@ -76,7 +98,22 @@ type actions = {
     messages : msg list
   }
 
+(** adjust the trim on an axis given its name *)
+let trim_adjust = fun axis_name adjustment ->
+  None
+
+(** Get message class type *)
+let get_message_type = fun class_name ->
+  match class_name with
+    "datalink" -> "Message"
+  | "ground" -> "Message"
+  | "trim_plus" -> "Trim"
+  | "trim_minus" -> "Trim"
+  | "trim_save" -> "Trim"
+  | _ -> failwith class_name    
+
 (** Get a message description from its name (and class name) *)
+(**   class_names with entries above as "Message" should be listed here  *)
 let get_message = fun class_name msg_name ->
   match class_name with
     "datalink" -> snd (DL.message_of_name msg_name)
@@ -159,8 +196,11 @@ let parse_input = fun input ->
       let value =
     match Xml.tag x with
       "axis" ->
+	     let trim = try ExtXml.float_attrib x "trim" with _ -> 0.0 in
+	     let exponent = try ExtXml.float_attrib x "exponent" with _ -> 0. in
+	     let limit = try ExtXml.float_attrib x "limit" with _ -> 1.0 in
              let deadband = try ExtXml.int_attrib x "deadband" with _ -> 0 in
-             Axis (index, deadband)
+             Axis (index, deadband, limit, exponent, ref trim)
     | "button" -> Button index
     | _ -> failwith "parse_input: unexepcted tag" in
       (name, value))
@@ -181,11 +221,16 @@ let parse_msg_field = fun msg_descr field ->
 (** Parse a complete message and build its representation *)
 let parse_msg = fun msg ->
   let msg_name = Xml.attrib msg "name"
-  and msg_class = Xml.attrib msg "class" in
+  and msg_class = Xml.attrib msg "class"
+  and send_always = (try ExtXml.int_attrib msg "send_always" with _ -> 0) in
 
+  let fields =
+    match get_message_type msg_class with
+      "Message" -> 
   let msg_descr = get_message msg_class msg_name in
-
-  let fields = List.map (parse_msg_field msg_descr) (Xml.children msg) in
+         List.map (parse_msg_field msg_descr) (Xml.children msg)
+    | "Trim" -> []
+    | _ -> failwith ("Unknown message class type") in
 
   let on_event =
     try Some (parse_value (Xml.attrib msg "on_event")) with _ -> None in
@@ -193,47 +238,103 @@ let parse_msg = fun msg ->
   { msg_name = msg_name;
     msg_class = msg_class;
     fields = fields;
-    on_event = on_event
+    on_event = on_event;
+    send_always = send_always
   }
 
-(** Parse the complete (input and messages) XML desxription *)
-let parse_descr = fun xml_file ->
+
+(** Verbose List.assco *)
+let my_assoc = fun x l ->
+  try List.assoc x l with Not_found ->
+    failwith (sprintf "my_assoc: %s not found" x)
+
+let first_of_two (x,_) = x
+
+let second_of_two (_,x) = x
+
+(** set a trim value given an inputs array and a trim_values tuple *)
+let trim_set = fun inputs value ->
+  let input = my_assoc (first_of_two value) inputs in
+  match input with
+    Axis (i, deadband, limit, exponent, trim) -> trim := (second_of_two value)
+  | Button i -> failwith "No trim for buttons"
+
+
+(** Input the trim file if it exists *)
+let parse_trim_file = fun trim_file_name inputs ->
+  if Sys.file_exists trim_file_name then begin
+    let trim = Xml.parse_file trim_file_name in
+    let trim_values = List.map
+      (fun x ->
+         let axis = ExtXml.attrib x "axis"
+         and trimval = ExtXml.float_attrib x "value" in
+         (axis, trimval))
+    (Xml.children trim) in
+    List.iter (trim_set inputs) trim_values;
+  end
+
+(** Parse the complete (input and messages) XML desxription 
+    Also parses the trim xml file if it exists *)
+let parse_descr = fun xml_file trim_file ->
   let xml = Xml.parse_file xml_file in
 
   let inputs = parse_input (ExtXml.child xml "input")
   and messages_xml = ExtXml.child xml "messages" in
 
-  let period_ms =truncate (1000.*.ExtXml.float_attrib messages_xml "period")
+  let period_ms = int_of_float (1000.*.ExtXml.float_attrib messages_xml "period")
   and messages = List.map parse_msg (Xml.children messages_xml) in
 
-  { period_ms = period_ms; inputs = inputs; messages = messages }
+  (** check for trim file *)
+  parse_trim_file trim_file inputs;
 
-(** Verbose List.assco *)
-let my_assoc = fun x l ->
-  try List.assoc x l with Not_found ->
-    failwith (sprintf "my_assoc: %s not found" x)
+  { period_ms = period_ms; inputs = inputs; messages = messages }
 
+(** apply deadband  - applied first *)
 let apply_deadband = fun x min ->
   if abs x < min then 0 else x
 
+(** apply limit - applied third *)
+let apply_limit = fun x limit ->
+  limit *. x
+
+(** apply exponent  - applied second *)
+let apply_exponent = fun x expon ->
+  let pow_value = (float_of_int x) ** 3. /. (float_of_int max_input) ** 2. in
+  ( (float_of_int x) *. (1. -. expon)) +. (pow_value *. expon)
+
+(** apply trim  - applied fourth *)
+let apply_trim = fun x trim ->
+  let x_new = (int_of_float (x +. trim)) in
+  if x_new > max_input then max_input else (if x_new < min_input then min_input else x_new)
+
 (** Access to an input value, button or axis *)
 let eval_input = fun buttons axis input ->
   match input with
-    Axis (i, deadband) -> apply_deadband axis.(i) deadband
+    Axis (i, deadband, limit, exponent, trim) ->  (apply_trim (apply_limit (apply_exponent (apply_deadband axis.(i) deadband) exponent) limit) trim.contents)
   | Button i -> (buttons lsr i) land 0x1
 
 (** Scale a value in the given bounds *)
 let scale = fun x min max ->
   min + ((x - min_input) * (max - min)) / (max_input - min_input)
 
+(** Scale a value in the given bounds *)
+let bound = fun x min max ->
+  if x < min then min else (if x > max then max else x)
+
 (** Eval a function call (TO BE COMPLETED) *)
 let eval_call = fun f args ->
   match f, args with
     "-", [a1; a2] -> a1 - a2
   | "+", [a1; a2] -> a1 + a2
   | "*", [a1; a2] -> a1 * a2
+  | "%", [a1; a2]  -> a1 / a2
   | "&&", [a1; a2] -> a1 land a2
+  | "||", [a1; a2] -> a1 lor a2
+  | "<",  [a1;a2]  ->  if a1 < a2 then 1 else 0
+  | ">",  [a1;a2]  ->  if a1 > a2 then 1 else 0
   | "Scale", [x; min; max] -> scale (x) (min) (max)
+  | "Bound", [x; min; max] -> bound (x) (min) (max)
+  | "JoystickID", [] -> !joystick_id
   | f, args -> failwith (sprintf "eval_call: unknown function '%s'" f)
 
 (** Eval an expression *)
@@ -262,6 +363,35 @@ let get_previous_values = fun msg_name ->
 let record_values = fun msg_name values ->
   Hashtbl.replace last_values msg_name values
 
+let second_list (_,x) = x
+let first_list (x,_) = x
+
+(** add a leaf *)
+let trim_save_add_leaf = fun x channel_pair ->
+  let chan_name = first_list channel_pair in
+  let channel = second_list channel_pair in
+  match channel with 
+    Axis (i, deadband, limit, exponent, trim) -> x := x.contents ^ (Printf.sprintf "<trim axis='%s' value = '%f'/>" chan_name trim.contents)
+  | Button i -> Printf.printf "%d" i
+
+(** save trim settings to file *)
+let trim_save = fun inputs ->
+  let xmlstring = ref "<trims>" in
+  List.iter (trim_save_add_leaf xmlstring) inputs;
+  xmlstring := xmlstring.contents ^ "</trims>";
+  let x = Xml.parse_string xmlstring.contents in
+  let pretty_xml_string = Xml.to_string_fmt x in
+  let output_trim_file = open_out trim_file_name.contents in
+  output_string output_trim_file pretty_xml_string;
+  close_out output_trim_file
+
+(** Adjust the trim on a specified channel *)
+let trim_adjust = fun axis_name adjustment inputs ->
+  let input = my_assoc axis_name inputs in
+  match input with
+    Axis (i, deadband, limit, exponent, trim) -> trim := trim.contents +. adjustment
+  | Button i -> failwith "No trim for buttons"
+
 (**Send an ivy message if needed: new values and/or 'on_event' condition true*)
 let execute_action = fun ac_id inputs buttons axis message ->
   let values =
@@ -275,11 +405,14 @@ let execute_action = fun ac_id inputs buttons axis message ->
     | Some expr -> eval_expr buttons axis inputs expr <> 0 in
 
   let previous_values = get_previous_values message.msg_name in
-  if (on_event, values) <> previous_values && on_event then begin
+  if ( ( (on_event, values) <> previous_values ) || message.send_always = 1 ) && on_event then begin
     let vs = ("ac_id", Pprz.Int ac_id) :: values in
     match message.msg_class with
       "datalink" -> DL.message_send "input2ivy" message.msg_name vs
     | "ground" -> G.message_send "input2ivy" message.msg_name vs
+    | "trim_plus" -> trim_adjust message.msg_name trim_step inputs
+    | "trim_minus" -> trim_adjust message.msg_name (-.trim_step) inputs 
+    | "trim_save" -> trim_save inputs
     | c -> failwith (sprintf "execute_action: unknown class '%s'" c)
   end;
   record_values message.msg_name (on_event, values)
@@ -287,18 +420,19 @@ let execute_action = fun ac_id inputs buttons axis message ->
 
 (** Output on stderr the values from the input device *)
 let print_inputs = fun nb_buttons buttons axis ->
-  fprintf stderr "buttons: ";
+  fprintf Pervasives.stderr "buttons: ";
   for i = 0 to nb_buttons - 1 do
-    fprintf stderr "%d:%d " i (eval_input buttons axis (Button i))
+    fprintf Pervasives.stderr "%d:%d " i (eval_input buttons axis (Button i))
   done;
-  fprintf stderr "\naxis: ";
+  fprintf Pervasives.stderr "\naxis: ";
   for i = 0 to Array.length axis - 1 do
-    fprintf stderr "%d:%d " i (eval_input buttons axis (Axis (i, 0)))
+    fprintf Pervasives.stderr "%d:%d " i (eval_input buttons axis (Axis (i, 0, 1.0, 0.0, ref 0.0)))
   done;
-  fprintf stderr "\n%!"
+  fprintf Pervasives.stderr "\n%!"
 
 
-(** Get the values from the input values and send messages *)
+(** Get the values from the input values and send messages
+     This is called at a rate programmed in the xml  *)
 let execute_actions = fun actions ac_id ->
   try
     let (nb_buttons, buttons, axis) = stick_read () in
@@ -311,11 +445,36 @@ let execute_actions = fun actions ac_id ->
     exc -> prerr_endline (Printexc.to_string exc)
 
 
+(**  process keyboard commands *)
+(**   used for adjusting trims interactively from the keyboard *)
+(**   this capability is mostly for bench-time trimming when a joystick does not have adequate buttons *)
+(**   it is not a very complete capability  *)
+let execute_kb_action = fun actions conditions -> 
+  let ch = input_byte Pervasives.stdin in
+  (** esdx for left stick
+      ijkm for right  *)
+
+  if true then begin
+  match ch with
+    101 -> trim_adjust "ly" 1.0 actions.inputs
+  | 115 -> trim_adjust "lx" (-1.0) actions.inputs
+  | 100 -> trim_adjust "lx" 1.0 actions.inputs
+  | 120 -> trim_adjust "ly" (-1.0) actions.inputs
+  | 105 -> trim_adjust "ry" 1.0 actions.inputs
+  | 106 -> trim_adjust "rx" (-1.0) actions.inputs
+  | 107 -> trim_adjust "rx" 1.0 actions.inputs
+  | 109 -> trim_adjust "ry" (-1.0) actions.inputs
+  | _ -> trim_adjust "ly" 0.0 actions.inputs
+  end;
+
+  true
+
+ 
 
 (************************************* MAIN **********************************)
 let () =
-let ivy_bus = ref Defivybus.default_ivy_bus  in
-  let device_name = ref ""
+  let ivy_bus = ref Defivybus.default_ivy_bus  in
+  let device_name = ref "/dev/input/js0"
   and ac_name = ref "MYAC"
   and xml_descr = ref "" in
 
@@ -325,6 +484,7 @@ let ivy_bus = ref Defivybus.default_ivy_bus  in
       "-ac",  Arg.Set_string ac_name, "<A/C name>";
       "-d",  Arg.Set_string device_name, "<device name>";
       "-v",  Arg.Set verbose, "Verbose mode (useful to identify the channels of an input device)";
+      "-id", Arg.Set_int joystick_id, "Joystick ID, from 0-255.  Each joystick requires a unique ID in a multiple joystick configuration.";
       "-", Arg.String anon_fun, "<xml file of actions>"
     ]
   and usage_msg = "Usage: " in
@@ -341,7 +501,13 @@ let ivy_bus = ref Defivybus.default_ivy_bus  in
   hash_index_of_settings !ac_name;
   hash_index_of_blocks !ac_name;
 
-  let actions = parse_descr !xml_descr in
+  Printf.printf "Joystick ID: %u\n" !joystick_id;
+
+  let joystick_conf_dir = conf_dir ^ "/joystick/" in
+  let xml_descr_full = joystick_conf_dir ^ !xml_descr in
+  trim_file_name := String.concat "." [xml_descr_full ; !ac_name ; "trim"];
+
+  let actions = parse_descr xml_descr_full trim_file_name.contents in
 
   if stick_init !device_name <> 0 then
     failwith (sprintf "Error: cannot open device %s\n" !device_name);
@@ -350,8 +516,15 @@ let ivy_bus = ref Defivybus.default_ivy_bus  in
   Ivy.init "Paparazzi joystick" "READY" (fun _ _ -> ());
   Ivy.start !ivy_bus;
 
+  (** setup stdin *)
+  let tstatus = (Unix.tcgetattr Unix.stdin) in
+  tstatus.c_icanon <- false;
+  Unix.tcsetattr Unix.stdin Unix.TCSANOW tstatus;
+
   ignore (Glib.Timeout.add actions.period_ms (fun () -> execute_actions actions ac_id; true));
+  ignore (Glib.Io.add_watch ~cond:[`IN] ~callback:(fun x -> execute_kb_action actions x) (Glib.Io.channel_of_descr Unix.stdin));
 
   (** Start the main loop *)
   let loop = Glib.Main.create true in
   while Glib.Main.is_running loop do ignore (Glib.Main.iteration true) done
+
diff --git a/sw/ground_segment/joystick/usb_stick.c b/sw/ground_segment/joystick/usb_stick.c
index 87c7a3fab87..8d78c8b3166 100644
--- a/sw/ground_segment/joystick/usb_stick.c
+++ b/sw/ground_segment/joystick/usb_stick.c
@@ -21,6 +21,11 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 
+/*  1/26/2011  - jpeverill added
+      support for joystick specific event interface.  During testing it was found that many devices do not work properly with the standard event interface (such as the PS3 joystick).  The joystick interface seems to work better.  The old event interface is used if "event" is found in the name, otherwise the joystick interface is used.
+      now kills the process in the event of a read error (joystick unplugs)
+*/
+
 //#define STICK_DBG 1
 
 #include "usb_stick.h"
@@ -33,11 +38,14 @@
 #include <errno.h>
 #include <math.h>
 #include <glib.h>
-#include <linux/input.h>
 #include <sys/ioctl.h>
 #include <Ivy/ivy.h>
 #include <Ivy/ivyglibloop.h>
 
+//needed for joystick interface
+#include <linux/input.h>
+#include <linux/joystick.h>
+
 #ifdef STICK_DBG
 #define dbgprintf fprintf
 #else
@@ -62,15 +70,19 @@
 int stick_device_handle;
 
 int8_t stick_axis_values[AXIS_COUNT] = {0, 0, 0, 0, 0, 0};
-int16_t stick_button_values = 0;
+int32_t stick_button_values = 0;
 
 int axis_code[AXIS_COUNT];
 int stick_axis_count = 0;
 int button_code[BUTTON_COUNT];
 int stick_button_count = 0;
+int hid_led_count = 0;
+
 
 int32_t axis_min[AXIS_COUNT], axis_max[AXIS_COUNT];
 
+int event_mode = STICK_MODE_UNKNOWN;
+
 struct stick_code_param_ stick_init_param = {
   0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
   0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
@@ -79,17 +91,56 @@ struct stick_code_param_ stick_init_param = {
 //struct ff_effect effect;
 
 
+void led_test( void ) {
+  
+  struct input_event ev; /* the event */
+
+  int lednum;
+
+  /* we turn off all the LEDs to start */
+  ev.type = EV_LED;
+
+  while (1) {
+    for (lednum=0;lednum<32;lednum++) {
+      ev.code = lednum;
+      ev.value = 1;
+      write(stick_device_handle, &ev, sizeof(struct input_event));
+    }
+
+    usleep(1000000);
+
+    for (lednum=0;lednum<32;lednum++) {
+      ev.code = lednum;
+      ev.value = 0;
+      write(stick_device_handle, &ev, sizeof(struct input_event));
+    }
+  }
+
+
+}
+
+
 int init_hid_device(char* device_name)
 {
   int cnt;
-  unsigned long key_bits[32],abs_bits[32];
+  unsigned long buttons,axes;
+  unsigned long key_bits[32],abs_bits[32],led_bits[32];
   //	unsigned long ff_bits[32];
   int valbuf[16];
-  char name[256] = "Unknown";
+  char name[MAX_NAME_LENGTH] = "Unknown";
 
   stick_button_count = 0;
   stick_axis_count = 0;
 
+  /* Detect device mode, event interface or joystick */
+  if (strstr( device_name, "event" ) == 0 ) {
+    dbgprintf(stderr," Using Joystick interface\n");
+    event_mode = STICK_MODE_JOYSTICK;
+  } else {
+    dbgprintf(stderr," Using Event interface\n");
+    event_mode = STICK_MODE_EVENT;
+  }
+
   /* Open event device read only (with write permission for ff) */
   stick_device_handle = open(device_name,O_RDONLY|O_NONBLOCK);
   if (stick_device_handle<0) {
@@ -98,12 +149,35 @@ int init_hid_device(char* device_name)
     return(1);
   }
 
+  /* LED testing */
+  
+//  led_test( );
+
+
   /* Which buttons has the device? */
   memset(key_bits,0,32*sizeof(unsigned long));
-  if (ioctl(stick_device_handle,EVIOCGBIT(EV_KEY,32*sizeof(unsigned long)),key_bits)<0) {
-    dbgprintf(stderr,"ERROR: can not get key bits (%s) [%s:%d]\n",
-        strerror(errno),__FILE__,__LINE__);
-    return(1);
+  memset(led_bits,0,32*sizeof(unsigned long));
+  if (event_mode == STICK_MODE_EVENT ) {
+    if (ioctl(stick_device_handle,EVIOCGBIT(EV_KEY,32*sizeof(unsigned long)),key_bits)<0) {
+      dbgprintf(stderr,"ERROR: can not get key bits (%s) [%s:%d]\n",
+          strerror(errno),__FILE__,__LINE__);
+      return(1);
+    }
+    //read LED count
+    if (ioctl(stick_device_handle,EVIOCGLED(32*sizeof(led_bits)),led_bits)<0) {
+      dbgprintf(stderr,"ERROR: can not get LED bits (%s) [%s:%d]\n",
+          strerror(errno),__FILE__,__LINE__);
+      return(1);
+    }
+    dbgprintf(stderr,"LEDs: %X",*led_bits);
+
+
+  } else {
+    if (ioctl(stick_device_handle,JSIOCGBUTTONS,&buttons)<0) {
+      dbgprintf(stderr,"ERROR: can not get key bits (%s) [%s:%d]\n",
+          strerror(errno),__FILE__,__LINE__);
+      return(1);
+    }
   }
 
   /* Store buttons */
@@ -118,13 +192,22 @@ int init_hid_device(char* device_name)
     memcpy(button_code,stick_init_param.button_code,BUTTON_COUNT*sizeof(int));
   }
   else {
-    for (cnt = MIN_BUTTON_CODE; cnt < MAX_BUTTON_CODE; cnt++) {
-      if (TEST_BIT(cnt, key_bits)) {
+    if (event_mode == STICK_MODE_EVENT) {
+      for (cnt = MIN_BUTTON_CODE; cnt < MAX_BUTTON_CODE; cnt++) {
+        if (TEST_BIT(cnt, key_bits)) {
+          button_code[stick_button_count++] = cnt;
+          dbgprintf(stderr,"Available button: %d (0x%x)\n",cnt,cnt);
+        }
+        if (stick_button_count == BUTTON_COUNT) break;
+      }
+    } else {
+      for (cnt = 0; cnt < buttons; cnt++) {
         button_code[stick_button_count++] = cnt;
         dbgprintf(stderr,"Available button: %d (0x%x)\n",cnt,cnt);
+        if (stick_button_count == BUTTON_COUNT) break;
       }
-      if (stick_button_count == BUTTON_COUNT) break;
     }
+
     if (stick_button_count == 0) {
       dbgprintf(stderr,"ERROR: no suitable buttons found [%s:%d]\n",__FILE__,__LINE__);
     }
@@ -132,10 +215,18 @@ int init_hid_device(char* device_name)
 
   /* Which axis has the device? */
   memset(abs_bits,0,32*sizeof(unsigned long));
-  if (ioctl(stick_device_handle,EVIOCGBIT(EV_ABS,32*sizeof(unsigned long)),abs_bits)<0) {
-    dbgprintf(stderr,"ERROR: can not get abs bits (%s) [%s:%d]\n",
-        strerror(errno),__FILE__,__LINE__);
-    return(1);
+  if (event_mode == STICK_MODE_EVENT ) {
+    if (ioctl(stick_device_handle,EVIOCGBIT(EV_ABS,32*sizeof(unsigned long)),abs_bits)<0) {
+      dbgprintf(stderr,"ERROR: can not get abs bits (%s) [%s:%d]\n",
+          strerror(errno),__FILE__,__LINE__);
+      return(1);
+    }
+  } else {
+    if (ioctl(stick_device_handle,JSIOCGAXES,&axes)<0) {
+      dbgprintf(stderr,"ERROR: can not get abs bits (%s) [%s:%d]\n",
+          strerror(errno),__FILE__,__LINE__);
+      return(1);
+    }
   }
 
   /* Store axis */
@@ -150,13 +241,22 @@ int init_hid_device(char* device_name)
     memcpy(axis_code,stick_init_param.axis_code,AXIS_COUNT*sizeof(int));
   }
   else {
-    for (cnt = MIN_ABS_CODE; cnt < MAX_ABS_CODE; cnt++) {
-      if (TEST_BIT(cnt, abs_bits)) {
+    if(event_mode == STICK_MODE_EVENT) {
+      for (cnt = MIN_ABS_CODE; cnt < MAX_ABS_CODE; cnt++) {
+        if (TEST_BIT(cnt, abs_bits)) {
+          axis_code[stick_axis_count++] = cnt;
+          dbgprintf(stderr,"Available axis: %d (0x%x)\n",cnt,cnt);
+        }
+        if (stick_axis_count == AXIS_COUNT) break;
+      }
+    } else {
+      for (cnt = 0; cnt < axes; cnt++) {
         axis_code[stick_axis_count++] = cnt;
         dbgprintf(stderr,"Available axis: %d (0x%x)\n",cnt,cnt);
+        if (stick_axis_count == AXIS_COUNT) break;
       }
-      if (stick_axis_count == AXIS_COUNT) break;
     }
+
     // at least 2 axis are needed in auto detection
     if (stick_axis_count < 2) {
       dbgprintf(stderr,"ERROR: no suitable axis found [%s:%d]\n",__FILE__,__LINE__);
@@ -167,14 +267,21 @@ int init_hid_device(char* device_name)
   /* Axis param */
   for (cnt = 0; cnt < stick_axis_count; cnt++)
   {
-    /* get axis value range */
-    if (ioctl(stick_device_handle,EVIOCGABS(axis_code[cnt]),valbuf)<0) {
-      dbgprintf(stderr,"ERROR: can not get axis %d value range (%s) [%s:%d]\n",
-          cnt,strerror(errno),__FILE__,__LINE__);
-      return(1);
-    }
-    axis_min[cnt]=valbuf[1];
-    axis_max[cnt]=valbuf[2];
+    if (event_mode == STICK_MODE_EVENT ) {
+      /* get axis value range */
+      if (ioctl(stick_device_handle,EVIOCGABS(axis_code[cnt]),valbuf)<0) {
+        dbgprintf(stderr,"ERROR: can not get axis %d value range (%s) [%s:%d]\n",
+            cnt,strerror(errno),__FILE__,__LINE__);
+        return(1);
+      }
+      axis_min[cnt]=valbuf[1];
+      axis_max[cnt]=valbuf[2];
+    } else {
+      // with joystick interface, all axes are signed 16 bit with full range
+      axis_min[cnt]=-32768;
+      axis_max[cnt]=32768;
+    }      
+
     if (axis_min[cnt]>=axis_max[cnt]) {
       dbgprintf(stderr,"ERROR: bad axis %d value range (%d,%d) [%s:%d]\n",
           cnt,axis_min[cnt],axis_max[cnt],__FILE__,__LINE__);
@@ -184,6 +291,9 @@ int init_hid_device(char* device_name)
         cnt,axis_min[cnt],axis_max[cnt]);
   }
 
+  //--------------------------------------------------
+  // force feedback, TBD feature
+  //--------------------------------------------------
 #if 0
   /* Now get some information about force feedback */
   memset(ff_bits,0,32*sizeof(unsigned long));
@@ -245,7 +355,11 @@ int init_hid_device(char* device_name)
   }
 #endif	
 
-  ioctl(stick_device_handle, EVIOCGNAME(sizeof(name)), name);
+  if (event_mode == STICK_MODE_EVENT ) {
+    ioctl(stick_device_handle, EVIOCGNAME(sizeof(name)), name);
+  } else {
+    ioctl(stick_device_handle, JSIOCGNAME(MAX_NAME_LENGTH), name);
+  }
   printf("Input device name: \"%s\" on device \"%s\"\n", name, device_name);
 
   return(0);
@@ -256,61 +370,79 @@ int stick_read( void ) {
 
   int cnt;
   struct input_event event;
+  struct js_event jsevent;
+
+  if (event_mode == STICK_MODE_EVENT ) {
+    /* Get events */
+    while (read(stick_device_handle,&event,sizeof(event))==sizeof(event)) {
+
+      switch (event.type) {
+        case EV_KEY:
+          for (cnt = 0; cnt < stick_button_count; cnt++) {
+            if (event.code == button_code[cnt]) {
+              if (event.value) stick_button_values |= (1 << cnt); // Set bit
+              else stick_button_values &= ~(1 << cnt); // Clear bit
+              break;
+            }
+          }
+          break;
+        case EV_ABS:
+          for (cnt = 0; cnt < stick_axis_count; cnt++) {
+            if (event.code == axis_code[cnt]) {
+              stick_axis_values[cnt] = (((event.value) - axis_min[cnt]))*ABS_MAX_VALUE / (axis_max[cnt] - axis_min[cnt]) - ABS_MID_VALUE;
+              break;
+            }
+          }
+          break;
+        default: break;
+      }
+    }
 
-  /* Get events */
-  while (read(stick_device_handle,&event,sizeof(event))==sizeof(event)) {
+  } else {
 
-    switch (event.type) {
-      case EV_KEY:
-        for (cnt = 0; cnt < stick_button_count; cnt++) {
-          if (event.code == button_code[cnt]) {
-            if (event.value) stick_button_values |= (1 << cnt); // Set bit
-            else stick_button_values &= ~(1 << cnt); // Clear bit
-            break;
+    /* Get joystick events */
+    while (read(stick_device_handle,&jsevent,sizeof(jsevent))==sizeof(jsevent)) {
+
+      switch (jsevent.type) {
+        case JS_EVENT_BUTTON:
+          for (cnt = 0; cnt < stick_button_count; cnt++) {
+            if (jsevent.number == button_code[cnt]) {
+              if (jsevent.value) stick_button_values |= (1 << cnt); // Set bit
+              else stick_button_values &= ~(1 << cnt); // Clear bit
+              break;
+            }
           }
-        }
-        break;
-      case EV_ABS:
-        for (cnt = 0; cnt < stick_axis_count; cnt++) {
-          if (event.code == axis_code[cnt]) {
-            stick_axis_values[cnt] = (((event.value) - axis_min[cnt]))*ABS_MAX_VALUE / (axis_max[cnt] - axis_min[cnt]) - ABS_MID_VALUE;
-            break;
+          break;
+        case JS_EVENT_AXIS:
+          for (cnt = 0; cnt < stick_axis_count; cnt++) {
+            if (jsevent.number == axis_code[cnt]) {
+              stick_axis_values[cnt] = (( (jsevent.value - axis_min[cnt]) * ABS_MAX_VALUE ) / (axis_max[cnt] - axis_min[cnt])) - ABS_MID_VALUE;    
+              break;
+            }
           }
-        }
-        break;
-      default: break;
+          break;
+        default: break;
+      }
     }
 
   }
 
-  dbgprintf(stderr,"buttons %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d | ",
-      (stick_button_values >> 0) & 1,
-      (stick_button_values >> 1) & 1,
-      (stick_button_values >> 2) & 1,
-      (stick_button_values >> 3) & 1,
-      (stick_button_values >> 4) & 1,
-      (stick_button_values >> 5) & 1,
-      (stick_button_values >> 6) & 1,
-      (stick_button_values >> 7) & 1,
-      (stick_button_values >> 8) & 1,
-      (stick_button_values >> 9) & 1,
-      (stick_button_values >> 10) & 1,
-      (stick_button_values >> 11) & 1,
-      (stick_button_values >> 12) & 1,
-      (stick_button_values >> 13) & 1,
-      (stick_button_values >> 14) & 1,
-      (stick_button_values >> 15) & 1);
-  dbgprintf(stderr,"axis %d %d %d %d %d %d %d %d %d %d\n",
-      stick_axis_values[0],
-      stick_axis_values[1],
-      stick_axis_values[2],
-      stick_axis_values[3],
-      stick_axis_values[4],
-      stick_axis_values[5],
-      stick_axis_values[6],
-      stick_axis_values[7],
-      stick_axis_values[8],
-      stick_axis_values[9]);
+  if (errno != EAGAIN) {
+    printf("Joystick read error!\n");
+    exit(1);
+  }
+
+  dbgprintf(stderr,"buttons ");
+  for (cnt = 0; cnt < stick_button_count; cnt++) {
+    dbgprintf(stderr,"%d ",(stick_button_values >> cnt) & 1 );
+  }
+
+  dbgprintf(stderr,"| axes ");
+  for (cnt = 0; cnt < stick_axis_count; cnt++) {
+    dbgprintf(stderr,"%d ",stick_axis_values[cnt]);
+  }
+
+  dbgprintf(stderr,"\n");
 
   return 0;
 }
@@ -318,7 +450,7 @@ int stick_read( void ) {
 
 int stick_init( char * device_name ) {
 
-  char devname[256];
+  char devname[MAX_NAME_LENGTH];
   int cnt = 0;
 
   /* test device_name, else look for a suitable device */
diff --git a/sw/ground_segment/joystick/usb_stick.h b/sw/ground_segment/joystick/usb_stick.h
index 65e709219ee..babde27e4da 100644
--- a/sw/ground_segment/joystick/usb_stick.h
+++ b/sw/ground_segment/joystick/usb_stick.h
@@ -27,17 +27,26 @@
 #include <linux/input.h>
 
 /* Max number of axis and buttons */
-#define STICK_BUTTON_COUNT    16
-#define STICK_AXIS_COUNT      10
+/*  Increased, many new controllers have pressure sensitive buttons that show up as axes */
+#define STICK_BUTTON_COUNT    32
+#define STICK_AXIS_COUNT      32
 
 /* Default values for the options */
 #define STICK_INPUT_DEV_MAX   15
 #define STICK_DEVICE_NAME     "/dev/input/event"
 
+/* Event mode switched */
+#define STICK_MODE_UNKNOWN    0
+#define STICK_MODE_EVENT      1
+#define STICK_MODE_JOYSTICK   2
+
+/* Max name length */
+#define MAX_NAME_LENGTH       255
+
 /* Global variables about the initialized device */
 extern int stick_device_handle;
 extern int8_t stick_axis_values[STICK_AXIS_COUNT];
-extern int16_t stick_button_values;
+extern int32_t stick_button_values;
 extern int stick_axis_count, stick_button_count;
 
 /* Structure for custom configuration

From c7fddd84cfc0cfea75c03c52ce5ec4ae5032807d Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Thu, 24 Mar 2011 18:24:22 +0100
Subject: [PATCH 02/99] send_always is now a boolean

---
 .gitignore                                |  7 ++--
 conf/joystick/aurora_skate_controller.xml | 32 +++++++--------
 conf/joystick/logitech_dual_action.xml    | 10 ++---
 conf/joystick/logitech_f710.xml           | 32 +++++++--------
 sw/ground_segment/joystick/input2ivy.ml   | 49 +++++++++++------------
 5 files changed, 65 insertions(+), 65 deletions(-)

diff --git a/.gitignore b/.gitignore
index 0d10b010ddb..a721f0a463d 100644
--- a/.gitignore
+++ b/.gitignore
@@ -78,6 +78,10 @@
 /sw/ground_segment/tmtc/server
 /sw/ground_segment/tmtc/diadec
 
+# /sw/ground_segment/joystick
+/sw/ground_segment/joystick/input2ivy
+/sw/ground_segment/joystick/test_stick
+
 # /sw/lib/ocaml/
 /sw/lib/ocaml/gtk_papget_editor.ml
 /sw/lib/ocaml/gtk_papget_text_editor.ml
@@ -109,9 +113,6 @@
 /sw/tools/fp_parser.ml
 /sw/tools/wiki_gen/wiki_gen
 
-# /sw/ground_segment/joystick
-/sw/ground_segment/joystick/test_stick
-
 # /sw/airborne/arch/lpc21/test/bootloader
 /sw/airborne/arch/lpc21/test/bootloader/bl.dmp
 /sw/airborne/arch/lpc21/test/bootloader/bl.hex
diff --git a/conf/joystick/aurora_skate_controller.xml b/conf/joystick/aurora_skate_controller.xml
index 9b3e579bd21..e803f2707fd 100644
--- a/conf/joystick/aurora_skate_controller.xml
+++ b/conf/joystick/aurora_skate_controller.xml
@@ -41,7 +41,7 @@ It has 9 buttons.
 <messages period="0.0333333333">
 
 
-  <message class="datalink" name="RC_4CH" send_always="1">
+  <message class="datalink" name="RC_4CH" send_always="true">
    <field name="joystick_id" value="JoystickID()"/>
    <field name="throttle"    value="Scale(ly,-10,127)"/>
    <field name="roll"        value="rx"/>
@@ -51,44 +51,44 @@ It has 9 buttons.
 
 
   <!-- trim commands -->
-  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="b1">
+  <message class="ground" name="DL_SETTING_MOD" on_event="b1">
     <field name="index" value="IndexOfSetting('rc_roll_trim')"/>
     <field name="mod" value="10"/>
   </message>
-  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="b0">
+  <message class="ground" name="DL_SETTING_MOD" on_event="b0">
     <field name="index" value="IndexOfSetting('rc_roll_trim')"/>
     <field name="mod" value="0-10"/>
   </message>
 
-  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="b3">
+  <message class="ground" name="DL_SETTING_MOD" on_event="b3">
     <field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
     <field name="mod" value="10"/>
   </message>
-  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="b2">
+  <message class="ground" name="DL_SETTING_MOD" on_event="b2">
     <field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
     <field name="mod" value="0-10"/>
   </message>
 
-  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="b11">
+  <message class="ground" name="DL_SETTING_MOD" on_event="b11">
     <field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
     <field name="mod" value="10"/>
   </message>
-  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="b10">
+  <message class="ground" name="DL_SETTING_MOD" on_event="b10">
     <field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
     <field name="mod" value="0-10"/>
   </message>
 
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b10 && b11">
+  <message class="ground" name="DL_SETTING" on_event="b10 && b11">
     <field name="index" value="IndexOfSetting('rc_roll_trim')"/>
     <field name="value" value="0"/>
   </message>
 
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b10 && b11">
+  <message class="ground" name="DL_SETTING" on_event="b10 && b11">
     <field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
     <field name="value" value="0"/>
   </message>
 
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b10 && b11">
+  <message class="ground" name="DL_SETTING" on_event="b10 && b11">
     <field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
     <field name="value" value="0-10"/>
   </message>
@@ -105,36 +105,36 @@ It has 9 buttons.
 
 <!-- Camera switch. Currently uses same buttons as yaw trim -->
 
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b1">
+  <message class="ground" name="DL_SETTING" on_event="b1">
     <field name="index" value="IndexOfSetting('cam_switch')"/>
     <field name="value" value="1"/> 
   </message>
 
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b0">
+  <message class="ground" name="DL_SETTING" on_event="b0">
     <field name="index" value="IndexOfSetting('cam_switch')"/>
     <field name="value" value="0"/> 
   </message>
 
 
   <!-- Red Button -->  <!-- AUTO2 mode -->
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b8">
+  <message class="ground" name="DL_SETTING" on_event="b8">
     <field name="index" value="IndexOfSetting('pprz_mode')"/>
     <field name="value" value="2"/> 
   </message>
 
   <!-- Yellow Button -->   <!-- AUTO1 mode -->
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b7">
+  <message class="ground" name="DL_SETTING" on_event="b7">
     <field name="index" value="IndexOfSetting('pprz_mode')"/>
     <field name="value" value="1"/>
   </message>
 
   <!-- Green Button -->  <!-- MANUAL mode -->
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b9">
+  <message class="ground" name="DL_SETTING" on_event="b9">
     <field name="index" value="IndexOfSetting('pprz_mode')"/>
     <field name="value" value="0"/> 
   </message>
 
- <message class="ground" name="DL_SETTING" send_always="0" on_event="b7 || b8 || b9">
+ <message class="ground" name="DL_SETTING" on_event="b7 || b8 || b9">
     <field name="index" value="IndexOfSetting('rc_dl_active_joystick')"/>
     <field name="value" value="JoystickID()"/> 
   </message>
diff --git a/conf/joystick/logitech_dual_action.xml b/conf/joystick/logitech_dual_action.xml
index ecbe18f3ebc..e9d7572d7ca 100644
--- a/conf/joystick/logitech_dual_action.xml
+++ b/conf/joystick/logitech_dual_action.xml
@@ -38,7 +38,7 @@ The "mode" button swaps the axes on the left stick and the d pad.
 <messages period="0.017">
 
 
-  <message class="datalink" name="RC_4CH" send_always="1">
+  <message class="datalink" name="RC_4CH" send_always="true">
    <field name="throttle" value="Scale(0-ly,0,255)"/>
    <field name="roll" value="rx"/>
    <field name="yaw" value="lx"/>
@@ -60,23 +60,23 @@ The "mode" button swaps the axes on the left stick and the d pad.
  -->
 
   <!-- Button 5 top left bumper) -->  <!-- AUTO1 mode -->
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b4">
+  <message class="ground" name="DL_SETTING" on_event="b4">
     <field name="index" value="5"/>
     <field name="value" value="1"/> 
   </message>
  
   <!-- Button 7 bottom left bumper -->   <!-- AUTO2 mode -->
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b6">
+  <message class="ground" name="DL_SETTING" on_event="b6">
     <field name="index" value="5"/>
     <field name="value" value="2"/>
   </message>
 
   <!-- Buttons 6+8 (right pad) -->  <!-- KILL mode -->
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b5">
+  <message class="ground" name="DL_SETTING" on_event="b5">
     <field name="index" value="5"/>
     <field name="value" value="5"/> 
   </message>
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b7">
+  <message class="ground" name="DL_SETTING" on_event="b7">
     <field name="index" value="5"/>
     <field name="value" value="5"/> 
   </message>
diff --git a/conf/joystick/logitech_f710.xml b/conf/joystick/logitech_f710.xml
index 66bd8f9d21f..dc6c75ef6b4 100644
--- a/conf/joystick/logitech_f710.xml
+++ b/conf/joystick/logitech_f710.xml
@@ -50,7 +50,7 @@ It has 8 buttons.
 <messages period="0.0333333333">
 
 
-  <message class="datalink" name="RC_4CH" send_always="1">
+  <message class="datalink" name="RC_4CH" send_always="true">
    <field name="joystick_id" value="JoystickID()"/>
    <field name="throttle"    value="Bound(0-ly,0,127)"/>
    <field name="roll"        value="rx"/>
@@ -60,44 +60,44 @@ It has 8 buttons.
 
 
   <!-- trim commands -->
-  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="dx > 100">
+  <message class="ground" name="DL_SETTING_MOD" on_event="dx > 100">
     <field name="index" value="IndexOfSetting('rc_roll_trim')"/>
     <field name="mod" value="10"/>
   </message>
-  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="0-dx > 100"	>
+  <message class="ground" name="DL_SETTING_MOD" on_event="0-dx > 100"	>
     <field name="index" value="IndexOfSetting('rc_roll_trim')"/>
     <field name="mod" value="0-10"/>	
   </message>
 
-  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="dy > 100">
+  <message class="ground" name="DL_SETTING_MOD" on_event="dy > 100">
     <field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
     <field name="mod" value="10"/>
   </message>
-  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="0-dy > 100">
+  <message class="ground" name="DL_SETTING_MOD" on_event="0-dy > 100">
     <field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
     <field name="mod" value="0-10"/>
   </message>
 
-  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="b_rtb">
+  <message class="ground" name="DL_SETTING_MOD" on_event="b_rtb">
     <field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
     <field name="mod" value="10"/>
   </message>
-  <message class="ground" name="DL_SETTING_MOD" send_always="0" on_event="b_ltb">
+  <message class="ground" name="DL_SETTING_MOD" on_event="b_ltb">
     <field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
     <field name="mod" value="0-10"/>
   </message>
 
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b_ltb && b_rtb">
+  <message class="ground" name="DL_SETTING" on_event="b_ltb && b_rtb">
     <field name="index" value="IndexOfSetting('rc_roll_trim')"/>
     <field name="value" value="0"/>
   </message>
 
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b_ltb && b_rtb">
+  <message class="ground" name="DL_SETTING" on_event="b_ltb && b_rtb">
     <field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
     <field name="value" value="0"/>
   </message>
 
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b_ltb && b_rtb">
+  <message class="ground" name="DL_SETTING" on_event="b_ltb && b_rtb">
     <field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
     <field name="value" value="0-10"/>
   </message>
@@ -114,36 +114,36 @@ It has 8 buttons.
 
 <!-- Camera switch. Currently uses same buttons as yaw trim -->
 
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b_start">
+  <message class="ground" name="DL_SETTING" on_event="b_start">
     <field name="index" value="IndexOfSetting('cam_switch')"/>
     <field name="value" value="1"/> 
   </message>
 
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b_back">
+  <message class="ground" name="DL_SETTING" on_event="b_back">
     <field name="index" value="IndexOfSetting('cam_switch')"/>
     <field name="value" value="0"/> 
   </message>
 
 
   <!-- Red Button -->  <!-- AUTO2 mode -->
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b_red">
+  <message class="ground" name="DL_SETTING" on_event="b_red">
     <field name="index" value="IndexOfSetting('pprz_mode')"/>
     <field name="value" value="2"/> 
   </message>
 
   <!-- Yellow Button -->   <!-- AUTO1 mode -->
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b_yellow">
+  <message class="ground" name="DL_SETTING" on_event="b_yellow">
     <field name="index" value="IndexOfSetting('pprz_mode')"/>
     <field name="value" value="1"/>
   </message>
 
   <!-- Green Button -->  <!-- MANUAL mode -->
-  <message class="ground" name="DL_SETTING" send_always="0" on_event="b_green">
+  <message class="ground" name="DL_SETTING" on_event="b_green">
     <field name="index" value="IndexOfSetting('pprz_mode')"/>
     <field name="value" value="0"/> 
   </message>
 
- <message class="ground" name="DL_SETTING" send_always="0" on_event="b_green || b_yellow || b_red">
+ <message class="ground" name="DL_SETTING" on_event="b_green || b_yellow || b_red">
     <field name="index" value="IndexOfSetting('rc_dl_active_joystick')"/>
     <field name="value" value="JoystickID()"/> 
   </message>
diff --git a/sw/ground_segment/joystick/input2ivy.ml b/sw/ground_segment/joystick/input2ivy.ml
index a4d665aeb4a..6e653b9e9c8 100644
--- a/sw/ground_segment/joystick/input2ivy.ml
+++ b/sw/ground_segment/joystick/input2ivy.ml
@@ -88,7 +88,7 @@ type msg = {
     msg_class : string;
     fields : (string * Syntax.expression) list;
     on_event : Syntax.expression option;
-    send_always : int
+    send_always : bool
   }
 
 (** Represenation of an input device and of the messages to send *)
@@ -189,22 +189,21 @@ let eval_settings_and_blocks = fun field_descr expr ->
 
 (** Parse an XML list of input channels *)
 let parse_input = fun input ->
-  List.map
-    (fun x ->
-      let name = Xml.attrib x "name"
-      and index = ExtXml.int_attrib x "index" in
-      let value =
-    match Xml.tag x with
-      "axis" ->
-	     let trim = try ExtXml.float_attrib x "trim" with _ -> 0.0 in
-	     let exponent = try ExtXml.float_attrib x "exponent" with _ -> 0. in
-	     let limit = try ExtXml.float_attrib x "limit" with _ -> 1.0 in
-             let deadband = try ExtXml.int_attrib x "deadband" with _ -> 0 in
-             Axis (index, deadband, limit, exponent, ref trim)
-    | "button" -> Button index
-    | _ -> failwith "parse_input: unexepcted tag" in
-      (name, value))
-    (Xml.children input)
+  List.map (fun x ->
+    let name = Xml.attrib x "name"
+    and index = ExtXml.int_attrib x "index" in
+    let value =
+      match Xml.tag x with
+        "axis" ->
+          let trim = try ExtXml.float_attrib x "trim" with _ -> 0.0 in
+          let exponent = try ExtXml.float_attrib x "exponent" with _ -> 0.0 in
+          let limit = try ExtXml.float_attrib x "limit" with _ -> 1.0 in
+          let deadband = try ExtXml.int_attrib x "deadband" with _ -> 0 in
+          Axis (index, deadband, limit, exponent, ref trim)
+      | "button" -> Button index
+      | _ -> failwith "parse_input: unexepcted tag" in
+    (name, value))
+  (Xml.children input)
 
 (** Parse a 'à la C' expression *)
 let parse_value = fun s ->
@@ -222,7 +221,7 @@ let parse_msg_field = fun msg_descr field ->
 let parse_msg = fun msg ->
   let msg_name = Xml.attrib msg "name"
   and msg_class = Xml.attrib msg "class"
-  and send_always = (try ExtXml.int_attrib msg "send_always" with _ -> 0) in
+  and send_always = (try (Xml.attrib msg "send_always") = "true" with _ -> false) in
 
   let fields =
     match get_message_type msg_class with
@@ -327,11 +326,11 @@ let eval_call = fun f args ->
     "-", [a1; a2] -> a1 - a2
   | "+", [a1; a2] -> a1 + a2
   | "*", [a1; a2] -> a1 * a2
-  | "%", [a1; a2]  -> a1 / a2
+  | "%", [a1; a2] -> a1 / a2
   | "&&", [a1; a2] -> a1 land a2
   | "||", [a1; a2] -> a1 lor a2
-  | "<",  [a1;a2]  ->  if a1 < a2 then 1 else 0
-  | ">",  [a1;a2]  ->  if a1 > a2 then 1 else 0
+  | "<",  [a1; a2] -> if a1 < a2 then 1 else 0
+  | ">",  [a1; a2] -> if a1 > a2 then 1 else 0
   | "Scale", [x; min; max] -> scale (x) (min) (max)
   | "Bound", [x; min; max] -> bound (x) (min) (max)
   | "JoystickID", [] -> !joystick_id
@@ -341,12 +340,12 @@ let eval_call = fun f args ->
 let eval_expr = fun buttons axis inputs expr ->
   let rec eval = function
       Syntax.Ident ident ->
-    let input = my_assoc ident inputs in
-    eval_input buttons axis input
+        let input = my_assoc ident inputs in
+        eval_input buttons axis input
     | Syntax.Int int -> int
     | Syntax.Float float -> failwith "eval_expr: float"
     | Syntax.Call (ident, exprs) | Syntax.CallOperator (ident, exprs) ->
-    eval_call ident (List.map eval exprs)
+        eval_call ident (List.map eval exprs)
     | Syntax.Index _ -> failwith "eval_expr: index"
     | Syntax.Field _ -> failwith "eval_expr: Field" in
 
@@ -405,7 +404,7 @@ let execute_action = fun ac_id inputs buttons axis message ->
     | Some expr -> eval_expr buttons axis inputs expr <> 0 in
 
   let previous_values = get_previous_values message.msg_name in
-  if ( ( (on_event, values) <> previous_values ) || message.send_always = 1 ) && on_event then begin
+  if ( ( (on_event, values) <> previous_values ) || message.send_always ) && on_event then begin
     let vs = ("ac_id", Pprz.Int ac_id) :: values in
     match message.msg_class with
       "datalink" -> DL.message_send "input2ivy" message.msg_name vs

From e543a355b8b958e15d2f18264ebfc601fc8ae39a Mon Sep 17 00:00:00 2001
From: Vandeportaele Bertrand <bvandepo@laas.fr>
Date: Tue, 5 Apr 2011 17:08:09 +0200
Subject: [PATCH 03/99] first commit of modified aircraft config files

---
 conf/airframes/LAAS/mmlaas_N1.xml           |  10 +-
 conf/airframes/LAAS/mmlaas_N1_carto_cam.xml | 271 ++++++++++++++++++++
 2 files changed, 276 insertions(+), 5 deletions(-)
 create mode 100644 conf/airframes/LAAS/mmlaas_N1_carto_cam.xml

diff --git a/conf/airframes/LAAS/mmlaas_N1.xml b/conf/airframes/LAAS/mmlaas_N1.xml
index 8c08511f7b8..470ae5aa946 100644
--- a/conf/airframes/LAAS/mmlaas_N1.xml
+++ b/conf/airframes/LAAS/mmlaas_N1.xml
@@ -24,8 +24,8 @@
     <subsystem name="control"/>
     <!-- Sensors -->
     <subsystem name="attitude" 		type="infrared">
-      <configure name="ADC_IR1" value="ADC_0"/>
-      <configure name="ADC_IR2" value="ADC_1"/>
+      <configure name="ADC_IR1" value="ADC_1"/>
+      <configure name="ADC_IR2" value="ADC_0"/>
       <configure name="ADC_IR_TOP" value="ADC_2"/>
     </subsystem>
     <subsystem name="gps" 		    type="ublox_lea4p"/>
@@ -40,7 +40,7 @@
 
   <modules>
     <load name="formation_flight.xml"/>
-    <load name="tcas.xml"/>
+   <!-- <load name="tcas.xml"/> -->
     <!--load name="potential.xml"/-->
   </modules>
 
@@ -95,7 +95,7 @@
     <define name="PITCH_NEUTRAL_DEFAULT" value="8.6" unit="deg"/>
 
     <define name="HORIZ_SENSOR_TILTED" value="1"/>
-    <define name="IR2_SIGN" value="-1"/>
+    <define name="IR2_SIGN" value="1"/>
     <define name="TOP_SIGN" value="1"/>
 
     <!--linear name="RollOfIrs" arity="2" coeff1="0.7" coeff2="0.7"/>
@@ -107,7 +107,7 @@
     <define name="ADC_TOP_NEUTRAL" value="515"/>
 
     <define name="LATERAL_CORRECTION" value="1."/>
-    <define name="LONGITUDINAL_CORRECTION" value="1."/>
+    <define name="LONGITUDINAL_CORRECTION" value="-1."/>
     <define name="VERTICAL_CORRECTION" value="1.5"/>
   </section>
 
diff --git a/conf/airframes/LAAS/mmlaas_N1_carto_cam.xml b/conf/airframes/LAAS/mmlaas_N1_carto_cam.xml
new file mode 100644
index 00000000000..bebd51bf5d7
--- /dev/null
+++ b/conf/airframes/LAAS/mmlaas_N1_carto_cam.xml
@@ -0,0 +1,271 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
+<airframe name="MiniMag Laas N1 Tiny 0.99">
+
+<!-- commands section -->
+  <servos>
+    <servo name="THROTTLE"   no="0" min="1200" neutral="1200" max="2000"/>
+<!-- Aileron gauche (blanc) oppose de l'aileron droit (jaune) -->
+<!-- max = valeur aileron vers le bas-->
+    <servo name="AILERON_LEFT"  no="3" min="1900" neutral="1450" max="1100"/>
+    <servo name="AILERON_RIGHT" no="1" min="1900" neutral="1520" max="1100"/>
+    <servo name="RUDDER" no="4" min="1850" neutral="1500" max="1150"/>
+    <servo name="ELEVATOR" no="5" min="2000" neutral="1568" max="1150"/>
+
+  </servos>
+
+  <commands>
+    <axis name="THROTTLE" failsafe_value="0"/>
+    <axis name="ROLL"     failsafe_value="0"/>
+    <axis name="PITCH"    failsafe_value="0"/>
+    <axis name="YAW"      failsafe_value="0"/>
+    <axis name="HATCH"      failsafe_value="0"/>
+  </commands>
+
+  <rc_commands>
+    <set command="THROTTLE" value="@THROTTLE"/>
+    <set command="ROLL"     value="@ROLL"/>
+    <set command="PITCH"    value="@PITCH"/>
+    <set command="YAW"    value="@YAW"/>
+  </rc_commands>
+
+  <section name="MIXER">
+     <define name="AILERON_DIFF" value="0.66"/>
+     <define name="COMBI_SWITCH" value="1.0"/>
+  </section>
+
+  <command_laws>
+    <set servo="THROTTLE"           value="@THROTTLE"/>
+    <set servo="ELEVATOR" value="@PITCH"/>
+    <set servo="RUDDER" value="@YAW + @ROLL*COMBI_SWITCH"/>
+
+    <let var="roll" value="@ROLL"/>
+    <set servo="AILERON_LEFT" value="($roll > 0 ? 1 : AILERON_DIFF) * $roll"/>
+    <set servo="AILERON_RIGHT" value="($roll > 0 ? AILERON_DIFF : 1) * $roll"/>
+  </command_laws>
+
+  <section name="AUTO1" prefix="AUTO1_">
+    <define name="MAX_ROLL" value="0.80"/>
+    <define name="MAX_PITCH" value="0.80"/>
+  </section>
+
+  <section name="adc" prefix="ADC_CHANNEL_">
+    <define name="IR1" value="ADC_0"/>
+    <define name="IR2" value="ADC_1"/>
+    <define name="IR_TOP" value="ADC_2"/>
+    <define name="IR_NB_SAMPLES" value="16"/>
+  </section>
+
+  <section name="INFRARED" prefix="IR_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="-2.6" unit="deg"/>
+<!--    <define name="PITCH_NEUTRAL_DEFAULT" value="8.6" unit="deg"/>-->
+    <define name="PITCH_NEUTRAL_DEFAULT" value="10.6" unit="deg"/>
+
+    <linear name="RollOfIrs" arity="2" coeff1="0.7" coeff2="0.7"/>
+    <linear name="PitchOfIrs" arity="2" coeff1="-0.7" coeff2="0.7"/>
+    <linear name="TopOfIr" arity="1" coeff1="1.5"/>
+
+    <define name="ADC_IR1_NEUTRAL" value="507"/>
+    <define name="ADC_IR2_NEUTRAL" value="509"/>
+    <define name="ADC_TOP_NEUTRAL" value="515"/>
+
+    <define name="CORRECTION_UP" value="1."/>
+    <define name="CORRECTION_DOWN" value="1."/>
+    <define name="CORRECTION_LEFT" value="1."/>
+    <define name="CORRECTION_RIGHT" value="1."/>
+
+    <define name="LATERAL_CORRECTION" value="1."/>
+    <define name="LONGITUDINAL_CORRECTION" value="1."/>
+    <define name="VERTICAL_CORRECTION" value="1."/>
+  </section>
+
+  <section name="BAT">
+    <define name="MILLIAMP_PER_PERCENT" value="0.86"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
+  </section>
+ 
+  <section name="MISC">
+    <define name="NOMINAL_AIRSPEED" value="11." unit="m/s"/>
+    <define name="CARROT" value="3." unit="s"/>
+    <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
+    <define name="CONTROL_RATE" value="60" unit="Hz"/>
+<!--    <define name="XBEE_INIT" value="\"ATPL2\rATRN1\rATTT80\rATBD6\rATWR\r\""/> -->
+    <define name="XBEE_INIT" value="\"ATPL2\rATRN1\rATTT80\r\""/>
+<!--    <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
+    <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
+    <define name="TRIGGER_DELAY" value="1."/>
+    <define name="DEFAULT_CIRCLE_RADIUS" value="60."/>
+  </section>
+ 
+  <section name="VERTICAL CONTROL" prefix="V_CTL_">
+
+    <define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
+    <!-- outer loop proportional gain -->
+    <define name="ALTITUDE_PGAIN" value="-0.15"/>
+    <!-- outer loop saturation -->
+    <define name="ALTITUDE_MAX_CLIMB" value="2."/>
+
+    <!-- auto throttle inner loop -->
+    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.55"/>  
+    <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.30"/>
+    <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.90"/>
+    <define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
+    <define name="AUTO_THROTTLE_DASH_TRIM" value="-2000"/>
+    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
+    <define name="AUTO_THROTTLE_PGAIN" value="-0.05"/>
+    <define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
+    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
+
+    <!-- auto pitch inner loop -->
+    <define name="AUTO_PITCH_PGAIN" value="-0.065"/>
+    <define name="AUTO_PITCH_IGAIN" value="0.15"/>
+    <define name="AUTO_PITCH_MAX_PITCH" value="0.35"/>
+    <define name="AUTO_PITCH_MIN_PITCH" value="-0.35"/>
+
+   <define name="THROTTLE_SLEW" value="0.05"/>
+
+  </section>
+
+  <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
+    <define name="COURSE_PGAIN" value="-1."/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+
+
+    <define name="ROLL_PGAIN" value="10000."/>
+    <define name="AILERON_OF_THROTTLE" value="0.0"/>
+    <define name="PITCH_PGAIN" value="-12000."/>
+    <define name="PITCH_DGAIN" value="1.5"/>
+    <define name="ELEVATOR_OF_ROLL" value="1250"/>
+    
+  </section>
+
+  <section name="NAV">
+    <define name="NAV_PITCH" value="0."/>
+    <define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
+    <define name="NAV_GROUND_SPEED_PGAIN" value="-0.015"/>
+    <define name="NAV_FOLLOW_PGAIN" value="-0.05"/>
+  </section>
+
+  <section name="FORMATION" prefix="FORM_">
+    <define name="CARROT" value="3." unit="s"/> <!-- carrot distance for followers -->
+    <define name="POS_PGAIN" value="0.02"/> <!-- coef on position error -->
+    <define name="SPEED_PGAIN" value="0.4"/> <!-- coef on speed error -->
+    <define name="COURSE_PGAIN" value="0.8"/> <!-- coef on course error (override course pgain for followers) -->
+    <define name="ALTITUDE_PGAIN" value="0.1"/> <!-- coef on altitude error -->
+    <define name="PROX" value="60." unit="m"/> <!-- proximity distance -->
+    <define name="MODE" value="0"/> <!-- mode 0 = global, 1 = local -->
+  </section>
+
+  <section name="TCAS" prefix="TCAS_">
+    <define name="TAU_TA" value="10." unit="s"/> <!-- traffic advisory -->
+    <define name="TAU_RA" value="6." unit="s"/> <!-- resolution advisory -->
+    <define name="ALIM" value="15." unit="m"/> <!-- altitude limitation -->
+    <define name="DT_MAX" value="2000" unit="ms"/> <!-- lost comm or timeout -->
+  </section>
+
+  <section name="AGGRESSIVE" prefix="AGR_">
+    <define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
+    <define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
+    <define name="CLIMB_THROTTLE" value="0.8"/><!-- Gaz for Aggressive Climb -->
+    <define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
+    <define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
+    <define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
+    <define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
+    <define name="DESCENT_NAV_RATIO" value="1.0"/>
+    </section>
+
+  <section name="FAILSAFE" prefix="FAILSAFE_">
+	<define name="DELAY_WITHOUT_GPS" value="1" unit="s"/>
+    	<define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
+    	<define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
+    	<define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
+	<define name="HOME_RADIUS" value="100" unit="m"/>
+</section>
+ 
+ <section name="DATALINK" prefix="DATALINK_">
+    <define name="DEVICE_TYPE" value="XBEE"/>
+    <define name="DEVICE_ADDRESS" value="...."/>
+  </section>
+
+
+ <section name="SIMU">
+    <define name="YAW_RESPONSE_FACTOR" value="0.5"/>
+ </section>
+ 
+ <makefile>
+include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile
+
+FLASH_MODE=IAP
+
+ap.CFLAGS +=  -DFBW -DAP -DCONFIG=\"tiny_0_99.h\" -DLED -DTIME_LED=1
+ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_fbw.c main_ap.c main.c
+
+ap.srcs += commands.c
+
+ap.CFLAGS += -DACTUATORS=\"servos_4015_MAT_hw.h\" -DSERVOS_4015_MAT
+ap.srcs += $(SRC_ARCH)/servos_4015_MAT_hw.c actuators.c
+
+ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
+ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c  
+
+ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B57600
+ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c xbee.c
+
+ap.CFLAGS += -DINTER_MCU
+ap.srcs += inter_mcu.c 
+
+ap.CFLAGS += -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2
+ap.srcs += $(SRC_ARCH)/adc_hw.c
+
+ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400
+
+ap.srcs += gps_ubx.c gps.c latlong.c
+
+ap.CFLAGS += -DINFRARED -DALT_KALMAN
+ap.srcs += infrared.c estimator.c
+
+ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
+ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c nav_survey_rectangle.c nav_line.c 
+
+# Hack to use the same tuning file than slayer1
+#�ap.CFLAGS += -DUSE_GPIO
+# ap.srcs += $(SRC_ARCH)/gpio.c
+
+# formation
+ap.CFLAGS += -DFORMATION -DTRAFFIC_INFO
+ap.srcs += traffic_info.c formation.c snav.c
+
+# external module
+ap.srcs += external.c
+
+# tcas
+ap.CFLAGS += -DTCAS
+ap.srcs += tcas.c
+
+# Harware In The Loop
+
+#ap.CFLAGS += -DHITL
+
+# Losange Cartography with Onboard Camera
+ap.CFLAGS += -DUSE_ONBOARD_CAMERA
+ap.srcs +=nav_survey_losange_carto.c 
+
+ 
+#################################################################################################
+# Config for SITL simulation
+include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
+sim.CFLAGS += -DCONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DFORMATION -DTRAFFIC_INFO -DTCAS
+sim.srcs += nav_survey_rectangle.c nav_line.c external.c traffic_info.c formation.c snav.c tcas.c 
+
+# Losange Cartography with Onboard Camera
+sim.CFLAGS += -DUSE_ONBOARD_CAMERA
+sim.srcs += nav_survey_losange_carto.c 
+
+#pour les printf de debug
+sim.CFLAGS += -DDEBUG_PRINTF
+
+
+  </makefile>
+</airframe>

From c5bbe861904f1fdf1eadde97cf759b03901d14a5 Mon Sep 17 00:00:00 2001
From: Allen <aibara@jobyenergy.com>
Date: Thu, 7 Apr 2011 20:35:47 -0700
Subject: [PATCH 04/99] Convert HMC5843 driver to poll DRDY line directly,
 rather than polling a variable which is set by the interrupt

---
 .../arch/stm32/peripherals/hmc5843_arch.c     |  4 +
 .../arch/stm32/peripherals/hmc5843_arch.h     |  8 ++
 sw/airborne/peripherals/hmc5843.c             | 93 ++++++++++++-------
 sw/airborne/peripherals/hmc5843.h             | 10 +-
 4 files changed, 76 insertions(+), 39 deletions(-)

diff --git a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c b/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c
index f50c285037d..d968eb88ada 100644
--- a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c
+++ b/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c
@@ -40,6 +40,7 @@ void hmc5843_arch_init( void ) {
   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
   GPIO_Init(GPIOB, &GPIO_InitStructure);
 
+#ifdef HMC5843_USE_INT
   GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, GPIO_PinSource5);
   EXTI_InitTypeDef EXTI_InitStructure;
   EXTI_InitStructure.EXTI_Line = EXTI_Line5;
@@ -54,6 +55,7 @@ void hmc5843_arch_init( void ) {
   NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
   NVIC_Init(&NVIC_InitStructure);
+#endif
 }
 
 void hmc5843_arch_reset(void)
@@ -67,5 +69,7 @@ void exti9_5_irq_handler(void) {
   if(EXTI_GetITStatus(EXTI_Line5) != RESET)
     EXTI_ClearITPendingBit(EXTI_Line5);
 
+#ifdef HMC5843_USE_INT
     hmc5843.ready_for_read = TRUE;
+#endif
 }
diff --git a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.h b/sw/airborne/arch/stm32/peripherals/hmc5843_arch.h
index db2ff4426bf..bfb078ab1fb 100644
--- a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.h
+++ b/sw/airborne/arch/stm32/peripherals/hmc5843_arch.h
@@ -24,4 +24,12 @@
 #ifndef HMC5843_ARCH_H
 #define HMC5843_ARCH_H
 
+#include <stm32/gpio.h>
+
+/* returns true if conversion done */
+static inline int mag_eoc(void)
+{
+  return GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_5);
+}
+
 #endif /* HMC5843_ARCH_H */
diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index f02c1483121..d5209968314 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -9,10 +9,10 @@ void exti9_5_irq_handler(void);
 
 void hmc5843_init(void)
 {
-    hmc5843.i2c_trans.status = I2CTransSuccess;
-    hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
+  hmc5843.i2c_trans.status = I2CTransSuccess;
+  hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
 
-    hmc5843_arch_init();
+  hmc5843_arch_init();
 }
 
 // blocking, only intended to be called for initialization
@@ -23,61 +23,86 @@ static void send_config(void)
   hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2);
   hmc5843.i2c_trans.len_w = 2;
   i2c_submit(&i2c2,&hmc5843.i2c_trans);
-    while(hmc5843.i2c_trans.status == I2CTransPending);
+  while(hmc5843.i2c_trans.status == I2CTransPending);
 
   hmc5843.i2c_trans.type = I2CTransTx;
   hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB;  // set to gain to 1 Gauss
   hmc5843.i2c_trans.buf[1] = 0x01<<5;
   hmc5843.i2c_trans.len_w = 2;
   i2c_submit(&i2c2,&hmc5843.i2c_trans);
-    while(hmc5843.i2c_trans.status == I2CTransPending);
+  while(hmc5843.i2c_trans.status == I2CTransPending);
 
   hmc5843.i2c_trans.type = I2CTransTx;
   hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE;  // set to continuous mode
   hmc5843.i2c_trans.buf[1] = 0x00;
   hmc5843.i2c_trans.len_w = 2;
   i2c_submit(&i2c2,&hmc5843.i2c_trans);
-    while(hmc5843.i2c_trans.status == I2CTransPending);
+  while(hmc5843.i2c_trans.status == I2CTransPending);
 
 }
 
 void hmc5843_idle_task(void)
 {
-    if (hmc5843.initialized && hmc5843.ready_for_read && (hmc5843.i2c_trans.status == I2CTransSuccess || hmc5843.i2c_trans.status == I2CTransFailed)) {
-      if (i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
-        hmc5843.ready_for_read = FALSE;
-        hmc5843.i2c_trans.type = I2CTransRx;
-        hmc5843.i2c_trans.len_r = 7;
-        i2c_submit(&i2c2, &hmc5843.i2c_trans);
-        hmc5843.reading = TRUE;
-      }
+  if (hmc5843.i2c_trans.status == I2CTransFailed) {
+    hmc5843.sent_tx = 0;
+    hmc5843.sent_rx = 0;
+  }
+
+  if (hmc5843.i2c_trans.status == I2CTransRunning || hmc5843.i2c_trans.status == I2CTransPending) return;
+
+  if (hmc5843.initialized && mag_eoc() && !hmc5843.sent_tx && !hmc5843.sent_rx) {
+    if (i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
+      hmc5843.i2c_trans.type = I2CTransTx;
+      hmc5843.i2c_trans.len_w = 1;
+      hmc5843.i2c_trans.buf[0] = 0x3;
+      i2c_submit(&i2c2, &hmc5843.i2c_trans);
+      hmc5843.sent_tx = 1;
+      return;
     }
+  }
+
+  if (hmc5843.sent_tx) {
+    hmc5843.i2c_trans.type = I2CTransRx;
+    hmc5843.i2c_trans.len_r = 6;
+    hmc5843.i2c_trans.len_w = 1;
+    hmc5843.i2c_trans.buf[0] = 0x3;
+    i2c_submit(&i2c2, &hmc5843.i2c_trans);
+    hmc5843.sent_rx = 1;
+    hmc5843.sent_tx = 0;
+    return;
+  }
 
-    if (hmc5843.reading && hmc5843.i2c_trans.status == I2CTransSuccess) {
-        hmc5843.timeout = 0;
-        hmc5843.data_available = TRUE;
-        hmc5843.reading = FALSE;
-        memcpy(hmc5843.data.buf, (const void *) hmc5843.i2c_trans.buf, 6);
-        for (int i = 0; i < 3; i++) {
-            hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]);
-        }
+  if (hmc5843.sent_rx && hmc5843.i2c_trans.status == I2CTransSuccess) {
+    hmc5843.sent_rx = 0;
+    hmc5843.sent_tx = 0;
+    hmc5843.timeout = 0;
+    hmc5843.data_available = TRUE;
+    memcpy(hmc5843.data.buf, (const void *) hmc5843.i2c_trans.buf, 6);
+    for (int i = 0; i < 3; i++) {
+      hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]);
     }
+  }
 }
 
 void hmc5843_periodic(void)
 {
-    if (!hmc5843.initialized) {
-        send_config();
-        hmc5843.initialized = TRUE;
-    } else if (hmc5843.timeout++ > HMC5843_TIMEOUT && i2c2.status == I2CIdle && i2c_idle(&i2c2)){
+  if (!hmc5843.initialized) {
+    send_config();
+    hmc5843.initialized = TRUE;
+  } else if (hmc5843.timeout++ > HMC5843_TIMEOUT && i2c2.status == I2CIdle && i2c_idle(&i2c2)){
 #ifdef USE_HMC59843_ARCH_RESET
-        hmc5843_arch_reset();
+    hmc5843_arch_reset();
 #endif
-        hmc5843.i2c_trans.type = I2CTransRx;
-        hmc5843.i2c_trans.len_r = 7;
-        i2c_submit(&i2c2, &hmc5843.i2c_trans);
-        hmc5843.reading = TRUE;
-        hmc5843.ready_for_read = FALSE;
-        hmc5843.timeout = 0;
-    }
+    hmc5843.i2c_trans.type = I2CTransTx;
+    hmc5843.i2c_trans.len_w = 1;
+    hmc5843.i2c_trans.buf[0] = 0x3;
+    i2c_submit(&i2c2, &hmc5843.i2c_trans);
+    while(hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning);
+
+    hmc5843.i2c_trans.type = I2CTransRx;
+    hmc5843.i2c_trans.len_r = 6;
+    i2c_submit(&i2c2, &hmc5843.i2c_trans);
+    while(hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning);
+    hmc5843.timeout = 0;
+  }
 }
diff --git a/sw/airborne/peripherals/hmc5843.h b/sw/airborne/peripherals/hmc5843.h
index ed31ebeed33..16a3d37351c 100644
--- a/sw/airborne/peripherals/hmc5843.h
+++ b/sw/airborne/peripherals/hmc5843.h
@@ -31,15 +31,15 @@
 
 struct Hmc5843 {
     struct i2c_transaction i2c_trans;
+    uint32_t timeout;
+    uint8_t sent_tx;
+    uint8_t sent_rx;
+    uint8_t initialized;
+    uint8_t data_available;
   union {
     uint8_t buf[7];
     int16_t value[3];
   } data;
-    uint8_t initialized;
-    uint8_t reading;
-    volatile uint8_t ready_for_read;
-    uint8_t data_available;
-    uint32_t timeout;
 };
 
 extern struct Hmc5843 hmc5843;

From 230a2d4024546834fec348158d56a201b06400ed Mon Sep 17 00:00:00 2001
From: Allen <aibara@jobyenergy.com>
Date: Thu, 7 Apr 2011 20:38:44 -0700
Subject: [PATCH 05/99] aspirin: Start off i2c transaction failed

---
 sw/airborne/subsystems/imu/imu_aspirin.c | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/sw/airborne/subsystems/imu/imu_aspirin.c b/sw/airborne/subsystems/imu/imu_aspirin.c
index c8922066d28..d1b938de64f 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin.c
+++ b/sw/airborne/subsystems/imu/imu_aspirin.c
@@ -37,6 +37,7 @@ void imu_impl_init(void) {
   imu_aspirin.i2c_trans_gyro.slave_addr = ITG3200_ADDR;
   imu_aspirin.i2c_trans_gyro.len_w = 1;
   imu_aspirin.i2c_trans_gyro.len_r = 6;
+  imu_aspirin.i2c_trans_gyro.status = I2CTransFailed;
 
   imu_aspirin_arch_init();
   hmc5843_init();
@@ -78,9 +79,9 @@ static void configure_gyro(void) {
   t.buf[0] = ITG3200_REG_PWR_MGM;
   t.buf[1] = 0x01;
   send_i2c_msg_with_retry(&t);
-  /* enable interrupt on data ready, idle hight */
+  /* enable interrupt on data ready, idle high, latch until read any register */
   t.buf[0] = ITG3200_REG_INT_CFG;
-  t.buf[1] = (0x01 | 0x01<<7);
+  t.buf[1] = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7);
   send_i2c_msg_with_retry(&t);
 
 }

From 4200fbbe9e6bfa48d7fdf3b0f036cc7908bc294d Mon Sep 17 00:00:00 2001
From: Allen <aibara@jobyenergy.com>
Date: Thu, 7 Apr 2011 20:55:48 -0700
Subject: [PATCH 06/99] Convert aspirin gyro to poll DRDY line directly rather
 than polling varibale set via IRQ

---
 .../stm32/subsystems/imu/imu_aspirin_arch.c   |  6 ++
 .../stm32/subsystems/imu/imu_aspirin_arch.h   |  9 ++-
 sw/airborne/subsystems/imu/imu_aspirin.c      |  2 -
 sw/airborne/subsystems/imu/imu_aspirin.h      | 73 ++++++++++++-------
 4 files changed, 60 insertions(+), 30 deletions(-)

diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c
index 37153420091..1abb944aa05 100644
--- a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c
@@ -24,11 +24,13 @@ void dma1_c4_irq_handler(void);
 void imu_aspirin_arch_int_enable(void) {
   NVIC_InitTypeDef NVIC_InitStructure;
 
+#ifdef ASPIRIN_USE_GYRO_INT
   NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
   NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
   NVIC_Init(&NVIC_InitStructure);
+#endif
 
   NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn;
   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
@@ -83,12 +85,14 @@ void imu_aspirin_arch_init(void) {
   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
   GPIO_Init(GPIOC, &GPIO_InitStructure);
 
+#ifdef ASPIRIN_USE_GYRO_INT
   GPIO_EXTILineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource14);
   EXTI_InitStructure.EXTI_Line = EXTI_Line14;
   EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
   EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
   EXTI_InitStructure.EXTI_LineCmd = ENABLE;
   EXTI_Init(&EXTI_InitStructure);
+#endif
 
   /* Accel */
   /* set accel slave select as output and assert it ( on PB12) */
@@ -233,8 +237,10 @@ void exti15_10_irq_handler(void) {
   if(EXTI_GetITStatus(EXTI_Line14) != RESET)
     EXTI_ClearITPendingBit(EXTI_Line14);
 
+#ifdef ASPIRIN_USE_GYRO_INT
   imu_aspirin.gyro_eoc = TRUE;
   imu_aspirin.status = AspirinStatusReadingGyro;
+#endif
 
 }
 
diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.h b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.h
index 4345e8b14c1..5891d37df56 100644
--- a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.h
+++ b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.h
@@ -2,8 +2,7 @@
 #define IMU_ASPIRIN_ARCH_H
 
 #include "subsystems/imu.h"
-
-#include "led.h"
+#include <stm32/gpio.h>
 
 extern void imu_aspirin_arch_init(void);
 extern void imu_aspirin_arch_int_enable(void);
@@ -12,5 +11,11 @@ extern void adxl345_write_to_reg(uint8_t addr, uint8_t val);
 extern void adxl345_clear_rx_buf(void);
 extern void adxl345_start_reading_data(void);
 
+static inline int imu_aspirin_eoc(void)
+{
+  return !GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_14);
+}
+
+
 
 #endif /* IMU_ASPIRIN_ARCH_H */
diff --git a/sw/airborne/subsystems/imu/imu_aspirin.c b/sw/airborne/subsystems/imu/imu_aspirin.c
index d1b938de64f..26d0c36c394 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin.c
+++ b/sw/airborne/subsystems/imu/imu_aspirin.c
@@ -26,9 +26,7 @@ static void send_i2c_msg_with_retry(struct i2c_transaction* t) {
 void imu_impl_init(void) {
 
   imu_aspirin.status = AspirinStatusUninit;
-  imu_aspirin.gyro_available = FALSE;
   imu_aspirin.gyro_available_blaaa = FALSE;
-  imu_aspirin.mag_ready_for_read = FALSE;
   imu_aspirin.mag_available = FALSE;
   imu_aspirin.accel_available = FALSE;
 
diff --git a/sw/airborne/subsystems/imu/imu_aspirin.h b/sw/airborne/subsystems/imu/imu_aspirin.h
index f937fb8fd54..8f761640a0b 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin.h
+++ b/sw/airborne/subsystems/imu/imu_aspirin.h
@@ -63,15 +63,13 @@ struct ImuAspirin {
   volatile enum AspirinStatus status;
   struct i2c_transaction i2c_trans_gyro;
   struct i2c_transaction i2c_trans_mag;
-  uint8_t gyro_eoc;
-  uint8_t gyro_available;
-  uint8_t gyro_available_blaaa;
-  uint8_t mag_available;
-  volatile uint8_t mag_ready_for_read;
   volatile uint8_t accel_available;
   volatile uint8_t accel_tx_buf[7];
   volatile uint8_t accel_rx_buf[7];
   uint32_t time_since_last_reading;
+  uint8_t mag_available;
+  uint8_t reading_gyro;
+  uint8_t gyro_available_blaaa;
 };
 
 extern struct ImuAspirin imu_aspirin;
@@ -99,6 +97,7 @@ static inline void gyro_read_i2c(void)
 {
   imu_aspirin.i2c_trans_gyro.buf[0] = ITG3200_REG_GYRO_XOUT_H;
   i2c_submit(&i2c2,&imu_aspirin.i2c_trans_gyro);
+  imu_aspirin.reading_gyro = 1;
 }
 
 static inline void gyro_copy_i2c(void)
@@ -117,45 +116,67 @@ static inline void accel_copy_spi(void)
   VECT3_ASSIGN(imu.accel_unscaled, ax, ay, az);
 }
 
+static inline void imu_gyro_event(void (* _gyro_handler)(void))
+{
+
+}
+
 static inline void imu_aspirin_event(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
 {
   if (imu_aspirin.status == AspirinStatusUninit) return;
 
   imu_aspirin_arch_int_disable();
-  ImuMagEvent(_mag_handler);
-  if (imu_aspirin.time_since_last_reading > ASPIRIN_GYRO_TIMEOUT) {
-    imu_aspirin.gyro_eoc = FALSE;
-    imu_aspirin.status = AspirinStatusIdle;
-    i2c2_er_irq_handler();
-    gyro_read_i2c();
-    imu_aspirin.time_since_last_reading = 0;
+  if (imu_aspirin.accel_available) {
+    imu_aspirin.accel_available = FALSE;
+    accel_copy_spi();
+    _accel_handler();
   }
-  if (imu_aspirin.status == AspirinStatusReadingGyro && imu_aspirin.gyro_eoc &&
-      imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess && i2c_idle(&i2c2)) {
-    gyro_copy_i2c();
-    imu_aspirin.status = AspirinStatusIdle;
-    if (imu_aspirin.gyro_available_blaaa) {
-      imu_aspirin.gyro_available_blaaa = FALSE;
-      _gyro_handler();
+
+  // Try again later if transaction is in progress
+  if (imu_aspirin.i2c_trans_gyro.status == I2CTransPending || imu_aspirin.i2c_trans_gyro.status == I2CTransRunning) return;
+
+  ImuMagEvent(_mag_handler);
+
+  // Try back later if things are not idle
+  if (i2c2.status != I2CIdle) return;
+  if (!i2c_idle(&i2c2)) return;
+
+  if (imu_aspirin.reading_gyro) {
+    if (imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) {
+      gyro_copy_i2c();
+      if (imu_aspirin.gyro_available_blaaa) {
+        imu_aspirin.gyro_available_blaaa = FALSE;
+        _gyro_handler();
+      }
     }
+    imu_aspirin.reading_gyro = 0;
+    imu_aspirin_arch_int_enable();
+    return;
   }
-  if (imu_aspirin.gyro_eoc && i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
+
+  // If we're not already waiting for read, and conversion is complete, schedule a read
+  if (!imu_aspirin.reading_gyro && imu_aspirin_eoc() && i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
     if (imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) {
       imu_aspirin.time_since_last_reading = 0;
     }
-    imu_aspirin.gyro_eoc = FALSE;
     gyro_read_i2c();
+    imu_aspirin_arch_int_enable();
+    return;
   }
-  if (imu_aspirin.accel_available) {
-    imu_aspirin.accel_available = FALSE;
-    accel_copy_spi();
-    _accel_handler();
+
+  // Reset everything if we've been waiting too long
+  if (imu_aspirin.time_since_last_reading > ASPIRIN_GYRO_TIMEOUT) {
+    i2c2_er_irq_handler();
+    gyro_read_i2c();
+    imu_aspirin.time_since_last_reading = 0;
+    imu_aspirin_arch_int_enable();
+    return;
   }
   imu_aspirin_arch_int_enable();
 }
 
 #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) do {		\
   imu_aspirin_event(_gyro_handler, _accel_handler, _mag_handler); \
-  } while(0);
+} while(0);
 
 #endif /* IMU_ASPIRIN_H */

From 817ff01bdb573e2f713ec79c0e49c67c6a04a5b3 Mon Sep 17 00:00:00 2001
From: Allen <aibara@jobyenergy.com>
Date: Thu, 7 Apr 2011 21:08:20 -0700
Subject: [PATCH 07/99] Aspirin event handler: Be sure to turn interrupts back
 on in all cases

---
 sw/airborne/subsystems/imu/imu_aspirin.h | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/sw/airborne/subsystems/imu/imu_aspirin.h b/sw/airborne/subsystems/imu/imu_aspirin.h
index 8f761640a0b..b6894912872 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin.h
+++ b/sw/airborne/subsystems/imu/imu_aspirin.h
@@ -133,13 +133,19 @@ static inline void imu_aspirin_event(void (* _gyro_handler)(void), void (* _acce
   }
 
   // Try again later if transaction is in progress
-  if (imu_aspirin.i2c_trans_gyro.status == I2CTransPending || imu_aspirin.i2c_trans_gyro.status == I2CTransRunning) return;
+  if (imu_aspirin.i2c_trans_gyro.status == I2CTransPending || imu_aspirin.i2c_trans_gyro.status == I2CTransRunning) 
+  {
+    imu_aspirin_arch_int_enable();
+    return;
+  }
 
   ImuMagEvent(_mag_handler);
 
   // Try back later if things are not idle
-  if (i2c2.status != I2CIdle) return;
-  if (!i2c_idle(&i2c2)) return;
+  if ((i2c2.status != I2CIdle) || !i2c_idle(&i2c2)) {
+    imu_aspirin_arch_int_enable();
+    return;
+  }
 
   if (imu_aspirin.reading_gyro) {
     if (imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) {

From e86f5bae52b3bb4dda26ea29c45c0809e48ccfa1 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 8 Apr 2011 16:05:49 +0200
Subject: [PATCH 08/99] comments

---
 sw/airborne/math/pprz_algebra_int.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h
index 0a410b2e6a5..f8cc0c926a5 100644
--- a/sw/airborne/math/pprz_algebra_int.h
+++ b/sw/airborne/math/pprz_algebra_int.h
@@ -182,6 +182,7 @@ struct Int64Vect3 {
 };
 
 
+// Real (floating point) ->  Binary Fixed Point  (int)
 #define BFP_OF_REAL(_vr, _frac)    ((_vr)*(1<<(_frac)))
 #define FLOAT_OF_BFP(_vbfp, _frac) ((float)(_vbfp)/(1<<(_frac)))
 #define RATE_BFP_OF_REAL(_af)   BFP_OF_REAL((_af), INT32_RATE_FRAC)

From c6c9c4586e30a03a3dc677329ecf47e511d732f1 Mon Sep 17 00:00:00 2001
From: Allen <aibara@jobyenergy.com>
Date: Fri, 8 Apr 2011 12:53:22 -0700
Subject: [PATCH 09/99] Move reset handler to top of event handler -- otherwise
 we may abort before we get to it

---
 sw/airborne/subsystems/imu/imu_aspirin.h | 17 +++++++++--------
 1 file changed, 9 insertions(+), 8 deletions(-)

diff --git a/sw/airborne/subsystems/imu/imu_aspirin.h b/sw/airborne/subsystems/imu/imu_aspirin.h
index b6894912872..ccf6f25f154 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin.h
+++ b/sw/airborne/subsystems/imu/imu_aspirin.h
@@ -131,6 +131,15 @@ static inline void imu_aspirin_event(void (* _gyro_handler)(void), void (* _acce
     accel_copy_spi();
     _accel_handler();
   }
+  
+  // Reset everything if we've been waiting too long
+  if (imu_aspirin.time_since_last_reading > ASPIRIN_GYRO_TIMEOUT) {
+    i2c2_er_irq_handler();
+    gyro_read_i2c();
+    imu_aspirin.time_since_last_reading = 0;
+    imu_aspirin_arch_int_enable();
+    return;
+  }
 
   // Try again later if transaction is in progress
   if (imu_aspirin.i2c_trans_gyro.status == I2CTransPending || imu_aspirin.i2c_trans_gyro.status == I2CTransRunning) 
@@ -170,14 +179,6 @@ static inline void imu_aspirin_event(void (* _gyro_handler)(void), void (* _acce
     return;
   }
 
-  // Reset everything if we've been waiting too long
-  if (imu_aspirin.time_since_last_reading > ASPIRIN_GYRO_TIMEOUT) {
-    i2c2_er_irq_handler();
-    gyro_read_i2c();
-    imu_aspirin.time_since_last_reading = 0;
-    imu_aspirin_arch_int_enable();
-    return;
-  }
   imu_aspirin_arch_int_enable();
 }
 

From 773a513425e26097ea0b23759c10d3e71f19fbfa Mon Sep 17 00:00:00 2001
From: Allen <aibara@jobyenergy.com>
Date: Fri, 8 Apr 2011 15:38:36 -0700
Subject: [PATCH 10/99] Don't disable gyro irq if we're not using it

---
 sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c
index 1abb944aa05..c1cb472cbfe 100644
--- a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c
@@ -43,11 +43,13 @@ void imu_aspirin_arch_int_enable(void) {
 void imu_aspirin_arch_int_disable(void) {
   NVIC_InitTypeDef NVIC_InitStructure;
 
+#ifdef ASPIRIN_USE_GYRO_INT
   NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
   NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
   NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
   NVIC_Init(&NVIC_InitStructure);
+#endif
 
   NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn;
   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;

From 03df9d4c3fba4b70de6b29b90334332274b01ad0 Mon Sep 17 00:00:00 2001
From: Allen <aibara@jobyenergy.com>
Date: Fri, 8 Apr 2011 17:22:28 -0700
Subject: [PATCH 11/99] Aspirin: Add accel timeout/reconfigure

---
 sw/airborne/subsystems/imu/imu_aspirin.c |  8 ++++++--
 sw/airborne/subsystems/imu/imu_aspirin.h | 12 +++++-------
 2 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/sw/airborne/subsystems/imu/imu_aspirin.c b/sw/airborne/subsystems/imu/imu_aspirin.c
index 26d0c36c394..86cb2a87640 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin.c
+++ b/sw/airborne/subsystems/imu/imu_aspirin.c
@@ -50,10 +50,14 @@ void imu_periodic(void) {
     configure_accel();
     imu_aspirin_arch_int_enable();
     imu_aspirin.status = AspirinStatusIdle;
-  }
-  else {
+  } else {
     imu_aspirin.gyro_available_blaaa = TRUE;
     imu_aspirin.time_since_last_reading++;
+    imu_aspirin.time_since_last_accel_reading++;
+    if (imu_aspirin.time_since_last_accel_reading > ASPIRIN_ACCEL_TIMEOUT) {
+      configure_accel();
+      imu_aspirin.time_since_last_accel_reading=0;
+    }
   }
 
 }
diff --git a/sw/airborne/subsystems/imu/imu_aspirin.h b/sw/airborne/subsystems/imu/imu_aspirin.h
index ccf6f25f154..1da0d051063 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin.h
+++ b/sw/airborne/subsystems/imu/imu_aspirin.h
@@ -67,6 +67,7 @@ struct ImuAspirin {
   volatile uint8_t accel_tx_buf[7];
   volatile uint8_t accel_rx_buf[7];
   uint32_t time_since_last_reading;
+  uint32_t time_since_last_accel_reading;
   uint8_t mag_available;
   uint8_t reading_gyro;
   uint8_t gyro_available_blaaa;
@@ -74,6 +75,7 @@ struct ImuAspirin {
 
 extern struct ImuAspirin imu_aspirin;
 #define ASPIRIN_GYRO_TIMEOUT 3
+#define ASPIRIN_ACCEL_TIMEOUT 3
 
 #include "peripherals/hmc5843.h"
 #define foo_handler() {}
@@ -125,26 +127,26 @@ static inline void imu_aspirin_event(void (* _gyro_handler)(void), void (* _acce
 {
   if (imu_aspirin.status == AspirinStatusUninit) return;
 
-  imu_aspirin_arch_int_disable();
+  //imu_aspirin_arch_int_disable();
   if (imu_aspirin.accel_available) {
+    imu_aspirin.time_since_last_accel_reading = 0;
     imu_aspirin.accel_available = FALSE;
     accel_copy_spi();
     _accel_handler();
   }
+  //imu_aspirin_arch_int_enable();
   
   // Reset everything if we've been waiting too long
   if (imu_aspirin.time_since_last_reading > ASPIRIN_GYRO_TIMEOUT) {
     i2c2_er_irq_handler();
     gyro_read_i2c();
     imu_aspirin.time_since_last_reading = 0;
-    imu_aspirin_arch_int_enable();
     return;
   }
 
   // Try again later if transaction is in progress
   if (imu_aspirin.i2c_trans_gyro.status == I2CTransPending || imu_aspirin.i2c_trans_gyro.status == I2CTransRunning) 
   {
-    imu_aspirin_arch_int_enable();
     return;
   }
 
@@ -152,7 +154,6 @@ static inline void imu_aspirin_event(void (* _gyro_handler)(void), void (* _acce
 
   // Try back later if things are not idle
   if ((i2c2.status != I2CIdle) || !i2c_idle(&i2c2)) {
-    imu_aspirin_arch_int_enable();
     return;
   }
 
@@ -165,7 +166,6 @@ static inline void imu_aspirin_event(void (* _gyro_handler)(void), void (* _acce
       }
     }
     imu_aspirin.reading_gyro = 0;
-    imu_aspirin_arch_int_enable();
     return;
   }
 
@@ -175,11 +175,9 @@ static inline void imu_aspirin_event(void (* _gyro_handler)(void), void (* _acce
       imu_aspirin.time_since_last_reading = 0;
     }
     gyro_read_i2c();
-    imu_aspirin_arch_int_enable();
     return;
   }
 
-  imu_aspirin_arch_int_enable();
 }
 
 #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) do {		\

From a935c235727b48d7ef7ab5ea3cf1e8c2f806e719 Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Tue, 12 Apr 2011 00:25:00 +0200
Subject: [PATCH 12/99] added weather station from Andreas Gaeb

---
 .gitignore                         |   4 +
 Makefile                           |   6 +-
 conf/control_panel.xml.example     |   5 +
 conf/messages.xml                  |  11 +-
 sw/ground_segment/misc/Makefile    |   7 +
 sw/ground_segment/misc/davis2ivy.c | 269 +++++++++++++++++++++++++++++
 sw/ground_segment/misc/readme.txt  |   3 +
 7 files changed, 301 insertions(+), 4 deletions(-)
 create mode 100644 sw/ground_segment/misc/Makefile
 create mode 100644 sw/ground_segment/misc/davis2ivy.c
 create mode 100644 sw/ground_segment/misc/readme.txt

diff --git a/.gitignore b/.gitignore
index 0d10b010ddb..e2bf7c81ceb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -112,6 +112,10 @@
 # /sw/ground_segment/joystick
 /sw/ground_segment/joystick/test_stick
 
+# /sw/ground_segment/misc
+/sw/ground_segment/misc/davis2ivy
+
+
 # /sw/airborne/arch/lpc21/test/bootloader
 /sw/airborne/arch/lpc21/test/bootloader/bl.dmp
 /sw/airborne/arch/lpc21/test/bootloader/bl.hex
diff --git a/Makefile b/Makefile
index 52bbc5537ab..5e928b0fa79 100644
--- a/Makefile
+++ b/Makefile
@@ -43,6 +43,7 @@ AIRBORNE=sw/airborne
 COCKPIT=sw/ground_segment/cockpit
 TMTC=sw/ground_segment/tmtc
 MULTIMON=sw/ground_segment/multimon
+MISC=sw/ground_segment/misc
 LOGALIZER=sw/logalizer
 SIMULATOR=sw/simulator
 MAKE=make PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME)
@@ -69,7 +70,7 @@ OCAMLRUN=$(shell which ocamlrun)
 
 all: commands static conf
 
-static : lib center tools cockpit multimon tmtc logalizer lpc21iap sim_static static_h usb_lib
+static : lib center tools cockpit multimon tmtc misc logalizer lpc21iap sim_static static_h usb_lib
 
 conf: conf/conf.xml conf/control_panel.xml
 
@@ -98,6 +99,9 @@ cockpit: lib
 tmtc: lib cockpit
 	cd $(TMTC); $(MAKE) all
 
+misc:
+	cd $(MISC); $(MAKE) all
+
 multimon:
 	cd $(MULTIMON); $(MAKE)
 
diff --git a/conf/control_panel.xml.example b/conf/control_panel.xml.example
index e9a2ea99a18..36f12526fa1 100644
--- a/conf/control_panel.xml.example
+++ b/conf/control_panel.xml.example
@@ -63,6 +63,11 @@
     </program>
 
     <program name="Http Server" command="sw/ground_segment/tmtc/boa"/>
+
+    <program name="Weather Station" command="sw/ground_segment/misc/davis2ivy">
+      <arg flag="-b" variable="ivy_bus"/>
+      <arg flag="-d" constant="/dev/ttyUSB1"/>
+    </program>
  </section>
 
 
diff --git a/conf/messages.xml b/conf/messages.xml
index 27a0c434413..1df1cbbb957 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -451,7 +451,7 @@
     <field name="mode"       type="uint8"  unit="byte_mask"/>
     <field name="gps_nb_err" type="uint8"/>
   </message>
- 
+
   <message name="H_CTL_A" id="60">
     <field name="roll_sum_err" type="float"/>
     <field name="ref_roll_angle" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.3"/>
@@ -1332,7 +1332,7 @@
     <field name="bq"     type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
     <field name="br"     type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
   </message>
-    
+
 
  <!--179 is free -->
 
@@ -1603,7 +1603,12 @@
  <!--216 is free -->
  <!--217 is free -->
  <!--218 is free -->
- <!--219 is free -->
+  <message name="WEATHER" id="219">
+    <field name="p_amb" type="float" unit="Pa" alt_unit="mBar" alt_unit_coef="0.01"/>
+    <field name="t_amb" type="float" unit="deg C"/>
+    <field name="windspeed" type="float" unit="m/s"/>
+    <field name="wind_from" type="float" unit="deg"/>
+  </message>
 
   <message name="IMU_TURNTABLE" id="220">
     <field name="omega" type="float" unit="rad/s" alt_unit="deg/s" alt_unit_coef="57.29578"/>
diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile
new file mode 100644
index 00000000000..018ab409052
--- /dev/null
+++ b/sw/ground_segment/misc/Makefile
@@ -0,0 +1,7 @@
+all: davis2ivy
+
+davis2ivy: davis2ivy.o
+	g++ -o davis2ivy davis2ivy.o -s -livy
+
+%.o : %.c
+	gcc -c -O2 -Wall $<
diff --git a/sw/ground_segment/misc/davis2ivy.c b/sw/ground_segment/misc/davis2ivy.c
new file mode 100644
index 00000000000..f167bb78155
--- /dev/null
+++ b/sw/ground_segment/misc/davis2ivy.c
@@ -0,0 +1,269 @@
+/*
+ * Paparazzi $Id$
+ *
+ * Copyright (C) 2011 Andreas Gaeb
+ *
+ * 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.
+ *
+ */
+
+/** \file davis2ivy.c
+ *  \brief Connect a Davis VantagePro weather station to the Paparazzi system
+ *
+ * The program communicates with a Davis VantagePro(2) weather station connected
+ * to a serial port.  It asks for new data (Davis' LOOP command) in the
+ * specified intervals, extracts the relevant data (ambient pressure and
+ * temperature, wind speed and direction) and broadcasts this via the Ivy bus.
+ *
+ * At the moment, the Ivy messages should be sent with the ID of the actually
+ * flying aircraft, which integrates them into the log file, as long as the
+ * aircraft sends its alive message.
+ *
+ * Useful links:
+ * - <a href="http://www.davisnet.com/weather/products/vantagepro.asp">Weather Stations</a>
+ * - <a href="http://www.davisnet.com/support/weather/download/VantageSerialProtocolDocs_v230.pdf">Communication docs</a>
+ *
+ */
+
+
+ #include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <ctype.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <termios.h>
+
+#include <signal.h>
+
+#include <Ivy/ivy.h>
+#include <Ivy/ivyloop.h>
+#include <Ivy/timer.h>
+
+
+typedef enum { FALSE = 0, TRUE } BOOL;
+
+#define PACKET_LENGTH 99
+
+
+// global variables
+int fd, ac_id = 1;
+const char *device;
+unsigned char packet[PACKET_LENGTH];
+TimerId tid;
+BOOL want_alive_msg = FALSE;
+
+
+/// Handler for Ctrl-C, exits the main loop
+void sigint_handler(int sig) {
+  IvyStop();
+  TimerRemove(tid);
+  close(fd);
+}
+
+/// open the serial port with the appropiate settings
+void open_port(const char* device) {
+  fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY);
+  if (fd == -1) {
+    fprintf(stderr, "open_port: unable to open device %s - ", device);
+    perror(NULL);
+    exit(EXIT_FAILURE);
+  }
+  // setup connection options
+  struct termios options;
+
+  // get the current options
+  tcgetattr(fd, &options);
+
+  // set local mode, enable receiver, set comm. options:
+  // 8 data bits, 1 stop bit, no parity, 19200 Baud
+  options.c_cflag = CLOCAL | CREAD | CS8 | B19200;
+
+  // write options back to port
+  tcsetattr(fd, TCSANOW, &options);
+
+}
+
+/// disable transactions and empty queue
+void reset_station() {
+  char newline = '\n', bytes = 0;
+  fprintf(stderr, "Resetting communication\n");
+  // send a \n (wakeup and cancel all running transmits)
+  bytes = write(fd, &newline, 1);
+  // read and discard everything that might be left in the queue
+  close(fd);
+  sleep(1);
+  open_port(device);
+}
+
+/// send a wakeup call to the station
+BOOL wakeup(int tries) {
+  int loops = tries, bytes;
+  BOOL woken = FALSE;
+  char buf[] = {0, 0};
+  char newline = '\n';
+  do {
+    // send a \n
+    bytes = write(fd, &newline, 1);
+    // wait until station answers with \n\r
+    usleep(30000);
+    bytes = read(fd, buf, sizeof(buf));
+    woken = (buf[0] == 10) && (buf[1] == 13);
+  } while (!woken && loops-- > 0);
+  if (!woken) {
+    fprintf(stderr, "Could not wake up station: ");
+    if (bytes < 1) fprintf(stderr, "no bytes received\n");
+    else fprintf(stderr, "received %02x:%02x instead of \\n\\r\n", buf[0], buf[1]);
+    reset_station();
+  }
+  return woken;
+}
+
+/// send a LOOP command (read sensor data) to the station and get the packet back
+BOOL send_loop() {
+  char msg[32], ack;
+  // TODO maybe ask for more packets?
+  snprintf(msg, sizeof(msg), "LOOP %i\n", 1);
+  int bytes = write(fd, msg, strlen(msg));
+  usleep(120000);
+  bytes = read(fd, &ack, 1);
+  if (bytes < 1 || ack != 0x06) {
+    fprintf(stderr, "Failed to receive ACK from station\n");
+    reset_station();
+    return FALSE;
+  }
+  bytes = read(fd, packet, PACKET_LENGTH);
+  if (bytes < PACKET_LENGTH) {
+    fprintf(stderr, "Received packet is incomplete, only %i of %i bytes\n",
+            bytes, PACKET_LENGTH);
+    reset_station();
+    return FALSE;
+  } else {
+    return TRUE;
+  }
+}
+
+/// get the relevant data from the packet and sent it as Ivy message
+void decode_and_send_to_ivy() {
+
+  // check packet integrity
+  char expected[] = "LOO";
+  if (strncmp((char *)packet, expected, 3) != 0) {
+    fprintf(stderr, "Received packet from the weather station which does not match the expected format\n");
+    reset_station();
+    return;
+  }
+
+  // TODO CRC checking (is rather involved for the Davis protocol)
+
+  // get relevant data and convert to SI units
+  // see chapter IX.1 of the protocol definition
+  float
+    pstatic_Pa = (packet[7] | packet[8] << 8)*3.386388640341, // original is inches Hg / 1000
+    temp_degC = ((packet[12] | packet[13] << 8)/10.0 - 32.0)*5.0/9.0, // original is deg F / 10
+    windspeed_mps = packet[14]*0.44704, // original is miles per hour
+    winddir_deg = packet[16] | packet[17] << 8;
+
+
+  // TODO get the real MD5 for the aircraft id
+  if (want_alive_msg)
+    IvySendMsg("%d ALIVE 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0\n", ac_id);
+
+  // format has to match declaration in conf/messages.xml
+  IvySendMsg("%d WEATHER %f %f %f %f\n",
+    ac_id, pstatic_Pa, temp_degC, windspeed_mps, winddir_deg);
+}
+
+/// Get data from the station and send it via Ivy
+/** This function is executed by the timer
+ */
+void handle_timer (TimerId id, void *data, unsigned long delta) {
+  if (wakeup(3) && send_loop()) decode_and_send_to_ivy();
+}
+
+void print_usage(int argc, char ** argv) {
+  fprintf(stderr, "Usage: %s [-a] [-b <bus>] [-d <device>] [-i <aircraft_id>] [-s <delay time in seconds>]\n",
+          argv[0]);
+};
+
+/// Main function
+int main(int argc, char **argv) {
+  // default values for options
+  const char
+    *defaultbus = "127.255.255.255:2010",
+    *bus = defaultbus,
+    *defaultdevice = "/dev/ttyUSB1";
+  device = defaultdevice;
+  long delay = 1000;
+
+  // parse options
+  char c;
+  while ((c = getopt (argc, argv, "hab:d:i:s:")) != EOF) {
+    switch (c) {
+    case 'h':
+      print_usage(argc, argv);
+      exit(EXIT_SUCCESS);
+      break;
+    case 'a':
+      want_alive_msg = TRUE;
+      break;
+    case 'b':
+      bus = optarg;
+      break;
+    case 'd':
+      device = optarg;
+      break;
+    case 'i':
+      ac_id = atoi(optarg);
+      break;
+    case 's':
+      delay = atoi(optarg)*1000;
+      break;
+    case '?':
+      if (optopt == 'a' || optopt == 'b' || optopt == 'd' || optopt == 's')
+        fprintf (stderr, "Option -%c requires an argument.\n", optopt);
+      else if (isprint (optopt))
+        fprintf (stderr, "Unknown option `-%c'.\n", optopt);
+      else
+        fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt);
+      print_usage(argc, argv);
+      exit(EXIT_FAILURE);
+    default:
+      abort ();
+    }
+  }
+
+
+  // make Ctrl-C stop the main loop and clean up properly
+  signal(SIGINT, sigint_handler);
+
+  bzero (packet, PACKET_LENGTH);
+  open_port(device);
+
+  // setup Ivy communication
+  IvyInit("davis2ivy", "READY", 0, 0, 0, 0);
+  IvyStart(bus);
+
+  // create timer
+  tid = TimerRepeatAfter (0, delay, handle_timer, 0);
+
+  IvyMainLoop();
+
+  return 0;
+}
diff --git a/sw/ground_segment/misc/readme.txt b/sw/ground_segment/misc/readme.txt
new file mode 100644
index 00000000000..5221c38c9f3
--- /dev/null
+++ b/sw/ground_segment/misc/readme.txt
@@ -0,0 +1,3 @@
+davis2ivy:
+  A wrapper to communicate with a Davis VantagePro/VantagePro2 weather
+  station and integrate weather data into the telemetry link.

From 4a9e5b142b26ff6a8b79cf7300a204a7d781f34c Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Wed, 13 Apr 2011 18:16:20 +0200
Subject: [PATCH 13/99] arduimu at higher integration rate; returns corrected
 gyro values

---
 conf/modules/ins_arduimu_basic.xml            |  2 +-
 .../arduimu_basic/ADC.pde                     | 28 ++------
 .../arduimu_basic/DCM.pde                     | 70 ++-----------------
 .../arduimu_basic/Output.pde                  | 35 +++++-----
 .../arduimu_basic/arduimu_basic.pde           | 19 +++--
 sw/airborne/modules/ins/ins_arduimu_basic.c   |  2 +-
 6 files changed, 38 insertions(+), 118 deletions(-)

diff --git a/conf/modules/ins_arduimu_basic.xml b/conf/modules/ins_arduimu_basic.xml
index b1d4493404e..bc29ed822e2 100644
--- a/conf/modules/ins_arduimu_basic.xml
+++ b/conf/modules/ins_arduimu_basic.xml
@@ -6,7 +6,7 @@
   </header>
   <init fun="ArduIMU_init()"/>
   <periodic fun="ArduIMU_periodic()" freq="60"/>
-  <periodic fun="ArduIMU_periodicGPS()" freq="8"/>
+  <periodic fun="ArduIMU_periodicGPS()" freq="4"/>
   <event fun="ArduIMU_event()"/>
   <makefile target="ap">
     <file name="ins_arduimu_basic.c"/>
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde
index 3d4728d1681..9f8473b7e2f 100644
--- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde
@@ -31,29 +31,9 @@ float read_adc(int select)
   float temp;
   if (SENSOR_SIGN[select]<0){
     temp = (AN_OFFSET[select]-AN[select]);
-    if (abs(temp)>900) {
-#if PRINT_DEBUG != 0
-    Serial.print("!!!ADC:1,VAL:");
-    Serial.print (temp);
-    Serial.println("***");
-#endif
-#if PERFORMANCE_REPORTING == 1
-    adc_constraints++;   
-#endif 
-    }
     return constrain(temp,-900,900);             //Throw out nonsensical values
   } else {
     temp = (AN[select]-AN_OFFSET[select]); 
-    if (abs(temp)>900) {
-#if PRINT_DEBUG != 0
-    Serial.print("!!!ADC:2,VAL:");
-    Serial.print (temp);
-    Serial.println("***");
-#endif    
-#if PERFORMANCE_REPORTING == 1
-    adc_constraints++; 
-#endif
-    } 
     return constrain(temp,-900,900);
   }
 }
@@ -61,8 +41,8 @@ float read_adc(int select)
 //Activating the ADC interrupts. 
 void Analog_Init(void)
 {
- ADCSRA|=(1<<ADIE)|(1<<ADEN);
- ADCSRA|= (1<<ADSC);
+  ADCSRA|=(1<<ADIE)|(1<<ADEN);
+  ADCSRA|= (1<<ADSC);
 }
 
 //
@@ -80,8 +60,8 @@ ISR(ADC_vect)
   high = ADCH;
 
   if(analog_count[MuxSel]<63) {
-        analog_buffer[MuxSel] += (high << 8) | low;   // cumulate analog values
-        analog_count[MuxSel]++;
+    analog_buffer[MuxSel] += (high << 8) | low;   // cumulate analog values
+    analog_count[MuxSel]++;
   }
   MuxSel++;
   MuxSel &= 0x07;   //if(MuxSel >=8) MuxSel=0;
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde
index 305c2bac57b..ed30a701901 100644
--- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde
@@ -21,27 +21,13 @@ void Normalize(void)
     renorm= .5 * (3-renorm);                                                 //eq.21
   } else if (renorm < 100.0f && renorm > 0.01f) {
     renorm= 1. / sqrt(renorm);
-#if PERFORMANCE_REPORTING == 1  
+#if PERFORMANCE_REPORTING == 1
     renorm_sqrt_count++;
-#endif
-#if PRINT_DEBUG != 0
-    Serial.print("???SQT:1,RNM:");
-    Serial.print (renorm);
-    Serial.print (",ERR:");
-    Serial.print (error);
-    Serial.println("***");    
 #endif
   } else {
     problem = TRUE;
 #if PERFORMANCE_REPORTING == 1
     renorm_blowup_count++;
-#endif
-#if PRINT_DEBUG != 0
-    Serial.print("???PRB:1,RNM:");
-    Serial.print (renorm);
-    Serial.print (",ERR:");
-    Serial.print (error);
-    Serial.println("***");    
 #endif
   }
   Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
@@ -53,25 +39,11 @@ void Normalize(void)
     renorm= 1. / sqrt(renorm);  
 #if PERFORMANCE_REPORTING == 1    
     renorm_sqrt_count++;
-#endif
-#if PRINT_DEBUG != 0
-    Serial.print("???SQT:2,RNM:");
-    Serial.print (renorm);
-    Serial.print (",ERR:");
-    Serial.print (error);
-    Serial.println("***");    
 #endif
   } else {
     problem = TRUE;
 #if PERFORMANCE_REPORTING == 1
     renorm_blowup_count++;
-#endif
-#if PRINT_DEBUG != 0
-    Serial.print("???PRB:2,RNM:");
-    Serial.print (renorm);
-    Serial.print (",ERR:");
-    Serial.print (error);
-    Serial.println("***");    
 #endif
   }
   Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
@@ -83,23 +55,11 @@ void Normalize(void)
     renorm= 1. / sqrt(renorm);   
 #if PERFORMANCE_REPORTING == 1 
     renorm_sqrt_count++;
-#endif
-#if PRINT_DEBUG != 0
-    Serial.print("???SQT:3,RNM:");
-    Serial.print (renorm);
-    Serial.print (",ERR:");
-    Serial.print (error);
-    Serial.println("***");    
 #endif
   } else {
     problem = TRUE;  
 #if PERFORMANCE_REPORTING == 1
     renorm_blowup_count++;
-#endif
-#if PRINT_DEBUG != 0
-    Serial.print("???PRB:3,RNM:");
-    Serial.print (renorm);
-    Serial.println("***");    
 #endif
   }
   Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
@@ -170,23 +130,18 @@ void Drift_correction(void)
   Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
   if (Integrator_magnitude > ToRad(300)) {
     Vector_Scale(Omega_I,Omega_I,0.5f*ToRad(300)/Integrator_magnitude);
-#if PRINT_DEBUG != 0
-    Serial.print("!!!INT:1,MAG:");
-    Serial.print (ToDeg(Integrator_magnitude));
-
-    Serial.println("***");    
-#endif
   }
 
 }
+
 /**************************************************/
 void Accel_adjust(void)
 {
   Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]);  // Centrifugal force on Acc_y = GPS_speed*GyroZ
   Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]);  // Centrifugal force on Acc_z = GPS_speed*GyroY 
 }
-/**************************************************/
 
+/**************************************************/
 void Matrix_update(void)
 {
   Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll
@@ -202,7 +157,6 @@ void Matrix_update(void)
 
   Accel_adjust();    //Remove centrifugal acceleration.
 
-#if OUTPUTMODE==1         
   Update_Matrix[0][0]=0;
   Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
   Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
@@ -212,17 +166,6 @@ void Matrix_update(void)
   Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
   Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
   Update_Matrix[2][2]=0;
-#else                    // Uncorrected data (no drift correction)
-  Update_Matrix[0][0]=0;
-  Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
-  Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
-  Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
-  Update_Matrix[1][1]=0;
-  Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
-  Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
-  Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
-  Update_Matrix[2][2]=0;
-#endif
 
   Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
 
@@ -235,16 +178,11 @@ void Matrix_update(void)
   }
 }
 
+/**************************************************/
 void Euler_angles(void)
 {
-#if (OUTPUTMODE==2)         // Only accelerometer info (debugging purposes)
-  roll = atan2(Accel_Vector[1],Accel_Vector[2]);    // atan2(acc_y,acc_z)
-  pitch = -asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x)
-  yaw = 0;
-#else
   pitch = -asin(DCM_Matrix[2][0]);
   roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
   yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
-#endif
 }
 
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde
index 49170c774ca..2ee09a3d7cd 100644
--- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde
@@ -1,24 +1,28 @@
 
 //*****I2C Output************************************************************
-
-void requestEvent(){
-#if PRINT_DEBUG != 0
-  Serial.println("Sending IMU Data");
-#endif
-
+void fill_I2C_message() {
   // Message Array : Roll; Pitch; YaW; GyroX; GyroY; GyroZ; ACCX; ACCY; ACCZ
   // Float Number is multipited with 10000 and converted to an Integer, for sending via I2C.
   // Resolution for angles: 12, rates: 12, accel:10 (from pprza BFP math lib)
   I2C_Message_ar[0] = int(roll*(1<<12));
   I2C_Message_ar[1] = int(pitch*(1<<12));
   I2C_Message_ar[2] = int(yaw*(1<<12));
-  I2C_Message_ar[3] = int(Gyro_Vector[0]*(1<<12));
-  I2C_Message_ar[4] = int(Gyro_Vector[1]*(1<<12));
-  I2C_Message_ar[5] = int(Gyro_Vector[2]*(1<<12));
+  I2C_Message_ar[3] = int(Omega_Vector[0]*(1<<12));
+  I2C_Message_ar[4] = int(Omega_Vector[1]*(1<<12));
+  I2C_Message_ar[5] = int(Omega_Vector[2]*(1<<12));
   I2C_Message_ar[6] = int((9.81*Accel_Vector[0]/GRAVITY)*(1<<10));
   I2C_Message_ar[7] = int((9.81*Accel_Vector[1]/GRAVITY)*(1<<10));
   I2C_Message_ar[8] = int((9.81*Accel_Vector[2]/GRAVITY)*(1<<10));
 
+}
+
+void requestEvent(){
+#if PRINT_DEBUG != 0
+  Serial.println("Sending IMU Data");
+#endif
+
+  fill_I2C_message();
+
   byte* pointer;
   pointer = (byte*) &I2C_Message_ar;
   Wire.send(pointer, 18);
@@ -51,6 +55,7 @@ void receiveEvent(int howMany){
 void printdata(void){    
 
 #if PRINT_I2C_MSG == 1
+  fill_I2C_message();
   Serial.print("Time ");
   Serial.print(millis());
   Serial.print(";  Roll ");
@@ -89,8 +94,7 @@ void printdata(void){
   Serial.print (",AN4:");
   Serial.print(read_adc(4));
   Serial.print (",AN5:");
-  Serial.print(read_adc(5));
-  Serial.print (",");
+  Serial.println(read_adc(5));
 #endif
 
 #if PRINT_DCM == 1
@@ -111,8 +115,7 @@ void printdata(void){
   Serial.print (",EX7:");
   Serial.print(convert_to_dec(DCM_Matrix[2][1]));
   Serial.print (",EX8:");
-  Serial.print(convert_to_dec(DCM_Matrix[2][2]));
-  Serial.print (",");
+  Serial.println(convert_to_dec(DCM_Matrix[2][2]));
 #endif
 
 #if PRINT_EULER == 1
@@ -123,8 +126,7 @@ void printdata(void){
   Serial.print(",YAW:");
   Serial.print(ToDeg(yaw));
   Serial.print(",IMUH:");
-  Serial.print((imu_health>>8)&0xff);
-  Serial.print (",");
+  Serial.println((imu_health>>8)&0xff);
 #endif
 
 #if PRINT_GPS == 1
@@ -141,6 +143,7 @@ void printdata(void){
     gps_messages_sent++;
 #endif
   }
+  Serial.println("");
 #endif
 
 }
@@ -174,7 +177,7 @@ void printPerfData(long time)
   Serial.print(gps_messages_sent,DEC);
   Serial.print(",imu:");
   Serial.print((imu_health>>8),DEC);
-  Serial.print(",***");
+  Serial.println(",***");
 
   // Reset counters
   mainLoop_count = 0;
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde
index df41fd57789..7c5f550e0a9 100644
--- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde
@@ -30,9 +30,6 @@
 /*For debugging propurses*/
 #define PRINT_DEBUG 0   //Will print Debug messages
 
-//OUTPUTMODE=1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 will print accelerometer only data
-#define OUTPUTMODE 1
-
 #define PRINT_I2C_MSG 0 //Will print the I2C output buffer
 #define PRINT_DCM 0     //Will print the whole direction cosine matrix
 #define PRINT_ANALOGS 0 //Will print the analog raw data
@@ -49,7 +46,7 @@ int I2C_Message_ar[9];
 
 
 // *** NOTE!   To use ArduIMU with ArduPilot you must select binary output messages (change to 1 here)
-#define PRINT_BINARY 0   //Will print binary message and suppress ASCII messages (above)
+#define PRINT_BINARY 1   //Will print binary message and suppress ASCII messages (above)
 
 // *** NOTE!   Performance reporting is only supported for Ublox.  Set to 0 for others
 #define PERFORMANCE_REPORTING 1  //Will include performance reports in the binary output ~ 1/2 min
@@ -70,10 +67,9 @@ int I2C_Message_ar[9];
 #define ToDeg(x) (x*57.2957795131)  // *180/pi
 
 // LPR530 & LY530 Sensitivity (from datasheet) => 3.33mV/º/s, 3.22mV/ADC step => 1.03
-// Tested values : 0.96,0.96,0.94
-#define Gyro_Gain_X 0.92 //X axis Gyro gain
-#define Gyro_Gain_Y 0.92 //Y axis Gyro gain
-#define Gyro_Gain_Z 0.94 //Z axis Gyro gain
+#define Gyro_Gain_X 1.0 //X axis Gyro gain
+#define Gyro_Gain_Y 1.0 //Y axis Gyro gain
+#define Gyro_Gain_Z 1.0 //Z axis Gyro gain
 #define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
 #define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
 #define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second
@@ -84,7 +80,7 @@ int I2C_Message_ar[9];
 //#define Kp_YAW 2.5      //High yaw drift correction gain - use with caution!
 #define Ki_YAW 0.00005
 
-#define ADC_WARM_CYCLES 75
+#define ADC_WARM_CYCLES 100
 
 #define FALSE 0
 #define TRUE 1
@@ -268,7 +264,10 @@ void loop() //Main Loop
 {
   timeNow = millis();
 
-  if((timeNow-timer)>=20)  // Main loop runs at 50Hz
+  // 20 -> Main loop runs at 50Hz
+  // 5 -> Main loop runs at 200Hz
+  // Max measured duty time around 3ms
+  if((timeNow-timer)>=5)
   {
     timer_old = timer;
     timer = timeNow;
diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c
index 1ff1c7151ad..18b9fe654bc 100644
--- a/sw/airborne/modules/ins/ins_arduimu_basic.c
+++ b/sw/airborne/modules/ins/ins_arduimu_basic.c
@@ -132,7 +132,7 @@ void ArduIMU_event( void ) {
     estimator_p = arduimu_rates.p;
     ardu_ins_trans.status = I2CTransDone;
 
-    RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
+    //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
     RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r));
     RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z));
   }

From 02eb95dab92ee409d00e62341a3c415405b6cc37 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Wed, 13 Apr 2011 18:19:06 +0200
Subject: [PATCH 14/99] correct sign error in new vertical control

---
 conf/settings/tuning_ctl_new.xml              | 67 ++++---------------
 .../fixedwing/guidance/guidance_v_n.c         |  4 +-
 2 files changed, 14 insertions(+), 57 deletions(-)

diff --git a/conf/settings/tuning_ctl_new.xml b/conf/settings/tuning_ctl_new.xml
index 06caf025ac7..8ace2c0b9ee 100644
--- a/conf/settings/tuning_ctl_new.xml
+++ b/conf/settings/tuning_ctl_new.xml
@@ -4,67 +4,27 @@
 
 <settings>
   <dl_settings>
-    <dl_settings NAME="flight params">
-      <dl_setting MAX="1000" MIN="0" STEP="10" VAR="flight_altitude" shortname="altitude"/>
-      <dl_setting MAX="10" MIN="-10" STEP="0.5" VAR="wind_east"/>
-      <dl_setting MAX="10" MIN="-10" STEP="0.5" VAR="wind_north"/>
-    </dl_settings>
-
-    <dl_settings NAME="mode">
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="pprz_mode" module="autopilot"/>
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="alt_kalman_enabled" shortname="alt_kalman" module="estimator"/>
-      <dl_setting MAX="0" MIN="0" STEP="1" VAR="estimator_flight_time" shortname="flight time"/>
-      <dl_setting MAX="1000" MIN="0" STEP="1" VAR="stage_time"/>
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="launch"/>
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink"/>
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink"/>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps_reset" module="gps_ubx" handler="Reset" shortname="GPS reset"/>
-
-      <dl_setting MAX="200" MIN="-200" STEP="10" VAR="nav_radius" module="subsystems/nav" handler="SetNavRadius">
-        <strip_button icon="circle-right.png" name="Circle right" value="1" group="circle"/>
-        <strip_button icon="circle-left.png" name="Circle left" value="-1" group="circle"/>
-        <key_press key="greater" value="1"/>
-        <key_press key="less" value="-1"/>
-      </dl_setting>
-    </dl_settings>
 
     <dl_settings NAME="control">
-      <dl_settings NAME="infrared">
-        <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="infrared.roll_neutral"  shortname="roll_neutral"         param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" module="subsystems/sensors/infrared" />
-        <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="infrared.pitch_neutral" shortname="pitch_neutral"        param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
-
-        <dl_setting MAX="1.5" MIN="0." STEP="0.1"    VAR="infrared.lateral_correction"      shortname="lat_corr"   param="IR_LATERAL_CORRECTION"/>
-        <dl_setting MAX="1.5" MIN="0." STEP="0.1"    VAR="infrared.longitudinal_correction" shortname="log_corr"   param="IR_LONGITUDINAL_CORRECTION"/>
-        <dl_setting MAX="1.5" MIN="0.5" STEP="0.1"   VAR="infrared.vertical_correction"     shortname="vert_corr"  param="IR_VERTICAL_CORRECTION"/>
-
-        <dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_left"           shortname="corr_left"  param="IR_CORRECTION_LEFT"/>
-        <dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_right"          shortname="corr_right" param="IR_CORRECTION_RIGHT"/>
-        <dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_up"             shortname="corr_up"    param="IR_CORRECTION_UP"/>
-        <dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_down"           shortname="corr_down"  param="IR_CORRECTION_DOWN"/>
-
-      </dl_settings>
-
-
       <dl_settings NAME="attitude">
-        <dl_setting MAX="1" MIN="0" STEP="1" var="use_airspeed_ratio" values="FALSE|TRUE"/>
-        <dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="fw_h_ctl"/>
+        <dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
         <dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN"/>
-        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_igain" shortname="roll_igain" module="fw_h_ctl_a" handler="SetRollIGain"/>
+        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_igain" shortname="roll_igain" param="H_CTL_ROLL_IGAIN" handler="SetRollIGain"/>
         <dl_setting MAX="0" MIN="-55000" STEP="250" VAR="h_ctl_pitch_pgain" shortname="pitch_pgain" param="H_CTL_PITCH_PGAIN"/>
         <dl_setting MAX="0" MIN="-55000" STEP="250" VAR="h_ctl_pitch_dgain" shortname="pitch_dgain" param="H_CTL_PITCH_DGAIN"/>
-        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_igain" shortname="pitch_igain" module="fw_h_ctl_a" handler="SetPitchIGain"/>
-        <dl_setting MAX=".3" MIN="0." STEP="0.001" VAR="h_ctl_pitch_of_roll" shortname="pitch_of_roll" param="H_CTL_PITCH_OF_ROLL" module="fw_h_ctl_a"/>
-        <dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_aileron_of_throttle" shortname="aileron_of_throttle"/>
+        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_igain" shortname="pitch_igain" param="H_CTL_PITCH_IGAIN" handler="SetPitchIGain"/>
+        <dl_setting MAX=".3" MIN="0." STEP="0.001" VAR="h_ctl_pitch_of_roll" shortname="pitch_of_roll" param="H_CTL_PITCH_OF_ROLL"/>
+        <dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_aileron_of_throttle" shortname="aileron_of_throttle" module="stabilization/stabilization_adaptive"/>
 
         <dl_setting MAX="60" MIN="0" STEP="1" VAR="h_ctl_roll_max_setpoint" shortname="max_roll" param="H_CTL_ROLL_MAX_SETPOINT" unit="rad" alt_unit="deg" alt_unit_coef="0.0174533"/>
+        <dl_setting MAX="1" MIN="0" STEP="1" var="use_airspeed_ratio" values="FALSE|TRUE"/>
       </dl_settings>
 
       <dl_settings NAME="feedforward">
-        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kffa" module="fw_h_ctl_a" shortname="roll_Kffa"/>
-        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kffd" module="fw_h_ctl_a" shortname="roll_Kffd"/>
-        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_Kffa" module="fw_h_ctl_a" shortname="pitch_Kffa"/>
-        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_Kffd" module="fw_h_ctl_a" shortname="pitch_Kffd"/>
+        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kffa" shortname="roll_Kffa" param="H_CTL_ROLL_KFFA"/>
+        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kffd" shortname="roll_Kffd" param="H_CTL_ROLL_KFFD"/>
+        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_Kffa" shortname="pitch_Kffa" param="H_CTL_PITCH_KFFA"/>
+        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_Kffd" shortname="pitch_Kffd" param="H_CTL_PITCH_KFFD"/>
       </dl_settings>
 
       <dl_settings name="alt">
@@ -77,11 +37,11 @@
           <strip_button name="AS" value="1" group="speed_mode"/>
           <strip_button name="GS" value="2" group="speed_mode"/>
         </dl_setting>
-        <dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_cruise_throttle" shortname="cruise throttle" module="fw_v_ctl" handler="SetCruiseThrottle" param="V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE"/>
+        <dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_cruise_throttle" shortname="cruise throttle" module="guidance/guidance_v" handler="SetCruiseThrottle" param="V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE"/>
 
         <dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_pitch_pgain" shortname="pitch_p" param="V_CTL_AUTO_PITCH_PGAIN"/>
         <dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_pitch_igain" shortname="pitch_i" param="V_CTL_AUTO_PITCH_IGAIN"/>
-        <dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_pitch_dgain" shortname="pitch_d" param="V_CTL_AUTO_PITCH_DGAIN" module="fw_v_ctl_n"/>
+        <dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_pitch_dgain" shortname="pitch_d" param="V_CTL_AUTO_PITCH_DGAIN" module="guidance/guidance_v_n"/>
 
         <dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_throttle_pgain" shortname="throttle_p" param="V_CTL_AUTO_THROTTLE_PGAIN"/>
         <dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_throttle_igain" shortname="throttle_i" param="V_CTL_AUTO_THROTTLE_IGAIN"/>
@@ -114,13 +74,10 @@
         <dl_setting MAX="2" MIN="1" STEP="1" VAR="nav_mode"/>
         <dl_setting MAX="5" MIN="-5" STEP="0.5" VAR="nav_climb"/>
         <dl_setting MAX="20" MIN="-20" STEP="1" VAR="fp_pitch"/>
-        <dl_setting MAX="1" MIN="0" STEP="0.1" VAR="fp_throttle"/>
         <dl_setting MAX="50" MIN="-50" STEP="5" VAR="nav_shift" module="subsystems/nav" handler="IncreaseShift" shortname="inc. shift"/>
         <dl_setting MAX="50" MIN="5" STEP="0.5" VAR="nav_ground_speed_setpoint" shortname="ground speed"/>
         <dl_setting MAX="0." MIN="-0.2" STEP="0.01" VAR="nav_ground_speed_pgain" shortname="ground speed pgain"/>
         <dl_setting MAX="500" MIN="50" STEP="5" VAR="nav_survey_shift"/>
-        <dl_setting MAX="0.3" MIN="0" STEP="0.01" VAR="cte_igain"/>
-        <dl_setting MAX="20" MIN="0" STEP="1" VAR="cte_max"/>
       </dl_settings>
     </dl_settings>
   </dl_settings>
diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
index 02ecaaf8263..fcc14344f19 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
+++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
@@ -198,7 +198,7 @@ static inline void v_ctl_set_pitch ( void ) {
 
   if (v_ctl_auto_pitch_igain < 0.) {
     v_ctl_auto_pitch_sum_err += err*(1./60.);
-    BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain);
+    BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / (-v_ctl_auto_pitch_igain));
   }
 
   // PI loop + feedforward ctl
@@ -223,7 +223,7 @@ static inline void v_ctl_set_throttle( void ) {
 
   if (v_ctl_auto_throttle_igain < 0.) {
     v_ctl_auto_throttle_sum_err += err*(1./60.);
-    BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / v_ctl_auto_throttle_igain);
+    BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / (-v_ctl_auto_throttle_igain));
   }
 
   // PID loop + feedforward ctl

From b238c7f89a3fe654da478f4851a9674c8ed650a4 Mon Sep 17 00:00:00 2001
From: Piotr Esden-Tempski <piotr@esden.net>
Date: Wed, 13 Apr 2011 11:52:35 -0700
Subject: [PATCH 15/99] Moved IMU calibration into an include file.

---
 conf/airframes/esden/calib/aspirin_012.xml | 32 ++++++++++++++++++++++
 conf/airframes/esden/lisa_pwm_aspirin.xml  | 31 ++-------------------
 2 files changed, 34 insertions(+), 29 deletions(-)
 create mode 100644 conf/airframes/esden/calib/aspirin_012.xml

diff --git a/conf/airframes/esden/calib/aspirin_012.xml b/conf/airframes/esden/calib/aspirin_012.xml
new file mode 100644
index 00000000000..484ce882c4e
--- /dev/null
+++ b/conf/airframes/esden/calib/aspirin_012.xml
@@ -0,0 +1,32 @@
+<section name="IMU" prefix="IMU_">
+  <define name="GYRO_P_NEUTRAL" value="93"/>
+  <define name="GYRO_Q_NEUTRAL" value="131"/>
+  <define name="GYRO_R_NEUTRAL" value="-25"/>
+  <define name="GYRO_P_SENS" value="5.0" integer="16"/>
+  <define name="GYRO_Q_SENS" value="5.0" integer="16"/>
+  <define name="GYRO_R_SENS" value="5.0" integer="16"/>
+  <define name="GYRO_PQ_SENS" value="0.0" integer="16"/>
+  <define name="GYRO_PR_SENS" value="0.0" integer="16"/>
+  <define name="GYRO_QR_SENS" value="0.0" integer="16"/>
+
+  <define name="ACCEL_X_NEUTRAL" value="4"/>
+  <define name="ACCEL_Y_NEUTRAL" value="-14"/>
+  <define name="ACCEL_Z_NEUTRAL" value="-5"/>
+  <define name="ACCEL_X_SENS" value="39" integer="16"/>
+  <define name="ACCEL_Y_SENS" value="39" integer="16"/>
+  <define name="ACCEL_Z_SENS" value="39" integer="16"/>
+  <define name="ACCEL_XY_SENS" value="0.0" integer="16"/>
+  <define name="ACCEL_XZ_SENS" value="0.0" integer="16"/>
+  <define name="ACCEL_YZ_SENS" value="0.0" integer="16"/>
+
+  <define name="MAG_X_NEUTRAL" value="-21"/>
+  <define name="MAG_Y_NEUTRAL" value="10"/>
+  <define name="MAG_Z_NEUTRAL" value="12"/>
+  <define name="MAG_X_SENS" value="3.17378921476" integer="16"/>
+  <define name="MAG_Y_SENS" value="3.14663275967" integer="16"/>
+  <define name="MAG_Z_SENS" value="3.26531022727" integer="16"/>
+  <define name="MAG_XY_SENS" value="0.0" integer="16"/>
+  <define name="MAG_XZ_SENS" value="0.0" integer="16"/>
+  <define name="MAG_YZ_SENS" value="0.0" integer="16"/>
+
+</section>
diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml
index 01bd57bcdf7..fbc75e43a92 100644
--- a/conf/airframes/esden/lisa_pwm_aspirin.xml
+++ b/conf/airframes/esden/lisa_pwm_aspirin.xml
@@ -51,36 +51,9 @@
     <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
   </section>
 
+  <include href="conf/airframes/esden/calib/aspirin_012.xml"/>
+
   <section name="IMU" prefix="IMU_">
-    <define name="GYRO_P_NEUTRAL" value="27"/>
-    <define name="GYRO_Q_NEUTRAL" value="-10"/>
-    <define name="GYRO_R_NEUTRAL" value="20"/>
-    <define name="GYRO_P_SENS" value="5.0" integer="16"/>
-    <define name="GYRO_Q_SENS" value="5.0" integer="16"/>
-    <define name="GYRO_R_SENS" value="5.0" integer="16"/>
-    <define name="GYRO_PQ_SENS" value="0.0" integer="16"/>
-    <define name="GYRO_PR_SENS" value="0.0" integer="16"/>
-    <define name="GYRO_QR_SENS" value="0.0" integer="16"/>
-    
-    <define name="ACCEL_X_NEUTRAL" value="4"/>
-    <define name="ACCEL_Y_NEUTRAL" value="-14"/>
-    <define name="ACCEL_Z_NEUTRAL" value="-5"/>
-    <define name="ACCEL_X_SENS" value="39" integer="16"/>
-    <define name="ACCEL_Y_SENS" value="39" integer="16"/>
-    <define name="ACCEL_Z_SENS" value="39" integer="16"/>
-    <define name="ACCEL_XY_SENS" value="0.0" integer="16"/>
-    <define name="ACCEL_XZ_SENS" value="0.0" integer="16"/>
-    <define name="ACCEL_YZ_SENS" value="0.0" integer="16"/>
-    
-    <define name="MAG_X_NEUTRAL" value="0"/>
-    <define name="MAG_Y_NEUTRAL" value="0"/>
-    <define name="MAG_Z_NEUTRAL" value="0"/>
-    <define name="MAG_X_SENS" value="1." integer="16"/>
-    <define name="MAG_Y_SENS" value="1." integer="16"/>
-    <define name="MAG_Z_SENS" value="1." integer="16"/>
-    <define name="MAG_XY_SENS" value="0.0" integer="16"/>
-    <define name="MAG_XZ_SENS" value="0.0" integer="16"/>
-    <define name="MAG_YZ_SENS" value="0.0" integer="16"/>
     
     <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(   0. )"/>
     <define name="BODY_TO_IMU_THETA" value="RadOfDeg(   0. )"/>

From c251430b81354aaaa8efd4819e74aa942c9e0afd Mon Sep 17 00:00:00 2001
From: Piotr Esden-Tempski <piotr@esden.net>
Date: Wed, 13 Apr 2011 12:04:09 -0700
Subject: [PATCH 16/99] Changed estimator to ic.

---
 conf/airframes/esden/lisa_pwm_aspirin.xml | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml
index fbc75e43a92..94fd40b4ac9 100644
--- a/conf/airframes/esden/lisa_pwm_aspirin.xml
+++ b/conf/airframes/esden/lisa_pwm_aspirin.xml
@@ -130,6 +130,13 @@
 
   </section>
 
+  <section name="AHRS" prefix="AHRS_">
+    <define name="PROPAGATE_FREQUENCY" value="512"/>
+    <define name="H_X" value=" 0.51562740288882"/>
+    <define name="H_Y" value="-0.05707735220832"/>
+    <define name="H_Z" value=" 0.85490967783446"/>
+  </section>
+
   <section name="INS" prefix="INS_">
     <define name="BARO_SENS" value="10." integer="16"/>
   </section>
@@ -151,7 +158,6 @@
    <define name="INV_M" value ="0.15"/>
   </section>
 
-
  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
    <define name="PGAIN" value="-100"/>
    <define name="DGAIN" value="-100"/>
@@ -197,7 +203,8 @@
     <subsystem name="gps"           type="ublox">
 	    <configure name="GPS_BAUD"    value="B57600"/>
     </subsystem>
-    <subsystem name="ahrs"          type="cmpl"/>
+    <!--subsystem name="ahrs"          type="cmpl"/-->
+    <subsystem name="ahrs"          type="ic"/>
   </firmware>
 
   <firmware name="lisa_l_test_progs">

From ac50f56d021c06d04b7e4c47b923315533c16dd9 Mon Sep 17 00:00:00 2001
From: Piotr Esden-Tempski <piotr@esden.net>
Date: Wed, 13 Apr 2011 12:06:08 -0700
Subject: [PATCH 17/99] Bumped gains as the motor controllers are linear again.

---
 conf/airframes/esden/lisa_pwm_aspirin.xml | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml
index 94fd40b4ac9..fa18d1f0044 100644
--- a/conf/airframes/esden/lisa_pwm_aspirin.xml
+++ b/conf/airframes/esden/lisa_pwm_aspirin.xml
@@ -110,6 +110,7 @@
     <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
+    <!-- Old settings with exponential throttle motor controllers
     <!-- feedback -->
     <define name="PHI_PGAIN"  value="-250"/>
     <define name="PHI_DGAIN"  value="-200"/>
@@ -127,6 +128,25 @@
     <define name="PHI_DDGAIN"   value=" 200"/>
     <define name="THETA_DDGAIN" value=" 200"/>
     <define name="PSI_DDGAIN"   value=" 200"/>
+    -->
+
+    <!-- feedback -->
+    <define name="PHI_PGAIN"  value="-1900"/>
+    <define name="PHI_DGAIN"  value="-380"/>
+    <define name="PHI_IGAIN"  value="-200"/>
+
+    <define name="THETA_PGAIN"  value="-1900"/>
+    <define name="THETA_DGAIN"  value="-380"/>
+    <define name="THETA_IGAIN"  value="-200"/>
+
+    <define name="PSI_PGAIN"  value="-2000"/>
+    <define name="PSI_DGAIN"  value="-400"/>
+    <define name="PSI_IGAIN"  value="-10"/>
+
+    <!-- feedforward -->
+    <define name="PHI_DDGAIN"   value=" 300"/>
+    <define name="THETA_DDGAIN" value=" 300"/>
+    <define name="PSI_DDGAIN"   value=" 300"/>
 
   </section>
 

From f449e04a1216275b37f653ae69fae037b506e632 Mon Sep 17 00:00:00 2001
From: Piotr Esden-Tempski <piotr@esden.net>
Date: Wed, 13 Apr 2011 12:07:06 -0700
Subject: [PATCH 18/99] Bumped min motor to decrease the probability of cut out
 at low throttle.

---
 conf/airframes/esden/lisa_pwm_aspirin.xml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml
index fa18d1f0044..d9b7df70c5e 100644
--- a/conf/airframes/esden/lisa_pwm_aspirin.xml
+++ b/conf/airframes/esden/lisa_pwm_aspirin.xml
@@ -38,7 +38,7 @@
 
 
   <section name="SUPERVISION" prefix="SUPERVISION_">
-    <define name="MIN_MOTOR" value="1090"/>
+    <define name="MIN_MOTOR" value="1150"/>
     <define name="MAX_MOTOR" value="2000"/>
     <define name="TRIM_A" value="0"/>
     <define name="TRIM_E" value="0"/>

From 7ef265b0a82c1e85b1907a658e2b1c6c0b4fcd00 Mon Sep 17 00:00:00 2001
From: Piotr Esden-Tempski <piotr@esden.net>
Date: Wed, 13 Apr 2011 12:10:14 -0700
Subject: [PATCH 19/99] The new airframes have the imu rotated by 180.
 Corrected.

---
 conf/airframes/esden/lisa_pwm_aspirin.xml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml
index d9b7df70c5e..8b4db240fd8 100644
--- a/conf/airframes/esden/lisa_pwm_aspirin.xml
+++ b/conf/airframes/esden/lisa_pwm_aspirin.xml
@@ -57,7 +57,7 @@
     
     <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(   0. )"/>
     <define name="BODY_TO_IMU_THETA" value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(  45. )"/>
+    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg( 225. )"/>
 
   </section>
 

From 8188c8ad89d3958ea120ce99c3cfd8e9c1a31c55 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 15 Apr 2011 16:10:11 +0200
Subject: [PATCH 20/99] Higher Speed Servo updates with only 5 servo pulses
 instead of 10

---
 conf/airframes/CDW/ChimuTinyFwSpi.xml | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/conf/airframes/CDW/ChimuTinyFwSpi.xml b/conf/airframes/CDW/ChimuTinyFwSpi.xml
index a1fc625fec3..71b912ba1fa 100644
--- a/conf/airframes/CDW/ChimuTinyFwSpi.xml
+++ b/conf/airframes/CDW/ChimuTinyFwSpi.xml
@@ -7,11 +7,11 @@
 <airframe name="Yapa v1">
 
   <servos>
-    <servo name="THROTTLE" no="8" min="1000" neutral="1000" max="2000"/>
-    <servo name="AILERON_LEFT" no="9" min="1000" neutral="1500" max="2000"/>
-    <servo name="AILERON_RIGHT" no="4" min="2000" neutral="1500" max="1000"/>
-    <servo name="ELEVATOR" no="5" min="2000" neutral="1500" max="1000"/>
-    <servo name="RUDDER" no="3" min="1100" neutral="1500" max="1900"/>
+    <servo name="THROTTLE" no="0" min="1000" neutral="1000" max="2000"/>
+    <servo name="AILERON_LEFT" no="1" min="1000" neutral="1500" max="2000"/>
+    <servo name="AILERON_RIGHT" no="2" min="2000" neutral="1500" max="1000"/>
+    <servo name="ELEVATOR" no="3" min="2000" neutral="1500" max="1000"/>
+    <servo name="RUDDER" no="4" min="1100" neutral="1500" max="1900"/>
   </servos>
 
   <commands>
@@ -168,6 +168,7 @@
       <define name="WIND_INFO_RET"/>
       <define name="LOITER_TRIM"/>
       <define name="ALT_KALMAN"/>
+      <define name="NB_CHANNELS" value="5" /> 
     </target>
     <target name="sim" board="pc"/>
 

From 629ed9d9ce5e872c189ec8dc7a79d0d36aad748c Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Mon, 18 Apr 2011 18:06:39 +0200
Subject: [PATCH 21/99] improve input to ivy with variables

---
 conf/joystick/attack3_booz_nav.xml      |  18 +++-
 conf/joystick/cockpit_sx.xml            |  22 +++++
 conf/joystick/ikarus_usb.xml            |  20 +++++
 sw/ground_segment/joystick/input2ivy.ml | 104 ++++++++++++++++++++----
 4 files changed, 146 insertions(+), 18 deletions(-)
 create mode 100644 conf/joystick/cockpit_sx.xml
 create mode 100644 conf/joystick/ikarus_usb.xml

diff --git a/conf/joystick/attack3_booz_nav.xml b/conf/joystick/attack3_booz_nav.xml
index 583f3171023..fbeb7114ee2 100644
--- a/conf/joystick/attack3_booz_nav.xml
+++ b/conf/joystick/attack3_booz_nav.xml
@@ -11,8 +11,17 @@
     <button index="5" name="button6"/>
     <button index="6" name="button7"/>
     <button index="7" name="button8"/>
+    <button index="8" name="button9"/>
+    <button index="9" name="button10"/>
+    <button index="10" name="button11"/>
   </input>
 
+  <variables>
+    <var name="mode" default="0"/>
+    <set var="mode" value="0" on_event="button10"/>
+    <set var="mode" value="1" on_event="button11"/>
+  </variables>
+
   <messages period="0.1">
     <!--message class="datalink" name="BOOZ2_FMS_COMMAND">
       <field name="h_mode" value="IndexOfEnum(NAV)"/>
@@ -34,12 +43,17 @@
       <field name="index" value="IndexOfSetting(booz2_cam_tilt_pwm)"/>
       <field name="value" value="Scale(throttle, 1000, 2000)"/>
     </message-->
+    <message class="ground" name="DL_SETTING" on_event="button11 || button10">
+      <field name="index" value="IndexOfSetting(kill_throttle)"/>
+      <field name="value" value="mode"/>
+    </message>
+
 
     <message class="ground" name="JUMP_TO_BLOCK" on_event="shoot && button7">
       <field name="block_id" value="IndexOfBlock('land here')"/>
     </message>
-    <message class="ground" name="JUMP_TO_BLOCK" on_event="shoot && button8">
+    <!--message class="ground" name="JUMP_TO_BLOCK" on_event="shoot && button8">
       <field name="block_id" value="IndexOfBlock('Joystick')"/>
-    </message>
+    </message-->
   </messages>
 </joystick>
diff --git a/conf/joystick/cockpit_sx.xml b/conf/joystick/cockpit_sx.xml
new file mode 100644
index 00000000000..d94603ceb1f
--- /dev/null
+++ b/conf/joystick/cockpit_sx.xml
@@ -0,0 +1,22 @@
+<joystick>
+  <input>
+    <axis index="0" name="roll"/>
+    <axis index="1" name="pitch"/>
+    <axis index="2" name="yaw"/>
+    <axis index="3" name="spoiler"/>
+    <axis index="4" name="throttle"/>
+    <button index="3" name="mode"/>
+  </input>
+
+  <messages period="0.1">
+
+    <message class="datalink" name="RC_4CH" send_always="true">
+      <field name="mode" value="mode + 1"/> <!-- only AUTO1 and AUTO2 available -->
+      <field name="throttle" value="Bound(Fit(throttle,0-100,100)+127,0,256)"/>
+      <field name="roll" value="roll"/>
+      <field name="yaw" value="yaw"/>
+      <field name="pitch" value="pitch"/>
+    </message>
+
+  </messages>
+</joystick>
diff --git a/conf/joystick/ikarus_usb.xml b/conf/joystick/ikarus_usb.xml
new file mode 100644
index 00000000000..6134a9da82a
--- /dev/null
+++ b/conf/joystick/ikarus_usb.xml
@@ -0,0 +1,20 @@
+<joystick>
+  <input>
+    <axis index="0" name="roll"/>
+    <axis index="1" name="pitch"/>
+    <axis index="2" name="throttle"/>
+    <axis index="4" name="mode"/>
+  </input>
+
+  <messages period="0.1">
+
+    <message class="datalink" name="RC_4CH" send_always="true">
+      <field name="mode" value="PprzMode(0-mode)"/>
+      <field name="throttle" value="127-throttle"/>
+      <field name="roll" value="roll"/>
+      <field name="yaw" value="0"/>
+      <field name="pitch" value="pitch"/>
+    </message>
+
+  </messages>
+</joystick>
diff --git a/sw/ground_segment/joystick/input2ivy.ml b/sw/ground_segment/joystick/input2ivy.ml
index 356bd99cd62..61b0af2f1c8 100644
--- a/sw/ground_segment/joystick/input2ivy.ml
+++ b/sw/ground_segment/joystick/input2ivy.ml
@@ -91,11 +91,18 @@ type msg = {
     send_always : bool
   }
 
-(** Represenation of an input device and of the messages to send *)
+(** Representation of a variable *)
+type var = {
+  mutable value : int;
+  var_event : (int * Syntax.expression) list;
+}
+
+(** Represenation of an input device, the messages to send and the variables *)
 type actions = {
     period_ms : int;
     inputs : (string*input) list;
-    messages : msg list
+    messages : msg list;
+    variables : (string*var) list;
   }
 
 (** adjust the trim on an axis given its name *)
@@ -226,8 +233,8 @@ let parse_msg = fun msg ->
   let fields =
     match get_message_type msg_class with
       "Message" -> 
-  let msg_descr = get_message msg_class msg_name in
-         List.map (parse_msg_field msg_descr) (Xml.children msg)
+        let msg_descr = get_message msg_class msg_name in
+        List.map (parse_msg_field msg_descr) (Xml.children msg)
     | "Trim" -> []
     | _ -> failwith ("Unknown message class type") in
 
@@ -241,8 +248,33 @@ let parse_msg = fun msg ->
     send_always = send_always
   }
 
-
-(** Verbose List.assco *)
+(** Parse an XML list of variables and set function *)
+let parse_variables = fun variables ->
+  let l = ref [] in
+  List.iter (fun x ->
+    match Xml.tag x with
+      "var" ->
+        let name = Xml.attrib x "name"
+        and default = ExtXml.int_attrib x "default" in
+        if List.mem_assoc name !l then failwith (sprintf "Variable %s already declared" name);
+        (* filter all "set" node for this variable *)
+        let set = List.filter (fun vs ->
+            (ExtXml.tag_is vs "set") &&
+            (compare (ExtXml.attrib_or_default vs "var" "") name) = 0)
+          (Xml.children variables) in
+        let var_event = List.map (fun s ->
+          let value = ExtXml.int_attrib s "value"
+          and on_event = parse_value (Xml.attrib s "on_event") in
+          (value, on_event)
+          ) set in
+        l := (name, { value = default; var_event = var_event; }) :: !l;
+        ()
+    | _ -> ()
+  ) (Xml.children variables);
+  !l
+
+
+(** Verbose List.assoc *)
 let my_assoc = fun x l ->
   try List.assoc x l with Not_found ->
     failwith (sprintf "my_assoc: %s not found" x)
@@ -278,7 +310,8 @@ let parse_descr = fun xml_file trim_file ->
   let xml = Xml.parse_file xml_file in
 
   let inputs = parse_input (ExtXml.child xml "input")
-  and messages_xml = ExtXml.child xml "messages" in
+  and messages_xml = ExtXml.child xml "messages"
+  and variables = try parse_variables (ExtXml.child xml "variables") with _ -> [] in
 
   let period_ms = int_of_float (1000.*.ExtXml.float_attrib messages_xml "period")
   and messages = List.map parse_msg (Xml.children messages_xml) in
@@ -286,7 +319,7 @@ let parse_descr = fun xml_file trim_file ->
   (** check for trim file *)
   parse_trim_file trim_file inputs;
 
-  { period_ms = period_ms; inputs = inputs; messages = messages }
+  { period_ms = period_ms; inputs = inputs; messages = messages; variables = variables }
 
 (** apply deadband  - applied first *)
 let apply_deadband = fun x min ->
@@ -316,10 +349,25 @@ let eval_input = fun buttons axis input ->
 let scale = fun x min max ->
   min + ((x - min_input) * (max - min)) / (max_input - min_input)
 
+(** Fit a given interval of value into [min_input; max_input] *)
+let fit = fun x min max ->
+  min_input + ((x - min) * (max_input - min_input)) / (max - min)
+
 (** Scale a value in the given bounds *)
 let bound = fun x min max ->
   if x < min then min else (if x > max then max else x)
 
+(** Return a pprz RC mode
+  * mode > max -> 2
+  * mode < min -> 0
+  * else 1
+  *)
+let pprz_threshold = max_input / 2
+let pprz_mode = fun mode ->
+  if mode > pprz_threshold then 2
+  else if mode < -pprz_threshold then 0
+  else 1
+
 (** Eval a function call (TO BE COMPLETED) *)
 let eval_call = fun f args ->
   match f, args with
@@ -332,16 +380,25 @@ let eval_call = fun f args ->
   | "<",  [a1; a2] -> if a1 < a2 then 1 else 0
   | ">",  [a1; a2] -> if a1 > a2 then 1 else 0
   | "Scale", [x; min; max] -> scale (x) (min) (max)
+  | "Fit", [x; min; max] -> fit (x) (min) (max)
   | "Bound", [x; min; max] -> bound (x) (min) (max)
+  | "PprzMode", [x] -> pprz_mode (x)
   | "JoystickID", [] -> !joystick_id
   | f, args -> failwith (sprintf "eval_call: unknown function '%s'" f)
 
 (** Eval an expression *)
-let eval_expr = fun buttons axis inputs expr ->
+let eval_expr = fun buttons axis inputs variables expr ->
   let rec eval = function
       Syntax.Ident ident ->
-        let input = my_assoc ident inputs in
-        eval_input buttons axis input
+        (* try input first, then variables *)
+        let i = match (List.mem_assoc ident inputs, List.mem_assoc ident variables) with
+          (true, _) -> eval_input buttons axis (List.assoc ident inputs)
+        | (false, true) ->
+            let v = List.assoc ident variables in
+            v.value
+        | (false, false) -> failwith (sprintf "eval_expr: %s not found" ident)
+        in
+        i
     | Syntax.Int int -> int
     | Syntax.Float float -> failwith "eval_expr: float"
     | Syntax.Call (ident, exprs) | Syntax.CallOperator (ident, exprs) ->
@@ -392,19 +449,32 @@ let trim_adjust = fun axis_name adjustment inputs ->
     Axis (i, deadband, limit, exponent, trim) -> trim := trim.contents +. adjustment
   | Button i -> failwith "No trim for buttons"
 
-(**Send an ivy message if needed: new values and/or 'on_event' condition true*)
-let execute_action = fun ac_id inputs buttons axis message ->
+(** Update variables state *)
+let update_variables = fun inputs buttons axis variables ->
+  List.iter (fun (_,var) ->
+    List.iter (fun (value, expr) ->
+      let event = eval_expr buttons axis inputs variables expr in
+      if event <> 0 then begin
+        (* remove and add again ? *)
+        var.value <- value
+      end
+    ) var.var_event
+  ) variables
+
+(** Send an ivy message if needed: new values and/or 'on_event' condition true*)
+let execute_action = fun ac_id inputs buttons axis variables message ->
   let values =
     List.map
-      (fun (name, expr) -> (name, Pprz.Int (eval_expr buttons axis inputs expr)))
+      (fun (name, expr) -> (name, Pprz.Int (eval_expr buttons axis inputs variables expr)))
       message.fields
 
   and on_event =
     match message.on_event with
       None -> true
-    | Some expr -> eval_expr buttons axis inputs expr <> 0 in
+    | Some expr -> eval_expr buttons axis inputs variables expr <> 0 in
 
   let previous_values = get_previous_values message.msg_name in
+  (* FIXME ((value <> previous) && on_event) || send_always ??? *)
   if ( ( (on_event, values) <> previous_values ) || message.send_always ) && on_event then begin
     let vs = ("ac_id", Pprz.Int ac_id) :: values in
     match message.msg_class with
@@ -440,7 +510,9 @@ let execute_actions = fun actions ac_id ->
     if !verbose then
       print_inputs nb_buttons buttons axis;
 
-    List.iter (execute_action ac_id actions.inputs buttons axis) actions.messages
+    (* TODO update variables before msg *)
+    update_variables actions.inputs buttons axis actions.variables;
+    List.iter (execute_action ac_id actions.inputs buttons axis actions.variables) actions.messages
   with
     exc -> prerr_endline (Printexc.to_string exc)
 

From 5bb3d894c28b847a012452c96a5cd82534f83fd3 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Mon, 18 Apr 2011 19:08:43 +0200
Subject: [PATCH 22/99] rc_datalink working with RC_4CH message

---
 .../fixedwing/radio_control_datalink.makefile |  2 +-
 conf/joystick/cockpit_sx.xml                  |  4 ++--
 conf/joystick/ikarus_usb.xml                  |  2 +-
 conf/messages.xml                             |  2 +-
 sw/airborne/firmwares/fixedwing/datalink.c    |  6 ++---
 .../subsystems/radio_control/rc_datalink.c    | 23 ++++++++++++++++---
 .../subsystems/radio_control/rc_datalink.h    | 17 +++++++++++---
 sw/ground_segment/joystick/input2ivy.ml       |  4 ++--
 8 files changed, 44 insertions(+), 16 deletions(-)

diff --git a/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile b/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile
index e33a41c25ed..dc36fad9562 100644
--- a/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile
+++ b/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile
@@ -15,5 +15,5 @@ ifeq ($(NORADIO), False)
   $(TARGET).srcs		+= $(SRC_SUBSYSTEMS)/radio_control.c
   $(TARGET).srcs		+= $(SRC_SUBSYSTEMS)/radio_control/rc_datalink.c
 # arch only with sim target for compatibility (empty functions)
-	sim.srcs					+= $(SRC_ARCH)/radio_control/rc_datalink.c
+	sim.srcs					+= $(SRC_ARCH)/subsystems/radio_control/rc_datalink.c
 endif
diff --git a/conf/joystick/cockpit_sx.xml b/conf/joystick/cockpit_sx.xml
index d94603ceb1f..8bfa250d4d9 100644
--- a/conf/joystick/cockpit_sx.xml
+++ b/conf/joystick/cockpit_sx.xml
@@ -12,10 +12,10 @@
 
     <message class="datalink" name="RC_4CH" send_always="true">
       <field name="mode" value="mode + 1"/> <!-- only AUTO1 and AUTO2 available -->
-      <field name="throttle" value="Bound(Fit(throttle,0-100,100)+127,0,256)"/>
+      <field name="throttle" value="Fit(throttle,0-100,100,0,127)"/>
       <field name="roll" value="roll"/>
-      <field name="yaw" value="yaw"/>
       <field name="pitch" value="pitch"/>
+      <field name="yaw" value="yaw"/>
     </message>
 
   </messages>
diff --git a/conf/joystick/ikarus_usb.xml b/conf/joystick/ikarus_usb.xml
index 6134a9da82a..1f5bb2a379a 100644
--- a/conf/joystick/ikarus_usb.xml
+++ b/conf/joystick/ikarus_usb.xml
@@ -12,8 +12,8 @@
       <field name="mode" value="PprzMode(0-mode)"/>
       <field name="throttle" value="127-throttle"/>
       <field name="roll" value="roll"/>
-      <field name="yaw" value="0"/>
       <field name="pitch" value="pitch"/>
+      <field name="yaw" value="0"/>
     </message>
 
   </messages>
diff --git a/conf/messages.xml b/conf/messages.xml
index 45a085f9366..4c58df1c400 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1959,8 +1959,8 @@
     <field name="mode"        type="uint8"/>
     <field name="throttle"    type="uint8"/>
     <field name="roll"        type="int8"/>
-    <field name="yaw"         type="int8"/>
     <field name="pitch"       type="int8"/>
+    <field name="yaw"         type="int8"/>
   </message>
 
   <message name="KITE_COMMAND" id="96">
diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c
index 1178a0ef0b1..d94abb5d1d9 100644
--- a/sw/airborne/firmwares/fixedwing/datalink.c
+++ b/sw/airborne/firmwares/fixedwing/datalink.c
@@ -185,15 +185,15 @@ void dl_parse_msg(void) {
 #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
     if (msg_id == DL_RC_3CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) {
 LED_TOGGLE(3);
-      parse_rc_datalink(
+      parse_rc_3ch_datalink(
           DL_RC_3CH_throttle_mode(dl_buffer),
           DL_RC_3CH_roll(dl_buffer),
           DL_RC_3CH_pitch(dl_buffer));
     } else
-    if (msg_id == DL_RC_4CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) {
+    if (msg_id == DL_RC_4CH && DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
 LED_TOGGLE(3);
       parse_rc_4ch_datalink(
-          DL_RC_4CH_joystick_id(dl_buffer),
+          DL_RC_4CH_mode(dl_buffer),
           DL_RC_4CH_throttle(dl_buffer),
           DL_RC_4CH_roll(dl_buffer),
           DL_RC_4CH_pitch(dl_buffer),
diff --git a/sw/airborne/subsystems/radio_control/rc_datalink.c b/sw/airborne/subsystems/radio_control/rc_datalink.c
index 662a6b5ccda..1d7a39ccb5e 100644
--- a/sw/airborne/subsystems/radio_control/rc_datalink.c
+++ b/sw/airborne/subsystems/radio_control/rc_datalink.c
@@ -21,7 +21,7 @@
  * Boston, MA 02111-1307, USA.
  */
 
-#include "radio_control/rc_datalink.h"
+#include "subsystems/radio_control/rc_datalink.h"
 #include "subsystems/radio_control.h"
 
 int8_t rc_dl_values[ RC_DL_NB_CHANNEL ];
@@ -33,11 +33,11 @@ void radio_control_impl_init(void) {
 }
 
 
-void parse_rc_datalink( uint8_t throttle_mode,
+void parse_rc_3ch_datalink( uint8_t throttle_mode,
                         int8_t roll,
                         int8_t pitch)
 {
-  uint8_t throttle = throttle_mode & 0xFC;
+  uint8_t throttle = ((throttle_mode & 0xFC)>>2)*(128/64);
   uint8_t mode = throttle_mode & 0x03;
 
   rc_dl_values[RADIO_ROLL] = roll;
@@ -47,3 +47,20 @@ void parse_rc_datalink( uint8_t throttle_mode,
 
   rc_dl_frame_available = TRUE;
 }
+
+void parse_rc_4ch_datalink(
+    uint8_t mode,
+    uint8_t throttle,
+    int8_t roll,
+    int8_t pitch,
+    int8_t yaw)
+{
+  rc_dl_values[RADIO_MODE] = (int8_t)mode;
+  rc_dl_values[RADIO_THROTTLE] = (int8_t)throttle;
+  rc_dl_values[RADIO_ROLL] = roll;
+  rc_dl_values[RADIO_PITCH] = pitch;
+  rc_dl_values[RADIO_YAW] = yaw;
+
+  rc_dl_frame_available = TRUE;
+}
+
diff --git a/sw/airborne/subsystems/radio_control/rc_datalink.h b/sw/airborne/subsystems/radio_control/rc_datalink.h
index d10170febbe..ca72750480d 100644
--- a/sw/airborne/subsystems/radio_control/rc_datalink.h
+++ b/sw/airborne/subsystems/radio_control/rc_datalink.h
@@ -43,13 +43,24 @@ extern int8_t rc_dl_values[ RC_DL_NB_CHANNEL ];
 extern volatile bool_t rc_dl_frame_available;
 
 /**
- * Decode datalink message to get rc values
+ * Decode datalink message to get rc values with RC_3CH message
+ * Mode and throttle are merge in the same byte
  */
-extern void parse_rc_datalink(
+extern void parse_rc_3ch_datalink(
     uint8_t throttle_mode,
     int8_t roll,
     int8_t pitch);
 
+/**
+ * Decode datalink message to get rc values with RC_4CH message
+ */
+extern void parse_rc_4ch_datalink(
+    uint8_t mode,
+    uint8_t throttle,
+    int8_t roll,
+    int8_t pitch,
+    int8_t yaw);
+
 /**
  * Macro that normalize rc_dl_values to radio values
  */
@@ -60,7 +71,7 @@ extern void parse_rc_datalink(
   Bound(_out[RADIO_PITCH], MIN_PPRZ, MAX_PPRZ);                   \
   _out[RADIO_YAW] = 0;                                            \
   Bound(_out[RADIO_YAW], MIN_PPRZ, MAX_PPRZ);                     \
-  _out[RADIO_THROTTLE] = ((MAX_PPRZ/64) * _in[RADIO_THROTTLE]);   \
+  _out[RADIO_THROTTLE] = ((MAX_PPRZ/128) * _in[RADIO_THROTTLE]);   \
   Bound(_out[RADIO_THROTTLE], 0, MAX_PPRZ);                       \
   _out[RADIO_MODE] = MAX_PPRZ * (_in[RADIO_MODE] - 1);            \
   Bound(_out[RADIO_MODE], MIN_PPRZ, MAX_PPRZ);                    \
diff --git a/sw/ground_segment/joystick/input2ivy.ml b/sw/ground_segment/joystick/input2ivy.ml
index 61b0af2f1c8..594275b14cf 100644
--- a/sw/ground_segment/joystick/input2ivy.ml
+++ b/sw/ground_segment/joystick/input2ivy.ml
@@ -350,7 +350,7 @@ let scale = fun x min max ->
   min + ((x - min_input) * (max - min)) / (max_input - min_input)
 
 (** Fit a given interval of value into [min_input; max_input] *)
-let fit = fun x min max ->
+let fit = fun x min max min_input max_input ->
   min_input + ((x - min) * (max_input - min_input)) / (max - min)
 
 (** Scale a value in the given bounds *)
@@ -380,7 +380,7 @@ let eval_call = fun f args ->
   | "<",  [a1; a2] -> if a1 < a2 then 1 else 0
   | ">",  [a1; a2] -> if a1 > a2 then 1 else 0
   | "Scale", [x; min; max] -> scale (x) (min) (max)
-  | "Fit", [x; min; max] -> fit (x) (min) (max)
+  | "Fit", [x; min; max; min_input; max_input] -> fit (x) (min) (max) (min_input) (max_input)
   | "Bound", [x; min; max] -> bound (x) (min) (max)
   | "PprzMode", [x] -> pprz_mode (x)
   | "JoystickID", [] -> !joystick_id

From ca9322cf2c46834f296a2ab36e6465af0edec17f Mon Sep 17 00:00:00 2001
From: Vandeportaele Bertrand <bvandepo@laas.fr>
Date: Wed, 20 Apr 2011 12:35:18 +0200
Subject: [PATCH 23/99] add cartography module

---
 conf/airframes/LAAS/mmlaas_N1.xml             |   7 +-
 .../versatile_carto_fixe_muret.xml            | 160 ++++
 conf/modules/cartography.xml                  |  22 +
 sw/airborne/modules/cartography/cartography.c | 745 ++++++++++++++++++
 sw/airborne/modules/cartography/cartography.h |  52 ++
 5 files changed, 985 insertions(+), 1 deletion(-)
 create mode 100644 conf/flight_plans/versatile_carto_fixe_muret.xml
 create mode 100644 conf/modules/cartography.xml
 create mode 100644 sw/airborne/modules/cartography/cartography.c
 create mode 100644 sw/airborne/modules/cartography/cartography.h

diff --git a/conf/airframes/LAAS/mmlaas_N1.xml b/conf/airframes/LAAS/mmlaas_N1.xml
index 470ae5aa946..59cacc5ea6a 100644
--- a/conf/airframes/LAAS/mmlaas_N1.xml
+++ b/conf/airframes/LAAS/mmlaas_N1.xml
@@ -42,7 +42,12 @@
     <load name="formation_flight.xml"/>
    <!-- <load name="tcas.xml"/> -->
     <!--load name="potential.xml"/-->
-  </modules>
+  <load name="cartography.xml"/>
+</modules>
+
+
+
+
 
 <!-- commands section -->
   <servos>
diff --git a/conf/flight_plans/versatile_carto_fixe_muret.xml b/conf/flight_plans/versatile_carto_fixe_muret.xml
new file mode 100644
index 00000000000..21b8968ffe8
--- /dev/null
+++ b/conf/flight_plans/versatile_carto_fixe_muret.xml
@@ -0,0 +1,160 @@
+<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
+
+<flight_plan alt="240" ground_alt="190" lat0="43.4622" lon0="1.2729" max_dist_from_home="1200" name="Versatile" qfu="270" security_height="60"> 
+
+<!--<flight_plan alt="240" ground_alt="190" lat0="43.224622" lon0="1.2729" max_dist_from_home="900" name="Versatile" qfu="270" security_height="25"> -->
+  <header>
+#include "datalink.h"
+/*
+#ifndef VARDECLARED
+#define VARDECLARED
+float varsweepsize=110;
+#endif
+*/
+</header>
+  <waypoints>
+    <waypoint name="HOME" x="0" y="0"/>
+    <waypoint name="STDBY" x="20" y="80"/>
+    <waypoint name="1" x="44.8" y="102.2"/>
+    <waypoint name="2" x="-63.5" y="122.9"/>
+    <waypoint name="MOB" x="-11.5" y="-21.2"/>
+    <waypoint name="S1" x="-177.6" y="11.6"/>
+    <waypoint name="S2" x="173.9" y="-41.3"/>
+    <waypoint name="S3" x="-60.6" y="310.9"/>
+    <waypoint name="S4" x="-204.7" y="17.0"/>
+    <waypoint alt="30" name="AF" x="200" y="-10"/>
+    <waypoint alt="0" name="TD" x="80.0" y="20.0"/>
+    <waypoint name="BASELEG" x="26.9" y="-23.0"/>
+    <waypoint name="_1" x="-100" y="0"/>
+    <waypoint name="_2" x="-100" y="200"/>
+    <waypoint name="_3" x="100" y="200"/>
+    <waypoint name="_4" x="100" y="0"/>
+    <waypoint name="CLIMB" x="-122.5" y="35.4"/>
+    <waypoint name="SP1" x="-57.2" y="9.8"/>
+    <waypoint name="SP2" x="54.2" y="-41.7"/>
+    <waypoint name="SP3" x="-15.7" y="88.6"/>
+    <waypoint name="SP4" x="97.5" y="37.8"/>
+  </waypoints>
+<!--  <sectors>
+    <sector name="PolySectorS">
+      <corner name="S1"/>
+      <corner name="S2"/>
+      <corner name="S3"/>
+      <corner name="S4"/>
+    </sector>
+  </sectors>-->
+  <exceptions/>
+  <blocks>
+    <block name="Wait GPS">
+      <set value="1" var="kill_throttle"/>
+      <while cond="!GpsFixValid()"/>
+    </block>
+    <block name="Holding point">
+      <set value="1" var="kill_throttle"/>
+      <set value="NAV_MODE_ROLL" var="nav_mode"/>
+      <attitude roll="0" throttle="0" vmode="throttle"/>
+    </block>
+    <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
+      <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
+      <set value="0" var="kill_throttle"/>
+      <go wp="CLIMB"/>
+    </block>
+    <block name="Standby" strip_button="Standby" strip_icon="home.png">
+      <circle radius="nav_radius" wp="STDBY"/>
+    </block>
+
+ 
+
+    <block name="IncRailNumber" strip_button="Standby" strip_icon="home.png">
+		    <call fun="nav_survey_Inc_railnumberSinceBoot()"/>		    
+	    <circle radius="nav_radius" wp="STDBY"/>
+    </block>
+
+    <block name="Snapshoot" strip_button="Standby" strip_icon="home.png">
+		    <call fun="nav_survey_Snapshoot()"/>		    
+	    <circle radius="nav_radius" wp="STDBY"/>
+    </block>
+
+    <block name="SnapshootContinu" strip_button="Standby" strip_icon="home.png">
+		    <call fun="nav_survey_Snapshoot_Continu()"/>		    
+	    <circle radius="nav_radius" wp="STDBY"/>
+    </block>
+
+    <block name="StopSnapshoot" strip_button="Standby" strip_icon="home.png">
+		    <call fun="nav_survey_StopSnapshoot()"/>		    
+	    <circle radius="nav_radius" wp="STDBY"/>
+    </block>
+
+
+
+
+    <block name="Oval 1-2" strip_button="Oval (wp 1-2)" strip_icon="oval.png">
+      <oval p1="1" p2="2" radius="nav_radius"/>
+    </block>
+    <block name="MOB" strip_button="Turn around here" strip_icon="mob.png">
+      <call fun="NavSetWaypointHere(WP_MOB)"/>
+      <circle radius="100" wp="MOB"/>
+    </block>
+ 
+
+
+  <block name="init_display_carto">
+      <call fun="nav_survey_computefourth_corner(WP_S1,WP_S2,WP_S3,WP_S4)"/>
+    </block>
+
+    <block name="Monter">
+      <call fun="NavSetWaypointHere(WP_MOB)"/>
+      <set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
+      <circle alt="ground_alt+105" radius="nav_radius" wp="MOB"/>
+      <exception cond="estimator_z > ground_alt+100" deroute="Carto survey S1-S2-S3"/>
+    </block>
+
+  
+    <block name="Carto survey S1-S2-S3" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
+      <call fun="nav_survey_losange_carto_init(WP_S1,WP_S2,WP_S3, 50,150)"/>
+      <call fun="nav_survey_losange_carto()"/>
+    </block>
+    <block name="Carto survey S1-S3-S2" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
+      <call fun="nav_survey_losange_carto_init(WP_S1,WP_S3,WP_S2, 0,150)"/>
+      <call fun="nav_survey_losange_carto()"/>
+    </block>
+    <block name="Fin Carto" strip_button="Standby" strip_icon="home.png">
+      <circle radius="nav_radius" wp="STDBY"/>
+    </block>
+
+
+  <block name="init_display_carto Fin">
+      <call fun="nav_survey_computefourth_corner(WP_SP1,WP_SP2,WP_SP3,WP_SP4)"/>
+    </block>
+
+
+    <block name="Descendre">
+      <call fun="NavSetWaypointHere(WP_MOB)"/>
+      <set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
+      <circle alt="ground_alt+35" radius="nav_radius" wp="MOB"/>
+      <exception cond="ground_alt+40 > estimator_z   " deroute="Carto survey SP1-SP2-SP3 fin"/>
+    </block>
+
+    <block name="Carto survey SP1-SP2-SP3 fin" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
+      <call fun="nav_survey_losange_carto_init(WP_SP1,WP_SP2,WP_SP3, 20,150)"/>
+      <call fun="nav_survey_losange_carto()"/>
+    </block>
+    <block name="Carto survey SP1-SP3-SP2 fin " strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
+      <call fun="nav_survey_losange_carto_init(WP_SP1,WP_SP3,WP_SP2, 0,150)"/>
+      <call fun="nav_survey_losange_carto()"/>
+    </block>
+    <block name="Fin Carto fin " strip_button="Standby" strip_icon="home.png">
+      <circle radius="nav_radius" wp="STDBY"/>
+    </block>
+
+
+    <block name="Geo init">
+      <while cond="LessThan(NavBlockTime(), 10)"/>
+      <call fun="NavSetGroundReferenceHere()"/>
+    </block>
+   
+    <block name="Survey S1-S2" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
+      <survey_rectangle grid="150" wp1="S1" wp2="S2"/>
+    </block>
+  </blocks>
+</flight_plan>
diff --git a/conf/modules/cartography.xml b/conf/modules/cartography.xml
new file mode 100644
index 00000000000..04e3d9d4de8
--- /dev/null
+++ b/conf/modules/cartography.xml
@@ -0,0 +1,22 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="cartography">
+  <header>
+    <file name="cartography.h"/>
+  </header>
+  <init fun="init_carto()"/>
+  <periodic fun="periodic_1Hz_carto()" freq="1." start="start_carto()" stop="stop_carto()" autorun="TRUE"/>
+  <periodic fun="periodic_10Hz_carto()" period="0.1" start="start_carto()" stop="stop_carto()" autorun="FALSE"/>
+  <makefile>
+    <raw>
+#Exemple of RAW makefile part
+    </raw>
+    <define name="DEMO_MODULE_LED" value="2"/>
+    <file name="cartography.c"/>
+  </makefile>
+  <makefile target="sim">
+    <define name="SOME_FLAG"/>
+    <configure name="SOME_DEFINE" value="bla"/>
+  </makefile>
+</module>
+
diff --git a/sw/airborne/modules/cartography/cartography.c b/sw/airborne/modules/cartography/cartography.c
new file mode 100644
index 00000000000..bd5febc0236
--- /dev/null
+++ b/sw/airborne/modules/cartography/cartography.c
@@ -0,0 +1,745 @@
+/*
+ * $Id: cartography.c  2011-04-20 10:43:13Z  $
+ *  
+ * Copyright (C) 2011  Vandeportaele Bertrand
+ *
+ * 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.
+ *
+ */
+/** \file cartography.c
+ *  \brief Navigation functions for cartography of the ground
+ *
+ */
+
+
+
+ 
+#include "estimator.h" 
+#include "stdio.h" 
+
+#include "subsystems/nav.h"
+#include "generated/flight_plan.h"
+
+#include "std.h"  //macros pas mal dans sw/include
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+//for fast debbuging, the simulation can be accelerated using the gaia software from an xterm console
+//              /home/bvdp/paparazzi3/sw/simulator/gaia 
+////////////////////////////////////////////////////////////////////////////////////////////////
+// for explanations about debugging macros:
+//http://gcc.gnu.org/onlinedocs/cpp/Stringification.html#Stringification
+
+// Be carefull not to use printf function in ap compilation, only use it in sim compilation
+// the DEBUG_PRINTF should be defined only in the sim part of the makefile airframe file
+#ifdef DEBUG_PRINTF
+int CPTDEBUG=0;	
+#define PRTDEB(TYPE,EXP) \
+printf("%5d: " #EXP ": %"#TYPE"\n",CPTDEBUG,EXP);fflush(stdout);CPTDEBUG++;
+#define PRTDEBSTR(EXP) \
+printf("%5d: STR: "#EXP"\n",CPTDEBUG);fflush(stdout);CPTDEBUG++;
+#else 
+#define PRTDEB(TYPE,EXP) \
+;
+
+#define PRTDEBSTR(EXP) \
+;
+#endif
+
+/*
+ exemple of use for theese macros
+ PRTDEBSTR(Init polysurvey)
+ PRTDEB(u,SurveySize)
+ 
+ PRTDEB(lf,PolygonCenter.x)
+ PRTDEB(lf,PolygonCenter.y)
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+#define DISTXY(P1X,P1Y,P2X,P2Y)\
+(sqrt(  ( (P2X-P1X) * (P2X-P1X) ) + ( (P2Y-P1Y) * (P2Y-P1Y) )  ) )
+
+#define NORMXY(P1X,P1Y)\
+(sqrt(  ( (P1X) * (P1X) ) + ( (P1Y) * (P1Y) )  ) )
+
+
+//max distance between the estimator position and an objective points to consider that the objective points is atteined
+
+#define DISTLIMIT 30
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+uint16_t railnumberSinceBoot=1; //used to count the number of rails the plane has achieved since the boot, to number the sequences of saved images
+//the number 1 is reserved for snapshot fonctions that take only one image, the 2-65535 numbers are used to number the following sequences
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+#define USE_ONBOARD_CAMERA
+
+#ifdef USE_ONBOARD_CAMERA
+bool_t CAMERA_SNAPSHOT_REQUIERED=FALSE;  //declared in main_ap.c
+uint16_t camera_snapshot_image_number=0; //declared in main_ap.c
+#endif
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+bool_t survey_losange_uturn;//this flag indicates if the aircraft is turning between 2 rails (1) or if it is flying straight forward in the rail direction (0) 
+
+int railnumber;  //indicate the number of the rail being acquired
+int numberofrailtodo; 
+
+float distrail;  //distance between adjacent rails in meters, the value is set in the init function
+float distplus;  //distance that the aircraft should travel before and after a rail before turning to the next rails in meters, the value is set in the init function
+
+float distrailinteractif=60; //a cheangable value that can be set interactively in the GCS, not used at that time, it can be used to choose the right value while the aircraft is flying
+
+
+static struct point point1,point2,point3; // 3 2D points used for navigation
+static struct point pointA,pointB,pointC; // 3 2D points used for navigation
+static struct point vec12,vec13;
+float norm12,norm13; // norms of 12 & 13  vectors
+
+
+
+
+float tempx,tempy;  //temporary points for exchanges
+float angle1213; //angle between 12 & 13  vectors
+float signforturn; //can be +1 or -1, it is used to compute the direction of the UTURN between rails
+
+float tempcircleradius;// used to compute the radius of the UTURN after a rail
+float circleradiusmin=40;
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+float normBM,normAM,distancefromrail;
+
+
+float course_next_rail;
+float angle_between;
+
+bool_t ProjectionInsideLimitOfRail;
+
+
+
+
+#include "modules/cartography/cartography.h"
+#include "led.h"
+
+#include "downlink.h"
+#include "mcu_periph/uart.h"
+#include "std.h"
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+void init_carto(void) {
+  // this part is already done by led_init in fact
+  LED_INIT(DEMO_MODULE_LED);
+  LED_OFF(DEMO_MODULE_LED);
+}
+
+void periodic_1Hz_carto(void) {
+	/*uint8_t  toto=3;
+	float lat=3.14159;
+	float lon=1.234;
+    LED_TOGGLE(DEMO_MODULE_LED);
+	DOWNLINK_SEND_MARK(DefaultChannel,&toto,&lat,&lon);
+	 */
+}
+
+void periodic_10Hz_carto(void) {
+  /* LED_TOGGLE(DEMO_MODULE_LED);
+   */
+}
+
+void start_carto(void) {
+ /* LED_ON(DEMO_MODULE_LED);
+  */
+}
+
+void stop_carto(void) {
+ /* LED_OFF(DEMO_MODULE_LED);
+  */
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+bool_t nav_survey_Inc_railnumberSinceBoot(void)
+{
+	railnumberSinceBoot++;
+	return FALSE; 
+}
+///////////////////////////////////////////////////////////////////////////////////////////////
+bool_t nav_survey_Snapshoot(void)
+{
+	camera_snapshot_image_number=railnumberSinceBoot;
+	PRTDEBSTR(SNAPSHOT)
+	CAMERA_SNAPSHOT_REQUIERED=TRUE;
+	return FALSE; 
+	
+}
+///////////////////////////////////////////////////////////////////////////////////////////////
+bool_t nav_survey_Snapshoot_Continu(void)
+{
+	camera_snapshot_image_number=railnumberSinceBoot;
+	PRTDEBSTR(SNAPSHOT)
+	CAMERA_SNAPSHOT_REQUIERED=TRUE;
+	return TRUE; 
+	
+}
+///////////////////////////////////////////////////////////////////////////////////////////////
+bool_t nav_survey_StopSnapshoot(void)
+{
+	camera_snapshot_image_number=0;
+	PRTDEBSTR(STOP SNAPSHOT)
+	CAMERA_SNAPSHOT_REQUIERED=TRUE;
+	return FALSE; 
+	
+}
+///////////////////////////////////////////////////////////////////////////////////////////////
+
+bool_t nav_survey_computefourth_corner(uint8_t wp1, uint8_t wp2,  uint8_t wp3, uint8_t wp4 )
+{
+	waypoints[wp4].x=waypoints[wp2].x+waypoints[wp3].x-waypoints[wp1].x;
+	waypoints[wp4].y=waypoints[wp2].y+waypoints[wp3].y-waypoints[wp1].y;
+	
+	PRTDEBSTR(nav_survey_computefourth_corner)
+	PRTDEB(f,waypoints[wp4].x)
+	PRTDEB(f,waypoints[wp4].y)
+	return FALSE; 
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+bool_t  nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float estimator_xf,float estimator_yf,float *normAMf,float *normBMf,float *distancefromrailf);
+
+
+bool_t  nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float estimator_xf,float estimator_yf,float *normAMf,float *normBMf,float *distancefromrailf)
+//return if the projection of the estimator on the AB line is located inside the AB interval
+{
+	float a,b,c,xa,xb,xc,ya,yb,yc;
+	float f;
+	float AA1;
+	float BB1;
+	float YP;
+	float XP;
+    	
+	float AMx,AMy,BMx,BMy;
+	//+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
+
+	
+	
+	xb=pointAf.x;
+	yb=pointAf.y;
+	
+	xc=pointBf.x;
+	yc=pointBf.y;
+	
+	xa=estimator_xf;
+	ya=estimator_yf;
+	
+	//calcul des parametres de la droite pointAf pointBf
+	a = yc - yb;
+	b = xb - xc;
+	c = (yb - yc) * xb + (xc - xb) * yb ;
+	
+	//calcul de la distance de la droite à l'avion
+	
+	
+	if (fabs(a)>1e-10)
+		*distancefromrailf = fabs((a * xa + b * ya + c) / sqrt(a * a + b * b)); //denominateur =0 iniquement si a=b=0 //peut arriver si 2 waypoints sont confondus
+	else  				
+		return 0;					
+	
+	PRTDEB(f,a)
+	PRTDEB(f,b)
+	PRTDEB(f,c)
+	PRTDEB(f,*distancefromrailf)
+	
+	
+	// calcul des coordonnées du projeté orthogonal M(xx,y) de A sur (BC)
+	AA1 = (xc - xb);
+	BB1 = (yc - yb);
+	if (fabs(AA1)>1e-10)
+	{
+		f=(b - (a * BB1 / AA1));
+		if (fabs(f)>1e-10)
+			YP = (-(a * xa) - (a * BB1 * ya / AA1) - c) / f; 
+		else  				
+			return 0;					
+	}
+	else
+		return 0;					
+	
+	
+	
+	
+	XP = (-c - b * YP) / a ; //a !=0 deja testé avant
+	//+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
+	//+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
+	//+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
+	
+	PRTDEB(f,AA1)
+	PRTDEB(f,BB1)
+	PRTDEB(f,YP)
+	PRTDEB(f,XP)
+	
+	AMx=XP-pointAf.x;
+	AMy=YP-pointAf.y;
+	BMx=XP-pointBf.x;
+	BMy=YP-pointBf.y;
+	
+	*normAMf=NORMXY(AMx,AMy);
+	*normBMf=NORMXY(BMx,BMy);
+	
+	PRTDEB(f,*normAMf)
+	PRTDEB(f,*normBMf)
+	
+	if ( ( (*normAMf) + (*normBMf) ) >1.05*DISTXY(pointBf.x,pointBf.y,pointAf.x,pointAf.y))
+	{
+		PRTDEBSTR(NOT INSIDE)
+		return 0;
+	}
+	else 
+	{
+		PRTDEBSTR(INSIDE)
+		return 1;
+	}
+}
+///////////////////////////////////////////////////////////////////////////
+//if distrailinit = 0, the aircraft travel from  wp1 -> wp2 then do the inverse travel passing through  the wp3, 
+//This mode could be use to register bands of images aquired in a first nav_survey_losange_carto, done perpendicularly
+bool_t nav_survey_losange_carto_init(uint8_t wp1, uint8_t wp2,  uint8_t wp3, float distrailinit, float distplusinit)
+{
+	//PRTDEBSTR(nav_survey_losange_carto_init)
+	survey_losange_uturn=FALSE;
+	
+	
+	point1.x=waypoints[wp1].x;  //the coordinates are in meter units, taken from the flight plan, in float type
+	point1.y=waypoints[wp1].y;
+	point2.x=waypoints[wp2].x;
+	point2.y=waypoints[wp2].y;
+	point3.x=waypoints[wp3].x;
+	point3.y=waypoints[wp3].y;
+	
+	PRTDEB(u,wp1)
+	PRTDEB(f,point1.x)
+	PRTDEB(f,point1.y)
+	
+	PRTDEB(u,wp2)
+	PRTDEB(f,point2.x)
+	PRTDEB(f,point2.y)
+	
+	PRTDEB(u,wp3)
+	PRTDEB(f,point3.x)
+	PRTDEB(f,point3.y)
+	
+	
+	
+	vec12.x=point2.x-point1.x;
+	vec12.y=point2.y-point1.y;
+	PRTDEB(f,vec12.x)
+	PRTDEB(f,vec12.y)
+	
+	//TODO gerer le cas ou un golio met les points à la meme position -> norm=0 > /0
+	norm12=NORMXY(vec12.x,vec12.y);
+	
+	PRTDEB(f,norm12)
+	
+	
+	vec13.x=point3.x-point1.x;
+	vec13.y=point3.y-point1.y;
+	PRTDEB(f,vec13.x)
+	PRTDEB(f,vec13.y)
+	
+	norm13=NORMXY(vec13.x,vec13.y);
+	PRTDEB(f,norm13)
+	
+	//if (distrail<1e-15)  //inutile distrail=0 pour recollage et dans ce cas, il prend la valeur norm13
+	// 	return FALSE;
+	
+	
+	if (fabs(distrailinit)<=1)
+	{ //is distrailinit==0, then the aircraft should do 2 passes to register the bands
+		distrail=norm13;
+		numberofrailtodo=1;
+	}
+	else
+	{//no, so normal trajectory
+		distrail=fabs(distrailinit);
+		numberofrailtodo=ceil( norm13 / distrail);//round to the upper integer
+	}
+	
+	distplus=fabs(distplusinit);
+	
+	
+	PRTDEB(f,distrail)
+	PRTDEB(f,distplus)
+	PRTDEB(d,numberofrailtodo)
+	PRTDEB(d,railnumber)
+	PRTDEB(d,railnumberSinceBoot)
+	
+	railnumber=-1; // the state is before the first rail, which is numbered 0
+	
+	if (norm12<1e-15)
+		return FALSE;
+	if (norm13<1e-15)
+		return FALSE;
+	
+	
+	angle1213=(180/3.14159) * acos( ( ((vec12.x*vec13.x ) + (vec12.y*vec13.y) ))/(norm12*norm13));//oriented angle between 12 and 13 vectors
+	
+	angle1213 = atan2f(vec13.y, vec13.x) - atan2f(vec12.y,vec12.x);
+	while ( angle1213 >= M_PI ) angle1213 -= 2*M_PI;
+	while ( angle1213 <= -M_PI ) angle1213 += 2*M_PI;
+	
+	PRTDEB(f,angle1213)
+	
+	if (angle1213 >0)
+		signforturn=-1;  
+	else
+		signforturn=1;
+	
+	
+	return FALSE; //Init function must return false, so that the next function in the flight plan is automatically executed
+	//dans le flight_plan.h
+	//        if (! (nav_survey_losange_carto()))
+	//          NextStageAndBreak();
+}
+////////////////////////////////////////////////////////////////////////////////////////////////
+bool_t nav_survey_losange_carto(void)
+{
+	//test pour modifier en vol la valeur distrail
+	
+	//distrail=distrailinteractif;
+	
+	
+	//by default, a 0 is sent in the message DOWNLINK_SEND_CAMERA_SNAPSHOT,
+	//if the aircraft is inside the region to map, camera_snapshot_image_number will be equal to the number of rail since the last boot (not since the nav_survey_losange_carto_init, in order to get different values for differents calls to the cartography function  (this number is used to name the images on the hard drive
+	camera_snapshot_image_number=0;
+	
+	
+	PRTDEB(f,distrail)
+	
+	
+	
+	PRTDEBSTR(nav_survey_losange_carto)
+	PRTDEB(d,railnumber)
+	
+	PRTDEB(d,railnumberSinceBoot)
+	
+	//PRTDEB(f,estimator_x)
+	//PRTDEB(f,estimator_y)
+	
+	//sortir du bloc si données abhérantes 
+	if (norm13<1e-15)
+	{
+		PRTDEBSTR(norm13<1e-15)
+		return FALSE;
+	}
+	if (norm12<1e-15)
+	{
+		PRTDEBSTR(norm13<1e-15)
+		return FALSE;
+	}
+	if (distrail<1e-15) 
+   	{
+		PRTDEBSTR(distrail<1e-15)
+		return FALSE;
+	}
+	
+	if (survey_losange_uturn==FALSE)
+	{
+		
+		if (railnumber==-1) 
+		{ //se diriger vers le début du 1°rail
+			PRTDEBSTR(approche debut rail 0)
+			pointA.x=point1.x-(vec12.x/norm12)*distplus*1.2; //on prend une marge plus grande pour arriver en ce point
+			pointA.y=point1.y-(vec12.y/norm12)*distplus*1.2; //car le virage n'est pas tres bien géré
+			
+			
+	        pointB.x=point2.x+(vec12.x/norm12)*distplus*1.2; //on prend une marge plus grande pour arriver en ce point
+			pointB.y=point2.y+(vec12.y/norm12)*distplus*1.2; //car le virage n'est pas tres bien géré
+			
+			//PRTDEB(f,pointA.x)
+			//PRTDEB(f,pointA.y)
+			
+			
+			//the following test can cause problem when the aircraft is quite close to the entry point, as it can turn around for infinte time
+			//if ( DISTXY(estimator_x,estimator_y,pointA.x,pointA.y)  >DISTLIMIT)
+			//if ( DISTXY(estimator_x,estimator_y,pointA.x,pointA.y)  >DISTLIMIT)
+			
+			
+			nav_survey_ComputeProjectionOnLine(pointA,pointB,estimator_x,estimator_y,&normAM,&normBM,&distancefromrail);
+			
+			if ((DISTXY(estimator_x,estimator_y,pointA.x,pointA.y)  >2* DISTLIMIT)  || (normBM<(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
+			{
+				nav_route_xy(estimator_x, estimator_y,pointA.x,pointA.y);
+				//nav_route_xy(pointB.x, pointB.y,pointA.x,pointA.y);      
+			}
+			else
+			{
+				PRTDEBSTR(debut rail 0)
+				//un fois arrivé, on commence le 1° rail;
+				railnumber=0;
+				railnumberSinceBoot++;
+				
+			}
+		}
+		
+		
+		if (railnumber>=0) 
+		{
+			pointA.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+			pointA.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
+			
+			pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+			pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
+			
+			
+			
+			
+			
+			if ((railnumber %2)==0) //rail n0, 2, 4, donc premiere direction, de wp1 vers wp2
+			{ 
+				//rien a faire
+			}
+			else //if ((railnumber %2)==1) //rail n1, 3, 5, donc seconde direction, de wp2 vers wp1
+			{ 
+				//echange pointA et B	
+				tempx=pointA.x;
+				tempy=pointA.y;
+				pointA.x=pointB.x;
+				pointA.y=pointB.y;
+				pointB.x=tempx;
+				pointB.y=tempy;
+				
+			}
+			
+			//	PRTDEB(f,pointA.x)
+			//	PRTDEB(f,pointA.y)	
+			//	PRTDEB(f,pointB.x)
+			//	PRTDEB(f,pointB.y)
+			ProjectionInsideLimitOfRail=nav_survey_ComputeProjectionOnLine(pointA,pointB,estimator_x,estimator_y,&normAM,&normBM,&distancefromrail);
+			
+			
+			
+			//	if ( ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y)  >DISTLIMIT)  && 
+			//		(normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
+			
+			
+			if (! ( ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y) <DISTLIMIT)  ||   ( ProjectionInsideLimitOfRail &&( normAM > (DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))))
+				//		(normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
+			{
+				nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y);
+				PRTDEBSTR(NAVROUTE)
+				
+				
+				//est ce que l'avion est dans la zone ou il doit prendre des images?
+				//DEJA APPELE AVANT LE IF
+				//	nav_survey_ComputeProjectionOnLine(pointA,pointB,estimator_x,estimator_y,&normAM,&normBM,&distancefromrail);
+				
+				if (  (normAM> distplus) && (normBM> distplus) && (distancefromrail<distrail/2))
+				{
+					//CAMERA_SNAPSHOT_REQUIERED=TRUE;
+					//camera_snapshot_image_number++;
+					camera_snapshot_image_number=railnumberSinceBoot;
+					PRTDEBSTR(SNAPSHOT)
+				}
+				
+			}
+			
+			else // virage
+			{
+				//PRTDEBSTR(debut rail suivant)
+				railnumber++;
+				railnumberSinceBoot++;
+				
+				PRTDEB(d,railnumber)
+				PRTDEB(d,railnumberSinceBoot)
+				//CAMERA_SNAPSHOT_REQUIERED=TRUE;
+				//camera_snapshot_image_number++;
+				
+				PRTDEBSTR(UTURN)
+				survey_losange_uturn=TRUE; 
+				
+			}
+			
+			if (railnumber>numberofrailtodo)
+			{		
+				PRTDEBSTR(fin nav_survey_losange_carto)
+				return FALSE; //apparament, l'avion va au bloc suivant lorsque la fonction renvoie false
+			}
+			
+		}
+	}
+	else // (survey_losange_uturn==TRUE)
+	{
+		
+		
+		if (distrail<200)
+		{
+			//tourne autour d'un point à mi chemin entre les 2 rails
+			
+			//attention railnumber a été incrémenté  en fin du rail précédent
+			
+			if ((railnumber %2)==1) //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2
+			{ 
+				PRTDEBSTR(UTURN-IMPAIR)
+				//fin du rail précédent	
+				pointA.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail);
+				pointA.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail);
+				//début du rail suivant
+				pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+				pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
+				//milieu
+				waypoints[0].x=(pointA.x+pointB.x)/2;
+				waypoints[0].y=(pointA.y+pointB.y)/2;
+				
+				tempcircleradius=distrail/2;
+				if(tempcircleradius<circleradiusmin) 
+					tempcircleradius=circleradiusmin;
+				
+				
+				//fin du rail suivant
+				pointC.x=(point1.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+				pointC.y=(point1.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
+				
+				
+				course_next_rail=atan2(pointC.x-pointB.x,pointC.y-pointB.y);
+				PRTDEB(f,course_next_rail )
+				PRTDEB(f,estimator_hspeed_dir)
+				
+				angle_between=(course_next_rail-estimator_hspeed_dir);
+				while (angle_between > M_PI) angle_between -= 2 * M_PI; 
+    			while (angle_between < -M_PI) angle_between += 2 * M_PI;
+				
+				angle_between= DegOfRad(angle_between);
+				PRTDEB(f,angle_between )
+				//if (angle_between> -10 && angle_between< 10)   PRTDEBSTR(ON SE CASSE)
+				
+				NavCircleWaypoint(0,signforturn*tempcircleradius);
+				if ( ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y)  <DISTLIMIT) || (angle_between> -10 && angle_between< 10) )  
+				{
+					//l'avion fait le rail suivant
+					survey_losange_uturn=FALSE;
+					PRTDEBSTR(FIN UTURN-IMPAIR)
+				}
+			}
+			else //if ((railnumber %2)==0) //rail précédent n1, 3, 5, donc seconde direction, de wp2 vers wp1
+			{ 
+				PRTDEBSTR(UTURN-PAIR)
+				//fin du rail précédent	
+				pointA.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail);
+				pointA.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail);
+				//début du rail suivant
+				pointB.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+				pointB.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
+				//milieu
+				waypoints[0].x=(pointA.x+pointB.x)/2;
+				waypoints[0].y=(pointA.y+pointB.y)/2;
+				
+				tempcircleradius=distrail/2;
+				if(tempcircleradius<circleradiusmin) 
+					tempcircleradius=circleradiusmin;
+				
+				
+				
+				
+				//fin du rail suivant
+				pointC.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+				pointC.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
+				
+				
+				course_next_rail=atan2(pointC.x-pointB.x,pointC.y-pointB.y);
+				PRTDEB(f,course_next_rail )
+				PRTDEB(f,estimator_hspeed_dir)
+				
+				angle_between=(course_next_rail-estimator_hspeed_dir);
+				while (angle_between > M_PI) angle_between -= 2 * M_PI; 
+    			while (angle_between < -M_PI) angle_between += 2 * M_PI;
+				
+				angle_between= DegOfRad(angle_between);
+				PRTDEB(f,angle_between )
+				//if (angle_between> -10 && angle_between< 10)   PRTDEBSTR(ON SE CASSE)
+				
+				NavCircleWaypoint(0,signforturn*(-1)*tempcircleradius);
+				if (( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y)  <DISTLIMIT) || (angle_between> -10 && angle_between< 10) )    
+				{
+					//l'avion fait le rail suivant
+					survey_losange_uturn=FALSE;
+					PRTDEBSTR(FIN UTURN-PAIR)
+				}
+			}
+		}
+		else
+		{ //Le virage serait trop grand, on va en ligne droite pour ne pas trop éloigner l'avion
+			
+			if ((railnumber %2)==1) //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2
+			{ 
+				PRTDEBSTR(TRANSIT-IMPAIR)
+				//fin du rail précédent	
+				pointA.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail);
+				pointA.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail);
+				//début du rail suivant
+				pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+				pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail); 
+				nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y);
+				if ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y)  <DISTLIMIT)  
+				{
+					//l'avion fait le rail suivant
+					survey_losange_uturn=FALSE;
+					PRTDEBSTR(FIN TRANSIT-IMPAIR)
+				}
+			}
+			else //if ((railnumber %2)==0) //rail précédent n1, 3, 5, donc seconde direction, de wp2 vers wp1
+			{ 
+				PRTDEBSTR(TRANSIT-PAIR)
+				//fin du rail précédent	
+				pointA.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail);
+				pointA.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail);
+				//début du rail suivant
+				pointB.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail);
+				pointB.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail);
+				nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y);
+				if ( DISTXY(estimator_x,estimator_y,pointB.x,pointB.y)  <DISTLIMIT)  
+				{
+					//l'avion fait le rail suivant
+					survey_losange_uturn=FALSE;
+					PRTDEBSTR(FIN TRANSIT-PAIR)
+				}
+				
+			}
+			
+		}
+	}
+	
+	
+	//////////////// FAUT IL METTRE l'APPEL A CES FONCTIONS???????????????,
+	//NavVerticalAutoThrottleMode(0.); /* No pitch */
+	//NavVerticalAltitudeMode(WaypointAlt(wp1), 0.); /* No preclimb */
+	
+	
+	
+	CAMERA_SNAPSHOT_REQUIERED=TRUE;
+	
+	
+	return TRUE; //apparament pour les fonctions de tache=> true
+}
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+
diff --git a/sw/airborne/modules/cartography/cartography.h b/sw/airborne/modules/cartography/cartography.h
new file mode 100644
index 00000000000..2ba83d9acfd
--- /dev/null
+++ b/sw/airborne/modules/cartography/cartography.h
@@ -0,0 +1,52 @@
+/** Automatic survey of an oriented rectangle (defined by three points)  */
+
+#ifndef CARTOGRAPHY_H
+#define CARTOGRAPHY_H
+
+
+
+ 
+void init_carto(void);
+void periodic_1Hz_carto(void);
+void periodic_10Hz_carto(void);
+void start_carto(void);
+void stop_carto(void);
+
+/*
+ typedef enum {NS, WE} survey_orientation_t;
+ */
+
+
+#ifdef USE_ONBOARD_CAMERA
+extern bool_t CAMERA_SNAPSHOT_REQUIERED;   
+extern uint16_t camera_snapshot_image_number;  
+#endif
+
+extern float distrailinteractif; //pour exporter la variable et pouvoir la changer depuis settings	
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+
+
+extern bool_t nav_survey_Inc_railnumberSinceBoot(void);
+extern bool_t nav_survey_Snapshoot(void);
+bool_t nav_survey_Snapshoot_Continu(void);
+extern bool_t nav_survey_StopSnapshoot(void);
+extern bool_t nav_survey_computefourth_corner(uint8_t wp1, uint8_t wp2,  uint8_t wp3, uint8_t wp4 );
+
+extern bool_t nav_survey_losange_carto_init(uint8_t wp1, uint8_t wp2,  uint8_t wp3, float distrail, float distplus);
+
+extern bool_t nav_survey_losange_carto(void);   // !!!! important il faut mettre void en parametres d'entrée, sinon le compilo dit: attention : function declaration isn»t a prototype
+
+//(uint8_t wp1, uint8_t wp2, uint8_t wp3);
+
+/*
+ #define NavSurveylosange_cartoInit(_wp1, _wp2, _grid, _distplus) nav_survey_losange_init(_wp1, _wp2, _wp3, _grid, _distplus)
+ #define NavSurveylosange_carto nav_survey_losange
+ */
+
+
+
+#endif
+ 
\ No newline at end of file

From 596ed48c7fdadd4a5d41b740722e36fe36750f25 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 20 Apr 2011 14:34:14 +0200
Subject: [PATCH 24/99] Chimu also in Lisa

---
 conf/airframes/CDW/ChimuLisaFw.xml       | 238 +++++++++++++----------
 sw/airborne/modules/ins/ins_chimu_uart.c |   4 +-
 2 files changed, 141 insertions(+), 101 deletions(-)

diff --git a/conf/airframes/CDW/ChimuLisaFw.xml b/conf/airframes/CDW/ChimuLisaFw.xml
index 8a6337cb4f6..0e2fef40aaa 100644
--- a/conf/airframes/CDW/ChimuLisaFw.xml
+++ b/conf/airframes/CDW/ChimuLisaFw.xml
@@ -1,68 +1,17 @@
 <!DOCTYPE airframe SYSTEM "../airframe.dtd">
 
-<!-- TwinStart Multiplex
-     Lisa/L board (http://paparazzi.enac.fr/wiki/Lisa)
-     XBee modem
-     LEA 5H GPS
-     robbe R617
+<!-- Microjet Multiplex (http://www.multiplex-rc.de/)
+     Tiny 1.1 board (http://paparazzi.enac.fr/wiki/Tiny)
+     Tilted infrared sensor
+     Xbee modem in API mode
 -->
 
-<airframe name="TwinStar CHIMU Lisa/L">
-
-  <firmware name="fixedwing">
-    <target name="sim" 			board="pc"/>
-    <target name="ap" 			board="lisa_l_1.0"/>
-
-    <subsystem name="radio_control"     type="ppm"/>
-    <subsystem name="telemetry" 	type="transparent"/>
-    <subsystem name="control"/>
-    <subsystem name="gps" 		type="ublox_lea5h">
-      <configure name="GPS_PORT"         value="UART3"/>
-    </subsystem>
-    <subsystem name="navigation"/>
-  </firmware>
-
-  <firmware name="lisa_l_test_progs">
-    <target name="tunnel"           board="lisa_l_1.0"/>
-    <target name="test_led"         board="lisa_l_1.0"/>
-    <target name="test_uart"        board="lisa_l_1.0"/>
-    <target name="test_servos"      board="lisa_l_1.0"/>
-    <target name="test_telemetry"   board="lisa_l_1.0"/>
-    <target name="test_baro"        board="lisa_l_1.0"/>
-    <target name="test_imu_b2"      board="lisa_l_1.0"/>
-    <target name="test_imu_b2_2"    board="lisa_l_1.0"/>
-    <target name="test_imu_aspirin" board="lisa_l_1.0"/>
-    <target name="test_rc_spektrum" board="lisa_l_1.0"/>
-    <target name="test_rc_ppm"      board="lisa_l_1.0"/>
-    <target name="test_adc"         board="lisa_l_1.0"/>
-    <target name="test_hmc5843"     board="lisa_l_1.0"/>
-    <target name="test_itg3200"     board="lisa_l_1.0"/>
-    <target name="test_adxl345"             board="lisa_l_1.0"/>
-    <target name="test_esc_mkk_simple"      board="lisa_l_1.0"/>
-    <target name="test_esc_asctecv1_simple" board="lisa_l_1.0"/>
-    <target name="test_actuators_mkk"       board="lisa_l_1.0"/>
-    <target name="test_actuators_asctecv1"  board="lisa_l_1.0"/>
-  </firmware>
-
-
-  <modules>
-    <load name="ins_chimu_uart.xml">
-      <configure name="CHIMU_UART_NR" value="1"/>
-    </load>
-
-    <load name="light.xml">
-      <define name="LIGHT_LED_STROBE" value="3"/>
-      <define name="LIGHT_LED_NAV" value="4"/>
-      <define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
-      <define name="NAV_LIGHT_MODE_DEFAULT" value="0"/>
-    </load>
-  </modules>
-
+<airframe name="Microjet Tiny 1.1">
 
   <servos>
-    <servo name="MOTOR"         no="0" min="1000" neutral="1000" max="2000"/>
-    <servo name="AILEVON_LEFT"  no="1" min="1900" neutral="1534" max="1100"/>
-    <servo name="AILEVON_RIGHT" no="2" min="1100" neutral="1468" max="1900"/>
+    <servo name="MOTOR"         no="0" min="1290" neutral="1290" max="1810"/>
+    <servo name="AILEVON_LEFT"  no="1" min="2000" neutral="1510" max="1000"/>
+    <servo name="AILEVON_RIGHT" no="2" min="1000" neutral="1535" max="2000"/>
   </servos>
 
   <commands>
@@ -78,95 +27,186 @@
   </rc_commands>
 
   <section name="MIXER">
-    <define name="AILEVON_AILERON_RATE" value="0.45"/>
-    <define name="AILEVON_ELEVATOR_RATE" value="0.8"/>
+    <define name="AILEVON_AILERON_RATE" value="0.75"/>
+    <define name="AILEVON_ELEVATOR_RATE" value="0.75"/>
   </section>
 
   <command_laws>
     <let var="aileron"  value="@ROLL  * AILEVON_AILERON_RATE"/>
     <let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
-    <set servo="MOTOR"           value="@THROTTLE"/>
+    <set servo="MOTOR"         value="@THROTTLE"/>
     <set servo="AILEVON_LEFT"  value="$elevator + $aileron"/>
     <set servo="AILEVON_RIGHT" value="$elevator - $aileron"/>
   </command_laws>
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="RadOfDeg(50)"/>
-    <define name="MAX_PITCH" value="RadOfDeg(35)"/>
+    <define name="MAX_ROLL" value="0.7"/>
+    <define name="MAX_PITCH" value="0.6"/>
   </section>
 
-  <section name="INS" prefix="INS_">
-    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
-    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+  <section name="INFRARED" prefix="IR_">
+    <define name="ADC_IR1_NEUTRAL" value="2048"/>
+    <define name="ADC_IR2_NEUTRAL" value="2048"/>
+    <define name="ADC_TOP_NEUTRAL" value="2048"/>
+
+    <define name="LATERAL_CORRECTION" value="1."/>
+    <define name="LONGITUDINAL_CORRECTION" value="1."/>
+    <define name="VERTICAL_CORRECTION" value="1.5"/>
+
+    <define name="HORIZ_SENSOR_TILTED" value="1"/>
+    <define name="IR2_SIGN" value="-1"/>
+    <define name="TOP_SIGN" value="-1"/>
+
+    <define name="ROLL_NEUTRAL_DEFAULT" value="-3.6" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="5" unit="deg"/>
+
+    <define name="CORRECTION_UP" value="1."/>
+    <define name="CORRECTION_DOWN" value="1."/>
+    <define name="CORRECTION_LEFT" value="1."/>
+    <define name="CORRECTION_RIGHT" value="1."/>
+  </section>
+
+  <section name="GYRO" prefix="GYRO_">
+    <define name="ADC_ROLL_NEUTRAL" value="500"/>
+    <define name="DYNAMIC_RANGE" value="300" unit="deg/s"/>
+    <define name="ADXRS300_RESISTOR_BRIDGE" value="(3.3/(3.3+1.8))"/>
+    <define name="ADXRS300_SENSITIVITY" value="5" unit="mV/(deg/s)"/>
+    <define name="ROLL_SCALE" value="3.3*1000./1024./(GYRO_ADXRS300_SENSITIVITY*GYRO_ADXRS300_RESISTOR_BRIDGE)" unit="deg/s/adc_unit"/>
+    <define name="ROLL_DIRECTION" value="1."/>
   </section>
 
   <section name="BAT">
+    <define name="MILLIAMP_AT_FULL_THROTTLE" value="2000"/>
+
     <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
   </section>
 
   <section name="MISC">
-    <define name="NOMINAL_AIRSPEED" value="15." unit="m/s"/>
+    <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
     <define name="CARROT" value="5." unit="s"/>
     <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
     <define name="CONTROL_RATE" value="60" unit="Hz"/>
+    <define name="XBEE_INIT" value="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/>
+<!--    <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
+    <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
+
+    <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="DEFAULT_CIRCLE_RADIUS" value="120."/>
+    <define name="GLIDE_AIRSPEED" value="10"/>
+    <define name="GLIDE_VSPEED" value="3."/>
+    <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
     <define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
     <!-- outer loop proportional gain -->
-    <define name="ALTITUDE_PGAIN" value="-0.06"/> <!-- -0.024 -->
+    <define name="ALTITUDE_PGAIN" value="-0.03"/>
     <!-- outer loop saturation -->
     <define name="ALTITUDE_MAX_CLIMB" value="2."/>
 
     <!-- auto throttle inner loop -->
-    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.45"/>
-    <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.35"/>
-    <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.85"/>
-    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.2" unit="%/(m/s)"/>
-    <define name="AUTO_THROTTLE_PGAIN" value="-0.023"/> <!-- -0.012 -->
-    <define name="AUTO_THROTTLE_IGAIN" value="0.01"/>
+    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
+    <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
+    <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
+    <define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
+    <define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
+    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
+    <define name="AUTO_THROTTLE_PGAIN" value="-0.01"/>
+    <define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
     <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
 
-    <!-- auto pitch inner loop -->
-    <define name="AUTO_PITCH_PGAIN" value="-0.06"/> <!-- -0.03 -->
-    <define name="AUTO_PITCH_IGAIN" value="0.0"/>
-    <define name="AUTO_PITCH_MAX_PITCH" value="0.35"/>
-    <define name="AUTO_PITCH_MIN_PITCH" value="-0.35"/>
-
-    <define name="THROTTLE_SLEW" value="0.1"/>
+    <define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
   </section>
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
-    <define name="COURSE_PGAIN" value="-0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
+    <define name="COURSE_PGAIN" value="-1.4"/>
+
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
     <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
     <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
 
-    <define name="ROLL_PGAIN" value="6600."/>
-    <define name="AILERON_OF_THROTTLE" value="0.0"/>
-    <define name="PITCH_PGAIN" value="-5500."/>
-    <define name="PITCH_DGAIN" value="0.4"/>
+    <define name="PITCH_PGAIN" value="-12000."/>
+    <define name="PITCH_DGAIN" value="1.5"/>
 
-    <define name="ELEVATOR_OF_ROLL" value="2400"/>
-  </section>
+    <define name="ELEVATOR_OF_ROLL" value="1250"/>
 
-  <section name="NAV">
-    <define name="NAV_PITCH" value="0."/>
-    <define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
+    <define name="ROLL_SLEW" value="0.1"/>
+
+    <define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
+    <define name="ROLL_RATE_GAIN" value="-1500"/>
   </section>
 
+  <section name="AGGRESSIVE" prefix="AGR_">
+    <define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
+    <define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
+    <define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
+    <define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
+    <define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
+    <define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
+    <define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
+    <define name="DESCENT_NAV_RATIO" value="1.0"/>
+    </section>
+
   <section name="FAILSAFE" prefix="FAILSAFE_">
     <define name="DELAY_WITHOUT_GPS" value="2" unit="s"/>
-        <define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
-        <define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
-        <define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
-    <define name="HOME_RADIUS" value="100" unit="m"/>
+    <define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
+    <define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
+    <define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
   </section>
 
+  <section name="DATALINK" prefix="DATALINK_">
+    <define name="DEVICE_TYPE" value="XBEE"/>
+    <define name="DEVICE_ADDRESS" value="...."/>
+  </section>
+  
+  
   <section name="SIMU">
-    <define name="YAW_RESPONSE_FACTOR" value="0.5"/>
+    <define name="JSBSIM_MODEL" value="&quot;Malolo1&quot;"/>
+    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="RadOfDeg(0.)"/>
+    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="RadOfDeg(0.)"/>
   </section>
 
+  <section name="INS" prefix="INS_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+  </section>
+
+  <modules>
+    <load name="ins_chimu_uart.xml">
+      <configure name="CHIMU_UART_NR" value="3"/>
+    </load>
+  </modules>
+
+  <firmware name="fixedwing">
+    <target name="ap" 			board="lisa_l_1.0"/>
+    <target name="sim" 			board="pc" />
+    <target name="jsbsim"       board="pc"/>
+
+    <define name="AGR_CLIMB" />
+    <define name="LOITER_TRIM" />
+    <define name="ALT_KALMAN" />
+
+    <subsystem name="radio_control" type="ppm"/>
+
+    <!-- Communication -->
+    <subsystem name="telemetry" 	type="transparent">
+      <configure name="MODEM_BAUD" 		value="B9600"/>
+    </subsystem>
+
+    <subsystem name="control"/>
+    <!-- Sensors 
+    <subsystem name="gyro"          type="roll"/> -->
+    <subsystem name="gps" 		    type="ublox_lea4p"/>
+    <subsystem name="navigation"/>
+
+  </firmware>
+
+
+  <firmware name="setup">
+    <target name="tunnel"           board="tiny_1.1" />
+    <target name="usb_tunnel_0"     board="tiny_1.1" />
+    <target name="usb_tunnel_1"     board="tiny_1.1" />
+    <target name="setup_actuators"  board="tiny_1.1" />
+  </firmware>
+
 </airframe>
diff --git a/sw/airborne/modules/ins/ins_chimu_uart.c b/sw/airborne/modules/ins/ins_chimu_uart.c
index 1805c2ff93a..2304d2f880e 100644
--- a/sw/airborne/modules/ins/ins_chimu_uart.c
+++ b/sw/airborne/modules/ins/ins_chimu_uart.c
@@ -33,8 +33,8 @@ volatile uint8_t new_ins_attitude;
 
 void ins_init( void ) 
 {
-//  uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0xab, 0x76 };	// 50Hz attitude only + SPI
-  uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0xab, 0xd3 }; // 25Hz attitude only + SPI
+  uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0xab, 0x76 };	// 50Hz attitude only + SPI
+//  uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0xab, 0xd3 }; // 25Hz attitude only + SPI
 //  uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI
   uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
   

From 418371d775c5ca5b63820e5b7bd7d96b5be2db96 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 20 Apr 2011 15:32:34 +0200
Subject: [PATCH 25/99] yapa_xsens compiles with no more warnings

---
 conf/airframes/TU_Delft/yapa_xsens.xml     | 4 +---
 sw/airborne/modules/ins/imu_chimu.h        | 4 ----
 sw/airborne/modules/ins/ins_xsens.h        | 3 ---
 sw/airborne/subsystems/navigation/spiral.c | 2 +-
 4 files changed, 2 insertions(+), 11 deletions(-)

diff --git a/conf/airframes/TU_Delft/yapa_xsens.xml b/conf/airframes/TU_Delft/yapa_xsens.xml
index 7d1b5657998..84d95e43c8d 100644
--- a/conf/airframes/TU_Delft/yapa_xsens.xml
+++ b/conf/airframes/TU_Delft/yapa_xsens.xml
@@ -215,11 +215,9 @@
     <!-- Actuators -->
     <subsystem name="control"/>
     <!-- Sensors -->
-    <subsystem name="navigation" type="extra"/>
+    <subsystem name="navigation"/>
     <subsystem name="gps" type="xsens"/>
 
-    <!--<subsystem name="i2c"/>-->
-
   </firmware>
 
 </airframe>
diff --git a/sw/airborne/modules/ins/imu_chimu.h b/sw/airborne/modules/ins/imu_chimu.h
index 3e1434daaa7..6fade8a21e0 100644
--- a/sw/airborne/modules/ins/imu_chimu.h
+++ b/sw/airborne/modules/ins/imu_chimu.h
@@ -93,11 +93,7 @@ typedef struct {
         unsigned char   m_MsgLen;
         unsigned char   m_TempDeviceID;
         unsigned char   m_DeviceID;
-<<<<<<< HEAD
 	unsigned char   m_Payload[CHIMU_RX_BUFFERSIZE];        // CHIMU data
-=======
-        unsigned char   m_Payload[CHIMU_RX_BUFFERSIZE];        // CHIMU data
->>>>>>> paparazzi/CHIMU
         unsigned char   m_FullMessage[CHIMU_RX_BUFFERSIZE];	// CHIMU data
         CHIMU_attitude_data m_attitude;
         CHIMU_attitude_data m_attrates;
diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h
index 96248cef20b..446faabad8f 100644
--- a/sw/airborne/modules/ins/ins_xsens.h
+++ b/sw/airborne/modules/ins/ins_xsens.h
@@ -31,9 +31,6 @@
 
 #include "std.h"
 
-extern float ins_pitch_neutral;
-extern float ins_roll_neutral;
-
 extern int8_t xsens_hour;
 extern int8_t xsens_min;
 extern int8_t xsens_sec;
diff --git a/sw/airborne/subsystems/navigation/spiral.c b/sw/airborne/subsystems/navigation/spiral.c
index c506be572c1..219e7786032 100644
--- a/sw/airborne/subsystems/navigation/spiral.c
+++ b/sw/airborne/subsystems/navigation/spiral.c
@@ -37,7 +37,7 @@ static float SRad;
 static float IRad;
 static float Alphalimit;
 static float Segmente;
-static float CamAngle;
+// static float CamAngle;
 static float ZPoint;
 static float nav_radius_min;
 

From 726007166114c610d0e0d32ca7caaf1a9b3f644d Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Wed, 20 Apr 2011 16:17:38 +0200
Subject: [PATCH 26/99] fix sim with arduimu_basic; add a high_accel flag to
 prevent updates on high acceleration

---
 conf/modules/ins_arduimu_basic.xml            |  2 +-
 .../arch/sim/modules/ins/ins_arduimu_basic.c  | 47 +++++++++++++++++++
 .../arduimu_basic/DCM.pde                     |  2 +
 .../arduimu_basic/GPS_pprz.pde                |  1 +
 .../arduimu_basic/arduimu_basic.pde           |  6 +--
 sw/airborne/modules/ins/ins_arduimu_basic.c   | 27 +++++++++--
 6 files changed, 76 insertions(+), 9 deletions(-)
 create mode 100644 sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c

diff --git a/conf/modules/ins_arduimu_basic.xml b/conf/modules/ins_arduimu_basic.xml
index bc29ed822e2..7af675ada2a 100644
--- a/conf/modules/ins_arduimu_basic.xml
+++ b/conf/modules/ins_arduimu_basic.xml
@@ -12,7 +12,7 @@
     <file name="ins_arduimu_basic.c"/>
   </makefile>
   <makefile target="sim">
-    <file_arch name="ins_arduimu.c"/>
+    <file_arch name="ins_arduimu_basic.c"/>
   </makefile>
 </module>
 
diff --git a/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c b/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c
new file mode 100644
index 00000000000..5b6e96f2d58
--- /dev/null
+++ b/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c
@@ -0,0 +1,47 @@
+/** ArduIMU simulation. OCaml binding.
+ *  Sim provides IR sensor reading and airspeed
+ */
+
+
+#include <inttypes.h>
+#include "generated/airframe.h"
+
+#include <caml/mlvalues.h>
+
+#include "estimator.h"
+
+// Prevent undefined reference (from estimator.c)
+#include "subsystems/sensors/infrared.h"
+struct Infrared infrared;
+
+// Arduimu empty implementation
+#include "modules/ins/ins_arduimu_basic.h"
+
+struct FloatEulers arduimu_eulers;
+struct FloatRates arduimu_rates;
+struct FloatVect3 arduimu_accel;
+
+float ins_roll_neutral;
+float ins_pitch_neutral;
+
+void ArduIMU_init( void ) {}
+void ArduIMU_periodic( void ) {}
+void ArduIMU_periodicGPS( void ) {}
+void ArduIMU_event( void ) {}
+
+// Updates from Ocaml sim
+float sim_air_speed;
+
+value set_ir_and_airspeed(
+    value roll __attribute__ ((unused)),
+    value front __attribute__ ((unused)),
+    value top __attribute__ ((unused)),
+    value air_speed
+    ) {
+  // Feed directly the estimator
+  estimator_phi = atan2(Int_val(roll), Int_val(top)) - ins_roll_neutral;
+  estimator_theta = atan2(Int_val(front), Int_val(top)) - ins_pitch_neutral;
+  sim_air_speed = Double_val(air_speed);
+  return Val_unit;
+}
+
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde
index ed30a701901..ead08bda940 100644
--- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde
@@ -97,6 +97,8 @@ void Drift_correction(void)
   // Dynamic weighting of accelerometer info (reliability filter)
   // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
   Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);  //  
+  // Set to 0 if high_accel_flag is true
+  if (high_accel_flag) Accel_weight = 0.;
 
 #if PERFORMANCE_REPORTING == 1
   tempfloat = ((Accel_weight - 0.5) * 256.0f);    //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde
index 6a2ddf704d4..d1850d86e96 100644
--- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde
@@ -25,6 +25,7 @@ void parse_pprz_gps() {
   ground_course = (float)join_4_bytes(&Paparazzi_GPS_buffer[8])/100000.0; // Heading 2D  8,9,10,11
   stGpsFix = Paparazzi_GPS_buffer[12];
   stFlags = Paparazzi_GPS_buffer[13];
+  high_accel_flag = Paparazzi_GPS_buffer[14];
 
   if((stGpsFix >= 0x03) && (stFlags&0x01)) {
     gpsFix = 0; //valid position
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde
index 7c5f550e0a9..b213b961b03 100644
--- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde
@@ -104,11 +104,7 @@ float Omega_P[3]= {0,0,0};//Omega Proportional correction
 float Omega_I[3]= {0,0,0};//Omega Integrator
 float Omega[3]= {0,0,0};
 
-//Magnetometer variables
-//int magnetom_x;
-//int magnetom_y;
-//int magnetom_z;
-//float MAG_Heading;
+boolean high_accel_flag = false; // disable update on accelerometers if true
 
 // Euler angles
 float roll;
diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c
index 18b9fe654bc..8d1f3c59533 100644
--- a/sw/airborne/modules/ins/ins_arduimu_basic.c
+++ b/sw/airborne/modules/ins/ins_arduimu_basic.c
@@ -13,9 +13,13 @@ Autoren@ZHAW:   schmiemi
 // test
 #include "estimator.h"
 
-// für das Senden von GPS-Daten an den ArduIMU
+// GPS data for ArduIMU
 #include "gps.h"
 
+// Command vector for thrust
+#include "generated/airframe.h"
+#include "inter_mcu.h"
+
 #define NB_DATA 9
 
 #ifndef ARDUIMU_I2C_DEV
@@ -24,7 +28,7 @@ Autoren@ZHAW:   schmiemi
 
 // Adresse des I2C Slaves:  0001 0110	letztes Bit ist für Read/Write
 // einzugebende Adresse im ArduIMU ist 0000 1011
-//da ArduIMU das Read/Write Bit selber anfügt.
+// da ArduIMU das Read/Write Bit selber anfügt.
 #define ArduIMU_SLAVE_ADDR 0x22
 
 #ifndef DOWNLINK_DEVICE
@@ -47,6 +51,11 @@ struct FloatVect3 arduimu_accel;
 float ins_roll_neutral;
 float ins_pitch_neutral;
 
+// High Accel Flag
+#define HIGH_ACCEL_LOW_SPEED 2.0
+#define HIGH_ACCEL_HIGH_THRUST (0.8*MAX_PPRZ)
+bool_t high_accel_flag;
+
 void ArduIMU_init( void ) {
   FLOAT_EULERS_ZERO(arduimu_eulers);
   FLOAT_RATES_ZERO(arduimu_rates);
@@ -57,6 +66,8 @@ void ArduIMU_init( void ) {
 
   ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
   ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+
+  high_accel_flag = FALSE;
 }
 
 #define FillBufWith32bit(_buf, _index, _value) {  \
@@ -70,12 +81,22 @@ void ArduIMU_periodicGPS( void ) {
 
   if (ardu_gps_trans.status != I2CTransDone) { return; }
 
+  // Test for high acceleration:
+  //  - low speed
+  //  - high thrust
+  if (gps_speed_3d < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE]) {
+    high_accel_flag = TRUE;
+  } else {
+    high_accel_flag = FALSE;
+  }
+
   FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps_speed_3d); // speed 3D
   FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps_gspeed);   // ground speed
   FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)gps_course);   // course
   ardu_gps_trans.buf[12] = gps_mode;                              // status gps fix
   ardu_gps_trans.buf[13] = gps_status_flags;                      // status flags
-  I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 14);
+  ardu_gps_trans.buf[14] = (uint8_t)high_accel_flag;              // high acceleration flag (disable accelerometers in the arduimu filter)
+  I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 15);
 
 }
 

From cd17ba34c40f93063e9dcc2aacf29f0016d431e5 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Thu, 21 Apr 2011 13:50:08 +0200
Subject: [PATCH 27/99] disable stdin in joystick controller, not working with
 paparazzi center

---
 sw/ground_segment/joystick/input2ivy.ml | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/sw/ground_segment/joystick/input2ivy.ml b/sw/ground_segment/joystick/input2ivy.ml
index 594275b14cf..2fef43079d9 100644
--- a/sw/ground_segment/joystick/input2ivy.ml
+++ b/sw/ground_segment/joystick/input2ivy.ml
@@ -588,13 +588,13 @@ let () =
   Ivy.init "Paparazzi joystick" "READY" (fun _ _ -> ());
   Ivy.start !ivy_bus;
 
-  (** setup stdin *)
-  let tstatus = (Unix.tcgetattr Unix.stdin) in
+  (** setup stdin *) (* TODO find a better way to change trim, use a GUI ? *)
+  (*let tstatus = (Unix.tcgetattr Unix.stdin) in
   tstatus.c_icanon <- false;
-  Unix.tcsetattr Unix.stdin Unix.TCSANOW tstatus;
+  Unix.tcsetattr Unix.stdin Unix.TCSANOW tstatus;*)
 
   ignore (Glib.Timeout.add actions.period_ms (fun () -> execute_actions actions ac_id; true));
-  ignore (Glib.Io.add_watch ~cond:[`IN] ~callback:(fun x -> execute_kb_action actions x) (Glib.Io.channel_of_descr Unix.stdin));
+  (*ignore (Glib.Io.add_watch ~cond:[`IN] ~callback:(fun x -> execute_kb_action actions x) (Glib.Io.channel_of_descr Unix.stdin));*)
 
   (** Start the main loop *)
   let loop = Glib.Main.create true in

From 0d221ce14adebdacd8b15ebc27792c786a552a3e Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Fri, 22 Apr 2011 13:47:45 +0200
Subject: [PATCH 28/99] applied patch from Andreas Gaeb, adding settings to
 jsbsim target

---
 conf/autopilot/subsystems/fixedwing/autopilot.makefile | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/conf/autopilot/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile
index 47d2fd97df2..7d491dd3382 100644
--- a/conf/autopilot/subsystems/fixedwing/autopilot.makefile
+++ b/conf/autopilot/subsystems/fixedwing/autopilot.makefile
@@ -213,6 +213,9 @@ jsbsim.LDFLAGS		+= `pkg-config glib-2.0 --libs` -lm -lpcre -lglibivy -L/usr/lib
 jsbsim.CFLAGS 		+= -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport
 jsbsim.srcs 		+= downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/jsbsim_transport.c
 
+jsbsim.srcs 		+= subsystems/settings.c
+jsbsim.srcs 		+= $(SRC_ARCH)/subsystems/settings_arch.c
+
 ######################################################################
 ##
 ## Final Target Allocations

From 93ee429f328060b679e4379622ee5fafbe0138ca Mon Sep 17 00:00:00 2001
From: Martin Mueller <martin@pfump.org>
Date: Sat, 23 Apr 2011 07:31:30 +0200
Subject: [PATCH 29/99] needed; still I2CTransceive() seems broken

---
 sw/airborne/modules/meteo/humid_sht_i2c.c | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/sw/airborne/modules/meteo/humid_sht_i2c.c b/sw/airborne/modules/meteo/humid_sht_i2c.c
index 8fdccf61e45..3360eaa1a2c 100644
--- a/sw/airborne/modules/meteo/humid_sht_i2c.c
+++ b/sw/airborne/modules/meteo/humid_sht_i2c.c
@@ -35,6 +35,10 @@
 #include "messages.h"
 #include "downlink.h"
 
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
 #ifndef SHT_I2C_DEV
 #define SHT_I2C_DEV i2c0
 #endif

From 2c7614b2907dbcf138ef04cdc52ac4e364650aee Mon Sep 17 00:00:00 2001
From: Allen <aibara@jobyenergy.com>
Date: Tue, 26 Apr 2011 12:53:47 -0700
Subject: [PATCH 30/99] Tuning for lisa_m quad with MKK controllers

---
 conf/airframes/esden/lisa_m_pwm.xml | 26 +++++++++++++-------------
 1 file changed, 13 insertions(+), 13 deletions(-)

diff --git a/conf/airframes/esden/lisa_m_pwm.xml b/conf/airframes/esden/lisa_m_pwm.xml
index 3f78a121e12..bb9c4c53d37 100644
--- a/conf/airframes/esden/lisa_m_pwm.xml
+++ b/conf/airframes/esden/lisa_m_pwm.xml
@@ -38,8 +38,8 @@
 
 
   <section name="SUPERVISION" prefix="SUPERVISION_">
-    <define name="MIN_MOTOR" value="1090"/>
-    <define name="MAX_MOTOR" value="2000"/>
+    <define name="MIN_MOTOR" value="1200"/>
+    <define name="MAX_MOTOR" value="1920"/>
     <define name="TRIM_A" value="0"/>
     <define name="TRIM_E" value="0"/>
     <define name="TRIM_R" value="0"/>
@@ -122,12 +122,12 @@
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(600)"/>
+    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
     <define name="REF_ZETA_P"   value="0.85"/>
     <define name="REF_MAX_P"    value="RadOfDeg(400.)"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(600)"/>
+    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
     <define name="REF_MAX_Q"    value="RadOfDeg(400.)"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
@@ -138,22 +138,22 @@
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback -->
-    <define name="PHI_PGAIN"  value="-550"/>
-    <define name="PHI_DGAIN"  value="-210"/>
+    <define name="PHI_PGAIN"  value="-1000"/>
+    <define name="PHI_DGAIN"  value="-400"/>
     <define name="PHI_IGAIN"  value="-200"/>
 
-    <define name="THETA_PGAIN"  value="-550"/>
-    <define name="THETA_DGAIN"  value="-210"/>
+    <define name="THETA_PGAIN"  value="-1000"/>
+    <define name="THETA_DGAIN"  value="-400"/>
     <define name="THETA_IGAIN"  value="-200"/>
 
-    <define name="PSI_PGAIN"  value="-600"/>
-    <define name="PSI_DGAIN"  value="-200"/>
+    <define name="PSI_PGAIN"  value="-1000"/>
+    <define name="PSI_DGAIN"  value="-400"/>
     <define name="PSI_IGAIN"  value="-10"/>
 
     <!-- feedforward -->
-    <define name="PHI_DDGAIN"   value=" 200"/>
-    <define name="THETA_DDGAIN" value=" 200"/>
-    <define name="PSI_DDGAIN"   value=" 200"/>
+    <define name="PHI_DDGAIN"   value=" 300"/>
+    <define name="THETA_DDGAIN" value=" 300"/>
+    <define name="PSI_DDGAIN"   value=" 300"/>
 
   </section>
 

From 12e45e067bca9e050e9ced61416bae0236087b6c Mon Sep 17 00:00:00 2001
From: Allen <aibara@jobyenergy.com>
Date: Tue, 26 Apr 2011 12:54:28 -0700
Subject: [PATCH 31/99] Fix lisa pwm aspirin xml

---
 conf/airframes/esden/lisa_pwm_aspirin.xml | 20 --------------------
 1 file changed, 20 deletions(-)

diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml
index 8b4db240fd8..da03ef8484e 100644
--- a/conf/airframes/esden/lisa_pwm_aspirin.xml
+++ b/conf/airframes/esden/lisa_pwm_aspirin.xml
@@ -110,26 +110,6 @@
     <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
-    <!-- Old settings with exponential throttle motor controllers
-    <!-- feedback -->
-    <define name="PHI_PGAIN"  value="-250"/>
-    <define name="PHI_DGAIN"  value="-200"/>
-    <define name="PHI_IGAIN"  value="-150"/>
-
-    <define name="THETA_PGAIN"  value="-250"/>
-    <define name="THETA_DGAIN"  value="-200"/>
-    <define name="THETA_IGAIN"  value="-150"/>
-
-    <define name="PSI_PGAIN"  value="-900"/>
-    <define name="PSI_DGAIN"  value="-200"/>
-    <define name="PSI_IGAIN"  value="-10"/>
-
-    <!-- feedforward -->
-    <define name="PHI_DDGAIN"   value=" 200"/>
-    <define name="THETA_DDGAIN" value=" 200"/>
-    <define name="PSI_DDGAIN"   value=" 200"/>
-    -->
-
     <!-- feedback -->
     <define name="PHI_PGAIN"  value="-1900"/>
     <define name="PHI_DGAIN"  value="-380"/>

From 038ffb0525e4fddcff4b4831049cf226eb4dfe53 Mon Sep 17 00:00:00 2001
From: Allen <aibara@jobyenergy.com>
Date: Tue, 26 Apr 2011 12:56:02 -0700
Subject: [PATCH 32/99] Fix aspirin include xml file for esden airframe

---
 conf/airframes/esden/calib/aspirin_012.xml | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/conf/airframes/esden/calib/aspirin_012.xml b/conf/airframes/esden/calib/aspirin_012.xml
index 484ce882c4e..fe21765684d 100644
--- a/conf/airframes/esden/calib/aspirin_012.xml
+++ b/conf/airframes/esden/calib/aspirin_012.xml
@@ -1,3 +1,4 @@
+<airframe>
 <section name="IMU" prefix="IMU_">
   <define name="GYRO_P_NEUTRAL" value="93"/>
   <define name="GYRO_Q_NEUTRAL" value="131"/>
@@ -30,3 +31,4 @@
   <define name="MAG_YZ_SENS" value="0.0" integer="16"/>
 
 </section>
+</airframe>

From 6c76fe5fe368bab8f19e0a3564bf655b54030346 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Wed, 27 Apr 2011 10:45:54 +0200
Subject: [PATCH 33/99] should fix i2c transceive for now, needs to be fixed
 correctly

---
 sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
index f8f02fc83e8..11c51bbaecd 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
+++ b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
@@ -139,7 +139,7 @@ __attribute__ ((always_inline)) static inline void I2cAutomaton(int32_t state, s
         p->idx_buf++;
       } else {
         if (trans->type == I2CTransTxRx) {
-          //trans->type = I2CTransRx;	/* FIXME should not change type */
+          trans->type = I2CTransRx;	/* FIXME should not change type */
           p->idx_buf = 0;
           trans->slave_addr |= 1;
           I2cSendStart(p);

From e036f01fd1d773fe12075bced9bc23ab472b49d5 Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Wed, 27 Apr 2011 15:22:01 +0200
Subject: [PATCH 34/99] removed gps lost counter and gps_periodic. Now
 gps.last_fix_time is always used to determine gps timeout since it is
 independent of main loop frequency.

---
 sw/airborne/firmwares/fixedwing/main_ap.c |  2 +-
 sw/airborne/firmwares/rotorcraft/main.c   |  5 ++---
 sw/airborne/subsystems/gps.h              | 17 ++++++----------
 sw/airborne/subsystems/gps/gps_sim.h      |  3 +--
 sw/airborne/subsystems/gps/gps_sim_nps.h  |  2 +-
 sw/airborne/subsystems/gps/gps_skytraq.h  | 24 +++++++++++------------
 sw/airborne/subsystems/gps/gps_ubx.h      |  3 +--
 7 files changed, 24 insertions(+), 32 deletions(-)

diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 653304e0007..0cd47e82c32 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -269,7 +269,7 @@ static inline void telecommand_task( void ) {
 
 
 #ifdef FAILSAFE_DELAY_WITHOUT_GPS
-#define GpsTimeoutError (cpu_time_sec - gps.last_msg_time > FAILSAFE_DELAY_WITHOUT_GPS)
+#define GpsTimeoutError (cpu_time_sec - gps.last_fix_time > FAILSAFE_DELAY_WITHOUT_GPS)
 #endif
 
 /** \fn void navigation_task( void )
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index 12da21923b4..b0808d6202f 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -169,10 +169,9 @@ STATIC_INLINE void main_periodic( void ) {
     } );
 
 #ifdef USE_GPS
-  if (radio_control.status != RC_OK &&				\
+  if (radio_control.status != RC_OK &&                  \
       autopilot_mode == AP_MODE_NAV && GpsIsLost())		\
-    autopilot_set_mode(AP_MODE_FAILSAFE);			\
-  gps_periodic();
+    autopilot_set_mode(AP_MODE_FAILSAFE);
 #endif
 
   modules_periodic_task();
diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h
index ace5586b3db..21a30c6bb1f 100644
--- a/sw/airborne/subsystems/gps.h
+++ b/sw/airborne/subsystems/gps.h
@@ -78,8 +78,7 @@ struct GpsState {
   uint8_t nb_channels;           ///< Number of scanned satellites
   struct SVinfo svinfos[GPS_NB_CHANNELS];
 
-  uint8_t  lost_counter;         /* updated at 4Hz        */
-  uint16_t last_msg_time;
+  uint16_t last_fix_time;        ///< cpu time in sec at last valid fix
   uint16_t reset;                ///< hotstart, warmstart, coldstart
 };
 
@@ -99,15 +98,11 @@ extern void gps_init(void);
 extern void gps_impl_init(void);
 
 
-
-//TODO
-// this is only true for a 512Hz main loop
-// needs to work with different main loop frequencies
-static inline void gps_periodic( void ) {
-  RunOnceEvery(128, gps.lost_counter++; );
-}
-
-#define GpsIsLost() (gps.lost_counter > 20) /* 4Hz -> 5s */
+/* mark GPS as lost when no valid 3D fix was received for GPS_TIMEOUT secs */
+#ifndef GPS_TIMEOUT
+#define GPS_TIMEOUT 5
+#endif
+#define GpsIsLost() (cpu_time_sec - gps.last_fix_time > GPS_TIMEOUT)
 
 
 //TODO
diff --git a/sw/airborne/subsystems/gps/gps_sim.h b/sw/airborne/subsystems/gps/gps_sim.h
index 7e20ed92351..7794ea9c248 100644
--- a/sw/airborne/subsystems/gps/gps_sim.h
+++ b/sw/airborne/subsystems/gps/gps_sim.h
@@ -14,8 +14,7 @@ extern void gps_impl_init(void);
 #define GpsEvent(_sol_available_callback) {     \
     if (gps_available) {                        \
       if (gps.fix == GPS_FIX_3D) {              \
-        gps.lost_counter = 0;                   \
-        gps.last_msg_time = cpu_time_sec;       \
+        gps.last_fix_time = cpu_time_sec;       \
       }                                         \
       _sol_available_callback();                \
       gps_available = FALSE;                    \
diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.h b/sw/airborne/subsystems/gps/gps_sim_nps.h
index c5284b2e64a..c14e3f8f946 100644
--- a/sw/airborne/subsystems/gps/gps_sim_nps.h
+++ b/sw/airborne/subsystems/gps/gps_sim_nps.h
@@ -14,7 +14,7 @@ extern void gps_impl_init();
 #define GpsEvent(_sol_available_callback) {     \
     if (gps_available) {                        \
       if (gps.fix == GPS_FIX_3D)                \
-        gps.lost_counter = 0;                   \
+        gps.last_fix_time = cpu_time_sec;       \
       _sol_available_callback();				\
       gps_available = FALSE;                    \
     }                                           \
diff --git a/sw/airborne/subsystems/gps/gps_skytraq.h b/sw/airborne/subsystems/gps/gps_skytraq.h
index c5d6964c92b..fea8398af0c 100644
--- a/sw/airborne/subsystems/gps/gps_skytraq.h
+++ b/sw/airborne/subsystems/gps/gps_skytraq.h
@@ -88,19 +88,19 @@ extern struct GpsSkytraq gps_skytraq;
 
 #define GpsBuffer() GpsLink(ChAvailable())
 
-#define GpsEvent(_sol_available_callback) {				\
-    if (GpsBuffer()) {							\
-      ReadGpsBuffer();							\
-    }									\
-    if (gps_skytraq.msg_available) {				\
-      gps_skytraq_read_message();					\
+#define GpsEvent(_sol_available_callback) {                     \
+    if (GpsBuffer()) {                                          \
+      ReadGpsBuffer();                                          \
+    }                                                           \
+    if (gps_skytraq.msg_available) {                            \
+      gps_skytraq_read_message();                               \
       if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) {	\
-        if (gps.fix == GPS_FIX_3D)                     \
-          gps.lost_counter = 0;				\
-    _sol_available_callback();					\
-      }									\
-      gps_skytraq.msg_available = FALSE;				\
-    }									\
+        if (gps.fix == GPS_FIX_3D)                              \
+          gps.last_fix_time = cpu_time_sec;                     \
+        _sol_available_callback();                              \
+      }                                                         \
+      gps_skytraq.msg_available = FALSE;                        \
+    }                                                           \
   }
 
 #define ReadGpsBuffer() {						\
diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h
index 321bd24810e..186090e6baa 100644
--- a/sw/airborne/subsystems/gps/gps_ubx.h
+++ b/sw/airborne/subsystems/gps/gps_ubx.h
@@ -83,8 +83,7 @@ extern bool_t gps_configuring;
       if (gps_ubx.msg_class == UBX_NAV_ID &&       \
           gps_ubx.msg_id == UBX_NAV_VELNED_ID) {   \
         if (gps.fix == GPS_FIX_3D) {               \
-          gps.lost_counter = 0;                    \
-          gps.last_msg_time = cpu_time_sec;        \
+          gps.last_fix_time = cpu_time_sec;        \
         }                                          \
         _sol_available_callback();                 \
       }                                            \

From 0e15a116ceeeafbf05fd238386c2136ec13e592f Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Wed, 27 Apr 2011 15:50:58 +0200
Subject: [PATCH 35/99] added ublox specific status and sol flags

---
 sw/airborne/modules/ins/ins_arduimu.c       |  9 ++++++---
 sw/airborne/modules/ins/ins_arduimu_basic.c | 15 +++++++++------
 sw/airborne/subsystems/gps/gps_ubx.c        |  5 +++++
 sw/airborne/subsystems/gps/gps_ubx.h        |  3 +++
 4 files changed, 23 insertions(+), 9 deletions(-)

diff --git a/sw/airborne/modules/ins/ins_arduimu.c b/sw/airborne/modules/ins/ins_arduimu.c
index cc10e38d0c8..dbea8c92f2d 100644
--- a/sw/airborne/modules/ins/ins_arduimu.c
+++ b/sw/airborne/modules/ins/ins_arduimu.c
@@ -14,6 +14,9 @@ Autoren@ZHAW:   schmiemi
 #include "estimator.h"
 
 // für das Senden von GPS-Daten an den ArduIMU
+#ifndef UBX
+#error "currently only compatible with uBlox GPS modules"
+#endif
 #include "subsystems/gps.h"
 int32_t GPS_Data[14];
 
@@ -87,11 +90,11 @@ void ArduIMU_periodicGPS( void ) {
         GPS_Data [6] = gps.gspeed;		//ground speed
         GPS_Data [7] = DegOfRad(gps.course / 1e6);	//Kurs
         //status
-        GPS_Data [8] = gps_mode;		//fix
-        GPS_Data [9] = gps_status_flags;	//flags
+        GPS_Data [8] = gps.fix;		//fix
+        GPS_Data [9] = gps_ubx.status_flags;	//flags
         //sol
         GPS_Data [10] = gps.fix;		//fix
-        //GPS_Data [11] = gps_sol_flags;		//flags
+        //GPS_Data [11] = gps_ubx.sol_flags;		//flags
         GPS_Data [12] = -gps.ned_vel.z;
         GPS_Data [13] = gps.num_sv;
 
diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c
index 8d1f3c59533..0352c58b2d0 100644
--- a/sw/airborne/modules/ins/ins_arduimu_basic.c
+++ b/sw/airborne/modules/ins/ins_arduimu_basic.c
@@ -14,7 +14,10 @@ Autoren@ZHAW:   schmiemi
 #include "estimator.h"
 
 // GPS data for ArduIMU
-#include "gps.h"
+#ifndef UBX
+#error "currently only compatible with uBlox GPS modules"
+#endif
+#include "subsystems/gps.h"
 
 // Command vector for thrust
 #include "generated/airframe.h"
@@ -90,11 +93,11 @@ void ArduIMU_periodicGPS( void ) {
     high_accel_flag = FALSE;
   }
 
-  FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps_speed_3d); // speed 3D
-  FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps_gspeed);   // ground speed
-  FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)gps_course);   // course
-  ardu_gps_trans.buf[12] = gps_mode;                              // status gps fix
-  ardu_gps_trans.buf[13] = gps_status_flags;                      // status flags
+  FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps.speed_3d); // speed 3D
+  FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps.gspeed);   // ground speed
+  FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)DegOfRad(gps.course / 1e6));   // course
+  ardu_gps_trans.buf[12] = gps.fix;                              // status gps fix
+  ardu_gps_trans.buf[13] = gps_ubx.status_flags;                      // status flags
   ardu_gps_trans.buf[14] = (uint8_t)high_accel_flag;              // high acceleration flag (disable accelerometers in the arduimu filter)
   I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 15);
 
diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c
index 911ead5a351..cdb834db13c 100644
--- a/sw/airborne/subsystems/gps/gps_ubx.c
+++ b/sw/airborne/subsystems/gps/gps_ubx.c
@@ -179,6 +179,11 @@ void gps_ubx_read_message(void) {
         gps.svinfos[i].azim = UBX_NAV_SVINFO_Azim(gps_ubx.msg_buf, i);
       }
     }
+    else if (gps_ubx.msg_id == UBX_NAV_STATUS_ID) {
+      gps.fix = UBX_NAV_STATUS_GPSfix(gps_ubx.msg_buf);
+      gps_ubx.status_flags = UBX_NAV_STATUS_Flags(gps_ubx.msg_buf);
+      gps_ubx.sol_flags = UBX_NAV_SOL_Flags(gps_ubx.msg_buf);
+    }
   }
 }
 
diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h
index 186090e6baa..e1f85d4d410 100644
--- a/sw/airborne/subsystems/gps/gps_ubx.h
+++ b/sw/airborne/subsystems/gps/gps_ubx.h
@@ -48,6 +48,9 @@ struct GpsUbx {
   uint8_t send_ck_a, send_ck_b;
   uint8_t error_cnt;
   uint8_t error_last;
+
+  uint8_t status_flags;
+  uint8_t sol_flags;
 };
 
 extern struct GpsUbx gps_ubx;

From 24692fb8962a472f1112c95458b1b02060704347 Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Wed, 27 Apr 2011 20:12:31 +0200
Subject: [PATCH 36/99] added board and mcu specific includes to settings_arch
 so it compiles if not already included by accident

---
 sw/airborne/arch/lpc21/subsystems/settings_arch.c | 4 ++++
 sw/airborne/firmwares/fixedwing/datalink.c        | 1 +
 2 files changed, 5 insertions(+)

diff --git a/sw/airborne/arch/lpc21/subsystems/settings_arch.c b/sw/airborne/arch/lpc21/subsystems/settings_arch.c
index c493cb65218..f8763b31478 100644
--- a/sw/airborne/arch/lpc21/subsystems/settings_arch.c
+++ b/sw/airborne/arch/lpc21/subsystems/settings_arch.c
@@ -40,6 +40,10 @@
 
 #include "subsystems/settings.h"
 
+#include "LPC21xx.h"
+#include BOARD_CONFIG
+#include "armVIC.h"
+
 #define IAP_LOCATION 0x7FFFFFF1
 
 #define IAP_PREPARE_SECTORS     50
diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c
index d94abb5d1d9..00c3c879a73 100644
--- a/sw/airborne/firmwares/fixedwing/datalink.c
+++ b/sw/airborne/firmwares/fixedwing/datalink.c
@@ -41,6 +41,7 @@
 
 #if defined NAV || defined WIND_INFO
 #include "estimator.h"
+#include "subsystems/nav.h"
 #endif
 
 #ifdef USE_JOYSTICK

From b4a20ee8c11ac6183aee7362d16793a7ab464aac Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Wed, 27 Apr 2011 22:40:01 +0200
Subject: [PATCH 37/99] check if ivy version 3.8 is still used and call main
 loop appropriately

---
 sw/ground_segment/misc/davis2ivy.c | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/sw/ground_segment/misc/davis2ivy.c b/sw/ground_segment/misc/davis2ivy.c
index f167bb78155..7e5c70aa344 100644
--- a/sw/ground_segment/misc/davis2ivy.c
+++ b/sw/ground_segment/misc/davis2ivy.c
@@ -55,6 +55,7 @@
 #include <Ivy/ivy.h>
 #include <Ivy/ivyloop.h>
 #include <Ivy/timer.h>
+#include <Ivy/version.h>
 
 
 typedef enum { FALSE = 0, TRUE } BOOL;
@@ -263,7 +264,11 @@ int main(int argc, char **argv) {
   // create timer
   tid = TimerRepeatAfter (0, delay, handle_timer, 0);
 
-  IvyMainLoop();
+#if IVYMINOR_VERSION == 8
+  IvyMainLoop (NULL,NULL);
+#else
+  IvyMainLoop ();
+#endif
 
   return 0;
 }

From 2b3de73d929df61c76a8f230f6dd56f7d1d4a209 Mon Sep 17 00:00:00 2001
From: lamestllama <eric@eparsonage.com>
Date: Thu, 28 Apr 2011 08:01:59 +0930
Subject: [PATCH 38/99] fixed sw/ground_segment/misc/makefile to build under
 osx

---
 sw/ground_segment/misc/Makefile | 13 +++++++++++--
 1 file changed, 11 insertions(+), 2 deletions(-)

diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile
index 018ab409052..096dbdfaaa9 100644
--- a/sw/ground_segment/misc/Makefile
+++ b/sw/ground_segment/misc/Makefile
@@ -1,7 +1,16 @@
+UNAME = $(shell uname -s)
+
+ifeq ("$(UNAME)","Darwin")
+  LIBRARYS = -L/opt/local/lib
+else 
+  LIBRARYS = -s 
+endif
+
+
 all: davis2ivy
 
 davis2ivy: davis2ivy.o
-	g++ -o davis2ivy davis2ivy.o -s -livy
+	g++ -o davis2ivy davis2ivy.o  $(LIBRARYS)  -livy
 
 %.o : %.c
-	gcc -c -O2 -Wall $<
+	gcc -c -O2 -Wall -I /opt/local/include/  $<

From c25f1bccc16de3c7ba104f8ef942ac09ae884947 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 28 Apr 2011 13:10:17 +0200
Subject: [PATCH 39/99] Attempt 1: aspirin on Tiny

---
 conf/airframes/CDW/PPZIMUTinyFw.xml           | 197 ++++++++++++++++++
 .../lpc21/subsystems/imu/imu_aspirin_arch.c   |  78 +++++++
 .../lpc21/subsystems/imu/imu_aspirin_arch.h   |  16 ++
 3 files changed, 291 insertions(+)
 create mode 100644 conf/airframes/CDW/PPZIMUTinyFw.xml
 create mode 100644 sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c
 create mode 100644 sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.h

diff --git a/conf/airframes/CDW/PPZIMUTinyFw.xml b/conf/airframes/CDW/PPZIMUTinyFw.xml
new file mode 100644
index 00000000000..364ca50ac2a
--- /dev/null
+++ b/conf/airframes/CDW/PPZIMUTinyFw.xml
@@ -0,0 +1,197 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
+<!--
+	YAPA + XSens + XBee
+-->
+
+<airframe name="Yapa v1">
+
+  <servos>
+    <servo name="THROTTLE" no="0" min="1000" neutral="1000" max="2000"/>
+    <servo name="AILERON_LEFT" no="1" min="1000" neutral="1500" max="2000"/>
+    <servo name="AILERON_RIGHT" no="2" min="2000" neutral="1500" max="1000"/>
+    <servo name="ELEVATOR" no="3" min="2000" neutral="1500" max="1000"/>
+    <servo name="RUDDER" no="4" min="1100" neutral="1500" max="1900"/>
+  </servos>
+
+  <commands>
+    <axis name="THROTTLE" failsafe_value="0"/>
+    <axis name="ROLL" failsafe_value="0"/>
+    <axis name="PITCH" failsafe_value="0"/>
+    <axis name="BRAKE" failsafe_value="0"/>	<!-- both elerons up as butterfly brake ? -->
+  </commands>
+
+  <rc_commands>
+    <set command="THROTTLE" value="@THROTTLE"/>
+    <set command="ROLL" value="@ROLL"/>
+    <set command="PITCH" value="@PITCH"/>
+    <set command="BRAKE" value="@YAW"/>
+  </rc_commands>
+
+  <section name="SERVO_MIXER_GAINS">
+    <define name="AILERON_RATE_UP" value="0.50f"/>
+    <define name="AILERON_RATE_DOWN" value="0.25f"/>
+
+    <define name="AILERON_RATE_UP_BRAKE" value="0.5f"/>
+    <define name="AILERON_RATE_DOWN_BRAKE" value="0.9f"/>
+
+    <define name="PITCH_GAIN" value="0.9f"/>
+
+    <define name="YAW_THRUST" value="0.0f"/>
+    <define name="BRAKE_AILEVON" value="-0.68f"/>
+    <define name="BRAKE_PITCH" value="0.0f"/>
+    <define name="MAX_BRAKE_RATE" value="150"/>
+
+    <define name="LIMIT(X,XL,XH)" value="(    ((X)>(XH))   ?    (XH)    :    ( ((X)>(XL)) ? (X) : (XL) )    )"/>
+  </section>
+
+  <command_laws>
+    <!-- Differential Aileron Depending on Brake Value -->
+    <set servo="AILERON_LEFT" value="@ROLL"/>
+    <set servo="AILERON_RIGHT" value="@ROLL"/>
+
+    <!-- Differential Thurst -->
+    <set servo="THROTTLE" value="@THROTTLE"/>
+
+    <!-- Pitch with Brake-Trim Function -->
+    <set servo="ELEVATOR" value="@PITCH"/>
+  </command_laws>
+
+  <section name="INS" prefix="INS_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+  </section>
+
+  <section name="AUTO1" prefix="AUTO1_">
+    <define name="MAX_ROLL" value="0.7"/>
+    <define name="MAX_PITCH" value="0.7"/>
+  </section>
+
+  <section name="BAT">
+    <define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/>
+
+    <define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="10" unit="V"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
+  </section>
+
+  <section name="MISC">
+    <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
+    <define name="CARROT" value="5." unit="s"/>
+    <define name="CONTROL_RATE" value="60" unit="Hz"/>
+    <define name="XBEE_INIT" value="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/>
+<!--    <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
+    <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
+
+    <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
+
+    <define name="GLIDE_AIRSPEED" value="10"/>
+    <define name="GLIDE_VSPEED" value="3."/>
+    <define name="GLIDE_PITCH" value="45" unit="deg"/>
+  </section>
+
+  <section name="VERTICAL CONTROL" prefix="V_CTL_">
+    <define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
+    <!-- outer loop proportional gain -->
+    <define name="ALTITUDE_PGAIN" value="-0.03"/>
+    <!-- outer loop saturation -->
+    <define name="ALTITUDE_MAX_CLIMB" value="2."/>
+
+    <!-- auto throttle inner loop -->
+    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
+    <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
+    <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
+    <define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
+    <define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
+    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
+    <define name="AUTO_THROTTLE_PGAIN" value="-0.01"/>
+    <define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
+    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
+
+    <define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
+  </section>
+
+  <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
+    <define name="COURSE_PGAIN" value="-1.20000004768"/>
+    <define name="COURSE_DGAIN" value="0.3"/>
+    <define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
+
+    <define name="ROLL_MAX_SETPOINT" value="0.9" unit="radians"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+
+    <define name="PITCH_PGAIN" value="-12000."/>
+    <define name="PITCH_DGAIN" value="0"/>
+
+    <define name="ELEVATOR_OF_ROLL" value="1000."/>
+
+    <define name="ROLL_ATTITUDE_GAIN" value="-11500"/>
+    <define name="ROLL_RATE_GAIN" value="-600."/>
+  </section>
+
+  <section name="AGGRESSIVE" prefix="AGR_">
+    <define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
+    <define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
+    <define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
+    <define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
+    <define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
+    <define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
+    <define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
+    <define name="DESCENT_NAV_RATIO" value="1.0"/>
+  </section>
+
+  <section name="FAILSAFE" prefix="FAILSAFE_">
+    <define name="DEFAULT_THROTTLE" value="0.35" unit="%"/>
+    <define name="DEFAULT_ROLL" value="0.17" unit="rad"/>
+    <define name="DEFAULT_PITCH" value="0.08" unit="rad"/>
+
+    <define name="HOME_RADIUS" value="DEFAULT_CIRCLE_RADIUS" unit="m"/>
+    <define name="KILL_MODE_DISTANCE" value="(MAX_DIST_FROM_HOME*1.5)"/>
+    <define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
+  </section>
+
+  <section name="DIGITAL_CAMERA" prefix="DC_">
+    <define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
+    <define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
+  </section>
+
+
+  <modules>
+    <load name="baro_bmp.xml" >
+      <define name="SENSOR_SYNC_SEND" />
+      <define name="BMP_I2C_DEV" value="i2c0" />
+    </load>
+
+  </modules>
+
+  <firmware name="fixedwing">
+    <target name="ap" board="tiny_2.11">
+      <define name="STRONG_WIND"/>
+      <define name="WIND_INFO"/>
+      <define name="WIND_INFO_RET"/>
+      <define name="LOITER_TRIM"/>
+      <define name="ALT_KALMAN"/>
+      <define name="NB_CHANNELS" value="5" />
+    </target>
+    <target name="sim" board="pc"/>
+
+    <subsystem name="radio_control" type="ppm"/>
+    <subsystem name="gps" type="ublox_lea5h" />
+
+    <!-- Communication -->
+    <subsystem name="telemetry" type="xbee_api">
+      <configure name="MODEM_BAUD" value="B57600"/>
+    </subsystem>
+
+    <!-- Actuators -->
+    <subsystem name="control"/>
+    <!-- Sensors -->
+    <subsystem name="navigation"/>
+<!--    <subsystem name="gps"    type="ublox_lea5h"/> -->
+
+    <subsystem name="i2c"/>
+    <subsystem name="imu" type="aspirin" />
+
+  </firmware>
+
+</airframe>
diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c
new file mode 100644
index 00000000000..4e401891331
--- /dev/null
+++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c
@@ -0,0 +1,78 @@
+#include "subsystems/imu.h"
+
+
+#include "mcu_periph/i2c.h"
+
+
+void imu_aspirin_arch_int_enable(void)
+{
+}
+
+void imu_aspirin_arch_int_disable(void)
+{
+}
+
+void imu_aspirin_arch_init(void)
+{
+}
+
+
+void adxl345_write_to_reg(uint8_t addr, uint8_t val) {
+
+//  Adxl345Select();
+//  SPI_I2S_SendData(SPI2, addr);
+//  while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
+//  SPI_I2S_SendData(SPI2, val);
+//  while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
+//  while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_BSY) == SET);
+//  Adxl345Unselect();
+
+}
+
+void adxl345_clear_rx_buf(void) {
+}
+
+//void adxl345_start_reading_data(void) {
+//}
+
+/*
+ *
+ * Gyro data ready
+ *
+ */
+
+void exti15_10_irq_handler(void) {
+
+  /* clear EXTI */
+//  if(EXTI_GetITStatus(EXTI_Line14) != RESET)
+//    EXTI_ClearITPendingBit(EXTI_Line14);
+
+  imu_aspirin.gyro_eoc = TRUE;
+  imu_aspirin.status = AspirinStatusReadingGyro;
+
+}
+
+/*
+ *
+ * Accel data ready
+ *
+ */
+void exti2_irq_handler(void) {
+
+  /* clear EXTI */
+//  if(EXTI_GetITStatus(EXTI_Line2) != RESET)
+//    EXTI_ClearITPendingBit(EXTI_Line2);
+
+//  adxl345_start_reading_data();
+
+}
+
+/*
+ *
+ * Accel end of DMA transfert
+ *
+ */
+void dma1_c4_irq_handler(void) {
+
+  imu_aspirin.accel_available = TRUE;
+}
diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.h b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.h
new file mode 100644
index 00000000000..4345e8b14c1
--- /dev/null
+++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.h
@@ -0,0 +1,16 @@
+#ifndef IMU_ASPIRIN_ARCH_H
+#define IMU_ASPIRIN_ARCH_H
+
+#include "subsystems/imu.h"
+
+#include "led.h"
+
+extern void imu_aspirin_arch_init(void);
+extern void imu_aspirin_arch_int_enable(void);
+extern void imu_aspirin_arch_int_disable(void);
+extern void adxl345_write_to_reg(uint8_t addr, uint8_t val);
+extern void adxl345_clear_rx_buf(void);
+extern void adxl345_start_reading_data(void);
+
+
+#endif /* IMU_ASPIRIN_ARCH_H */

From 1987bbf7ec15f407a0893563e5f00ce2e78cb626 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Fri, 29 Apr 2011 23:36:58 +0200
Subject: [PATCH 40/99] timer call arch indep

---
 sw/airborne/subsystems/gps/gps_ubx.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c
index cdb834db13c..2fdcdccc20c 100644
--- a/sw/airborne/subsystems/gps/gps_ubx.c
+++ b/sw/airborne/subsystems/gps/gps_ubx.c
@@ -102,7 +102,7 @@ void gps_ubx_read_message(void) {
     if (gps_ubx.msg_id == UBX_NAV_SOL_ID) {
 #ifdef GPS_TIMESTAMP
       /* get hardware clock ticks */
-      gps.t0 = T0TC;
+      SysTimeTimerStart(gps.t0);
       gps.t0_tow        = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf);
       gps.t0_tow_frac   = UBX_NAV_SOL_Frac(gps_ubx.msg_buf);
 #endif

From fa3bef84829810e27ea3cdc817fffd43b9654f90 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Fri, 29 Apr 2011 23:38:33 +0200
Subject: [PATCH 41/99] use more auto alt_unit conversion

---
 conf/messages.xml     | 32 ++++++++++++++++----------------
 sw/lib/ocaml/pprz.ml  |  2 ++
 sw/lib/ocaml/pprz.mli |  4 ++++
 3 files changed, 22 insertions(+), 16 deletions(-)

diff --git a/conf/messages.xml b/conf/messages.xml
index 4a19a4de908..9bcbe6d2dc7 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -37,12 +37,12 @@
 
    <message name="GPS" id="8">
      <field name="mode"       type="uint8"  unit="byte_mask"/>
-     <field name="utm_east"   type="int32"  unit="cm" alt_unit="m" alt_unit_coef="0.01"/>
-     <field name="utm_north"  type="int32"  unit="cm" alt_unit="m" alt_unit_coef="0.01"/>
-     <field name="course"     type="int16"  unit="decideg" alt_unit="deg" alt_unit_coef="0.1"/>
-     <field name="alt"        type="int32"  unit="mm" alt_unit="m" alt_unit_coef="0.001"/>
-     <field name="speed"      type="uint16" unit="cm/s" alt_unit="m/s" alt_unit_coef="0.01"/>
-     <field name="climb"      type="int16"  unit="cm/s" alt_unit="m/s" alt_unit_coef="0.01"/>
+     <field name="utm_east"   type="int32"  unit="cm" alt_unit="m"/>
+     <field name="utm_north"  type="int32"  unit="cm" alt_unit="m"/>
+     <field name="course"     type="int16"  unit="decideg" alt_unit="deg"/>
+     <field name="alt"        type="int32"  unit="mm" alt_unit="m"/>
+     <field name="speed"      type="uint16" unit="cm/s" alt_unit="m/s"/>
+     <field name="climb"      type="int16"  unit="cm/s" alt_unit="m/s"/>
      <field name="week"       type="uint16" unit="weeks"/>
      <field name="itow"       type="uint32" unit="ms"/>
      <field name="utm_zone"   type="uint8"/>
@@ -1104,18 +1104,18 @@
   </message>
 
   <message name="GPS_INT" id="155">
-    <field name="ecef_x"  type="int32" unit="cm"   alt_unit="m"   alt_unit_coef="0.01"/>
-    <field name="ecef_y"  type="int32" unit="cm"   alt_unit="m"   alt_unit_coef="0.01"/>
-    <field name="ecef_z"  type="int32" unit="cm"   alt_unit="m"   alt_unit_coef="0.01"/>
+    <field name="ecef_x"  type="int32" unit="cm"   alt_unit="m"/>
+    <field name="ecef_y"  type="int32" unit="cm"   alt_unit="m"/>
+    <field name="ecef_z"  type="int32" unit="cm"   alt_unit="m"/>
     <field name="lat"     type="int32" alt_unit="deg" alt_unit_coef="0.0000057296"/>
     <field name="lon"     type="int32" alt_unit="deg" alt_unit_coef="0.0000057296"/>
-    <field name="alt"     type="int32" unit="mm"   alt_unit="m"   alt_unit_coef="0.001"/>
-    <field name="hmsl"    type="int32" unit="mm"   alt_unit="m"   alt_unit_coef="0.001"/>
-    <field name="ecef_xd" type="int32" unit="cm/s" alt_unit="m/s" alt_unit_coef="0.01"/>
-    <field name="ecef_yd" type="int32" unit="cm/s" alt_unit="m/s" alt_unit_coef="0.01"/>
-    <field name="ecef_zd" type="int32" unit="cm/s" alt_unit="m/s" alt_unit_coef="0.01"/>
-    <field name="pacc"    type="int32" unit="cm"   alt_unit="m"   alt_unit_coef="0.01"/>
-    <field name="sacc"    type="int32" unit="cm/s" alt_unit="m/s" alt_unit_coef="0.01"/>
+    <field name="alt"     type="int32" unit="mm"   alt_unit="m"/>
+    <field name="hmsl"    type="int32" unit="mm"   alt_unit="m"/>
+    <field name="ecef_xd" type="int32" unit="cm/s" alt_unit="m/s"/>
+    <field name="ecef_yd" type="int32" unit="cm/s" alt_unit="m/s"/>
+    <field name="ecef_zd" type="int32" unit="cm/s" alt_unit="m/s"/>
+    <field name="pacc"    type="int32" unit="cm"   alt_unit="m"/>
+    <field name="sacc"    type="int32" unit="cm/s" alt_unit="m/s"/>
     <field name="tow"     type="uint32"/>
     <field name="pdop"    type="uint16"/>
     <field name="numsv"   type="uint8"/>
diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml
index 774b7f1ea7f..ba2c64c93a3 100644
--- a/sw/lib/ocaml/pprz.ml
+++ b/sw/lib/ocaml/pprz.ml
@@ -168,6 +168,8 @@ let alt_unit_coef_of_xml = function xml ->
     | ("rad", "deg") | ("rad/s", "deg/s") -> string_of_float (180. /. pi)
     | ("m", "cm") | ("m/s", "cm/s") -> "100."
     | ("cm", "m") | ("cm/s", "m/s") -> "0.01"
+    | ("m", "mm") | ("m/s", "mm/s") -> "1000."
+    | ("mm", "m") | ("mm/s", "m/s") -> "0.001"
     | ("decideg", "deg") -> "0.1"
     | (_, _) -> "1."
 
diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli
index 855dd2938cf..b52124d3d55 100644
--- a/sw/lib/ocaml/pprz.mli
+++ b/sw/lib/ocaml/pprz.mli
@@ -93,8 +93,12 @@ val alt_unit_coef_of_xml : Xml.xml -> string
     rad -> deg
     m -> cm
     cm -> m
+    m -> mm
+    mm -> m
     m/s -> cm/s
     cm/s -> m/s
+    m/s -> mm/s
+    mm/s -> m/s
     decideg -> deg
     *)
 

From 554a0cc418a67fd114e01a9063a52a21cb20e1e1 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Sat, 30 Apr 2011 00:02:06 +0200
Subject: [PATCH 42/99] example of wrapper for ubx gps (not tested !)

---
 conf/modules/gps_ubx_uart.xml          | 22 +++++++++++++++++
 sw/airborne/modules/gps/gps_ubx_uart.h | 34 ++++++++++++++++++++++++++
 2 files changed, 56 insertions(+)
 create mode 100644 conf/modules/gps_ubx_uart.xml
 create mode 100644 sw/airborne/modules/gps/gps_ubx_uart.h

diff --git a/conf/modules/gps_ubx_uart.xml b/conf/modules/gps_ubx_uart.xml
new file mode 100644
index 00000000000..55ea724d27e
--- /dev/null
+++ b/conf/modules/gps_ubx_uart.xml
@@ -0,0 +1,22 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="gps_ubx" dir="gps">
+  <header>
+    <file name="gps_ubx_uart.h"/>
+  </header>
+  <init fun="gps_init()"/>
+  <event fun="GpsEvent(on_gps_solution)"/>
+  <makefile>
+    <file name="gps.c" dir="subsystems"/>
+  </makefile>
+  <makefile target="ap">
+    <file name="gps_ubx.c" dir="subsystems/gps"/>
+    <define name="GPS_TYPE_H" value="\"subsystems/gps/gps_ubx.h\""/>
+  </makefile>
+  <makefile target="sim|jsbsim">
+    <file name="gps_sim.c" dir="subsystems/gps"/>
+    <define name="GPS_TYPE_H" value="\"subsystems/gps/gps_sim.h\""/>
+  </makefile>
+</module>
+
+
diff --git a/sw/airborne/modules/gps/gps_ubx_uart.h b/sw/airborne/modules/gps/gps_ubx_uart.h
new file mode 100644
index 00000000000..3349680f443
--- /dev/null
+++ b/sw/airborne/modules/gps/gps_ubx_uart.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (C) 2011 Gautier Hattenberger
+ *
+ * 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.
+ *
+ */
+
+/*
+ * Wrapper for UBX gps on uart
+ */
+
+#ifndef MODULE_GPS_UBX_UART_H
+#define MODULE_GPS_UBX_UART_H
+
+#include "subsystems/gps.h"
+#include "subsystems/gps/gps_ubx.h"
+
+#endif // MODULE_GPS_UBX_UART_H
+

From 8e0aeb10ccb8b6775fb3541307cc06d990344990 Mon Sep 17 00:00:00 2001
From: Allen <aibara@jobyenergy.com>
Date: Fri, 29 Apr 2011 16:23:15 -0700
Subject: [PATCH 43/99] Enable/Disable all interrupts for aspirin for event
 handler

---
 .../stm32/subsystems/imu/imu_aspirin_arch.c   | 36 ++++++++++++++-----
 sw/airborne/subsystems/imu/imu_aspirin.h      |  4 +--
 2 files changed, 29 insertions(+), 11 deletions(-)

diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c
index c1cb472cbfe..676a890331e 100644
--- a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c
@@ -38,6 +38,15 @@ void imu_aspirin_arch_int_enable(void) {
   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
   NVIC_Init(&NVIC_InitStructure);
 
+  /* Enable DMA1 channel4 IRQ Channel ( SPI RX) */
+  NVIC_InitTypeDef NVIC_init_struct = {
+    .NVIC_IRQChannel = DMA1_Channel4_IRQn,
+    .NVIC_IRQChannelPreemptionPriority = 0,
+    .NVIC_IRQChannelSubPriority = 0,
+    .NVIC_IRQChannelCmd = ENABLE
+  };
+  NVIC_Init(&NVIC_init_struct);
+
 }
 
 void imu_aspirin_arch_int_disable(void) {
@@ -57,6 +66,15 @@ void imu_aspirin_arch_int_disable(void) {
   NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
   NVIC_Init(&NVIC_InitStructure);
 
+  /* Enable DMA1 channel4 IRQ Channel ( SPI RX) */
+  NVIC_InitTypeDef NVIC_init_struct = {
+    .NVIC_IRQChannel = DMA1_Channel4_IRQn,
+    .NVIC_IRQChannelPreemptionPriority = 0,
+    .NVIC_IRQChannelSubPriority = 0,
+    .NVIC_IRQChannelCmd = DISABLE
+  };
+  NVIC_Init(&NVIC_init_struct);
+
 }
 
 void imu_aspirin_arch_init(void) {
@@ -143,14 +161,6 @@ void imu_aspirin_arch_init(void) {
   SPI_InitStructure.SPI_CRCPolynomial = 7;
   SPI_Init(SPI2, &SPI_InitStructure);
 
-  /* Enable DMA1 channel4 IRQ Channel ( SPI RX) */
-  NVIC_InitTypeDef NVIC_init_struct = {
-    .NVIC_IRQChannel = DMA1_Channel4_IRQn,
-    .NVIC_IRQChannelPreemptionPriority = 0,
-    .NVIC_IRQChannelSubPriority = 0,
-    .NVIC_IRQChannelCmd = ENABLE
-  };
-  NVIC_Init(&NVIC_init_struct);
 
   /* Enable SPI_2 DMA clock ---------------------------------------------------*/
   RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
@@ -268,6 +278,15 @@ void exti2_irq_handler(void) {
  */
 void dma1_c4_irq_handler(void) {
   Adxl345Unselect();
+  if (DMA_GetITStatus(DMA1_IT_TC4)) {
+		// clear int pending bit
+		DMA_ClearITPendingBit(DMA1_IT_GL4);
+
+    // mark as available
+    imu_aspirin.accel_available = TRUE;
+	}
+
+  // disable DMA Channel
   DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE);
   /* Disable SPI_2 Rx and TX request */
   SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, DISABLE);
@@ -276,5 +295,4 @@ void dma1_c4_irq_handler(void) {
   DMA_Cmd(DMA1_Channel4, DISABLE);
   DMA_Cmd(DMA1_Channel5, DISABLE);
 
-  imu_aspirin.accel_available = TRUE;
 }
diff --git a/sw/airborne/subsystems/imu/imu_aspirin.h b/sw/airborne/subsystems/imu/imu_aspirin.h
index 1da0d051063..f9ac1d4f0a4 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin.h
+++ b/sw/airborne/subsystems/imu/imu_aspirin.h
@@ -127,14 +127,14 @@ static inline void imu_aspirin_event(void (* _gyro_handler)(void), void (* _acce
 {
   if (imu_aspirin.status == AspirinStatusUninit) return;
 
-  //imu_aspirin_arch_int_disable();
+  imu_aspirin_arch_int_disable();
   if (imu_aspirin.accel_available) {
     imu_aspirin.time_since_last_accel_reading = 0;
     imu_aspirin.accel_available = FALSE;
     accel_copy_spi();
     _accel_handler();
   }
-  //imu_aspirin_arch_int_enable();
+  imu_aspirin_arch_int_enable();
   
   // Reset everything if we've been waiting too long
   if (imu_aspirin.time_since_last_reading > ASPIRIN_GYRO_TIMEOUT) {

From 1d5ad17674fff665084dfb32513615a2867062b0 Mon Sep 17 00:00:00 2001
From: Allen <aibara@jobyenergy.com>
Date: Fri, 29 Apr 2011 19:20:46 -0700
Subject: [PATCH 44/99] Switch from ic ahrs to cmpl (ic seems broken?)

---
 conf/airframes/esden/lisa_pwm_aspirin.xml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml
index da03ef8484e..b4869e722a2 100644
--- a/conf/airframes/esden/lisa_pwm_aspirin.xml
+++ b/conf/airframes/esden/lisa_pwm_aspirin.xml
@@ -203,8 +203,8 @@
     <subsystem name="gps"           type="ublox">
 	    <configure name="GPS_BAUD"    value="B57600"/>
     </subsystem>
-    <!--subsystem name="ahrs"          type="cmpl"/-->
-    <subsystem name="ahrs"          type="ic"/>
+    <subsystem name="ahrs"          type="cmpl"/>
+    <!--<subsystem name="ahrs"          type="ic"/>-->
   </firmware>
 
   <firmware name="lisa_l_test_progs">

From 365812b796dd3c6e3c1f20c540fe26c7077cd7dd Mon Sep 17 00:00:00 2001
From: Martin Mueller <martin@pfump.org>
Date: Sat, 30 Apr 2011 23:14:57 +0200
Subject: [PATCH 45/99] module time slots might get randmomized if using more
 than one module, force SHT init to be on first slot for correct delays

---
 conf/modules/humid_sht_i2c.xml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/conf/modules/humid_sht_i2c.xml b/conf/modules/humid_sht_i2c.xml
index 949d3acf7c4..a9587748e3d 100644
--- a/conf/modules/humid_sht_i2c.xml
+++ b/conf/modules/humid_sht_i2c.xml
@@ -10,7 +10,7 @@
     <file name="humid_sht_i2c.h"/>
   </header>
   <init fun="humid_sht_init()"/>
-  <periodic fun="humid_sht_periodic()" freq="4"/>
+  <periodic fun="humid_sht_periodic()" freq="4" delay="0"/>
   <periodic fun="humid_sht_p_temp()" freq="4" delay="6"/>
   <periodic fun="humid_sht_p_humid()"  freq="4" delay="9"/>
   <event fun="humid_sht_event()"/>

From 7209c3133c7e136d3f5a566c46a6871c235bf183 Mon Sep 17 00:00:00 2001
From: Martin Mueller <martin@pfump.org>
Date: Sun, 1 May 2011 00:05:23 +0200
Subject: [PATCH 46/99] use VIC_SLOT 11 as default for i2c1

---
 sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c | 5 +++++
 sw/airborne/arch/lpc21/vic_slots_fw.txt      | 1 +
 2 files changed, 6 insertions(+)

diff --git a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
index 11c51bbaecd..98f65140a99 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
+++ b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
@@ -270,6 +270,11 @@ void i2c0_hw_init ( void ) {
 #endif
 #endif
 
+#ifndef I2C1_VIC_SLOT
+#define I2C1_VIC_SLOT 11
+#endif
+
+
 void i2c1_ISR(void) __attribute__((naked));
 
 void i2c1_ISR(void) {
diff --git a/sw/airborne/arch/lpc21/vic_slots_fw.txt b/sw/airborne/arch/lpc21/vic_slots_fw.txt
index 6c94aa7bfe4..86f238d5a80 100644
--- a/sw/airborne/arch/lpc21/vic_slots_fw.txt
+++ b/sw/airborne/arch/lpc21/vic_slots_fw.txt
@@ -17,6 +17,7 @@ SSP_VIC_SLOT               9          imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2
 MICROMAG_DRDY_VIC_SLOT     9          micromag
 hardcoded, no define      10          usb, e.g. telemetry_transparent_usb
 hardcoded, no define      11          EXTINT in baro_scp module
+I2C1_VIC_SLOT              ? (11)     (11 is default in mcu_periph/i2c_arch.c)
 I2C1_VIC_SLOT             12          ami601 in imu_b2_v1.0, imu_crista
 MS2100_DRDY_VIC_SLOT      12          ms2100 mag in imu_b2_v1.1
 MAX11040_DRDY_VIC_SLOT     ?          max11040 adc module

From 817735686f12fefd029fa806cfdbb5d3bc95b7df Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Tue, 3 May 2011 14:40:51 +0200
Subject: [PATCH 47/99] added print macros for uarts 2,3 and 5. Only define
 macros if respective UART is actually used (USE_UARTx).

---
 sw/airborne/print.h | 51 +++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 51 insertions(+)

diff --git a/sw/airborne/print.h b/sw/airborne/print.h
index 8f51c6286df..c62bee3872f 100644
--- a/sw/airborne/print.h
+++ b/sw/airborne/print.h
@@ -59,6 +59,8 @@
     _PrintHex16(out_fun, low32);				\
 }
 
+#ifdef USE_UART0
+
 #define Uart0PrintHex(c) _PrintHex(uart0_transmit, c)
 #define Uart0PrintHex16(c) _PrintHex16(uart0_transmit, c)
 #define Uart0PrintHex32(c) _PrintHex32(uart0_transmit, c)
@@ -69,8 +71,13 @@
 #define UART0PrintHex32   Uart0PrintHex32
 #define UART0PrintString  Uart0PrintString
 
+#endif /* USE_UART0 */
+
+#ifdef USE_UART1
+
 #define Uart1PrintHex(c) _PrintHex(uart1_transmit, c)
 #define Uart1PrintHex16(c) _PrintHex16(uart1_transmit, c)
+#define Uart1PrintHex32(c) _PrintHex32(uart1_transmit, c)
 #define Uart1PrintString(s) _PrintString(uart1_transmit, s)
 
 #define UART1PrintHex     Uart1PrintHex
@@ -78,6 +85,50 @@
 #define UART1PrintHex32   Uart1PrintHex32
 #define UART1PrintString  Uart1PrintString
 
+#endif /* USE_UART1 */
+
+#ifdef USE_UART2
+
+#define Uart2PrintHex(c) _PrintHex(uart2_transmit, c)
+#define Uart2PrintHex16(c) _PrintHex16(uart2_transmit, c)
+#define Uart2PrintHex32(c) _PrintHex32(uart2_transmit, c)
+#define Uart2PrintString(s) _PrintString(uart2_transmit, s)
+
+#define UART2PrintHex     Uart2PrintHex
+#define UART2PrintHex16   Uart2PrintHex16
+#define UART2PrintHex32   Uart2PrintHex32
+#define UART2PrintString  Uart2PrintString
+
+#endif /* USE_UART2 */
+
+#ifdef USE_UART3
+
+#define Uart3PrintHex(c) _PrintHex(uart3_transmit, c)
+#define Uart3PrintHex16(c) _PrintHex16(uart3_transmit, c)
+#define Uart3PrintHex32(c) _PrintHex32(uart3_transmit, c)
+#define Uart3PrintString(s) _PrintString(uart3_transmit, s)
+
+#define UART3PrintHex     Uart3PrintHex
+#define UART3PrintHex16   Uart3PrintHex16
+#define UART3PrintHex32   Uart3PrintHex32
+#define UART3PrintString  Uart3PrintString
+
+#endif /* USE_UART3 */
+
+#ifdef USE_UART5
+
+#define Uart5PrintHex(c) _PrintHex(uart5_transmit, c)
+#define Uart5PrintHex16(c) _PrintHex16(uart5_transmit, c)
+#define Uart5PrintHex32(c) _PrintHex32(uart5_transmit, c)
+#define Uart5PrintString(s) _PrintString(uart5_transmit, s)
+
+#define UART5PrintHex     Uart5PrintHex
+#define UART5PrintHex16   Uart5PrintHex16
+#define UART5PrintHex32   Uart5PrintHex32
+#define UART5PrintString  Uart5PrintString
+
+#endif /* USE_UART5 */
+
 #define UsbSPrintHex(c) _PrintHex(VCOM_putchar, c)
 #define UsbSPrintHex16(c) _PrintHex16(VCOM_putchar, c)
 #define UsbSPrintString(s) _PrintString(VCOM_putchar, s)

From 2222350ae2d0dbd6830350773e8841b2d25a2024 Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Tue, 3 May 2011 15:25:45 +0200
Subject: [PATCH 48/99] fixed definition of UBX_GPS_BAUD for gps configuration

---
 sw/airborne/subsystems/gps/gps_ubx.c | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c
index 2fdcdccc20c..0e2f9bd25a5 100644
--- a/sw/airborne/subsystems/gps/gps_ubx.c
+++ b/sw/airborne/subsystems/gps/gps_ubx.c
@@ -293,11 +293,9 @@ void gps_ubx_parse( uint8_t c ) {
 #define GPS_PORT_ID GPS_PORT_UART1
 #endif
 
-#if GPS_LINK == UART0
-#define UBX_GPS_BAUD UART0_BAUD
-#elif GPS_LINK == UART1
-#define UBX_GPS_BAUD UART1_BAUD
-#endif
+#define __UBX_GPS_BAUD(_u) _u##_BAUD
+#define _UBX_GPS_BAUD(_u) __UBX_GPS_BAUD(_u)
+#define UBX_GPS_BAUD _UBX_GPS_BAUD(GPS_LINK)
 
 /* Configure the GPS baud rate using the current uart baud rate. Busy
    wait for the end of the transmit. Then, BEFORE waiting for the ACK,

From a43abc929713141d02f161c95e2b6bebbb1207e4 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Tue, 3 May 2011 15:29:34 +0200
Subject: [PATCH 49/99] hmc5843 module -no-irq

---
 conf/modules/mag_hmc5843.xml              | 19 ++++++++
 sw/airborne/modules/sensors/mag_hmc5843.c | 55 +++++++++++++++++++++++
 sw/airborne/modules/sensors/mag_hmc5843.h | 33 ++++++++++++++
 sw/airborne/peripherals/hmc5843.c         | 46 ++++++++++++-------
 sw/airborne/peripherals/hmc5843.h         |  6 ++-
 5 files changed, 142 insertions(+), 17 deletions(-)
 create mode 100644 conf/modules/mag_hmc5843.xml
 create mode 100644 sw/airborne/modules/sensors/mag_hmc5843.c
 create mode 100644 sw/airborne/modules/sensors/mag_hmc5843.h

diff --git a/conf/modules/mag_hmc5843.xml b/conf/modules/mag_hmc5843.xml
new file mode 100644
index 00000000000..c8439d8e3e0
--- /dev/null
+++ b/conf/modules/mag_hmc5843.xml
@@ -0,0 +1,19 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="sensors">
+  <!-- <depend conflict="ins" -->
+  <header>
+    <file name="mag_hmc5843.h"/>
+  </header>
+  <init fun="hmc5843_module_init()"/>
+  <periodic fun="hmc5843_module_periodic()" freq="60"/>
+  <event fun="hmc5843_module_event()"/>
+  <makefile>
+    <define name="USE_I2C" />
+    <file name="mag_hmc5843.c"/>
+    <file name="../../peripherals/hmc5843.c"/>
+    <define name="HMC5843_I2C_DEVICE" value="i2c0" />
+    <define name="USE_I2C0" />
+    <define name="HMC5843_NO_IRQ" />
+  </makefile>
+</module>
diff --git a/sw/airborne/modules/sensors/mag_hmc5843.c b/sw/airborne/modules/sensors/mag_hmc5843.c
new file mode 100644
index 00000000000..dfffde60947
--- /dev/null
+++ b/sw/airborne/modules/sensors/mag_hmc5843.c
@@ -0,0 +1,55 @@
+/*
+ * 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 "estimator.h"
+#include "mcu_periph/i2c.h"
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+#include <math.h>
+
+#include "../../peripherals/hmc5843.h"
+
+
+int32_t mag_x, mag_y, mag_z;
+bool_t mag_valid;
+
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+
+void hmc5843_module_init( void ) {
+  hmc5843_init();
+}
+
+void hmc5843_module_periodic ( void )
+{
+  hmc5843_periodic();
+  mag_x = hmc5843.data.value[0];
+  mag_y = hmc5843.data.value[1];
+  mag_z = hmc5843.data.value[2];
+  RunOnceEvery(1,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z));
+}
+
+void hmc5843_module_event( void )
+{
+  hmc5843_idle_task();
+}
diff --git a/sw/airborne/modules/sensors/mag_hmc5843.h b/sw/airborne/modules/sensors/mag_hmc5843.h
new file mode 100644
index 00000000000..d21dbb50379
--- /dev/null
+++ b/sw/airborne/modules/sensors/mag_hmc5843.h
@@ -0,0 +1,33 @@
+/*
+ * 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.
+ *
+ */
+
+#ifndef HMC5843__H
+#define HMC5843__H
+
+#include "std.h"
+#include "mcu_periph/i2c.h"
+
+extern int32_t mag_x, mag_y, mag_z;
+
+extern void hmc5843_module_init( void );
+extern void hmc5843_module_periodic( void );
+extern void hmc5843_module_event( void );
+
+#endif // HMC5843__H
diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index f02c1483121..9ba12d90e2d 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -1,4 +1,6 @@
 #include "peripherals/hmc5843.h"
+#include "mcu_periph/i2c.h"
+#include "led.h"
 
 #define HMC5843_TIMEOUT 100
 
@@ -7,12 +9,18 @@
 struct Hmc5843 hmc5843;
 void exti9_5_irq_handler(void);
 
+#ifndef HMC5843_I2C_DEVICE
+#define HMC5843_I2C_DEVICE i2c2
+#endif
+
 void hmc5843_init(void)
 {
     hmc5843.i2c_trans.status = I2CTransSuccess;
     hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
 
+#ifndef HMC5843_NO_IRQ
     hmc5843_arch_init();
+#endif
 }
 
 // blocking, only intended to be called for initialization
@@ -22,37 +30,27 @@ static void send_config(void)
   hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA;  // set to rate to 50Hz
   hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2);
   hmc5843.i2c_trans.len_w = 2;
-  i2c_submit(&i2c2,&hmc5843.i2c_trans);
+  i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans);
     while(hmc5843.i2c_trans.status == I2CTransPending);
 
   hmc5843.i2c_trans.type = I2CTransTx;
   hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB;  // set to gain to 1 Gauss
   hmc5843.i2c_trans.buf[1] = 0x01<<5;
   hmc5843.i2c_trans.len_w = 2;
-  i2c_submit(&i2c2,&hmc5843.i2c_trans);
+  i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans);
     while(hmc5843.i2c_trans.status == I2CTransPending);
 
   hmc5843.i2c_trans.type = I2CTransTx;
   hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE;  // set to continuous mode
   hmc5843.i2c_trans.buf[1] = 0x00;
   hmc5843.i2c_trans.len_w = 2;
-  i2c_submit(&i2c2,&hmc5843.i2c_trans);
+  i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans);
     while(hmc5843.i2c_trans.status == I2CTransPending);
 
 }
 
 void hmc5843_idle_task(void)
 {
-    if (hmc5843.initialized && hmc5843.ready_for_read && (hmc5843.i2c_trans.status == I2CTransSuccess || hmc5843.i2c_trans.status == I2CTransFailed)) {
-      if (i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
-        hmc5843.ready_for_read = FALSE;
-        hmc5843.i2c_trans.type = I2CTransRx;
-        hmc5843.i2c_trans.len_r = 7;
-        i2c_submit(&i2c2, &hmc5843.i2c_trans);
-        hmc5843.reading = TRUE;
-      }
-    }
-
     if (hmc5843.reading && hmc5843.i2c_trans.status == I2CTransSuccess) {
         hmc5843.timeout = 0;
         hmc5843.data_available = TRUE;
@@ -61,6 +59,17 @@ void hmc5843_idle_task(void)
         for (int i = 0; i < 3; i++) {
             hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]);
         }
+        LED_TOGGLE(3);
+    }
+
+    if (hmc5843.initialized && hmc5843.ready_for_read && (hmc5843.i2c_trans.status == I2CTransSuccess || hmc5843.i2c_trans.status == I2CTransFailed)) {
+      if (HMC5843_I2C_DEVICE.status == I2CIdle && i2c_idle(&HMC5843_I2C_DEVICE)) {
+        hmc5843.ready_for_read = FALSE;
+        hmc5843.i2c_trans.type = I2CTransRx;
+        hmc5843.i2c_trans.len_r = 7;
+        i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans);
+        hmc5843.reading = TRUE;
+      }
     }
 }
 
@@ -69,15 +78,22 @@ void hmc5843_periodic(void)
     if (!hmc5843.initialized) {
         send_config();
         hmc5843.initialized = TRUE;
-    } else if (hmc5843.timeout++ > HMC5843_TIMEOUT && i2c2.status == I2CIdle && i2c_idle(&i2c2)){
+    }
+    else if (hmc5843.timeout++ > HMC5843_TIMEOUT && HMC5843_I2C_DEVICE.status == I2CIdle && i2c_idle(&HMC5843_I2C_DEVICE)){
 #ifdef USE_HMC59843_ARCH_RESET
         hmc5843_arch_reset();
 #endif
         hmc5843.i2c_trans.type = I2CTransRx;
         hmc5843.i2c_trans.len_r = 7;
-        i2c_submit(&i2c2, &hmc5843.i2c_trans);
+        i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans);
         hmc5843.reading = TRUE;
         hmc5843.ready_for_read = FALSE;
         hmc5843.timeout = 0;
     }
+
+#ifdef HMC5843_NO_IRQ
+    // < 50Hz
+    hmc5843.ready_for_read = TRUE;
+#endif
+
 }
diff --git a/sw/airborne/peripherals/hmc5843.h b/sw/airborne/peripherals/hmc5843.h
index ed31ebeed33..119d001256a 100644
--- a/sw/airborne/peripherals/hmc5843.h
+++ b/sw/airborne/peripherals/hmc5843.h
@@ -27,8 +27,6 @@
 #include "std.h"
 #include "mcu_periph/i2c.h"
 
-#include "peripherals/hmc5843_arch.h"
-
 struct Hmc5843 {
     struct i2c_transaction i2c_trans;
   union {
@@ -44,8 +42,12 @@ struct Hmc5843 {
 
 extern struct Hmc5843 hmc5843;
 
+#ifndef HMC5843_NO_IRQ
+#include "peripherals/hmc5843_arch.h"
+
 extern void hmc5843_arch_init( void );
 extern void hmc5843_arch_reset( void );
+#endif
 
 extern void hmc5843_init(void);
 extern void hmc5843_periodic(void);

From d9f78faecaf47b9b02bb666ea6cf5426ec7dd5d4 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 4 May 2011 13:06:12 +0200
Subject: [PATCH 50/99] XSens -> 230k4: otherwise half the data is lost. Fixed
 GPS_NB_CHANNELS issue with new GPS

---
 conf/airframes/TU_Delft/yapa_xsens.xml        | 14 ++--
 .../subsystems/fixedwing/imu_booz.makefile    |  1 -
 conf/modules/ins_xsens_MTiG_fixedwing.xml     |  5 +-
 conf/xsens_MTi-G.xml                          |  5 ++
 sw/airborne/ap_downlink.h                     |  2 +-
 sw/airborne/gps_xsens.c                       | 74 -------------------
 sw/airborne/gps_xsens.h                       | 48 ------------
 sw/airborne/modules/ins/ins_chimu_uart.c      | 22 +++---
 sw/airborne/modules/ins/ins_xsens.c           | 36 ++++++---
 sw/airborne/modules/sensors/mag_hmc5843.c     |  2 +-
 10 files changed, 58 insertions(+), 151 deletions(-)
 delete mode 100644 sw/airborne/gps_xsens.c
 delete mode 100644 sw/airborne/gps_xsens.h

diff --git a/conf/airframes/TU_Delft/yapa_xsens.xml b/conf/airframes/TU_Delft/yapa_xsens.xml
index 84d95e43c8d..10abe2b9f11 100644
--- a/conf/airframes/TU_Delft/yapa_xsens.xml
+++ b/conf/airframes/TU_Delft/yapa_xsens.xml
@@ -1,6 +1,6 @@
 <!DOCTYPE airframe SYSTEM "../airframe.dtd">
 
-<!-- 
+<!--
 	YAPA + XSens + XBee
 -->
 
@@ -51,7 +51,7 @@
     <!-- Brake Rate Limiter -->
     <let var="brake_value" value="Chop(-@BRAKE, 0, MAX_PPRZ)"/>
     <!--<let var="brake_value" value="RATELIMIT(  $brake_value , MAX_BRAKE_RATE )"/>
-    
+
     <let var="test; \
   	      static int16_t _var_brake_value = 0; \
               _var_brake_value += LIMIT(_var_brake_value_nofilt - _var_brake_value,-MAX_BRAKE_RATE,MAX_BRAKE_RATE); \
@@ -180,14 +180,16 @@
     <load name="ins_xsens_MTiG_fixedwing.xml">
       <configure name="XSENS_UART_NR" value="0"/>
     </load>
-
+<!--
     <load name="light.xml">
       <define name="LIGHT_LED_STROBE" value="3"/>
       <define name="LIGHT_LED_NAV" value="2"/>
       <define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
       <define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
     </load>
-<!--    <load name="digital_cam_i2c.xml"/>  -->
+    -->
+    <!--    <load name="digital_cam_i2c.xml"/>  -->
+    <load name="mag_hmc5843.xml" />
     <load name="digital_cam.xml" >
       <define name="DC_SHUTTER_LED" value="3"/>
     </load>
@@ -218,6 +220,8 @@
     <subsystem name="navigation"/>
     <subsystem name="gps" type="xsens"/>
 
-  </firmware>
+    <subsystem name="i2c"/>
+
+</firmware>
 
 </airframe>
diff --git a/conf/autopilot/subsystems/fixedwing/imu_booz.makefile b/conf/autopilot/subsystems/fixedwing/imu_booz.makefile
index e6ad633f17a..c6ee27d0c1f 100644
--- a/conf/autopilot/subsystems/fixedwing/imu_booz.makefile
+++ b/conf/autopilot/subsystems/fixedwing/imu_booz.makefile
@@ -52,7 +52,6 @@ imu_srcs += $(SRC_ARCH)/peripherals/max1168_arch.c
 #ifeq ($(ARCH), lpc21)
 imu_CFLAGS += -DSSP_VIC_SLOT=9
 imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
-#FIXME ms2100 not used on this imu
 #else ifeq ($(ARCH), stm32)
 #imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
 #imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT)
diff --git a/conf/modules/ins_xsens_MTiG_fixedwing.xml b/conf/modules/ins_xsens_MTiG_fixedwing.xml
index 4990c68b8cc..dcd3c87cb44 100644
--- a/conf/modules/ins_xsens_MTiG_fixedwing.xml
+++ b/conf/modules/ins_xsens_MTiG_fixedwing.xml
@@ -13,10 +13,13 @@
     <define name="AHRS_TYPE_H" value="\\\"modules/ins/ins_xsens.h\\\"" />
     <define name="USE_UART$(XSENS_UART_NR)"/>
     <define name="INS_LINK" value="Uart$(XSENS_UART_NR)"/>
-    <define name="UART$(XSENS_UART_NR)_BAUD" value="B115200"/>
+    <define name="UART$(XSENS_UART_NR)_BAUD" value="B230400"/>
     <define name="USE_GPS_XSENS"/>
     <define name="USE_GPS_XSENS_RAW_DATA" />
+    <define name="GPS_NB_CHANNELS" value="16" />
+    <define name="XSENS_OUTPUT_MODE" value="0x1836" />
     <file name="ins_xsens.c"/>
+    <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
   </makefile>
 </module>
 
diff --git a/conf/xsens_MTi-G.xml b/conf/xsens_MTi-G.xml
index 4db7955d4ce..721c801aef9 100644
--- a/conf/xsens_MTi-G.xml
+++ b/conf/xsens_MTi-G.xml
@@ -40,6 +40,11 @@
   </message>
   <message name="SetBaudrateAck" ID="0x19" to="host"/>
 
+  <message name="SetSyncOutSettings" ID="0xD8" to="MT" length="3">
+    <field name="param" format="U1"/>
+    <field name="value" format="U2"/>
+  </message>
+
   <message name="RestoreFactoryDef" ID="0x0E" to="MT"/>
   <message name="RestoreFactoryDefAck" ID="0x0F" to="host"/>
 
diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
index 248893f693f..8cb3248fb86 100644
--- a/sw/airborne/ap_downlink.h
+++ b/sw/airborne/ap_downlink.h
@@ -191,7 +191,7 @@
     int16_t climb = -gps.ned_vel.z;                                     \
     int16_t course = DegOfRad(gps.course / 10);                        \
     DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.lla_pos.alt, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \
-    if (i == gps.nb_channels) i = 0;                                    \
+    if (i >= gps.nb_channels) i = 0;                                    \
     if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) { \
       DOWNLINK_SEND_SVINFO(DefaultChannel, &i, &gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].elev, &gps.svinfos[i].azim); \
       last_cnos[i] = gps.svinfos[i].cno;                                \
diff --git a/sw/airborne/gps_xsens.c b/sw/airborne/gps_xsens.c
deleted file mode 100644
index 077d2353af5..00000000000
--- a/sw/airborne/gps_xsens.c
+++ /dev/null
@@ -1,74 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2005  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.
- *
- */
-
-/** \file gps_xsens.c
- *  \brief GPS hardware for Xsens MTi-G
- *
- */
-
-#include <stdlib.h>
-
-#include "gps.h"
-#include "sys_time.h"
-#include "generated/airframe.h"
-
-#include "mcu_periph/uart.h"
-#include "messages.h"
-#include "downlink.h"
-
-uint8_t gps_mode;
-uint16_t gps_week;
-uint32_t gps_itow;
-int32_t gps_alt;
-uint16_t gps_gspeed;
-int16_t gps_climb;
-int16_t gps_course;
-int32_t gps_utm_east, gps_utm_north;
-uint8_t gps_utm_zone;
-int32_t gps_lat, gps_lon;
-uint16_t gps_PDOP;
-uint32_t gps_Pacc, gps_Sacc;
-uint8_t gps_numSV;
-uint8_t gps_configuring;
-
-uint16_t gps_reset;
-
-void reset_gps_watchdog(void)
-{
-  last_gps_msg_t = cpu_time_sec;
-}
-
-volatile bool_t gps_msg_received;
-bool_t gps_pos_available;
-uint8_t gps_nb_ovrn;
-
-uint8_t gps_nb_channels;
-
-struct svinfo gps_svinfos[GPS_NB_CHANNELS];
-
-
-void gps_init( void ) {}
-void gps_configure( void ) {}
-void parse_gps_msg( void ) {}
-void gps_configure_uart( void ) {}
diff --git a/sw/airborne/gps_xsens.h b/sw/airborne/gps_xsens.h
deleted file mode 100644
index 2395793e5a0..00000000000
--- a/sw/airborne/gps_xsens.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
- * Paparazzi autopilot $Id: gps_ubx.h 4659 2010-03-10 16:55:04Z mmm $
- *
- * Copyright (C) 2004-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.
- *
- */
-
-/** \file gps_xsens.h
- * \brief XSens GPS
- *
-*/
-
-
-#ifndef XSENS_GPS_H
-#define XSENS_GPS_H
-
-extern uint16_t gps_reset;
-
-#define GPS_NB_CHANNELS 16
-
-extern void reset_gps_watchdog(void);
-
-
-#define GpsFixValid() (gps_mode > 0)
-
-#define gps_xsens_Reset(_val) { \
-  gps_reset = _val; \
-}
-
-
-#endif
diff --git a/sw/airborne/modules/ins/ins_chimu_uart.c b/sw/airborne/modules/ins/ins_chimu_uart.c
index 3929155ec45..7f6453795b4 100644
--- a/sw/airborne/modules/ins/ins_chimu_uart.c
+++ b/sw/airborne/modules/ins/ins_chimu_uart.c
@@ -34,22 +34,22 @@ INS_FORMAT ins_pitch_neutral;
 
 volatile uint8_t new_ins_attitude;
 
-void ins_init( void ) 
+void ins_init( void )
 {
   uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 };	// 50Hz attitude only + SPI
 //  uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
 //  uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI
   uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
-  
+
   CHIMU_Checksum(rate,12);
 
   new_ins_attitude = 0;
-  
+
   ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
   ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
-  
-  CHIMU_Init(&CHIMU_DATA);  
-  
+
+  CHIMU_Init(&CHIMU_DATA);
+
   // Quat Filter
   for (int i=0;i<7;i++)
   {
@@ -68,7 +68,7 @@ void ins_init( void )
   {
     InsUartSend1(rate[i]);
   }
-  
+
 }
 
 void parse_ins_msg( void )
@@ -76,7 +76,7 @@ void parse_ins_msg( void )
   while (InsLink(ChAvailable()))
   {
     uint8_t ch = InsLink(Getch());
-    
+
     if (CHIMU_Parse(ch, 0, &CHIMU_DATA))
     {
       if(CHIMU_DATA.m_MsgID==0x03)
@@ -87,10 +87,10 @@ void parse_ins_msg( void )
 	{
 	  CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
 	}
-		
+
 	EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
 	//EstimatorSetRate(ins_p,ins_q);
-	
+
 	DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
 
       }
@@ -100,7 +100,7 @@ void parse_ins_msg( void )
 
 
 //Frequency defined in conf *.xml
-void ins_periodic_task( void ) 
+void ins_periodic_task( void )
 {
   // Downlink Send
 }
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index adb9d0513ec..c26f0d0947e 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -68,6 +68,8 @@ INS_FORMAT ins_mz;
 float ins_pitch_neutral;
 float ins_roll_neutral;
 
+volatile uint8_t new_ins_attitude;
+
 //////////////////////////////////////////////////////////////////////////////////////////
 //
 //	XSens Specific
@@ -177,9 +179,6 @@ int8_t xsens_day;
 float xsens_lat;
 float xsens_lon;
 
-int8_t xsens_gps_nr_channels = 16;
-
-
 static uint8_t xsens_id;
 static uint8_t xsens_status;
 static uint8_t xsens_len;
@@ -205,7 +204,16 @@ void ins_init( void ) {
   XSENS_GoToConfig();
   XSENS_SetOutputMode(xsens_output_mode);
   XSENS_SetOutputSettings(xsens_output_settings);
+
+  uint8_t baud = 1;
+  XSENS_SetBaudrate(baud);
+  // Give pulses on SyncOut
+  XSENS_SetSyncOutSettings(0,0x0002);
+  // 1 pulse every 100 samples
+  // XSENS_SetSyncOutSettings(1,100);
   //XSENS_GoToMeasurment();
+
+  gps.nb_channels = 0;
 }
 
 void ins_periodic_task( void ) {
@@ -243,16 +251,22 @@ void parse_ins_msg( void ) {
   }
 #ifdef USE_GPS_XSENS
   else if (xsens_id == XSENS_GPSStatus_ID) {
-    xsens_gps_nr_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
-    gps.num_sv = xsens_gps_nr_channels;
+    gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
+    gps.num_sv = 0;
+
     uint8_t i;
-    for(i = 0; i < Min(xsens_gps_nr_channels, xsens_gps_nr_channels); i++) {
+    // Do not write outside buffer
+    for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
       uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i);
-      if (ch > xsens_gps_nr_channels) continue;
+      if (ch > gps.nb_channels) continue;
       gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i);
       gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i);
       gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i);
       gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i);
+      if (gps.svinfos[ch].flags > 0)
+      {
+        gps.num_sv++;
+      }
     }
   }
 #endif
@@ -267,15 +281,18 @@ void parse_ins_msg( void ) {
       }
       if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
 #if defined(USE_GPS_XSENS_RAW_DATA) && defined(USE_GPS_XSENS)
+    LED_TOGGLE(3);
         gps.week = 0; // FIXME
         gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
         gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset));
         gps.lla_pos.lon = RadOfDeg(XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset));
+
+
         /* Set the real UTM zone */
         gps.utm_pos.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
 
-        lla_f.lat = (float) gps.lla_pos.lat * 1e7;
-        lla_f.lon = (float) gps.lla_pos.lat * 1e7;
+        lla_f.lat = (float) gps.lla_pos.lat / 1e7;
+        lla_f.lon = (float) gps.lla_pos.lat / 1e7;
         utm_f.zone = nav_utm_zone0;
         /* convert to utm */
         utm_of_lla_f(&utm_f, &lla_f);
@@ -348,6 +365,7 @@ void parse_ins_msg( void ) {
         if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) {
           offset += XSENS_DATA_Matrix_LENGTH;
         }
+        new_ins_attitude = 1;
       }
       if (XSENS_MASK_Auxiliary(xsens_output_mode)) {
         uint8_t l = 0;
diff --git a/sw/airborne/modules/sensors/mag_hmc5843.c b/sw/airborne/modules/sensors/mag_hmc5843.c
index dfffde60947..cc3965c14de 100644
--- a/sw/airborne/modules/sensors/mag_hmc5843.c
+++ b/sw/airborne/modules/sensors/mag_hmc5843.c
@@ -46,7 +46,7 @@ void hmc5843_module_periodic ( void )
   mag_x = hmc5843.data.value[0];
   mag_y = hmc5843.data.value[1];
   mag_z = hmc5843.data.value[2];
-  RunOnceEvery(1,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z));
+  RunOnceEvery(30,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z));
 }
 
 void hmc5843_module_event( void )

From 4ee65bc49f17f26082be709a4795c70a3c07c5b6 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 4 May 2011 15:56:16 +0200
Subject: [PATCH 51/99] XSens bugfix lattitude-longitude issue

---
 sw/airborne/modules/ins/ins_xsens.c | 4 ++--
 sw/airborne/peripherals/hmc5843.c   | 6 +++---
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index c26f0d0947e..afb07a4aa6a 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -291,8 +291,8 @@ void parse_ins_msg( void ) {
         /* Set the real UTM zone */
         gps.utm_pos.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
 
-        lla_f.lat = (float) gps.lla_pos.lat / 1e7;
-        lla_f.lon = (float) gps.lla_pos.lat / 1e7;
+        lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
+        lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
         utm_f.zone = nav_utm_zone0;
         /* convert to utm */
         utm_of_lla_f(&utm_f, &lla_f);
diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index 381d2319334..188b3d18b07 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -31,21 +31,21 @@ static void send_config(void)
   hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2);
   hmc5843.i2c_trans.len_w = 2;
   i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans);
-  while(hmc5843.i2c_trans.status == I2CTransPending);
+    while(hmc5843.i2c_trans.status == I2CTransPending);
 
   hmc5843.i2c_trans.type = I2CTransTx;
   hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB;  // set to gain to 1 Gauss
   hmc5843.i2c_trans.buf[1] = 0x01<<5;
   hmc5843.i2c_trans.len_w = 2;
   i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans);
-  while(hmc5843.i2c_trans.status == I2CTransPending);
+    while(hmc5843.i2c_trans.status == I2CTransPending);
 
   hmc5843.i2c_trans.type = I2CTransTx;
   hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE;  // set to continuous mode
   hmc5843.i2c_trans.buf[1] = 0x00;
   hmc5843.i2c_trans.len_w = 2;
   i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans);
-  while(hmc5843.i2c_trans.status == I2CTransPending);
+    while(hmc5843.i2c_trans.status == I2CTransPending);
 
 }
 

From ea9a9cc4faf937953085f3e45ee26f5cdaf25dd0 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 4 May 2011 15:56:49 +0200
Subject: [PATCH 52/99] more SV-info when you need it, less SV-info when flying

---
 sw/airborne/ap_downlink.h | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
index 8cb3248fb86..256f0a1d68c 100644
--- a/sw/airborne/ap_downlink.h
+++ b/sw/airborne/ap_downlink.h
@@ -187,14 +187,13 @@
 //#define PERIODIC_SEND_GPS(_chan) gps_send() /* also sends svinfo */
 #define PERIODIC_SEND_GPS(_chan) {                                      \
     static uint8_t i;                                                   \
-    static uint8_t last_cnos[GPS_NB_CHANNELS];                          \
     int16_t climb = -gps.ned_vel.z;                                     \
     int16_t course = DegOfRad(gps.course / 10);                        \
     DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.lla_pos.alt, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \
-    if (i >= gps.nb_channels) i = 0;                                    \
-    if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) { \
+    if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0;                                    \
+    if (i >= gps.nb_channels * 2) i = 0;                                    \
+    if (i < gps.nb_channels && gps.svinfos[i].cno > 0) { \
       DOWNLINK_SEND_SVINFO(DefaultChannel, &i, &gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].elev, &gps.svinfos[i].azim); \
-      last_cnos[i] = gps.svinfos[i].cno;                                \
     }                                                                   \
     i++;                                                                \
 }

From 0d82d49960b2b228e36747fb484418cfbb9cc293 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Wed, 4 May 2011 17:54:53 +0200
Subject: [PATCH 53/99] add input2ivy to the set of default programs

---
 conf/control_panel.xml.example | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/conf/control_panel.xml.example b/conf/control_panel.xml.example
index 36f12526fa1..b54fddac02a 100644
--- a/conf/control_panel.xml.example
+++ b/conf/control_panel.xml.example
@@ -53,6 +53,10 @@
       <arg flag="-b" variable="ivy_bus"/>
     </program>
 
+    <program name="Joystick" command="sw/ground_segment/joystick/input2ivy">
+      <arg flag="-b" variable="ivy_bus"/>
+    </program>
+
     <program name="Hardware in the Loop" command="sw/simulator/simhitl">
       <arg flag="-fbw" variable="fbw_serial_port"/>
       <arg flag="-ap" variable="ap_serial_port"/>

From 15bf773a9486b39d71b1d4269e2ea70acb4bbc89 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 4 May 2011 21:57:33 +0200
Subject: [PATCH 54/99] PPZUAV-IMU universal module driver without interrupts

---
 .../fixed-wing/ppzimu_tiny.xml}               |  78 +++++--
 conf/modules/ins_ppzuavimu.xml                |  18 ++
 sw/airborne/modules/ins/ins_ppzuavimu.c       | 205 ++++++++++++++++++
 sw/airborne/modules/ins/ins_ppzuavimu.h       |  36 +++
 sw/airborne/peripherals/adxl345.h             |   2 +
 5 files changed, 315 insertions(+), 24 deletions(-)
 rename conf/airframes/{CDW/PPZIMUTinyFw.xml => PPZUAV/fixed-wing/ppzimu_tiny.xml} (72%)
 create mode 100644 conf/modules/ins_ppzuavimu.xml
 create mode 100644 sw/airborne/modules/ins/ins_ppzuavimu.c
 create mode 100644 sw/airborne/modules/ins/ins_ppzuavimu.h

diff --git a/conf/airframes/CDW/PPZIMUTinyFw.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
similarity index 72%
rename from conf/airframes/CDW/PPZIMUTinyFw.xml
rename to conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
index 364ca50ac2a..dfc634c298d 100644
--- a/conf/airframes/CDW/PPZIMUTinyFw.xml
+++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -8,8 +8,8 @@
 
   <servos>
     <servo name="THROTTLE" no="0" min="1000" neutral="1000" max="2000"/>
-    <servo name="AILERON_LEFT" no="1" min="1000" neutral="1500" max="2000"/>
-    <servo name="AILERON_RIGHT" no="2" min="2000" neutral="1500" max="1000"/>
+    <servo name="AILERON_LEFT" no="2" min="1000" neutral="1500" max="2000"/>
+    <servo name="AILERON_RIGHT" no="6" min="2000" neutral="1500" max="1000"/>
     <servo name="ELEVATOR" no="3" min="2000" neutral="1500" max="1000"/>
     <servo name="RUDDER" no="4" min="1100" neutral="1500" max="1900"/>
   </servos>
@@ -42,26 +42,39 @@
     <define name="BRAKE_PITCH" value="0.0f"/>
     <define name="MAX_BRAKE_RATE" value="150"/>
 
-    <define name="LIMIT(X,XL,XH)" value="(    ((X)>(XH))   ?    (XH)    :    ( ((X)>(XL)) ? (X) : (XL) )    )"/>
-  </section>
+    <define name="RATELIMIT(X,RATE)" value="( _rate_limited_value + Chop( (X) - _rate_limited_value, -(RATE), (RATE)  )); \
+       int16_t _rate_limited_value = 0;"/>
+
+</section>
 
   <command_laws>
+    <!-- Brake Rate Limiter -->
+    <let var="brake_value" value="Chop(-@BRAKE, 0, MAX_PPRZ)"/>
+    <!--<let var="brake_value" value="RATELIMIT(  $brake_value , MAX_BRAKE_RATE )"/>
+
+    <let var="test; \
+  	      static int16_t _var_brake_value = 0; \
+              _var_brake_value += LIMIT(_var_brake_value_nofilt - _var_brake_value,-MAX_BRAKE_RATE,MAX_BRAKE_RATE); \
+              int verwaarloos_deze_warning_CDW" value="0"/>
+		-->
+
     <!-- Differential Aileron Depending on Brake Value -->
-    <set servo="AILERON_LEFT" value="@ROLL"/>
-    <set servo="AILERON_RIGHT" value="@ROLL"/>
+    <let var="aileron_up_rate" value="(AILERON_RATE_UP   * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_UP_BRAKE   * $brake_value)"/>
+    <let var="aileron_down_rate" value="(AILERON_RATE_DOWN * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_DOWN_BRAKE * $brake_value)"/>
+    <let var="aileron_up" value="(@ROLL  *  (((float)$aileron_up_rate) / ((float)MAX_PPRZ)))"/>
+    <let var="aileron_down" value="(@ROLL  * (((float)$aileron_down_rate) / ((float)MAX_PPRZ)))"/>
+    <let var="leftturn" value="(@ROLL >= 0? 1 : 0)"/>
+    <let var="rightturn" value="(1 - $leftturn)"/>
+    <set servo="AILERON_LEFT" value="($aileron_up * $leftturn) + ($aileron_down * $rightturn) - $brake_value*(BRAKE_AILEVON)"/>
+    <set servo="AILERON_RIGHT" value="($aileron_up * $rightturn) + ($aileron_down * $leftturn) + $brake_value*(BRAKE_AILEVON)"/>
 
     <!-- Differential Thurst -->
     <set servo="THROTTLE" value="@THROTTLE"/>
 
     <!-- Pitch with Brake-Trim Function -->
-    <set servo="ELEVATOR" value="@PITCH"/>
+    <set servo="ELEVATOR" value="-@PITCH * PITCH_GAIN + BRAKE_PITCH * $brake_value"/>
   </command_laws>
 
-  <section name="INS" prefix="INS_">
-    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
-    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
-  </section>
-
   <section name="AUTO1" prefix="AUTO1_">
     <define name="MAX_ROLL" value="0.7"/>
     <define name="MAX_PITCH" value="0.7"/>
@@ -75,6 +88,11 @@
     <define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
   </section>
 
+  <section name="INS" prefix="INS_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+  </section>
+
   <section name="MISC">
     <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
     <define name="CARROT" value="5." unit="s"/>
@@ -116,17 +134,19 @@
     <define name="COURSE_DGAIN" value="0.3"/>
     <define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.9" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.75" unit="radians"/>
     <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
     <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
 
     <define name="PITCH_PGAIN" value="-12000."/>
-    <define name="PITCH_DGAIN" value="0"/>
+    <define name="PITCH_DGAIN" value="1.5"/>
 
     <define name="ELEVATOR_OF_ROLL" value="1000."/>
 
-    <define name="ROLL_ATTITUDE_GAIN" value="-11500"/>
-    <define name="ROLL_RATE_GAIN" value="-600."/>
+    <define name="ROLL_SLEW" value="1."/>
+
+    <define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
+    <define name="ROLL_RATE_GAIN" value="0."/>
   </section>
 
   <section name="AGGRESSIVE" prefix="AGR_">
@@ -157,26 +177,37 @@
 
 
   <modules>
-    <load name="baro_bmp.xml" >
-      <define name="SENSOR_SYNC_SEND" />
-      <define name="BMP_I2C_DEV" value="i2c0" />
+<!--
+    <load name="light.xml">
+      <define name="LIGHT_LED_STROBE" value="3"/>
+      <define name="LIGHT_LED_NAV" value="2"/>
+      <define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
+      <define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
     </load>
+    -->
+    <!--    <load name="digital_cam_i2c.xml"/>  -->
+
+    <load name="ins_ppzuavimu.xml" />
 
+<!--
+    <load name="digital_cam.xml" >
+      <define name="DC_SHUTTER_LED" value="3"/>
+    </load>
+    -->
   </modules>
 
   <firmware name="fixedwing">
+
     <target name="ap" board="tiny_2.11">
       <define name="STRONG_WIND"/>
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
       <define name="LOITER_TRIM"/>
       <define name="ALT_KALMAN"/>
-      <define name="NB_CHANNELS" value="5" />
     </target>
     <target name="sim" board="pc"/>
 
     <subsystem name="radio_control" type="ppm"/>
-    <subsystem name="gps" type="ublox_lea5h" />
 
     <!-- Communication -->
     <subsystem name="telemetry" type="xbee_api">
@@ -187,11 +218,10 @@
     <subsystem name="control"/>
     <!-- Sensors -->
     <subsystem name="navigation"/>
-<!--    <subsystem name="gps"    type="ublox_lea5h"/> -->
+    <subsystem name="gps" type="ublox_lea4p"/>
 
     <subsystem name="i2c"/>
-    <subsystem name="imu" type="aspirin" />
 
-  </firmware>
+</firmware>
 
 </airframe>
diff --git a/conf/modules/ins_ppzuavimu.xml b/conf/modules/ins_ppzuavimu.xml
new file mode 100644
index 00000000000..d3c02614213
--- /dev/null
+++ b/conf/modules/ins_ppzuavimu.xml
@@ -0,0 +1,18 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="ins_ppzuavimu" dir="ins">
+  <!-- <depend conflict="ins" -->
+  <header>
+    <file name="ins_ppzuavimu.h"/>
+  </header>
+  <init fun="ppzuavimu_module_init()"/>
+  <periodic fun="ppzuavimu_module_periodic()" freq="60"/>
+  <event fun="ppzuavimu_module_event()"/>
+  <makefile>
+    <define name="AHRS_TYPE_H" value="\\\"modules/ins/ins_ppzuavimu.h\\\"" />
+    <define name="USE_I2C" />
+    <file name="ins_ppzuavimu.c"/>
+    <define name="PPZUAVIMU_I2C_DEVICE" value="i2c0" />
+    <define name="USE_I2C0" />
+  </makefile>
+</module>
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c
new file mode 100644
index 00000000000..602c50d7bef
--- /dev/null
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.c
@@ -0,0 +1,205 @@
+/*
+ * 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 <math.h>
+#include "estimator.h"
+#include "mcu_periph/i2c.h"
+
+// Downlink
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+
+// Peripherials
+#define HMC5843_NO_IRQ
+#include "../../peripherals/itg3200.h"
+#include "../../peripherals/adxl345.h"
+#include "../../peripherals/hmc5843.h"
+
+// INS
+#include "ins_module.h"
+
+INS_FORMAT ins_roll_neutral;
+INS_FORMAT ins_pitch_neutral;
+
+// Results
+int32_t mag_x, mag_y, mag_z;
+bool_t mag_valid;
+
+int32_t gyr_x, gyr_y, gyr_z;
+bool_t gyr_valid;
+
+int32_t acc_x, acc_y, acc_z;
+bool_t acc_valid;
+
+
+// Communication
+struct i2c_transaction ppzuavimu_hmc5843;
+struct i2c_transaction ppzuavimu_itg3200;
+struct i2c_transaction ppzuavimu_adxl345;
+
+
+void ppzuavimu_module_init( void )
+{
+  /////////////////////////////////////////////////////////////////////
+  // ITG3200
+  ppzuavimu_itg3200.type = I2CTransTx;
+  ppzuavimu_itg3200.slave_addr = ITG3200_ADDR;
+  /* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */
+  ppzuavimu_itg3200.buf[0] = ITG3200_REG_DLPF_FS;
+  ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x04<<0);
+  ppzuavimu_itg3200.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
+    while(ppzuavimu_itg3200.status == I2CTransPending);
+
+  /* set sample rate to 66Hz: so at 60Hz there is always a new sample ready and you loose little */
+  ppzuavimu_itg3200.buf[0] = ITG3200_REG_SMPLRT_DIV;
+  ppzuavimu_itg3200.buf[1] = 0x0E;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
+    while(ppzuavimu_itg3200.status == I2CTransPending);
+
+  /* switch to gyroX clock */
+  ppzuavimu_itg3200.buf[0] = ITG3200_REG_PWR_MGM;
+  ppzuavimu_itg3200.buf[1] = 0x01;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
+    while(ppzuavimu_itg3200.status == I2CTransPending);
+
+  /* no interrupts for now */
+  ppzuavimu_itg3200.buf[0] = ITG3200_REG_INT_CFG;
+  ppzuavimu_itg3200.buf[1] = 0x00;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
+    while(ppzuavimu_itg3200.status == I2CTransPending);
+
+  /////////////////////////////////////////////////////////////////////
+  // ADXL345
+
+  /* set data rate to 100Hz */
+  ppzuavimu_adxl345.slave_addr = ADXL345_ADDR;
+  ppzuavimu_adxl345.type = I2CTransTx;
+  ppzuavimu_adxl345.buf[0] = ADXL345_REG_BW_RATE;
+  ppzuavimu_adxl345.buf[1] = 0x0a;  // normal power and 100Hz sampling, 50Hz BW
+  ppzuavimu_adxl345.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
+    while(ppzuavimu_adxl345.status == I2CTransPending);
+
+  /* switch to measurement mode */
+  ppzuavimu_adxl345.type = I2CTransTx;
+  ppzuavimu_adxl345.buf[0] = ADXL345_REG_POWER_CTL;
+  ppzuavimu_adxl345.buf[1] = 1<<3;
+  ppzuavimu_adxl345.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
+    while(ppzuavimu_adxl345.status == I2CTransPending);
+
+  /* Set range to 4g */
+  ppzuavimu_adxl345.type = I2CTransTx;
+  ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_FORMAT;
+  ppzuavimu_adxl345.buf[1] = 1<<3 | 0<<2 | 0x01;  // bit 3 is full resolution bit, bit 2 is left justify bit 0,1 are range: 00=2g 01=4g 10=8g 11=16g
+  ppzuavimu_adxl345.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
+    while(ppzuavimu_adxl345.status == I2CTransPending);
+
+  /////////////////////////////////////////////////////////////////////
+  // HMC5843
+  ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR;
+  ppzuavimu_hmc5843.type = I2CTransTx;
+  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA;  // set to rate to 50Hz
+  ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2);
+  ppzuavimu_hmc5843.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
+    while(ppzuavimu_hmc5843.status == I2CTransPending);
+
+  ppzuavimu_hmc5843.type = I2CTransTx;
+  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGB;  // set to gain to 1 Gauss
+  ppzuavimu_hmc5843.buf[1] = 0x01<<5;
+  ppzuavimu_hmc5843.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
+    while(ppzuavimu_hmc5843.status == I2CTransPending);
+
+  ppzuavimu_hmc5843.type = I2CTransTx;
+  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_MODE;  // set to continuous mode
+  ppzuavimu_hmc5843.buf[1] = 0x00;
+  ppzuavimu_hmc5843.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
+    while(ppzuavimu_hmc5843.status == I2CTransPending);
+
+}
+
+void ppzuavimu_module_periodic( void )
+{
+  // Start reading the latest gyroscope data
+  ppzuavimu_itg3200.type = I2CTransTxRx;
+  ppzuavimu_itg3200.len_r = 6;
+  ppzuavimu_itg3200.len_w = 1;
+  ppzuavimu_itg3200.buf[0] = ITG3200_REG_GYRO_XOUT_H;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_itg3200);
+
+  // Start reading the latest accelerometer data
+  ppzuavimu_adxl345.type = I2CTransTxRx;
+  ppzuavimu_adxl345.len_r = 6;
+  ppzuavimu_adxl345.len_w = 1;
+  ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_X0;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_adxl345);
+
+  // Start reading the latest magnetometer data
+  ppzuavimu_hmc5843.type = I2CTransTxRx;
+  ppzuavimu_hmc5843.len_r = 6;
+  ppzuavimu_hmc5843.len_w = 1;
+  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843);
+
+  RunOnceEvery(6,DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&gyr_x,&gyr_y,&gyr_z));
+  RunOnceEvery(6,DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&acc_x,&acc_y,&acc_z));
+  RunOnceEvery(6,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z));
+}
+
+void ppzuavimu_module_event( void )
+{
+  // If the itg3200 I2C transaction has succeeded: convert the data
+  if (ppzuavimu_itg3200.status == I2CTransSuccess)
+  {
+    gyr_x = (int16_t) ((ppzuavimu_itg3200.buf[0] << 8) | ppzuavimu_itg3200.buf[1]);
+    gyr_y = (int16_t) ((ppzuavimu_itg3200.buf[2] << 8) | ppzuavimu_itg3200.buf[3]);
+    gyr_z = (int16_t) ((ppzuavimu_itg3200.buf[4] << 8) | ppzuavimu_itg3200.buf[5]);
+    gyr_valid = TRUE;
+  }
+
+  // If the adxl345 I2C transaction has succeeded: convert the data
+  if (ppzuavimu_adxl345.status == I2CTransSuccess)
+  {
+    acc_x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
+    acc_y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
+    acc_z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
+    acc_valid = TRUE;
+  }
+
+  // If the hmc5843 I2C transaction has succeeded: convert the data
+  if (ppzuavimu_hmc5843.status == I2CTransSuccess)
+  {
+    mag_x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]);
+    mag_y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
+    mag_z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
+    mag_valid = TRUE;
+  }
+}
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.h b/sw/airborne/modules/ins/ins_ppzuavimu.h
new file mode 100644
index 00000000000..f528076c71a
--- /dev/null
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.h
@@ -0,0 +1,36 @@
+/*
+ * 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.
+ *
+ */
+
+#ifndef PPZUAVIMU_H
+#define PPZUAVIMU_H
+
+#include "std.h"
+// pitch/roll neutrals
+#include "ins_module.h"
+
+extern int32_t mag_x, mag_y, mag_z;
+extern int32_t gyr_x, gyr_y, gyr_z;
+extern int32_t acc_x, acc_y, acc_z;
+
+extern void ppzuavimu_module_init( void );
+extern void ppzuavimu_module_periodic( void );
+extern void ppzuavimu_module_event( void );
+
+#endif // PPZUAVIMU_H
diff --git a/sw/airborne/peripherals/adxl345.h b/sw/airborne/peripherals/adxl345.h
index de6fa023051..5e0b5559eff 100644
--- a/sw/airborne/peripherals/adxl345.h
+++ b/sw/airborne/peripherals/adxl345.h
@@ -1,6 +1,8 @@
 #ifndef ADXL345_H
 #define ADXL345_H
 
+#define ADXL345_ADDR            0xA6
+#define ADXL345_ADDR_ALT        0x3A
 
 #define ADXL345_REG_BW_RATE     0x2C
 #define ADXL345_REG_POWER_CTL   0x2D

From 0e519a2f4eedcb29e14fc8850287d994a25b4f90 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 4 May 2011 21:59:42 +0200
Subject: [PATCH 55/99] fixed path issue after moving files

---
 conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
index dfc634c298d..aba43cb4d3f 100644
--- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -1,4 +1,4 @@
-<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
 
 <!--
 	YAPA + XSens + XBee

From c5dcaa4204ac97833c3963c13588a591bbdf36b4 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 5 May 2011 09:38:30 +0200
Subject: [PATCH 56/99] Added AHRS to ppzimu

---
 .../PPZUAV/fixed-wing/ppzimu_tiny.xml         | 105 ++++++++++++++++--
 conf/modules/ins_ppzuavimu.xml                |   1 -
 sw/airborne/modules/ins/ins_ppzuavimu.c       |  12 +-
 sw/airborne/modules/ins/ins_ppzuavimu.h       |  29 ++++-
 4 files changed, 128 insertions(+), 19 deletions(-)

diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
index aba43cb4d3f..8628daf58ff 100644
--- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -75,6 +75,78 @@
     <set servo="ELEVATOR" value="-@PITCH * PITCH_GAIN + BRAKE_PITCH * $brake_value"/>
   </command_laws>
 
+ <!-- Local magnetic field -->
+ <section name="AHRS" prefix="AHRS_" >
+   <define name="H_X" value="0.51562740288882" />
+   <define name="H_Y" value="-0.05707735220832" />
+   <define name="H_Z" value="0.85490967783446" />
+ </section>
+
+
+  <section name="IMU" prefix="IMU_">
+    <!-- MAX1168 ADC CHANNELS
+    <define name="GYRO_P_CHAN" value="0"/>
+    <define name="GYRO_Q_CHAN" value="1"/>
+    <define name="GYRO_R_CHAN" value="2"/>
+
+    <define name="ACCEL_X_CHAN" value="4"/>
+    <define name="ACCEL_Y_CHAN" value="3"/>
+    <define name="ACCEL_Z_CHAN" value="5"/>
+    -->
+    <!-- Calibration Neutral -->
+    <define name="GYRO_P_NEUTRAL" value="0"/>
+    <define name="GYRO_Q_NEUTRAL" value="0"/>
+    <define name="GYRO_R_NEUTRAL" value="0"/>
+
+    <!-- SENS = 2mV/deg/sec * 57.6 deg/rad = 114.59 mV/rad/sec * 16 LSB/mV = 1833.465 LSB/rad/sec / 12bit FRAC -->
+    <define name="GYRO_P_SENS" value="2.234" integer="16"/>
+    <define name="GYRO_Q_SENS" value="2.234" integer="16"/>
+    <define name="GYRO_R_SENS" value="2.234" integer="16"/>
+
+    <define name="GYRO_P_Q" value="0."/>
+    <define name="GYRO_P_R" value="0"/>
+    <define name="GYRO_Q_P" value="0."/>
+    <define name="GYRO_Q_R" value="0."/>
+    <define name="GYRO_R_P" value="0."/>
+    <define name="GYRO_R_Q" value="0."/>
+
+    <define name="GYRO_P_SIGN" value="1"/>
+    <define name="GYRO_Q_SIGN" value="-1"/>
+    <define name="GYRO_R_SIGN" value="-1"/>
+
+    <define name="ACCEL_X_NEUTRAL" value="0"/>
+    <define name="ACCEL_Y_NEUTRAL" value="0"/>
+    <define name="ACCEL_Z_NEUTRAL" value="0"/>
+
+    <!-- SENS = 660mV/g / 9.81 ms2/g = 67.27 mV/ms2 * 16 LSB/mV = 1076 LSB/ms2 / 10bit FRAC -->
+    <define name="ACCEL_X_SENS" value="0.9372" integer="16"/>
+    <define name="ACCEL_Y_SENS" value="0.9346" integer="16"/>
+    <define name="ACCEL_Z_SENS" value="0.9178" integer="16"/>
+
+    <define name="ACCEL_X_SIGN" value="1"/>
+    <define name="ACCEL_Y_SIGN" value="1"/>
+    <define name="ACCEL_Z_SIGN" value="1"/>
+
+    <define name="MAG_X_NEUTRAL" value="0"/>
+    <define name="MAG_Y_NEUTRAL" value="0"/>
+    <define name="MAG_Z_NEUTRAL" value="0"/>
+
+    <define name="MAG_X_SENS" value="1" integer="16"/>
+    <define name="MAG_Y_SENS" value="1" integer="16"/>
+    <define name="MAG_Z_SENS" value="1" integer="16"/>
+
+<!--    <define name="MAG_45_HACK" value="1"/> -->
+
+    <define name="BODY_TO_IMU_PHI" value="0"/>
+    <define name="BODY_TO_IMU_THETA" value="0"/>
+    <define name="BODY_TO_IMU_PSI" value="0"/>
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+  </section>
+
   <section name="AUTO1" prefix="AUTO1_">
     <define name="MAX_ROLL" value="0.7"/>
     <define name="MAX_PITCH" value="0.7"/>
@@ -88,11 +160,6 @@
     <define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
   </section>
 
-  <section name="INS" prefix="INS_">
-    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
-    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
-  </section>
-
   <section name="MISC">
     <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
     <define name="CARROT" value="5." unit="s"/>
@@ -202,11 +269,29 @@
       <define name="STRONG_WIND"/>
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
-      <define name="LOITER_TRIM"/>
-      <define name="ALT_KALMAN"/>
+
+      <!--
+      <configure name="PERIODIC_FREQUENCY" value="960"/>
+      <configure name="AHRS_PROPAGATE_FREQUENCY" value="240"/>
+      <configure name="AHRS_CORRECT_FREQUENCY" value="60"/>
+      <configure name="AHRS_ALIGNER_LED" value="1"/>
+      -->
+
+      <configure name="CPU_LED" value="2"/>
     </target>
     <target name="sim" board="pc"/>
 
+    <define name="AGR_CLIMB"/>
+    <define name="LOITER_TRIM"/>
+    <define name="ALT_KALMAN"/>
+
+    <!-- Sensors -->
+    <!--
+    <subsystem name="imu" type="booz"/>
+    -->
+    <subsystem name="attitude" type="dcm"/>
+
+
     <subsystem name="radio_control" type="ppm"/>
 
     <!-- Communication -->
@@ -224,4 +309,10 @@
 
 </firmware>
 
+ <makefile>
+   ap.srcs +=  $(SRC_SUBSYSTEMS)/imu.c
+
+ap.CFLAGS +=    -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446
+ </makefile>
+
 </airframe>
diff --git a/conf/modules/ins_ppzuavimu.xml b/conf/modules/ins_ppzuavimu.xml
index d3c02614213..b6779b0f053 100644
--- a/conf/modules/ins_ppzuavimu.xml
+++ b/conf/modules/ins_ppzuavimu.xml
@@ -9,7 +9,6 @@
   <periodic fun="ppzuavimu_module_periodic()" freq="60"/>
   <event fun="ppzuavimu_module_event()"/>
   <makefile>
-    <define name="AHRS_TYPE_H" value="\\\"modules/ins/ins_ppzuavimu.h\\\"" />
     <define name="USE_I2C" />
     <file name="ins_ppzuavimu.c"/>
     <define name="PPZUAVIMU_I2C_DEVICE" value="i2c0" />
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c
index 602c50d7bef..3da102b6be2 100644
--- a/sw/airborne/modules/ins/ins_ppzuavimu.c
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.c
@@ -38,21 +38,15 @@
 #include "../../peripherals/adxl345.h"
 #include "../../peripherals/hmc5843.h"
 
-// INS
-#include "ins_module.h"
-
-INS_FORMAT ins_roll_neutral;
-INS_FORMAT ins_pitch_neutral;
-
 // Results
 int32_t mag_x, mag_y, mag_z;
-bool_t mag_valid;
+volatile bool_t mag_valid;
 
 int32_t gyr_x, gyr_y, gyr_z;
-bool_t gyr_valid;
+volatile bool_t gyr_valid;
 
 int32_t acc_x, acc_y, acc_z;
-bool_t acc_valid;
+volatile bool_t acc_valid;
 
 
 // Communication
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.h b/sw/airborne/modules/ins/ins_ppzuavimu.h
index f528076c71a..82d0bed4ca0 100644
--- a/sw/airborne/modules/ins/ins_ppzuavimu.h
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.h
@@ -22,15 +22,40 @@
 #define PPZUAVIMU_H
 
 #include "std.h"
-// pitch/roll neutrals
-#include "ins_module.h"
+#include "subsystems/imu.h"
 
 extern int32_t mag_x, mag_y, mag_z;
 extern int32_t gyr_x, gyr_y, gyr_z;
 extern int32_t acc_x, acc_y, acc_z;
 
+extern volatile bool_t gyr_valid;
+extern volatile bool_t acc_valid;
+extern volatile bool_t mag_valid;
+
+
 extern void ppzuavimu_module_init( void );
 extern void ppzuavimu_module_periodic( void );
 extern void ppzuavimu_module_event( void );
 
+
+extern volatile bool_t analog_imu_available;
+extern int imu_overrun;
+
+#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) {   \
+    if (gyr_valid) {                         \
+      gyr_valid = FALSE;                     \
+      _gyro_handler();                                  \
+    }                                                   \
+    if (acc_valid) {                         \
+      acc_valid = FALSE;                     \
+      _accel_handler();                                 \
+    }                                                   \
+    if (mag_valid) {                         \
+      mag_valid = FALSE;                     \
+      _mag_handler();                                 \
+    }                                                   \
+  }
+
+
+
 #endif // PPZUAVIMU_H

From 737d8b159203dd0ade384983af7c75423193a9b8 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 5 May 2011 21:35:45 +0200
Subject: [PATCH 57/99] PPZIMU calibrated and integrated with AHRS code

---
 .../PPZUAV/fixed-wing/ppzimu_tiny.xml         | 126 +++++-------------
 conf/modules/ins_ppzuavimu.xml                |   5 +-
 sw/airborne/firmwares/fixedwing/main_ap.c     |  12 +-
 sw/airborne/modules/ins/ins_ppzuavimu.c       |  26 +++-
 sw/airborne/modules/ins/ins_ppzuavimu.h       |  11 +-
 sw/airborne/subsystems/ahrs/ahrs_float_dcm.c  |   4 +-
 sw/airborne/subsystems/ahrs/ahrs_float_dcm.h  |   2 +
 7 files changed, 74 insertions(+), 112 deletions(-)

diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
index 8628daf58ff..d60dfa5734d 100644
--- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -7,11 +7,11 @@
 <airframe name="Yapa v1">
 
   <servos>
-    <servo name="THROTTLE" no="0" min="1000" neutral="1000" max="2000"/>
-    <servo name="AILERON_LEFT" no="2" min="1000" neutral="1500" max="2000"/>
-    <servo name="AILERON_RIGHT" no="6" min="2000" neutral="1500" max="1000"/>
-    <servo name="ELEVATOR" no="3" min="2000" neutral="1500" max="1000"/>
-    <servo name="RUDDER" no="4" min="1100" neutral="1500" max="1900"/>
+    <servo name="THROTTLE" no="4" min="1000" neutral="1000" max="2000"/>
+    <servo name="AILERON_LEFT" no="5" min="1000" neutral="1500" max="2000"/>
+    <servo name="AILERON_RIGHT" no="8" min="2000" neutral="1500" max="1000"/>
+    <servo name="ELEVATOR" no="1" min="2000" neutral="1500" max="1000"/>
+    <servo name="RUDDER" no="9" min="1100" neutral="1500" max="1900"/>
   </servos>
 
   <commands>
@@ -28,80 +28,30 @@
     <set command="BRAKE" value="@YAW"/>
   </rc_commands>
 
-  <section name="SERVO_MIXER_GAINS">
-    <define name="AILERON_RATE_UP" value="0.50f"/>
-    <define name="AILERON_RATE_DOWN" value="0.25f"/>
-
-    <define name="AILERON_RATE_UP_BRAKE" value="0.5f"/>
-    <define name="AILERON_RATE_DOWN_BRAKE" value="0.9f"/>
-
-    <define name="PITCH_GAIN" value="0.9f"/>
-
-    <define name="YAW_THRUST" value="0.0f"/>
-    <define name="BRAKE_AILEVON" value="-0.68f"/>
-    <define name="BRAKE_PITCH" value="0.0f"/>
-    <define name="MAX_BRAKE_RATE" value="150"/>
-
-    <define name="RATELIMIT(X,RATE)" value="( _rate_limited_value + Chop( (X) - _rate_limited_value, -(RATE), (RATE)  )); \
-       int16_t _rate_limited_value = 0;"/>
-
-</section>
-
   <command_laws>
-    <!-- Brake Rate Limiter -->
-    <let var="brake_value" value="Chop(-@BRAKE, 0, MAX_PPRZ)"/>
-    <!--<let var="brake_value" value="RATELIMIT(  $brake_value , MAX_BRAKE_RATE )"/>
-
-    <let var="test; \
-  	      static int16_t _var_brake_value = 0; \
-              _var_brake_value += LIMIT(_var_brake_value_nofilt - _var_brake_value,-MAX_BRAKE_RATE,MAX_BRAKE_RATE); \
-              int verwaarloos_deze_warning_CDW" value="0"/>
-		-->
-
-    <!-- Differential Aileron Depending on Brake Value -->
-    <let var="aileron_up_rate" value="(AILERON_RATE_UP   * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_UP_BRAKE   * $brake_value)"/>
-    <let var="aileron_down_rate" value="(AILERON_RATE_DOWN * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_DOWN_BRAKE * $brake_value)"/>
-    <let var="aileron_up" value="(@ROLL  *  (((float)$aileron_up_rate) / ((float)MAX_PPRZ)))"/>
-    <let var="aileron_down" value="(@ROLL  * (((float)$aileron_down_rate) / ((float)MAX_PPRZ)))"/>
-    <let var="leftturn" value="(@ROLL >= 0? 1 : 0)"/>
-    <let var="rightturn" value="(1 - $leftturn)"/>
-    <set servo="AILERON_LEFT" value="($aileron_up * $leftturn) + ($aileron_down * $rightturn) - $brake_value*(BRAKE_AILEVON)"/>
-    <set servo="AILERON_RIGHT" value="($aileron_up * $rightturn) + ($aileron_down * $leftturn) + $brake_value*(BRAKE_AILEVON)"/>
-
-    <!-- Differential Thurst -->
+    <set servo="AILERON_LEFT" value="@ROLL"/>
+    <set servo="AILERON_RIGHT" value="@ROLL"/>
     <set servo="THROTTLE" value="@THROTTLE"/>
-
-    <!-- Pitch with Brake-Trim Function -->
-    <set servo="ELEVATOR" value="-@PITCH * PITCH_GAIN + BRAKE_PITCH * $brake_value"/>
+    <set servo="ELEVATOR" value="@PITCH"/>
   </command_laws>
 
- <!-- Local magnetic field -->
- <section name="AHRS" prefix="AHRS_" >
-   <define name="H_X" value="0.51562740288882" />
-   <define name="H_Y" value="-0.05707735220832" />
-   <define name="H_Z" value="0.85490967783446" />
- </section>
-
+  <!-- Local magnetic field -->
+  <section name="AHRS" prefix="AHRS_" >
+    <define name="H_X" value="0.51562740288882" />
+    <define name="H_Y" value="-0.05707735220832" />
+    <define name="H_Z" value="0.85490967783446" />
+  </section>
 
   <section name="IMU" prefix="IMU_">
-    <!-- MAX1168 ADC CHANNELS
-    <define name="GYRO_P_CHAN" value="0"/>
-    <define name="GYRO_Q_CHAN" value="1"/>
-    <define name="GYRO_R_CHAN" value="2"/>
-
-    <define name="ACCEL_X_CHAN" value="4"/>
-    <define name="ACCEL_Y_CHAN" value="3"/>
-    <define name="ACCEL_Z_CHAN" value="5"/>
-    -->
     <!-- Calibration Neutral -->
-    <define name="GYRO_P_NEUTRAL" value="0"/>
-    <define name="GYRO_Q_NEUTRAL" value="0"/>
-    <define name="GYRO_R_NEUTRAL" value="0"/>
+    <define name="GYRO_P_NEUTRAL" value="-7"/>
+    <define name="GYRO_Q_NEUTRAL" value="-77"/>
+    <define name="GYRO_R_NEUTRAL" value="-23"/>
 
-    <!-- SENS = 2mV/deg/sec * 57.6 deg/rad = 114.59 mV/rad/sec * 16 LSB/mV = 1833.465 LSB/rad/sec / 12bit FRAC -->
-    <define name="GYRO_P_SENS" value="2.234" integer="16"/>
-    <define name="GYRO_Q_SENS" value="2.234" integer="16"/>
-    <define name="GYRO_R_SENS" value="2.234" integer="16"/>
+    <!-- SENS = 14.375 LSB/(deg/sec) * 57.6 deg/rad = 828 LSB/rad/sec / 12bit FRAC: 4096 / 828 -->
+    <define name="GYRO_P_SENS" value="4.947" integer="16"/>
+    <define name="GYRO_Q_SENS" value="4.947" integer="16"/>
+    <define name="GYRO_R_SENS" value="4.947" integer="16"/>
 
     <define name="GYRO_P_Q" value="0."/>
     <define name="GYRO_P_R" value="0"/>
@@ -110,21 +60,21 @@
     <define name="GYRO_R_P" value="0."/>
     <define name="GYRO_R_Q" value="0."/>
 
-    <define name="GYRO_P_SIGN" value="1"/>
-    <define name="GYRO_Q_SIGN" value="-1"/>
-    <define name="GYRO_R_SIGN" value="-1"/>
+    <define name="GYRO_P_SIGN" value="-1"/>
+    <define name="GYRO_Q_SIGN" value="1"/>
+    <define name="GYRO_R_SIGN" value="1"/>
 
-    <define name="ACCEL_X_NEUTRAL" value="0"/>
-    <define name="ACCEL_Y_NEUTRAL" value="0"/>
+    <define name="ACCEL_X_NEUTRAL" value="12"/>
+    <define name="ACCEL_Y_NEUTRAL" value="2"/>
     <define name="ACCEL_Z_NEUTRAL" value="0"/>
 
-    <!-- SENS = 660mV/g / 9.81 ms2/g = 67.27 mV/ms2 * 16 LSB/mV = 1076 LSB/ms2 / 10bit FRAC -->
-    <define name="ACCEL_X_SENS" value="0.9372" integer="16"/>
-    <define name="ACCEL_Y_SENS" value="0.9346" integer="16"/>
-    <define name="ACCEL_Z_SENS" value="0.9178" integer="16"/>
+    <!-- SENS = 256 LSB/g @ 2.5V [X&Y: 265 LSB/g @ 3.3V] / 9.81 ms2/g = 26.095 LSB/ms2 / 10bit FRAC: 1024 / 26.095 for z and 1024 / 27.01 for X&Y -->
+    <define name="ACCEL_X_SENS" value="37.9" integer="16"/>
+    <define name="ACCEL_Y_SENS" value="37.9" integer="16"/>
+    <define name="ACCEL_Z_SENS" value="39.24" integer="16"/>
 
     <define name="ACCEL_X_SIGN" value="1"/>
-    <define name="ACCEL_Y_SIGN" value="1"/>
+    <define name="ACCEL_Y_SIGN" value="-1"/>
     <define name="ACCEL_Z_SIGN" value="1"/>
 
     <define name="MAG_X_NEUTRAL" value="0"/>
@@ -135,8 +85,6 @@
     <define name="MAG_Y_SENS" value="1" integer="16"/>
     <define name="MAG_Z_SENS" value="1" integer="16"/>
 
-<!--    <define name="MAG_45_HACK" value="1"/> -->
-
     <define name="BODY_TO_IMU_PHI" value="0"/>
     <define name="BODY_TO_IMU_THETA" value="0"/>
     <define name="BODY_TO_IMU_PSI" value="0"/>
@@ -270,14 +218,12 @@
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
 
-      <!--
-      <configure name="PERIODIC_FREQUENCY" value="960"/>
-      <configure name="AHRS_PROPAGATE_FREQUENCY" value="240"/>
+      <configure name="PERIODIC_FREQUENCY" value="60"/>
+      <configure name="AHRS_PROPAGATE_FREQUENCY" value="60"/>
       <configure name="AHRS_CORRECT_FREQUENCY" value="60"/>
-      <configure name="AHRS_ALIGNER_LED" value="1"/>
-      -->
 
-      <configure name="CPU_LED" value="2"/>
+      <configure name="AHRS_ALIGNER_LED" value="3"/>
+      <configure name="CPU_LED" value="3"/>
     </target>
     <target name="sim" board="pc"/>
 
@@ -313,6 +259,8 @@
    ap.srcs +=  $(SRC_SUBSYSTEMS)/imu.c
 
 ap.CFLAGS +=    -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446
+ap.CFLAGS +=    -DIMU_TYPE_H=\"modules/ins/ins_ppzuavimu.h\"
+# -DOUTPUTMODE=2
  </makefile>
 
 </airframe>
diff --git a/conf/modules/ins_ppzuavimu.xml b/conf/modules/ins_ppzuavimu.xml
index b6779b0f053..9537dbe34db 100644
--- a/conf/modules/ins_ppzuavimu.xml
+++ b/conf/modules/ins_ppzuavimu.xml
@@ -5,13 +5,16 @@
   <header>
     <file name="ins_ppzuavimu.h"/>
   </header>
+
   <init fun="ppzuavimu_module_init()"/>
   <periodic fun="ppzuavimu_module_periodic()" freq="60"/>
+  <periodic fun="ppzuavimu_module_downlink_raw()" freq="5"/>
   <event fun="ppzuavimu_module_event()"/>
+
   <makefile>
-    <define name="USE_I2C" />
     <file name="ins_ppzuavimu.c"/>
     <define name="PPZUAVIMU_I2C_DEVICE" value="i2c0" />
+    <define name="USE_I2C" />
     <define name="USE_I2C0" />
   </makefile>
 </module>
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 0cd47e82c32..45fba324633 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -75,7 +75,7 @@
 #include "subsystems/ahrs.h"
 #include "subsystems/ahrs/ahrs_aligner.h"
 #include "subsystems/ahrs/ahrs_float_dcm.h"
-static inline void on_gyro_accel_event( void );
+static inline void on_gyro_event( void );
 static inline void on_accel_event( void );
 static inline void on_mag_event( void );
 #endif
@@ -398,7 +398,7 @@ static inline void attitude_loop( void ) {
       v_ctl_throttle_slew();
       ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
       ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint;
-      
+
       ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
 
 #if defined MCU_SPI_LINK
@@ -608,7 +608,7 @@ void event_task_ap( void ) {
 #endif
 
 #ifdef USE_AHRS
-  ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event);
+  ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
 #endif // USE_AHRS
 
 #ifdef USE_GPS
@@ -630,7 +630,7 @@ void event_task_ap( void ) {
   }
 
   modules_event_task();
-  
+
 #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
   if (new_ins_attitude > 0)
   {
@@ -639,7 +639,7 @@ void event_task_ap( void ) {
     new_ins_attitude = 0;
   }
 #endif
-  
+
 } /* event_task_ap() */
 
 
@@ -656,7 +656,7 @@ static inline void on_gps_solution( void ) {
 static inline void on_accel_event( void ) {
 }
 
-static inline void on_gyro_accel_event( void ) {
+static inline void on_gyro_event( void ) {
 
 #ifdef AHRS_CPU_LED
     LED_ON(AHRS_CPU_LED);
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c
index 3da102b6be2..73cf7ce5e77 100644
--- a/sw/airborne/modules/ins/ins_ppzuavimu.c
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.c
@@ -106,10 +106,10 @@ void ppzuavimu_module_init( void )
   i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
     while(ppzuavimu_adxl345.status == I2CTransPending);
 
-  /* Set range to 4g */
+  /* Set range to 16g but keeping full resolution of 3.9 mV/g */
   ppzuavimu_adxl345.type = I2CTransTx;
   ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_FORMAT;
-  ppzuavimu_adxl345.buf[1] = 1<<3 | 0<<2 | 0x01;  // bit 3 is full resolution bit, bit 2 is left justify bit 0,1 are range: 00=2g 01=4g 10=8g 11=16g
+  ppzuavimu_adxl345.buf[1] = 1<<3 | 0<<2 | 0x03;  // bit 3 is full resolution bit, bit 2 is left justify bit 0,1 are range: 00=2g 01=4g 10=8g 11=16g
   ppzuavimu_adxl345.len_w = 2;
   i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
     while(ppzuavimu_adxl345.status == I2CTransPending);
@@ -162,10 +162,13 @@ void ppzuavimu_module_periodic( void )
   ppzuavimu_hmc5843.len_w = 1;
   ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM;
   i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843);
+}
 
-  RunOnceEvery(6,DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&gyr_x,&gyr_y,&gyr_z));
-  RunOnceEvery(6,DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&acc_x,&acc_y,&acc_z));
-  RunOnceEvery(6,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z));
+void ppzuavimu_module_downlink_raw( void )
+{
+  DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&gyr_x,&gyr_y,&gyr_z);
+  DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&acc_x,&acc_y,&acc_z);
+  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z);
 }
 
 void ppzuavimu_module_event( void )
@@ -176,7 +179,15 @@ void ppzuavimu_module_event( void )
     gyr_x = (int16_t) ((ppzuavimu_itg3200.buf[0] << 8) | ppzuavimu_itg3200.buf[1]);
     gyr_y = (int16_t) ((ppzuavimu_itg3200.buf[2] << 8) | ppzuavimu_itg3200.buf[3]);
     gyr_z = (int16_t) ((ppzuavimu_itg3200.buf[4] << 8) | ppzuavimu_itg3200.buf[5]);
+
+#ifdef ASPIRIN_IMU
+    RATES_ASSIGN(imu.gyro_unscaled, gyr_x, gyr_y, gyr_z);
+#else
+    RATES_ASSIGN(imu.gyro_unscaled, gyr_x, gyr_y, gyr_z);
+#endif
+
     gyr_valid = TRUE;
+    ppzuavimu_itg3200.status = I2CTransDone;
   }
 
   // If the adxl345 I2C transaction has succeeded: convert the data
@@ -185,7 +196,11 @@ void ppzuavimu_module_event( void )
     acc_x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
     acc_y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
     acc_z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
+
+    VECT3_ASSIGN(imu.accel_unscaled, acc_x, acc_y, acc_z);
+
     acc_valid = TRUE;
+    ppzuavimu_adxl345.status = I2CTransDone;
   }
 
   // If the hmc5843 I2C transaction has succeeded: convert the data
@@ -195,5 +210,6 @@ void ppzuavimu_module_event( void )
     mag_y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
     mag_z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
     mag_valid = TRUE;
+    ppzuavimu_hmc5843.status = I2CTransDone;
   }
 }
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.h b/sw/airborne/modules/ins/ins_ppzuavimu.h
index 82d0bed4ca0..1da5a745d7b 100644
--- a/sw/airborne/modules/ins/ins_ppzuavimu.h
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.h
@@ -24,10 +24,6 @@
 #include "std.h"
 #include "subsystems/imu.h"
 
-extern int32_t mag_x, mag_y, mag_z;
-extern int32_t gyr_x, gyr_y, gyr_z;
-extern int32_t acc_x, acc_y, acc_z;
-
 extern volatile bool_t gyr_valid;
 extern volatile bool_t acc_valid;
 extern volatile bool_t mag_valid;
@@ -36,11 +32,9 @@ extern volatile bool_t mag_valid;
 extern void ppzuavimu_module_init( void );
 extern void ppzuavimu_module_periodic( void );
 extern void ppzuavimu_module_event( void );
+extern void ppzuavimu_module_downlink_raw( void );
 
 
-extern volatile bool_t analog_imu_available;
-extern int imu_overrun;
-
 #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) {   \
     if (gyr_valid) {                         \
       gyr_valid = FALSE;                     \
@@ -54,8 +48,7 @@ extern int imu_overrun;
       mag_valid = FALSE;                     \
       _mag_handler();                                 \
     }                                                   \
-  }
-
+}
 
 
 #endif // PPZUAVIMU_H
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
index 04e30d69db6..62a5e78f5b7 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
@@ -113,7 +113,7 @@ void ahrs_update_fw_estimator( void )
   estimator_psi   = ahrs_float.ltp_to_imu_euler.psi;
 
   estimator_p = Omega_Vector[0];
-
+/*
   RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel,
     &(DCM_Matrix[0][0]),
     &(DCM_Matrix[0][1]),
@@ -128,7 +128,7 @@ void ahrs_update_fw_estimator( void )
     &(DCM_Matrix[2][2])
 
   ));
-
+*/
 }
 
 
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
index ea54f0c1d45..f731cf5a71d 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
@@ -53,7 +53,9 @@ void ahrs_update_fw_estimator(void);
 #define GRAVITY 9.81
 
 
+#ifndef OUTPUTMODE
 #define OUTPUTMODE 1
+#endif
 // Mode 0 = DCM integration without Ki gyro bias
 // Mode 1 = DCM integration with Kp and Ki
 // Mode 2 = direct accelerometer -> euler

From 769efc2bf74b3de26787c18cb34c3f9e07e8d5b5 Mon Sep 17 00:00:00 2001
From: Piotr Esden-Tempski <piotr@esden.net>
Date: Thu, 5 May 2011 15:20:09 -0700
Subject: [PATCH 58/99] Added motor command output step binding in supervision.

---
 .../rotorcraft/actuators/supervision.c        | 39 +++++++++++++++++++
 1 file changed, 39 insertions(+)

diff --git a/sw/airborne/firmwares/rotorcraft/actuators/supervision.c b/sw/airborne/firmwares/rotorcraft/actuators/supervision.c
index 012b2402a0a..b158fae8255 100644
--- a/sw/airborne/firmwares/rotorcraft/actuators/supervision.c
+++ b/sw/airborne/firmwares/rotorcraft/actuators/supervision.c
@@ -38,6 +38,19 @@
 #define SUPERVISION_MIN_MOTOR_STARTUP SUPERVISION_MIN_MOTOR
 #endif
 
+#if defined (SUPERVISION_MAX_NEGATIVE_MOTOR_STEP) || defined (SUPERVISION_MAX_POSITIVE_MOTOR_STEP)
+#define SUPERVISION_USE_MAX_MOTOR_STEP_BINDING
+
+#ifndef SUPERVISION_MAX_NEGATIVE_MOTOR_STEP
+#define SUPERVISION_MAX_NEGATIVE_MOTOR_STEP INT32_MIN
+#endif
+/*
+#ifndef SUPERVISION_MAX_POSITIVE_MOTOR_STEP
+#define SUPERVISION_MAX_POSITIVE_MOTOR_STEP INT32_MAX
+#endif
+*/
+#endif
+
 static const int32_t roll_coef[SUPERVISION_NB_MOTOR]   = SUPERVISION_ROLL_COEF;
 static const int32_t pitch_coef[SUPERVISION_NB_MOTOR]  = SUPERVISION_PITCH_COEF;
 static const int32_t yaw_coef[SUPERVISION_NB_MOTOR]    = SUPERVISION_YAW_COEF;
@@ -72,6 +85,31 @@ __attribute__ ((always_inline)) static inline void bound_commands(void) {
           SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR);
 }
 
+#ifdef SUPERVISION_USE_MAX_MOTOR_STEP_BINDING
+__attribute__ ((always_inline)) static inline void bound_commands_step(void) {
+  uint8_t j;
+  static int32_t prev_commands[SUPERVISION_NB_MOTOR];
+  static uint8_t initialized = 0;
+
+  if (initialized == 1) {
+    for (j=0; j<SUPERVISION_NB_MOTOR; j++) {
+      int32_t new_command_diff = supervision.commands[j] - prev_commands[j];
+      Bound(new_command_diff,
+            SUPERVISION_MAX_NEGATIVE_MOTOR_STEP, SUPERVISION_MAX_POSITIVE_MOTOR_STEP);
+      supervision.commands[j] = prev_commands[j] + new_command_diff;
+    }
+  }else{
+    initialized = 1;
+  }
+
+  for (j=0; j<SUPERVISION_NB_MOTOR; j++)
+    prev_commands[j] = supervision.commands[j];
+}
+#else
+__attribute__ ((always_inline)) static inline void bound_commands_step(void) {
+}
+#endif
+
 void supervision_run_spinup(uint32_t counter, uint32_t max_counter)
 {
   int i;
@@ -126,6 +164,7 @@ void supervision_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[] ) {
       }
     }
     bound_commands();
+    bound_commands_step();
   }
   else
     for (i=0; i<SUPERVISION_NB_MOTOR; i++)

From c983ab57427c34f5989aeecc5331fc77c61e4bef Mon Sep 17 00:00:00 2001
From: Piotr Esden-Tempski <piotr@esden.net>
Date: Thu, 5 May 2011 15:24:29 -0700
Subject: [PATCH 59/99] Added dummy motor step binding value for supervision.

---
 conf/airframes/esden/lisa_pwm_aspirin.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml
index b4869e722a2..34491a043a3 100644
--- a/conf/airframes/esden/lisa_pwm_aspirin.xml
+++ b/conf/airframes/esden/lisa_pwm_aspirin.xml
@@ -49,6 +49,7 @@
     <define name="PITCH_COEF"  value="{  256, -256,    0,    0 }"/>
     <define name="YAW_COEF"    value="{ -256, -256,  256,  256 }"/>
     <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
+    <!--define name="MAX_POSITIVE_MOTOR_STEP" value="5"/-->
   </section>
 
   <include href="conf/airframes/esden/calib/aspirin_012.xml"/>

From 5595c6d7899250070d6a6823267a6cea7922e375 Mon Sep 17 00:00:00 2001
From: Piotr Esden-Tempski <piotr@esden.net>
Date: Thu, 5 May 2011 15:27:17 -0700
Subject: [PATCH 60/99] Added new synerani airframe, (just a copy of lisa pwm
 aspirin for now).

---
 conf/airframes/esden/synerani_4B.xml | 246 +++++++++++++++++++++++++++
 1 file changed, 246 insertions(+)
 create mode 100644 conf/airframes/esden/synerani_4B.xml

diff --git a/conf/airframes/esden/synerani_4B.xml b/conf/airframes/esden/synerani_4B.xml
new file mode 100644
index 00000000000..9dcd7df4446
--- /dev/null
+++ b/conf/airframes/esden/synerani_4B.xml
@@ -0,0 +1,246 @@
+<!-- synerani H frame with pwm motor controllers Lisa/L and aspirin -->
+
+<airframe name="synerani_4b">
+
+  <servos min="0" neutral="0" max="0xff">
+    <servo name="FRONT"   no="0" min="1000" neutral="1000" max="2000"/>
+    <servo name="BACK"    no="1" min="1000" neutral="1000" max="2000"/>
+    <servo name="LEFT"    no="2" min="1000" neutral="1000" max="2000"/>
+    <servo name="RIGHT"   no="3" min="1000" neutral="1000" max="2000"/>
+  </servos>
+
+  <commands>
+    <axis name="PITCH"  failsafe_value="0"/>
+    <axis name="ROLL"   failsafe_value="0"/>
+    <axis name="YAW"    failsafe_value="0"/>
+    <axis name="THRUST" failsafe_value="0"/>
+  </commands>
+
+  <command_laws>
+    <!--<let var="aileron"  value="@ROLL  * AILEVON_AILERON_RATE"/>
+    <let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
+    <set servo="MOTOR"         value="@THROTTLE"/>
+    <set servo="AILEVON_LEFT"  value="$elevator + $aileron"/>
+    <set servo="AILEVON_RIGHT" value="$elevator - $aileron"/>
+    -->
+    <set servo="FRONT" value="0"/>
+    <set servo="BACK" value="1"/>
+    <set servo="LEFT" value="2"/>
+    <set servo="RIGHT" value="3"/>
+
+  </command_laws>
+
+  <!-- for the sim -->
+  <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
+    <define name="NB" value="4"/>
+    <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
+  </section>
+
+
+  <section name="SUPERVISION" prefix="SUPERVISION_">
+    <define name="MIN_MOTOR" value="1150"/>
+    <define name="MAX_MOTOR" value="2000"/>
+    <define name="TRIM_A" value="0"/>
+    <define name="TRIM_E" value="0"/>
+    <define name="TRIM_R" value="0"/>
+    <define name="NB_MOTOR" value="4"/>
+    <define name="SCALE" value="256"/>
+    <define name="ROLL_COEF"   value="{  0  ,    0,  256, -256 }"/>
+    <define name="PITCH_COEF"  value="{  256, -256,    0,    0 }"/>
+    <define name="YAW_COEF"    value="{ -256, -256,  256,  256 }"/>
+    <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
+    <!--define name="MAX_POSITIVE_MOTOR_STEP" value="5"/-->
+  </section>
+
+  <include href="conf/airframes/esden/calib/aspirin_012.xml"/>
+
+  <section name="IMU" prefix="IMU_">
+    
+    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(   0. )"/>
+    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(   0. )"/>
+    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg( 225. )"/>
+
+  </section>
+
+ <section name="AUTOPILOT">
+   <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
+   <define name="MODE_AUTO1"  value="AP_MODE_ATTITUDE_RC_CLIMB"/>
+   <define name="MODE_AUTO2"  value="AP_MODE_HOVER_Z_HOLD"/>
+ </section>
+
+ <section name="BAT">
+   <define name="MILLIAMP_PER_PERCENT" value="0.86"/>
+   <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
+   <define name="BATTERY_SENS"   value="0.48" integer="16"/>
+ </section>
+
+
+  <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
+
+    <define name="SP_MAX_P" value="10000"/>
+    <define name="SP_MAX_Q" value="10000"/>
+    <define name="SP_MAX_R" value="10000"/>
+
+    <define name="GAIN_P" value="-400"/>
+    <define name="GAIN_Q" value="-400"/>
+    <define name="GAIN_R" value="-350"/>
+
+  </section>
+
+  <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
+
+    <!-- setpoints -->
+    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
+    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
+    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="DEADBAND_R"     value="250"/>
+
+    <!-- reference -->
+    <define name="REF_OMEGA_P"  value="RadOfDeg(600)"/>
+    <define name="REF_ZETA_P"   value="0.85"/>
+    <define name="REF_MAX_P"    value="RadOfDeg(400.)"/>
+    <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
+
+    <define name="REF_OMEGA_Q"  value="RadOfDeg(600)"/>
+    <define name="REF_ZETA_Q"   value="0.85"/>
+    <define name="REF_MAX_Q"    value="RadOfDeg(400.)"/>
+    <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
+
+    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_ZETA_R"   value="0.85"/>
+    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
+
+    <!-- feedback -->
+    <define name="PHI_PGAIN"  value="-1900"/>
+    <define name="PHI_DGAIN"  value="-380"/>
+    <define name="PHI_IGAIN"  value="-200"/>
+
+    <define name="THETA_PGAIN"  value="-1900"/>
+    <define name="THETA_DGAIN"  value="-380"/>
+    <define name="THETA_IGAIN"  value="-200"/>
+
+    <define name="PSI_PGAIN"  value="-2000"/>
+    <define name="PSI_DGAIN"  value="-400"/>
+    <define name="PSI_IGAIN"  value="-10"/>
+
+    <!-- feedforward -->
+    <define name="PHI_DDGAIN"   value=" 300"/>
+    <define name="THETA_DDGAIN" value=" 300"/>
+    <define name="PSI_DDGAIN"   value=" 300"/>
+
+  </section>
+
+  <section name="AHRS" prefix="AHRS_">
+    <define name="PROPAGATE_FREQUENCY" value="512"/>
+    <define name="H_X" value=" 0.51562740288882"/>
+    <define name="H_Y" value="-0.05707735220832"/>
+    <define name="H_Z" value=" 0.85490967783446"/>
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="BARO_SENS" value="10." integer="16"/>
+  </section>
+
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
+   <define name="MIN_ERR_Z"   value="POS_BFP_OF_REAL(-10.)"/>
+   <define name="MAX_ERR_Z"   value="POS_BFP_OF_REAL( 10.)"/>
+   <define name="MIN_ERR_ZD"  value="SPEED_BFP_OF_REAL(-10.)"/>
+   <define name="MAX_ERR_ZD"  value="SPEED_BFP_OF_REAL( 10.)"/>
+   <define name="MAX_SUM_ERR" value="2000000"/>
+   <define name="HOVER_KP"    value="-150"/>
+   <define name="HOVER_KD"    value="-80"/>
+   <!--define name="HOVER_KI"    value="-100"/-->
+   <define name="HOVER_KI"    value="0"/>
+   <!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
+   <define name="RC_CLIMB_COEF" value ="163"/>
+   <!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
+   <define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
+   <define name="INV_M" value ="0.15"/>
+  </section>
+
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
+   <define name="PGAIN" value="-100"/>
+   <define name="DGAIN" value="-100"/>
+   <define name="IGAIN" value="-0"/>
+ </section>
+
+ <section name="MISC">
+   <define name="FACE_REINJ_1"  value="1024"/>
+ </section>
+
+ <section name="SIMULATOR" prefix="NPS_">
+   <define name="ACTUATOR_NAMES"  value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
+   <define name="INITIAL_CONDITITONS" value="&quot;reset00&quot;"/>
+   <define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_booz2_a1.h&quot;"/>
+ </section>
+
+<!-- -->
+ <modules main_freq="512">
+   <load name="vehicle_interface_overo_link.xml"/>
+ </modules>
+<!-- -->
+
+  <firmware name="rotorcraft">
+    <target name="ap" board="lisa_l_1.1">
+<!--      <define name="BOOZ_START_DELAY" value="1"/> -->
+      <subsystem name="radio_control" type="spektrum"/>
+      <subsystem name="actuators"     type="pwm_supervision"/>
+      <subsystem name="telemetry"     type="transparent"/>
+      <define name="SERVO_HZ" value="400"/>
+      <define name="RADIO_MODE" value="RADIO_AUX2"/>
+      <define name="RADIO_KILL_SWITCH" value="RADIO_GEAR"/>
+      <define name="RADIO_CONTROL_SPEKTRUM_SIGNS" value="\{1,1,-1,1,-1,-1,-1,1,1,1,1,1\}"/>
+      <define name = "RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT" value = "UART5"/>
+      <define name = "OVERRIDE_UART5_IRQ_HANDLER"/>
+    </target>
+    <target name="sim" board="pc">
+      <subsystem name="fdm"           type="nps"/>
+      <subsystem name="radio_control" type="ppm"/>
+      <subsystem name="actuators"     type="mkk"/>
+    </target>
+
+    <subsystem name="imu"           type="aspirin"/>
+    <subsystem name="gps"           type="ublox">
+	    <configure name="GPS_BAUD"    value="B57600"/>
+    </subsystem>
+    <subsystem name="ahrs"          type="cmpl"/>
+    <!--<subsystem name="ahrs"          type="ic"/>-->
+  </firmware>
+
+  <firmware name="lisa_l_test_progs">
+    <target name="test_led"   board="lisa_l_1.1"/>
+    <target name="test_uart"   board="lisa_l_1.1"/>
+    <target name="test_servos"   board="lisa_l_1.1"/>
+    <target name="test_telemetry"   board="lisa_l_1.1"/>
+    <target name="test_baro"        board="lisa_l_1.1"/>
+    <target name="test_imu_b2"      board="lisa_l_1.1"/>
+    <target name="test_imu_b2_2"    board="lisa_l_1.1"/>
+    <target name="test_imu_aspirin" board="lisa_l_1.1"/>
+    <target name="test_rc_spektrum" board="lisa_l_1.1"/>
+    <target name="test_rc_ppm"      board="lisa_l_1.1"/>
+    <target name="test_adc"         board="lisa_l_1.1"/>
+    <target name="test_hmc5843"     board="lisa_l_1.1"/>
+    <target name="test_itg3200"     board="lisa_l_1.1"/>
+    <target name="test_adxl345"             board="lisa_l_1.1"/>
+    <target name="test_esc_mkk_simple"      board="lisa_l_1.1"/>
+    <target name="test_esc_asctecv1_simple" board="lisa_l_1.1"/>
+    <target name="test_actuators_mkk"       board="lisa_l_1.1"/>
+    <target name="test_actuators_asctecv1"  board="lisa_l_1.1"/>
+  </firmware>
+
+  <firmware name="lisa_passthrough">
+    <target name="overo_test_passthrough" board="lisa_l_1.1"  >
+      <param name="HOST"          value="A7"/>
+      <param name="USER"          value=""/>
+      <param name="TARGET_DIR"    value="~"/>
+      <param name="PERIODIC_FREQ" value="512"/>
+    </target>
+    <target name="stm_passthrough" board="lisa_l_1.1">
+      <subsystem name="radio_control" type="spektrum"/>
+      <subsystem name="imu" type="b2_v1.1"/>
+    </target>
+ </firmware>
+
+
+</airframe>

From e7a2c2e2b993b98aaa78a22b153f8dad20ef54da Mon Sep 17 00:00:00 2001
From: Piotr Esden-Tempski <piotr@esden.net>
Date: Thu, 5 May 2011 16:42:47 -0700
Subject: [PATCH 61/99] Changed estimator to ic, renamed servo actuators, set
 the body to imu rotations.

---
 conf/airframes/esden/synerani_4B.xml | 33 +++++++++++++++-------------
 1 file changed, 18 insertions(+), 15 deletions(-)

diff --git a/conf/airframes/esden/synerani_4B.xml b/conf/airframes/esden/synerani_4B.xml
index 9dcd7df4446..5146d71f5e2 100644
--- a/conf/airframes/esden/synerani_4B.xml
+++ b/conf/airframes/esden/synerani_4B.xml
@@ -3,10 +3,12 @@
 <airframe name="synerani_4b">
 
   <servos min="0" neutral="0" max="0xff">
-    <servo name="FRONT"   no="0" min="1000" neutral="1000" max="2000"/>
-    <servo name="BACK"    no="1" min="1000" neutral="1000" max="2000"/>
-    <servo name="LEFT"    no="2" min="1000" neutral="1000" max="2000"/>
-    <servo name="RIGHT"   no="3" min="1000" neutral="1000" max="2000"/>
+    <servo name="A1"            no="0" min="1000" neutral="1000" max="2000"/>
+    <servo name="A2"            no="1" min="1000" neutral="1000" max="2000"/>
+    <servo name="B1"            no="2" min="1000" neutral="1000" max="2000"/>
+    <servo name="B2"            no="3" min="1000" neutral="1000" max="2000"/>
+    <servo name="AILEVON_LEFT"  no="4" min="1000" neutral="1500" max="2000"/>
+    <servo name="AILEVON_RIGHT" no="5" min="1000" neutral="1500" max="2000"/>
   </servos>
 
   <commands>
@@ -15,20 +17,21 @@
     <axis name="YAW"    failsafe_value="0"/>
     <axis name="THRUST" failsafe_value="0"/>
   </commands>
-
+  
+  <!--
   <command_laws>
-    <!--<let var="aileron"  value="@ROLL  * AILEVON_AILERON_RATE"/>
+    <let var="aildderon"  value="@ROLL  * AILEVON_AILERON_RATE"/>
     <let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
     <set servo="MOTOR"         value="@THROTTLE"/>
     <set servo="AILEVON_LEFT"  value="$elevator + $aileron"/>
     <set servo="AILEVON_RIGHT" value="$elevator - $aileron"/>
-    -->
     <set servo="FRONT" value="0"/>
     <set servo="BACK" value="1"/>
     <set servo="LEFT" value="2"/>
     <set servo="RIGHT" value="3"/>
 
   </command_laws>
+  -->
 
   <!-- for the sim -->
   <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
@@ -45,9 +48,9 @@
     <define name="TRIM_R" value="0"/>
     <define name="NB_MOTOR" value="4"/>
     <define name="SCALE" value="256"/>
-    <define name="ROLL_COEF"   value="{  0  ,    0,  256, -256 }"/>
-    <define name="PITCH_COEF"  value="{  256, -256,    0,    0 }"/>
-    <define name="YAW_COEF"    value="{ -256, -256,  256,  256 }"/>
+    <define name="ROLL_COEF"   value="{  256, -256,  256, -256 }"/>
+    <define name="PITCH_COEF"  value="{ -256, -256,  256,  256 }"/>
+    <define name="YAW_COEF"    value="{    0,    0,    0,    0 }"/>
     <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
     <!--define name="MAX_POSITIVE_MOTOR_STEP" value="5"/-->
   </section>
@@ -56,9 +59,9 @@
 
   <section name="IMU" prefix="IMU_">
     
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg( 225. )"/>
+    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg( 180. )"/>
+    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(  90. )"/>
+    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(   0. )"/>
 
   </section>
 
@@ -204,8 +207,8 @@
     <subsystem name="gps"           type="ublox">
 	    <configure name="GPS_BAUD"    value="B57600"/>
     </subsystem>
-    <subsystem name="ahrs"          type="cmpl"/>
-    <!--<subsystem name="ahrs"          type="ic"/>-->
+    <!--subsystem name="ahrs"          type="cmpl"/-->
+    <subsystem name="ahrs"          type="ic"/>
   </firmware>
 
   <firmware name="lisa_l_test_progs">

From 9ca9e65d90a3f780ef1c0cc5bd991e5f678f2ee3 Mon Sep 17 00:00:00 2001
From: Piotr Esden-Tempski <piotr@esden.net>
Date: Thu, 5 May 2011 20:00:20 -0700
Subject: [PATCH 62/99] Switched to int comp estimator.

---
 conf/airframes/Poine/booz2_a7.xml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/conf/airframes/Poine/booz2_a7.xml b/conf/airframes/Poine/booz2_a7.xml
index 61428ed284e..213a7d95108 100644
--- a/conf/airframes/Poine/booz2_a7.xml
+++ b/conf/airframes/Poine/booz2_a7.xml
@@ -234,8 +234,8 @@
     <subsystem name="gps"           type="ublox">
       <configure name="GPS_BAUD"    value="B57600"/>
     </subsystem>
-    <subsystem name="ahrs"          type="cmpl"/>
- <!--     <subsystem name="ahrs"          type="ic"/>  -->
+    <!--subsystem name="ahrs"          type="cmpl"/-->
+    <subsystem name="ahrs"          type="ic"/>
     <subsystem name="ins"           type="hff"/>
   </firmware>
 

From 162005d918191a34f4c836545325cc3ea83fd27c Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 6 May 2011 10:36:57 +0200
Subject: [PATCH 63/99] Aspirin IMU now works with LPC2148 too using I2C only
 (intended for fixedwing)

---
 .../PPZUAV/fixed-wing/ppzimu_tiny.xml         | 19 ++++---
 sw/airborne/modules/ins/ins_ppzuavimu.c       | 54 +++++++++++--------
 2 files changed, 42 insertions(+), 31 deletions(-)

diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
index d60dfa5734d..0fd3bd0c2e9 100644
--- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -44,9 +44,9 @@
 
   <section name="IMU" prefix="IMU_">
     <!-- Calibration Neutral -->
-    <define name="GYRO_P_NEUTRAL" value="-7"/>
-    <define name="GYRO_Q_NEUTRAL" value="-77"/>
-    <define name="GYRO_R_NEUTRAL" value="-23"/>
+    <define name="GYRO_P_NEUTRAL" value="0"/>
+    <define name="GYRO_Q_NEUTRAL" value="0"/>
+    <define name="GYRO_R_NEUTRAL" value="0"/>
 
     <!-- SENS = 14.375 LSB/(deg/sec) * 57.6 deg/rad = 828 LSB/rad/sec / 12bit FRAC: 4096 / 828 -->
     <define name="GYRO_P_SENS" value="4.947" integer="16"/>
@@ -60,12 +60,12 @@
     <define name="GYRO_R_P" value="0."/>
     <define name="GYRO_R_Q" value="0."/>
 
-    <define name="GYRO_P_SIGN" value="-1"/>
+    <define name="GYRO_P_SIGN" value="1"/>
     <define name="GYRO_Q_SIGN" value="1"/>
     <define name="GYRO_R_SIGN" value="1"/>
 
-    <define name="ACCEL_X_NEUTRAL" value="12"/>
-    <define name="ACCEL_Y_NEUTRAL" value="2"/>
+    <define name="ACCEL_X_NEUTRAL" value="0"/>
+    <define name="ACCEL_Y_NEUTRAL" value="0"/>
     <define name="ACCEL_Z_NEUTRAL" value="0"/>
 
     <!-- SENS = 256 LSB/g @ 2.5V [X&Y: 265 LSB/g @ 3.3V] / 9.81 ms2/g = 26.095 LSB/ms2 / 10bit FRAC: 1024 / 26.095 for z and 1024 / 27.01 for X&Y -->
@@ -74,7 +74,7 @@
     <define name="ACCEL_Z_SENS" value="39.24" integer="16"/>
 
     <define name="ACCEL_X_SIGN" value="1"/>
-    <define name="ACCEL_Y_SIGN" value="-1"/>
+    <define name="ACCEL_Y_SIGN" value="1"/>
     <define name="ACCEL_Z_SIGN" value="1"/>
 
     <define name="MAG_X_NEUTRAL" value="0"/>
@@ -260,7 +260,10 @@
 
 ap.CFLAGS +=    -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446
 ap.CFLAGS +=    -DIMU_TYPE_H=\"modules/ins/ins_ppzuavimu.h\"
-# -DOUTPUTMODE=2
+
+# Test attitude using accelerometers only:
+# ap.CFLAGS +=   -DOUTPUTMODE=2
+# ap.CFLAGS +=   -DASPIRIN_IMU
  </makefile>
 
 </airframe>
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c
index 73cf7ce5e77..2f7394ab894 100644
--- a/sw/airborne/modules/ins/ins_ppzuavimu.c
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.c
@@ -39,16 +39,10 @@
 #include "../../peripherals/hmc5843.h"
 
 // Results
-int32_t mag_x, mag_y, mag_z;
 volatile bool_t mag_valid;
-
-int32_t gyr_x, gyr_y, gyr_z;
 volatile bool_t gyr_valid;
-
-int32_t acc_x, acc_y, acc_z;
 volatile bool_t acc_valid;
 
-
 // Communication
 struct i2c_transaction ppzuavimu_hmc5843;
 struct i2c_transaction ppzuavimu_itg3200;
@@ -166,38 +160,45 @@ void ppzuavimu_module_periodic( void )
 
 void ppzuavimu_module_downlink_raw( void )
 {
-  DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&gyr_x,&gyr_y,&gyr_z);
-  DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&acc_x,&acc_y,&acc_z);
-  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z);
+  DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);
+  DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);
+  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&imu.mag_unscaled.x,&imu.mag_unscaled.y,&imu.mag_unscaled.z);
 }
 
 void ppzuavimu_module_event( void )
 {
+  int32_t x, y, z;
+
   // If the itg3200 I2C transaction has succeeded: convert the data
   if (ppzuavimu_itg3200.status == I2CTransSuccess)
   {
-    gyr_x = (int16_t) ((ppzuavimu_itg3200.buf[0] << 8) | ppzuavimu_itg3200.buf[1]);
-    gyr_y = (int16_t) ((ppzuavimu_itg3200.buf[2] << 8) | ppzuavimu_itg3200.buf[3]);
-    gyr_z = (int16_t) ((ppzuavimu_itg3200.buf[4] << 8) | ppzuavimu_itg3200.buf[5]);
+    x = (int16_t) ((ppzuavimu_itg3200.buf[0] << 8) | ppzuavimu_itg3200.buf[1]);
+    y = (int16_t) ((ppzuavimu_itg3200.buf[2] << 8) | ppzuavimu_itg3200.buf[3]);
+    z = (int16_t) ((ppzuavimu_itg3200.buf[4] << 8) | ppzuavimu_itg3200.buf[5]);
 
+    // Signs depend on the way sensors are soldered on the board: so they are hardcoded
 #ifdef ASPIRIN_IMU
-    RATES_ASSIGN(imu.gyro_unscaled, gyr_x, gyr_y, gyr_z);
-#else
-    RATES_ASSIGN(imu.gyro_unscaled, gyr_x, gyr_y, gyr_z);
+    RATES_ASSIGN(imu.gyro_unscaled, x, -y, -z);
+#else // PPZIMU
+    RATES_ASSIGN(imu.gyro_unscaled, -x, y, -z);
 #endif
 
     gyr_valid = TRUE;
-    ppzuavimu_itg3200.status = I2CTransDone;
+    ppzuavimu_itg3200.status = I2CTransDone;  // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data
   }
 
   // If the adxl345 I2C transaction has succeeded: convert the data
   if (ppzuavimu_adxl345.status == I2CTransSuccess)
   {
-    acc_x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
-    acc_y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
-    acc_z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
+    x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
+    y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
+    z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
 
-    VECT3_ASSIGN(imu.accel_unscaled, acc_x, acc_y, acc_z);
+#ifdef ASPIRIN_IMU
+    VECT3_ASSIGN(imu.accel_unscaled, -x, y, z);
+#else // PPZIMU
+    VECT3_ASSIGN(imu.accel_unscaled, x, -y, z);
+#endif
 
     acc_valid = TRUE;
     ppzuavimu_adxl345.status = I2CTransDone;
@@ -206,9 +207,16 @@ void ppzuavimu_module_event( void )
   // If the hmc5843 I2C transaction has succeeded: convert the data
   if (ppzuavimu_hmc5843.status == I2CTransSuccess)
   {
-    mag_x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]);
-    mag_y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
-    mag_z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
+    x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]);
+    y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
+    z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
+
+#ifdef ASPIRIN_IMU
+    VECT3_ASSIGN(imu.mag_unscaled, x, y, z);
+#else // PPZIMU
+    VECT3_ASSIGN(imu.mag_unscaled, x, y, z);
+#endif
+
     mag_valid = TRUE;
     ppzuavimu_hmc5843.status = I2CTransDone;
   }

From 663196b10e8bcd59c651822d2fda87575214e2b4 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 6 May 2011 12:35:14 +0200
Subject: [PATCH 64/99] not include DCM directly: include AHRS_TYPE_H to allow
 other filters too

---
 sw/airborne/firmwares/fixedwing/main_ap.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 45fba324633..a99aa76e25c 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -74,7 +74,7 @@
 #ifdef USE_AHRS
 #include "subsystems/ahrs.h"
 #include "subsystems/ahrs/ahrs_aligner.h"
-#include "subsystems/ahrs/ahrs_float_dcm.h"
+#include AHRS_TYPE_H
 static inline void on_gyro_event( void );
 static inline void on_accel_event( void );
 static inline void on_mag_event( void );

From a1eac67a74facba51ff657478cfece1d1281990e Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 6 May 2011 14:00:37 +0200
Subject: [PATCH 65/99] Sign convention fix: stationary accelerometer measures
 [0 0 -9.81]

---
 conf/airframes/TU_Delft/MicrojetCDW.xml      | 16 ++++++++--------
 sw/airborne/modules/ins/ins_ppzuavimu.c      |  4 ++--
 sw/airborne/subsystems/ahrs/ahrs_float_dcm.c |  7 +++++++
 3 files changed, 17 insertions(+), 10 deletions(-)

diff --git a/conf/airframes/TU_Delft/MicrojetCDW.xml b/conf/airframes/TU_Delft/MicrojetCDW.xml
index 34d651058fe..baba44fc6f7 100644
--- a/conf/airframes/TU_Delft/MicrojetCDW.xml
+++ b/conf/airframes/TU_Delft/MicrojetCDW.xml
@@ -5,7 +5,7 @@
 <!-- commands section -->
   <servos>
     <servo name="THROTTLE" no="0" min="1050" neutral="1050" max="1900"/>
-    <servo name="AILEVON_RIGHT" no="1" min="1850" neutral="1480" max="1100"/> <!-- 400 - 380 --> 
+    <servo name="AILEVON_RIGHT" no="1" min="1850" neutral="1480" max="1100"/> <!-- 400 - 380 -->
     <servo name="AILEVON_LEFT" no="2" min="1250" neutral="1580" max="1980"/> <!-- 300 - 400 -->
   </servos>
 
@@ -28,7 +28,7 @@
 
   <command_laws>
     <let var="aileron" value="@ROLL  * AILEVON_AILERON_RATE"/>
-    <let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>  
+    <let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
 
     <set servo="THROTTLE" value="@THROTTLE"/>
     <set servo="AILEVON_LEFT" value="$elevator + $aileron"/>
@@ -42,7 +42,7 @@
 
 
   <section name="IMU" prefix="IMU_">
-    <!-- MAX1168 ADC CHANNELS --> 
+    <!-- MAX1168 ADC CHANNELS -->
     <define name="GYRO_P_CHAN" value="0"/>
     <define name="GYRO_Q_CHAN" value="1"/>
     <define name="GYRO_R_CHAN" value="2"/>
@@ -81,9 +81,9 @@
     <define name="ACCEL_Y_SENS" value="0.9346" integer="16"/>
     <define name="ACCEL_Z_SENS" value="0.9178" integer="16"/>
 
-    <define name="ACCEL_X_SIGN" value="-1"/>
-    <define name="ACCEL_Y_SIGN" value="1"/>
-    <define name="ACCEL_Z_SIGN" value="1"/>
+    <define name="ACCEL_X_SIGN" value="1"/>
+    <define name="ACCEL_Y_SIGN" value="-1"/>
+    <define name="ACCEL_Z_SIGN" value="-1"/>
 
     <define name="MAG_X_NEUTRAL" value="0"/>
     <define name="MAG_Y_NEUTRAL" value="0"/>
@@ -92,7 +92,7 @@
     <define name="MAG_X_SENS" value="1" integer="16"/>
     <define name="MAG_Y_SENS" value="1" integer="16"/>
     <define name="MAG_Z_SENS" value="1" integer="16"/>
-    
+
 <!--    <define name="MAG_45_HACK" value="1"/> -->
 
     <define name="BODY_TO_IMU_PHI" value="0"/>
@@ -161,7 +161,7 @@
     <define name="ROLL_ATTITUDE_GAIN" value="-6000."/>
     <define name="ROLL_RATE_GAIN" value="-600."/>
 
-    <define name="ROLL_SLEW" value="0.02"/> <!-- Maximal roll angle change per 1/60 of second: 0.02 rad/loop * 180/pi * 60 loop/sec = 60 deg/sec --> 
+    <define name="ROLL_SLEW" value="0.02"/> <!-- Maximal roll angle change per 1/60 of second: 0.02 rad/loop * 180/pi * 60 loop/sec = 60 deg/sec -->
   </section>
 
   <section name="AGGRESSIVE" prefix="AGR_">
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c
index 2f7394ab894..4f5d5ae9ea6 100644
--- a/sw/airborne/modules/ins/ins_ppzuavimu.c
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.c
@@ -195,9 +195,9 @@ void ppzuavimu_module_event( void )
     z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
 
 #ifdef ASPIRIN_IMU
-    VECT3_ASSIGN(imu.accel_unscaled, -x, y, z);
+    VECT3_ASSIGN(imu.accel_unscaled, x, -y, -z);
 #else // PPZIMU
-    VECT3_ASSIGN(imu.accel_unscaled, x, -y, z);
+    VECT3_ASSIGN(imu.accel_unscaled, -x, y, -z);
 #endif
 
     acc_valid = TRUE;
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
index 62a5e78f5b7..788acb1e8f8 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
@@ -213,6 +213,13 @@ void ahrs_update_accel(void)
 
   ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
 
+  // DCM filter uses g-force as positive
+  // accelerometer measures [0 0 -g] in a static case
+
+  accel_float.x = -accel_float.x;
+  accel_float.y = -accel_float.y;
+  accel_float.z = -accel_float.z;
+
 #ifdef USE_GPS
   if (gps.fix == GPS_FIX_3D) {    //Remove centrifugal acceleration.
     accel_float.y += gps.speed_3d/100. * Omega[2];  // Centrifugal force on Acc_y = GPS_speed*GyroZ

From ce328a978666ddc15f988f4c96b8d725a753b031 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 6 May 2011 14:38:15 +0200
Subject: [PATCH 66/99] Integer quaternion complementary attitude filter

---
 .../PPZUAV/fixed-wing/ppzimu_tiny.xml         |  3 +-
 .../subsystems/shared/ahrs_ic.makefile        | 27 ++++++++++--
 sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c   | 41 +++++++++++++------
 sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h   |  5 +++
 4 files changed, 59 insertions(+), 17 deletions(-)

diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
index 0fd3bd0c2e9..1b4a1040a06 100644
--- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -234,8 +234,9 @@
     <!-- Sensors -->
     <!--
     <subsystem name="imu" type="booz"/>
-    -->
     <subsystem name="attitude" type="dcm"/>
+    -->
+    <subsystem name="ahrs" type="ic"/>
 
 
     <subsystem name="radio_control" type="ppm"/>
diff --git a/conf/autopilot/subsystems/shared/ahrs_ic.makefile b/conf/autopilot/subsystems/shared/ahrs_ic.makefile
index bda8b08365d..3c3feb18fa9 100644
--- a/conf/autopilot/subsystems/shared/ahrs_ic.makefile
+++ b/conf/autopilot/subsystems/shared/ahrs_ic.makefile
@@ -10,9 +10,10 @@ ifdef AHRS_ALIGNER_LED
 AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
 AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl.h\"
-AHRS_SRCS   += subsystems/ahrs.c 
-AHRS_SRCS   += subsystems/ahrs/ahrs_int_cmpl.c 
-AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c 
+AHRS_SRCS   += subsystems/ahrs.c
+AHRS_SRCS   += subsystems/ahrs/ahrs_int_cmpl.c
+AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c
+AHRS_SRCS   += math/pprz_trig_int.c
 
 ap.CFLAGS += $(AHRS_CFLAGS)
 ap.srcs += $(AHRS_SRCS)
@@ -20,3 +21,23 @@ ap.srcs += $(AHRS_SRCS)
 sim.CFLAGS += $(AHRS_CFLAGS)
 sim.srcs += $(AHRS_SRCS)
 
+
+# Extra stuff for fixedwings
+
+ifdef CPU_LED
+  ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
+endif
+
+ifdef AHRS_PROPAGATE_FREQUENCY
+else
+  AHRS_PROPAGATE_FREQUENCY = 60
+endif
+
+ifdef AHRS_CORRECT_FREQUENCY
+else
+  AHRS_CORRECT_FREQUENCY = 60
+endif
+
+ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY)
+ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
+
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c
index a74f631cb38..d27af66705d 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c
@@ -100,7 +100,7 @@ void ahrs_init(void) {
 
 void ahrs_align(void) {
 
-  
+
   /* Compute an initial orientation using euler angles */
   ahrs_int_get_euler_from_accel_mag(&ahrs.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
   /* Convert initial orientation in quaternion and rotation matrice representations. */
@@ -215,12 +215,12 @@ void ahrs_update_mag(void) {
 
 
 static inline void ahrs_update_mag_full(void) {
-  const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X), 
-					  MAG_BFP_OF_REAL(AHRS_H_Y), 
+  const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
+					  MAG_BFP_OF_REAL(AHRS_H_Y),
 					  MAG_BFP_OF_REAL(AHRS_H_Z)};
   struct Int32Vect3 expected_imu;
   INT32_RMAT_VMULT(expected_imu, ahrs.ltp_to_imu_rmat, expected_ltp);
-  
+
   struct Int32Vect3 residual;
   INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
 
@@ -228,12 +228,12 @@ static inline void ahrs_update_mag_full(void) {
   ahrs_impl.rate_correction.q += residual.y/32/16;
   ahrs_impl.rate_correction.r += residual.z/32/16;
 
-  
+
   ahrs_impl.high_rez_bias.p += -residual.x/32*1024;
   ahrs_impl.high_rez_bias.q += -residual.y/32*1024;
   ahrs_impl.high_rez_bias.r += -residual.z/32*1024;
 
-  
+
   INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
 
 }
@@ -241,17 +241,17 @@ static inline void ahrs_update_mag_full(void) {
 
 static inline void ahrs_update_mag_2d(void) {
 
-  const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X), 
+  const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
 					  MAG_BFP_OF_REAL(AHRS_H_Y)};
 
   struct Int32Vect3 measured_ltp;
   INT32_RMAT_TRANSP_VMULT(measured_ltp, ahrs.ltp_to_imu_rmat, imu.mag);
-  
-  struct Int32Vect3 residual_ltp = 
+
+  struct Int32Vect3 residual_ltp =
     { 0,
       0,
       (measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x)/(1<<5)};
-  
+
   struct Int32Vect3 residual_imu;
   INT32_RMAT_VMULT(residual_imu, ahrs.ltp_to_imu_rmat, residual_ltp);
 
@@ -266,7 +266,7 @@ static inline void ahrs_update_mag_2d(void) {
   ahrs_impl.rate_correction.p += residual_imu.x/16;
   ahrs_impl.rate_correction.q += residual_imu.y/16;
   ahrs_impl.rate_correction.r += residual_imu.z/16;
-  
+
 
   // residual_ltp FRAC = 2 * MAG_FRAC = 22
   // high_rez_bias = RATE_FRAC+28 = 40
@@ -300,12 +300,12 @@ __attribute__ ((always_inline)) static inline void compute_imu_quat_and_rmat_fro
 /* Compute ltp to imu rotation in euler angles and rotation matrice representation
    from the quaternion representation */
 __attribute__ ((always_inline)) static inline void compute_imu_euler_and_rmat_from_quat(void) {
-  
+
   /* Compute LTP to IMU euler */
   INT32_EULERS_OF_QUAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_quat);
   /* Compute LTP to IMU rotation matrix */
   INT32_RMAT_OF_QUAT(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_quat);
-  
+
 }
 
 __attribute__ ((always_inline)) static inline void compute_body_orientation(void) {
@@ -320,3 +320,18 @@ __attribute__ ((always_inline)) static inline void compute_body_orientation(void
   INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate);
 
 }
+
+// TODO use ahrs result directly
+#include "estimator.h"
+void ahrs_update_fw_estimator(void)
+{
+  struct FloatEulers att;
+  // export results to estimator
+  EULERS_FLOAT_OF_BFP(att,ahrs.ltp_to_imu_euler);
+
+  estimator_phi   = att.phi;
+  estimator_theta = att.theta;
+  estimator_psi   = att.psi;
+
+  //estimator_p = Omega_Vector[0];
+}
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h
index f1f833b656f..3b2f8756f4c 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h
@@ -40,4 +40,9 @@ struct AhrsIntCmpl {
 
 extern struct AhrsIntCmpl ahrs_impl;
 
+
+// TODO copy ahrs to state instead of estimator
+void ahrs_update_fw_estimator(void);
+
+
 #endif /* AHRS_INT_CMPL_H */

From dfd47dfc226988e3298bd0bb7b0563f66f629ea4 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 6 May 2011 14:45:55 +0200
Subject: [PATCH 67/99] The current default imu-trimming for fixedwing...
 another FIXME

---
 sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c | 9 +++++++++
 sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h | 3 +++
 2 files changed, 12 insertions(+)

diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c
index d27af66705d..f398bdd891d 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c
@@ -323,6 +323,15 @@ __attribute__ ((always_inline)) static inline void compute_body_orientation(void
 
 // TODO use ahrs result directly
 #include "estimator.h"
+// remotely settable
+#ifndef INS_ROLL_NEUTRAL_DEFAULT
+#define INS_ROLL_NEUTRAL_DEFAULT 0
+#endif
+#ifndef INS_PITCH_NEUTRAL_DEFAULT
+#define INS_PITCH_NEUTRAL_DEFAULT 0
+#endif
+float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
 void ahrs_update_fw_estimator(void)
 {
   struct FloatEulers att;
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h
index 3b2f8756f4c..ab0a7d939dc 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h
@@ -43,6 +43,9 @@ extern struct AhrsIntCmpl ahrs_impl;
 
 // TODO copy ahrs to state instead of estimator
 void ahrs_update_fw_estimator(void);
+extern float ins_roll_neutral;
+extern float ins_pitch_neutral;
+
 
 
 #endif /* AHRS_INT_CMPL_H */

From 680fffd1f43a455b12c22c3b394fcd6dac93689f Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 6 May 2011 15:11:58 +0200
Subject: [PATCH 68/99] Magnetic vector moved to separate file, and defaults to
 [1 0 0] when not defined

---
 sw/airborne/subsystems/ahrs/ahrs_float_utils.h         |  2 ++
 sw/airborne/subsystems/ahrs/ahrs_int_utils.h           |  4 ++--
 .../subsystems/ahrs/ahrs_magnetic_field_model.h        | 10 ++++++++++
 3 files changed, 14 insertions(+), 2 deletions(-)
 create mode 100644 sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h

diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
index 03374c81498..69ac9c28c2a 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
@@ -1,6 +1,8 @@
 #ifndef AHRS_FLOAT_UTILS_H
 #define AHRS_FLOAT_UTILS_H
 
+#include "subsystems/ahrs/ahrs_magnetic_field_model.h"
+
 static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) {
   /* get phi and theta from accelerometer */
   struct FloatVect3 accelf;
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h
index 84918fcc620..73450f57916 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h
@@ -4,7 +4,7 @@
 //#include "../../test/pprz_algebra_print.h"
 #include "math/pprz_algebra_int.h"
 
-#include "generated/airframe.h"
+#include "subsystems/ahrs/ahrs_magnetic_field_model.h"
 
 static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) {
   //  DISPLAY_INT32_VECT3("# accel", (*accel));
@@ -17,7 +17,7 @@ static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, stru
   int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel->x, INT32_TRIG_FRAC);
   const float ftheta = atan2f(-cphi_ax, -accel->z);
   e->theta = ANGLE_BFP_OF_REAL(ftheta);
-    
+
   int32_t sphi;
   PPRZ_ITRIG_SIN(sphi, e->phi);
   int32_t stheta;
diff --git a/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h b/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h
new file mode 100644
index 00000000000..b548ea3b048
--- /dev/null
+++ b/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h
@@ -0,0 +1,10 @@
+#include "generated/airframe.h"
+
+#ifndef AHRS_H_X
+#ifdef AHRS_H_Y
+#error Either define both AHRS_H_X and AHRS_H_Y or none, but not half
+#endif
+#define AHRS_H_X 1
+#define AHRS_H_Y 0
+#endif
+

From c4034340022486fbdcaed69724432d869849f5c6 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 6 May 2011 15:14:04 +0200
Subject: [PATCH 69/99] Make ins_ppzuavimu module also run standalone for
 testing

---
 sw/airborne/modules/ins/ins_ppzuavimu.c | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c
index 4f5d5ae9ea6..ea3dc5e852c 100644
--- a/sw/airborne/modules/ins/ins_ppzuavimu.c
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.c
@@ -48,6 +48,10 @@ struct i2c_transaction ppzuavimu_hmc5843;
 struct i2c_transaction ppzuavimu_itg3200;
 struct i2c_transaction ppzuavimu_adxl345;
 
+// Standalone option: run module only
+#ifndef IMU_TYPE_H
+struct Imu imu;
+#endif
 
 void ppzuavimu_module_init( void )
 {

From 6b34203e27edc6dc48201ac88a8e72f26225b372 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 6 May 2011 16:12:24 +0200
Subject: [PATCH 70/99] ppzuav-imu as a <subsystem name=imu type=ppzuav>

---
 .../PPZUAV/fixed-wing/ppzimu_tiny.xml         | 14 ++---------
 conf/airframes/TU_Delft/yapa_xsens.xml        | 10 ++++----
 conf/autopilot/fixedwing.makefile             |  1 +
 .../subsystems/shared/imu_ppzuav.makefile     | 20 +++++++++++++++
 conf/modules/ins_aspirin_via_i2c.xml          | 25 +++++++++++++++++++
 conf/modules/ins_ppzuavimu.xml                |  8 ++++--
 sw/airborne/modules/ins/ins_ppzuavimu.c       |  6 ++---
 sw/airborne/modules/ins/ins_ppzuavimu.h       | 15 ++++++-----
 8 files changed, 71 insertions(+), 28 deletions(-)
 create mode 100644 conf/autopilot/subsystems/shared/imu_ppzuav.makefile
 create mode 100644 conf/modules/ins_aspirin_via_i2c.xml

diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
index 1b4a1040a06..06906f2ea41 100644
--- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -202,7 +202,7 @@
     -->
     <!--    <load name="digital_cam_i2c.xml"/>  -->
 
-    <load name="ins_ppzuavimu.xml" />
+ <!--    <load name="ins_ppzuavimu.xml" /> -->
 
 <!--
     <load name="digital_cam.xml" >
@@ -233,9 +233,9 @@
 
     <!-- Sensors -->
     <!--
-    <subsystem name="imu" type="booz"/>
     <subsystem name="attitude" type="dcm"/>
     -->
+    <subsystem name="imu" type="ppzuav"/>
     <subsystem name="ahrs" type="ic"/>
 
 
@@ -256,15 +256,5 @@
 
 </firmware>
 
- <makefile>
-   ap.srcs +=  $(SRC_SUBSYSTEMS)/imu.c
-
-ap.CFLAGS +=    -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446
-ap.CFLAGS +=    -DIMU_TYPE_H=\"modules/ins/ins_ppzuavimu.h\"
-
-# Test attitude using accelerometers only:
-# ap.CFLAGS +=   -DOUTPUTMODE=2
-# ap.CFLAGS +=   -DASPIRIN_IMU
- </makefile>
 
 </airframe>
diff --git a/conf/airframes/TU_Delft/yapa_xsens.xml b/conf/airframes/TU_Delft/yapa_xsens.xml
index 10abe2b9f11..f27b129aeb4 100644
--- a/conf/airframes/TU_Delft/yapa_xsens.xml
+++ b/conf/airframes/TU_Delft/yapa_xsens.xml
@@ -180,18 +180,18 @@
     <load name="ins_xsens_MTiG_fixedwing.xml">
       <configure name="XSENS_UART_NR" value="0"/>
     </load>
-<!--
+
     <load name="light.xml">
       <define name="LIGHT_LED_STROBE" value="3"/>
       <define name="LIGHT_LED_NAV" value="2"/>
       <define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
       <define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
     </load>
-    -->
-    <!--    <load name="digital_cam_i2c.xml"/>  -->
-    <load name="mag_hmc5843.xml" />
+
+<!--    <load name="digital_cam_i2c.xml"/>  -->
+  <!--    <load name="ins_ppzuavimu.xml"/>  -->
     <load name="digital_cam.xml" >
-      <define name="DC_SHUTTER_LED" value="3"/>
+      <define name="DC_SHUTTER_LED" value="2"/>
     </load>
 
   </modules>
diff --git a/conf/autopilot/fixedwing.makefile b/conf/autopilot/fixedwing.makefile
index 139a7ca919b..3b0758e8bf7 100644
--- a/conf/autopilot/fixedwing.makefile
+++ b/conf/autopilot/fixedwing.makefile
@@ -15,6 +15,7 @@ SRC_FIXEDWING_TEST=$(SRC_FIXEDWING)/
 
 SRC_FIRMWARE=firmwares/fixedwing
 SRC_SUBSYSTEMS=subsystems
+SRC_MODULES=modules
 
 FIXEDWING_INC = -I$(SRC_FIRMWARE) -I$(SRC_FIXEDWING)
 
diff --git a/conf/autopilot/subsystems/shared/imu_ppzuav.makefile b/conf/autopilot/subsystems/shared/imu_ppzuav.makefile
new file mode 100644
index 00000000000..6da0e416e42
--- /dev/null
+++ b/conf/autopilot/subsystems/shared/imu_ppzuav.makefile
@@ -0,0 +1,20 @@
+
+IMU_PPZUAVIMU_CFLAGS  = -DUSE_IMU
+IMU_PPZUAVIMU_CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_ppzuavimu.h\"
+
+IMU_PPZUAVIMU_SRCS    = $(SRC_SUBSYSTEMS)/imu.c             \
+                      $(SRC_MODULES)/ins/ins_ppzuavimu.c
+
+
+IMU_PPZUAVIMU_CFLAGS += -DUSE_I2C
+ifdef STM32
+	IMU_PPZUAVIMU_CFLAGS += -DUSE_I2C2
+	IMU_PPZUAVIMU_CFLAGS += -DPPZUAVIMU_I2C_DEVICE=i2c2
+else
+	IMU_PPZUAVIMU_CFLAGS += -DUSE_I2C0
+	IMU_PPZUAVIMU_CFLAGS += -DPPZUAVIMU_I2C_DEVICE=i2c0
+endif
+
+ap.CFLAGS += $(IMU_PPZUAVIMU_CFLAGS)
+ap.srcs   += $(IMU_PPZUAVIMU_SRCS)
+
diff --git a/conf/modules/ins_aspirin_via_i2c.xml b/conf/modules/ins_aspirin_via_i2c.xml
new file mode 100644
index 00000000000..d5f93cd3273
--- /dev/null
+++ b/conf/modules/ins_aspirin_via_i2c.xml
@@ -0,0 +1,25 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="ins_aspirin_via_i2c" dir="ins">
+  <!-- <depend conflict="ins" -->
+  <header>
+    <file name="ins_ppzuavimu.h"/>
+  </header>
+
+  <!-- default imu stuff -->
+  <init fun="imu_impl_init()"/>
+  <periodic fun="imu_periodic()" freq="60"/>
+  <!-- ImuEvent called directly from main_ap -->
+
+  <!-- extras to become a usefull module -->
+  <periodic fun="ppzuavimu_module_downlink_raw()" freq="5"/>
+  <event fun="ppzuavimu_module_event()"/>
+
+  <makefile>
+    <file name="ins_ppzuavimu.c"/>
+    <define name="PPZUAVIMU_I2C_DEVICE" value="i2c0" />
+    <define name="USE_I2C" />
+    <define name="USE_I2C0" />
+    <define name="ASPIRIN_IMU" />
+  </makefile>
+</module>
diff --git a/conf/modules/ins_ppzuavimu.xml b/conf/modules/ins_ppzuavimu.xml
index 9537dbe34db..a2544c204df 100644
--- a/conf/modules/ins_ppzuavimu.xml
+++ b/conf/modules/ins_ppzuavimu.xml
@@ -6,8 +6,12 @@
     <file name="ins_ppzuavimu.h"/>
   </header>
 
-  <init fun="ppzuavimu_module_init()"/>
-  <periodic fun="ppzuavimu_module_periodic()" freq="60"/>
+  <!-- default imu stuff -->
+  <init fun="imu_impl_init()"/>
+  <periodic fun="imu_periodic()" freq="60"/>
+  <!-- ImuEvent called directly from main_ap -->
+
+  <!-- extras to become a usefull module -->
   <periodic fun="ppzuavimu_module_downlink_raw()" freq="5"/>
   <event fun="ppzuavimu_module_event()"/>
 
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c
index ea3dc5e852c..be707312c80 100644
--- a/sw/airborne/modules/ins/ins_ppzuavimu.c
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.c
@@ -19,7 +19,7 @@
  */
 
 #include <math.h>
-#include "estimator.h"
+#include "ins_ppzuavimu.h"
 #include "mcu_periph/i2c.h"
 
 // Downlink
@@ -53,7 +53,7 @@ struct i2c_transaction ppzuavimu_adxl345;
 struct Imu imu;
 #endif
 
-void ppzuavimu_module_init( void )
+void imu_impl_init(void)
 {
   /////////////////////////////////////////////////////////////////////
   // ITG3200
@@ -138,7 +138,7 @@ void ppzuavimu_module_init( void )
 
 }
 
-void ppzuavimu_module_periodic( void )
+void imu_periodic( void )
 {
   // Start reading the latest gyroscope data
   ppzuavimu_itg3200.type = I2CTransTxRx;
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.h b/sw/airborne/modules/ins/ins_ppzuavimu.h
index 1da5a745d7b..8ffe325d299 100644
--- a/sw/airborne/modules/ins/ins_ppzuavimu.h
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.h
@@ -28,14 +28,13 @@ extern volatile bool_t gyr_valid;
 extern volatile bool_t acc_valid;
 extern volatile bool_t mag_valid;
 
-
-extern void ppzuavimu_module_init( void );
-extern void ppzuavimu_module_periodic( void );
-extern void ppzuavimu_module_event( void );
-extern void ppzuavimu_module_downlink_raw( void );
-
+/* must be defined in order to be IMU code: declared in imu.h
+extern void imu_impl_init(void);
+extern void imu_periodic(void);
+*/
 
 #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) {   \
+    ppzuavimu_module_event();                \
     if (gyr_valid) {                         \
       gyr_valid = FALSE;                     \
       _gyro_handler();                                  \
@@ -50,5 +49,9 @@ extern void ppzuavimu_module_downlink_raw( void );
     }                                                   \
 }
 
+/* Own Extra Functions */
+extern void ppzuavimu_module_event( void );
+extern void ppzuavimu_module_downlink_raw( void );
+
 
 #endif // PPZUAVIMU_H

From f84428131b4a63dc8d9f897209f3c3c1c4bae5d7 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 6 May 2011 16:46:53 +0200
Subject: [PATCH 71/99] Moved the 60Hz I2C routines to fixedwing only
 subsystems

---
 conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile | 5 +++++
 .../subsystems/{shared => fixedwing}/imu_ppzuav.makefile     | 0
 2 files changed, 5 insertions(+)
 create mode 100644 conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile
 rename conf/autopilot/subsystems/{shared => fixedwing}/imu_ppzuav.makefile (100%)

diff --git a/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile b/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile
new file mode 100644
index 00000000000..907645fc25a
--- /dev/null
+++ b/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile
@@ -0,0 +1,5 @@
+
+
+include $(CFG_FIXEDWING)/imu_ppzuavimu.makefile
+
+ap.CFLAGS += -DASPIRIN_IMU
diff --git a/conf/autopilot/subsystems/shared/imu_ppzuav.makefile b/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile
similarity index 100%
rename from conf/autopilot/subsystems/shared/imu_ppzuav.makefile
rename to conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile

From bacdc1b2cd32e5e11555ae5b59175f641539a817 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 6 May 2011 17:21:03 +0200
Subject: [PATCH 72/99] Magentometer integrated and calibrated for aspirin and
 ppzimu for ahrs_ic and ahrs_dcm

---
 conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml      | 11 +++++++++--
 .../subsystems/fixedwing/attitude_dcm.makefile        |  1 +
 .../subsystems/fixedwing/imu_aspirin_i2c.makefile     |  2 +-
 .../subsystems/fixedwing/imu_ppzuav.makefile          |  2 ++
 sw/airborne/firmwares/fixedwing/main_ap.c             |  9 +++++----
 sw/airborne/modules/ins/ins_ppzuavimu.c               |  8 +++++---
 sw/airborne/subsystems/ahrs/ahrs_float_dcm.c          | 10 +++++++++-
 7 files changed, 32 insertions(+), 11 deletions(-)

diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
index 06906f2ea41..8a0110b530c 100644
--- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -85,6 +85,10 @@
     <define name="MAG_Y_SENS" value="1" integer="16"/>
     <define name="MAG_Z_SENS" value="1" integer="16"/>
 
+    <define name="MAG_X_SIGN" value="1"/>
+    <define name="MAG_Y_SIGN" value="1"/>
+    <define name="MAG_Z_SIGN" value="1"/>
+
     <define name="BODY_TO_IMU_PHI" value="0"/>
     <define name="BODY_TO_IMU_THETA" value="0"/>
     <define name="BODY_TO_IMU_PSI" value="0"/>
@@ -233,10 +237,13 @@
 
     <!-- Sensors -->
     <!--
-    <subsystem name="attitude" type="dcm"/>
+    <subsystem name="ahrs" type="ic"/>
+    <subsystem name="imu" type="aspirin_i2c"/>
     -->
     <subsystem name="imu" type="ppzuav"/>
-    <subsystem name="ahrs" type="ic"/>
+    <subsystem name="attitude" type="dcm">
+      <define name="USE_MAGNETOMETER" />
+    </subsystem>
 
 
     <subsystem name="radio_control" type="ppm"/>
diff --git a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
index 91b1eb5ecd6..9aa26c0cdf3 100644
--- a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
+++ b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
@@ -10,6 +10,7 @@ ap.CFLAGS += -DUSE_AHRS
 ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs.c
 ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
 ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c
+ap.srcs   += math/pprz_trig_int.c
 
 ifdef AHRS_ALIGNER_LED
   ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
diff --git a/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile b/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile
index 907645fc25a..87d72683c7e 100644
--- a/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile
+++ b/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile
@@ -1,5 +1,5 @@
 
 
-include $(CFG_FIXEDWING)/imu_ppzuavimu.makefile
+include $(CFG_FIXEDWING)/imu_ppzuav.makefile
 
 ap.CFLAGS += -DASPIRIN_IMU
diff --git a/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile b/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile
index 6da0e416e42..8f224a91668 100644
--- a/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile
+++ b/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile
@@ -18,3 +18,5 @@ endif
 ap.CFLAGS += $(IMU_PPZUAVIMU_CFLAGS)
 ap.srcs   += $(IMU_PPZUAVIMU_SRCS)
 
+ap.CFLAGS += -DAHRS_MAG_UPDATE_YAW_ONLY
+
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index a99aa76e25c..e467ecdf7b0 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -723,13 +723,14 @@ static inline void on_gyro_event( void ) {
 
 }
 
-static inline void on_mag_event(void) {
-  /*
+static inline void on_mag_event(void)
+{
+#ifdef IMU_MAG_X_SIGN
   ImuScaleMag(imu);
   if (ahrs.status == AHRS_RUNNING) {
     ahrs_update_mag();
-    ahrs_update_fw_estimator();
+//    ahrs_update_fw_estimator();
   }
-  */
+#endif
 }
 #endif // USE_AHRS
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c
index be707312c80..b5f7521024f 100644
--- a/sw/airborne/modules/ins/ins_ppzuavimu.c
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.c
@@ -116,7 +116,7 @@ void imu_impl_init(void)
   // HMC5843
   ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR;
   ppzuavimu_hmc5843.type = I2CTransTx;
-  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA;  // set to rate to 50Hz
+  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA;  // set to rate to max speed: 50Hz no bias
   ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2);
   ppzuavimu_hmc5843.len_w = 2;
   i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
@@ -160,6 +160,8 @@ void imu_periodic( void )
   ppzuavimu_hmc5843.len_w = 1;
   ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM;
   i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843);
+
+  RunOnceEvery(10,ppzuavimu_module_downlink_raw());
 }
 
 void ppzuavimu_module_downlink_raw( void )
@@ -216,9 +218,9 @@ void ppzuavimu_module_event( void )
     z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
 
 #ifdef ASPIRIN_IMU
-    VECT3_ASSIGN(imu.mag_unscaled, x, y, z);
+    VECT3_ASSIGN(imu.mag_unscaled, x, -y, -z);
 #else // PPZIMU
-    VECT3_ASSIGN(imu.mag_unscaled, x, y, z);
+    VECT3_ASSIGN(imu.mag_unscaled, -y, -x, -z);
 #endif
 
     mag_valid = TRUE;
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
index 788acb1e8f8..c7ad209c922 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
@@ -230,9 +230,17 @@ void ahrs_update_accel(void)
   Drift_correction();
 }
 
+#ifdef USE_MAGNETOMETER
+#warning NO_PITCH_AND_ROLL_COMPENSATION_YET
+#endif
 void ahrs_update_mag(void)
 {
-  //TODO
+  //FIXME: pitch and roll compensation please!!!
+  /*
+  struct Int32Vect3 ltp_mag;
+  INT32_RMAT_VMULT(expected_imu, ahrs.imu_to_ltp_rmat, imu.mag);
+   */
+  MAG_Heading = atan2(imu.mag.y, -imu.mag.x);
 }
 
 void Normalize(void)

From b8c0344482841cde6142fd591a84bcc10132d26f Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Fri, 6 May 2011 20:38:14 +0200
Subject: [PATCH 73/99] macro FLOAT_QUAT_MULT is normal quaterion
 multiplicationl, q_a2c = q_b2c * q_a2b. QUAT_COMP has switched order

---
 sw/airborne/math/pprz_algebra_float.h | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
index 5a31345f6c4..9bb7f5efb19 100644
--- a/sw/airborne/math/pprz_algebra_float.h
+++ b/sw/airborne/math/pprz_algebra_float.h
@@ -518,7 +518,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
     FLOAT_QUAT_NORMALIZE(_a2c);						\
   }
 
-/* _a2c = _a2b comp _b2c , aka  _a2c = _a2b * _b2c */
+/* _a2c = _a2b comp _b2c , aka  _a2c = _b2c * _a2b */
 #define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c) {				\
     (_a2c).qi = (_a2b).qi*(_b2c).qi - (_a2b).qx*(_b2c).qx - (_a2b).qy*(_b2c).qy - (_a2b).qz*(_b2c).qz; \
     (_a2c).qx = (_a2b).qi*(_b2c).qx + (_a2b).qx*(_b2c).qi + (_a2b).qy*(_b2c).qz - (_a2b).qz*(_b2c).qy; \
@@ -526,7 +526,8 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
     (_a2c).qz = (_a2b).qi*(_b2c).qz + (_a2b).qx*(_b2c).qy - (_a2b).qy*(_b2c).qx + (_a2b).qz*(_b2c).qi; \
   }
 
-#define FLOAT_QUAT_MULT(_a2c, _a2b, _b2c) FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
+/* Quaternion multiplication q_a2c = q_b2c * q_a2b */
+#define FLOAT_QUAT_MULT(_a2c, _b2c, _a2b) FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
 
 /* _a2b = _a2c comp_inv _b2c , aka  _a2b = _a2c * inv(_b2c) */
 #define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) {		\

From eccd77c41eef680c041badd93df85f8ce8f35b8f Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Fri, 6 May 2011 22:24:28 +0200
Subject: [PATCH 74/99] fix typo in baro_ets sim

---
 sw/airborne/modules/sensors/baro_ets.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c
index 1f56ce70f7b..736b3782c2b 100644
--- a/sw/airborne/modules/sensors/baro_ets.c
+++ b/sw/airborne/modules/sensors/baro_ets.c
@@ -98,7 +98,7 @@ void baro_ets_read_periodic( void ) {
     I2CReceive(BARO_ETS_I2C_DEV, baro_ets_i2c_trans, BARO_ETS_ADDR, 2);
 #else // SITL
   baro_ets_adc = 0;
-  baro_ets_altitude = gps.hms / 1000.0;
+  baro_ets_altitude = gps.hmsl / 1000.0;
   baro_ets_valid = TRUE;
   EstimatorSetAlt(baro_ets_altitude);
 #endif

From 53a759115f55b70680e51bcd4dcfe0e24e8fea1d Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 6 May 2011 23:55:53 +0200
Subject: [PATCH 75/99] PPZIMU runs at 100Hz, with 100% sample processing with
 Soft EOC for perfect integration

---
 .../PPZUAV/fixed-wing/ppzimu_tiny.xml         |  6 +-
 sw/airborne/modules/ins/ins_ppzuavimu.c       | 61 +++++++++++++++----
 2 files changed, 53 insertions(+), 14 deletions(-)

diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
index 8a0110b530c..21d6c992732 100644
--- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -222,9 +222,9 @@
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
 
-      <configure name="PERIODIC_FREQUENCY" value="60"/>
-      <configure name="AHRS_PROPAGATE_FREQUENCY" value="60"/>
-      <configure name="AHRS_CORRECT_FREQUENCY" value="60"/>
+      <configure name="PERIODIC_FREQUENCY" value="120"/>
+      <configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
+      <configure name="AHRS_CORRECT_FREQUENCY" value="100"/>
 
       <configure name="AHRS_ALIGNER_LED" value="3"/>
       <configure name="CPU_LED" value="3"/>
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c
index b5f7521024f..556b83cb2d3 100644
--- a/sw/airborne/modules/ins/ins_ppzuavimu.c
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.c
@@ -21,6 +21,7 @@
 #include <math.h>
 #include "ins_ppzuavimu.h"
 #include "mcu_periph/i2c.h"
+#include "led.h"
 
 // Downlink
 #include "mcu_periph/uart.h"
@@ -53,22 +54,40 @@ struct i2c_transaction ppzuavimu_adxl345;
 struct Imu imu;
 #endif
 
+#ifndef PERIODIC_FREQUENCY
+#define PERIODIC_FREQUENCY 60
+#endif
+
 void imu_impl_init(void)
 {
   /////////////////////////////////////////////////////////////////////
   // ITG3200
   ppzuavimu_itg3200.type = I2CTransTx;
   ppzuavimu_itg3200.slave_addr = ITG3200_ADDR;
-  /* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */
   ppzuavimu_itg3200.buf[0] = ITG3200_REG_DLPF_FS;
+#if PERIODIC_FREQUENCY == 60
+  /* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */
   ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x04<<0);
+#else
+#  if PERIODIC_FREQUENCY == 120
+#  warning ITG3200 read at 120Hz
+  /* set gyro range to 2000deg/s and low pass at 42Hz (< 120Hz/2) internal sampling at 1kHz */
+  ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x03<<0);
+#  else
+#  error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates
+#  endif
+#endif
   ppzuavimu_itg3200.len_w = 2;
   i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
     while(ppzuavimu_itg3200.status == I2CTransPending);
 
   /* set sample rate to 66Hz: so at 60Hz there is always a new sample ready and you loose little */
   ppzuavimu_itg3200.buf[0] = ITG3200_REG_SMPLRT_DIV;
-  ppzuavimu_itg3200.buf[1] = 0x0E;
+#if PERIODIC_FREQUENCY == 60
+  ppzuavimu_itg3200.buf[1] = 19;  // 50Hz
+#else
+  ppzuavimu_itg3200.buf[1] = 9;  // 100Hz
+#endif
   i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
     while(ppzuavimu_itg3200.status == I2CTransPending);
 
@@ -78,9 +97,9 @@ void imu_impl_init(void)
   i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
     while(ppzuavimu_itg3200.status == I2CTransPending);
 
-  /* no interrupts for now */
+  /* no interrupts for now, but set data ready interrupt to enable reading status bits */
   ppzuavimu_itg3200.buf[0] = ITG3200_REG_INT_CFG;
-  ppzuavimu_itg3200.buf[1] = 0x00;
+  ppzuavimu_itg3200.buf[1] = 0x01;
   i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
     while(ppzuavimu_itg3200.status == I2CTransPending);
 
@@ -91,7 +110,11 @@ void imu_impl_init(void)
   ppzuavimu_adxl345.slave_addr = ADXL345_ADDR;
   ppzuavimu_adxl345.type = I2CTransTx;
   ppzuavimu_adxl345.buf[0] = ADXL345_REG_BW_RATE;
+#if PERIODIC_FREQUENCY == 60
+  ppzuavimu_adxl345.buf[1] = 0x09;  // normal power and 50Hz sampling, 50Hz BW
+#else
   ppzuavimu_adxl345.buf[1] = 0x0a;  // normal power and 100Hz sampling, 50Hz BW
+#endif
   ppzuavimu_adxl345.len_w = 2;
   i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
     while(ppzuavimu_adxl345.status == I2CTransPending);
@@ -142,9 +165,9 @@ void imu_periodic( void )
 {
   // Start reading the latest gyroscope data
   ppzuavimu_itg3200.type = I2CTransTxRx;
-  ppzuavimu_itg3200.len_r = 6;
+  ppzuavimu_itg3200.len_r = 9;
   ppzuavimu_itg3200.len_w = 1;
-  ppzuavimu_itg3200.buf[0] = ITG3200_REG_GYRO_XOUT_H;
+  ppzuavimu_itg3200.buf[0] = ITG3200_REG_INT_STATUS;
   i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_itg3200);
 
   // Start reading the latest accelerometer data
@@ -155,12 +178,17 @@ void imu_periodic( void )
   i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_adxl345);
 
   // Start reading the latest magnetometer data
+#if PERIODIC_FREQUENCY > 60
+  RunOnceEvery(2,{
+#endif
   ppzuavimu_hmc5843.type = I2CTransTxRx;
   ppzuavimu_hmc5843.len_r = 6;
   ppzuavimu_hmc5843.len_w = 1;
   ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM;
   i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843);
-
+#if PERIODIC_FREQUENCY > 60
+  });
+#endif
   RunOnceEvery(10,ppzuavimu_module_downlink_raw());
 }
 
@@ -178,9 +206,21 @@ void ppzuavimu_module_event( void )
   // If the itg3200 I2C transaction has succeeded: convert the data
   if (ppzuavimu_itg3200.status == I2CTransSuccess)
   {
-    x = (int16_t) ((ppzuavimu_itg3200.buf[0] << 8) | ppzuavimu_itg3200.buf[1]);
-    y = (int16_t) ((ppzuavimu_itg3200.buf[2] << 8) | ppzuavimu_itg3200.buf[3]);
-    z = (int16_t) ((ppzuavimu_itg3200.buf[4] << 8) | ppzuavimu_itg3200.buf[5]);
+#define ITG_STA_DAT_OFFSET 3
+    x = (int16_t) ((ppzuavimu_itg3200.buf[0+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[1+ITG_STA_DAT_OFFSET]);
+    y = (int16_t) ((ppzuavimu_itg3200.buf[2+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[3+ITG_STA_DAT_OFFSET]);
+    z = (int16_t) ((ppzuavimu_itg3200.buf[4+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[5+ITG_STA_DAT_OFFSET]);
+
+    // Is this is new data
+    if (ppzuavimu_itg3200.buf[0] & 0x01)
+    {
+      //LED_ON(3);
+      gyr_valid = TRUE;
+      //LED_OFF(3);
+    }
+    else
+    {
+    }
 
     // Signs depend on the way sensors are soldered on the board: so they are hardcoded
 #ifdef ASPIRIN_IMU
@@ -189,7 +229,6 @@ void ppzuavimu_module_event( void )
     RATES_ASSIGN(imu.gyro_unscaled, -x, y, -z);
 #endif
 
-    gyr_valid = TRUE;
     ppzuavimu_itg3200.status = I2CTransDone;  // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data
   }
 

From 3cc537013eb5a8d88121650101a4bdff57563096 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Mon, 9 May 2011 15:27:49 +0200
Subject: [PATCH 76/99] add camera_snapshot message

---
 .../versatile_carto_fixe_muret.xml            | 15 +++++---
 conf/messages.xml                             | 12 ++++---
 conf/modules/cartography.xml                  | 11 +-----
 sw/airborne/modules/cartography/cartography.c | 35 +++++--------------
 sw/airborne/modules/cartography/cartography.h |  5 ++-
 5 files changed, 29 insertions(+), 49 deletions(-)

diff --git a/conf/flight_plans/versatile_carto_fixe_muret.xml b/conf/flight_plans/versatile_carto_fixe_muret.xml
index 21b8968ffe8..848395badcf 100644
--- a/conf/flight_plans/versatile_carto_fixe_muret.xml
+++ b/conf/flight_plans/versatile_carto_fixe_muret.xml
@@ -54,6 +54,12 @@ float varsweepsize=110;
       <set value="NAV_MODE_ROLL" var="nav_mode"/>
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
+    <block name="Geo init">
+      <while cond="LessThan(NavBlockTime(), 10)"/>
+      <call fun="NavSetGroundReferenceHere()"/>
+      <deroute block="Holding Point"/>
+    </block>
+   
     <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
       <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
@@ -111,10 +117,12 @@ float varsweepsize=110;
 
   
     <block name="Carto survey S1-S2-S3" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
+      <call fun="nav_survey_computefourth_corner(WP_S1,WP_S2,WP_S3,WP_S4)"/>
       <call fun="nav_survey_losange_carto_init(WP_S1,WP_S2,WP_S3, 50,150)"/>
       <call fun="nav_survey_losange_carto()"/>
     </block>
     <block name="Carto survey S1-S3-S2" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
+      <call fun="nav_survey_computefourth_corner(WP_S1,WP_S2,WP_S3,WP_S4)"/>
       <call fun="nav_survey_losange_carto_init(WP_S1,WP_S3,WP_S2, 0,150)"/>
       <call fun="nav_survey_losange_carto()"/>
     </block>
@@ -136,10 +144,12 @@ float varsweepsize=110;
     </block>
 
     <block name="Carto survey SP1-SP2-SP3 fin" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
+      <call fun="nav_survey_computefourth_corner(WP_S1,WP_S2,WP_S3,WP_S4)"/>
       <call fun="nav_survey_losange_carto_init(WP_SP1,WP_SP2,WP_SP3, 20,150)"/>
       <call fun="nav_survey_losange_carto()"/>
     </block>
     <block name="Carto survey SP1-SP3-SP2 fin " strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
+      <call fun="nav_survey_computefourth_corner(WP_S1,WP_S2,WP_S3,WP_S4)"/>
       <call fun="nav_survey_losange_carto_init(WP_SP1,WP_SP3,WP_SP2, 0,150)"/>
       <call fun="nav_survey_losange_carto()"/>
     </block>
@@ -148,11 +158,6 @@ float varsweepsize=110;
     </block>
 
 
-    <block name="Geo init">
-      <while cond="LessThan(NavBlockTime(), 10)"/>
-      <call fun="NavSetGroundReferenceHere()"/>
-    </block>
-   
     <block name="Survey S1-S2" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
       <survey_rectangle grid="150" wp1="S1" wp2="S2"/>
     </block>
diff --git a/conf/messages.xml b/conf/messages.xml
index 4c58df1c400..0009e3d02f4 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -802,10 +802,14 @@
     <field name="speed_sp" type="float"/>
   </message>
 
- <!--126 is free -->
- <!--127 is free -->
- <!--128 is free -->
- <!--129 is free -->
+  <!--126 is free -->
+  <!--127 is free -->
+
+  <message name="CAMERA_SNAPSHOT" id="128">
+    <field name="snapshot_image_number" type="uint16"/>
+  </message>
+
+  <!--129 is free -->
 
   <message name="STAB_ATTITUDE_FLOAT" id="130">
     <field name="est_p"         type="float" alt_unit="degres/s" alt_unit_coef="57.29578"/>
diff --git a/conf/modules/cartography.xml b/conf/modules/cartography.xml
index 04e3d9d4de8..99e941a0f03 100644
--- a/conf/modules/cartography.xml
+++ b/conf/modules/cartography.xml
@@ -5,18 +5,9 @@
     <file name="cartography.h"/>
   </header>
   <init fun="init_carto()"/>
-  <periodic fun="periodic_1Hz_carto()" freq="1." start="start_carto()" stop="stop_carto()" autorun="TRUE"/>
-  <periodic fun="periodic_10Hz_carto()" period="0.1" start="start_carto()" stop="stop_carto()" autorun="FALSE"/>
+  <periodic fun="periodic_downlink_carto()" freq="2." autorun="FALSE"/>
   <makefile>
-    <raw>
-#Exemple of RAW makefile part
-    </raw>
-    <define name="DEMO_MODULE_LED" value="2"/>
     <file name="cartography.c"/>
   </makefile>
-  <makefile target="sim">
-    <define name="SOME_FLAG"/>
-    <configure name="SOME_DEFINE" value="bla"/>
-  </makefile>
 </module>
 
diff --git a/sw/airborne/modules/cartography/cartography.c b/sw/airborne/modules/cartography/cartography.c
index bd5febc0236..fe9abdef98e 100644
--- a/sw/airborne/modules/cartography/cartography.c
+++ b/sw/airborne/modules/cartography/cartography.c
@@ -90,8 +90,7 @@ uint16_t railnumberSinceBoot=1; //used to count the number of rails the plane ha
 #define USE_ONBOARD_CAMERA
 
 #ifdef USE_ONBOARD_CAMERA
-bool_t CAMERA_SNAPSHOT_REQUIERED=FALSE;  //declared in main_ap.c
-uint16_t camera_snapshot_image_number=0; //declared in main_ap.c
+uint16_t camera_snapshot_image_number=0;
 #endif
 
 
@@ -137,7 +136,7 @@ bool_t ProjectionInsideLimitOfRail;
 
 
 #include "modules/cartography/cartography.h"
-#include "led.h"
+#include "generated/modules.h"
 
 #include "downlink.h"
 #include "mcu_periph/uart.h"
@@ -147,33 +146,16 @@ bool_t ProjectionInsideLimitOfRail;
 ////////////////////////////////////////////////////////////////////////////////////////////////
 
 void init_carto(void) {
-  // this part is already done by led_init in fact
-  LED_INIT(DEMO_MODULE_LED);
-  LED_OFF(DEMO_MODULE_LED);
 }
 
-void periodic_1Hz_carto(void) {
-	/*uint8_t  toto=3;
-	float lat=3.14159;
-	float lon=1.234;
-    LED_TOGGLE(DEMO_MODULE_LED);
-	DOWNLINK_SEND_MARK(DefaultChannel,&toto,&lat,&lon);
-	 */
-}
-
-void periodic_10Hz_carto(void) {
-  /* LED_TOGGLE(DEMO_MODULE_LED);
-   */
+void periodic_downlink_carto(void) {
+	DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel,&camera_snapshot_image_number);
 }
 
 void start_carto(void) {
- /* LED_ON(DEMO_MODULE_LED);
-  */
 }
 
 void stop_carto(void) {
- /* LED_OFF(DEMO_MODULE_LED);
-  */
 }
 
 
@@ -189,7 +171,7 @@ bool_t nav_survey_Snapshoot(void)
 {
 	camera_snapshot_image_number=railnumberSinceBoot;
 	PRTDEBSTR(SNAPSHOT)
-	CAMERA_SNAPSHOT_REQUIERED=TRUE;
+  cartography_periodic_downlink_carto_status = MODULES_START;
 	return FALSE; 
 	
 }
@@ -198,7 +180,7 @@ bool_t nav_survey_Snapshoot_Continu(void)
 {
 	camera_snapshot_image_number=railnumberSinceBoot;
 	PRTDEBSTR(SNAPSHOT)
-	CAMERA_SNAPSHOT_REQUIERED=TRUE;
+  cartography_periodic_downlink_carto_status = MODULES_START;
 	return TRUE; 
 	
 }
@@ -207,7 +189,7 @@ bool_t nav_survey_StopSnapshoot(void)
 {
 	camera_snapshot_image_number=0;
 	PRTDEBSTR(STOP SNAPSHOT)
-	CAMERA_SNAPSHOT_REQUIERED=TRUE;
+  cartography_periodic_downlink_carto_status = MODULES_START;
 	return FALSE; 
 	
 }
@@ -735,8 +717,7 @@ bool_t nav_survey_losange_carto(void)
 	
 	
 	
-	CAMERA_SNAPSHOT_REQUIERED=TRUE;
-	
+  cartography_periodic_downlink_carto_status = MODULES_START;
 	
 	return TRUE; //apparament pour les fonctions de tache=> true
 }
diff --git a/sw/airborne/modules/cartography/cartography.h b/sw/airborne/modules/cartography/cartography.h
index 2ba83d9acfd..bbbaa843764 100644
--- a/sw/airborne/modules/cartography/cartography.h
+++ b/sw/airborne/modules/cartography/cartography.h
@@ -7,8 +7,7 @@
 
  
 void init_carto(void);
-void periodic_1Hz_carto(void);
-void periodic_10Hz_carto(void);
+void periodic_downlink_carto(void);
 void start_carto(void);
 void stop_carto(void);
 
@@ -49,4 +48,4 @@ extern bool_t nav_survey_losange_carto(void);   // !!!! important il faut mettre
 
 
 #endif
- 
\ No newline at end of file
+ 

From 002efa3295527e87c0ea930042f1356a6cd455d3 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Mon, 9 May 2011 15:44:21 +0200
Subject: [PATCH 77/99] fix typo and define

---
 conf/flight_plans/versatile_carto_fixe_muret.xml | 2 +-
 sw/airborne/modules/cartography/cartography.c    | 3 +++
 2 files changed, 4 insertions(+), 1 deletion(-)

diff --git a/conf/flight_plans/versatile_carto_fixe_muret.xml b/conf/flight_plans/versatile_carto_fixe_muret.xml
index 848395badcf..a30a00ba30e 100644
--- a/conf/flight_plans/versatile_carto_fixe_muret.xml
+++ b/conf/flight_plans/versatile_carto_fixe_muret.xml
@@ -57,7 +57,7 @@ float varsweepsize=110;
     <block name="Geo init">
       <while cond="LessThan(NavBlockTime(), 10)"/>
       <call fun="NavSetGroundReferenceHere()"/>
-      <deroute block="Holding Point"/>
+      <deroute block="Holding point"/>
     </block>
    
     <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
diff --git a/sw/airborne/modules/cartography/cartography.c b/sw/airborne/modules/cartography/cartography.c
index fe9abdef98e..4dc179586db 100644
--- a/sw/airborne/modules/cartography/cartography.c
+++ b/sw/airborne/modules/cartography/cartography.c
@@ -138,6 +138,9 @@ bool_t ProjectionInsideLimitOfRail;
 #include "modules/cartography/cartography.h"
 #include "generated/modules.h"
 
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
 #include "downlink.h"
 #include "mcu_periph/uart.h"
 #include "std.h"

From 58d11d0c1bd48c9823c4279501322f5119c2670c Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Tue, 10 May 2011 10:08:39 +0200
Subject: [PATCH 78/99] allows to use '-' instead of '0-' for minus; bound
 values after a 'fit'

---
 conf/joystick/cockpit_sx.xml            |  8 ++++----
 sw/ground_segment/joystick/input2ivy.ml | 12 +++++++-----
 2 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/conf/joystick/cockpit_sx.xml b/conf/joystick/cockpit_sx.xml
index 8bfa250d4d9..621b179458a 100644
--- a/conf/joystick/cockpit_sx.xml
+++ b/conf/joystick/cockpit_sx.xml
@@ -12,10 +12,10 @@
 
     <message class="datalink" name="RC_4CH" send_always="true">
       <field name="mode" value="mode + 1"/> <!-- only AUTO1 and AUTO2 available -->
-      <field name="throttle" value="Fit(throttle,0-100,100,0,127)"/>
-      <field name="roll" value="roll"/>
-      <field name="pitch" value="pitch"/>
-      <field name="yaw" value="yaw"/>
+      <field name="throttle" value="Fit(throttle,-100,100,0,127)"/>
+      <field name="roll" value="Fit(roll,-100,100,-127,127)"/>
+      <field name="pitch" value="Fit(pitch,-100,100,-127,127)"/>
+      <field name="yaw" value="Fit(yaw,-100,100,-127,127)"/>
     </message>
 
   </messages>
diff --git a/sw/ground_segment/joystick/input2ivy.ml b/sw/ground_segment/joystick/input2ivy.ml
index 2fef43079d9..a4eeaa9573c 100644
--- a/sw/ground_segment/joystick/input2ivy.ml
+++ b/sw/ground_segment/joystick/input2ivy.ml
@@ -349,14 +349,15 @@ let eval_input = fun buttons axis input ->
 let scale = fun x min max ->
   min + ((x - min_input) * (max - min)) / (max_input - min_input)
 
-(** Fit a given interval of value into [min_input; max_input] *)
-let fit = fun x min max min_input max_input ->
-  min_input + ((x - min) * (max_input - min_input)) / (max - min)
-
 (** Scale a value in the given bounds *)
 let bound = fun x min max ->
   if x < min then min else (if x > max then max else x)
 
+(** Fit a given interval of value into [min_input; max_input] *)
+let fit = fun x min max min_input max_input ->
+  let v = min_input + ((x - min) * (max_input - min_input)) / (max - min) in
+  bound v min_input max_input
+
 (** Return a pprz RC mode
   * mode > max -> 2
   * mode < min -> 0
@@ -371,7 +372,8 @@ let pprz_mode = fun mode ->
 (** Eval a function call (TO BE COMPLETED) *)
 let eval_call = fun f args ->
   match f, args with
-    "-", [a1; a2] -> a1 - a2
+    "-", [a] -> - a
+  | "-", [a1; a2] -> a1 - a2
   | "+", [a1; a2] -> a1 + a2
   | "*", [a1; a2] -> a1 * a2
   | "%", [a1; a2] -> a1 / a2

From 1d8f7f5100019ba4574c80c665c40406fb624f0f Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Tue, 10 May 2011 17:43:37 +0200
Subject: [PATCH 79/99] GPS fixes in dev branch...

---
 .../PPZUAV/fixed-wing/ppzimu_tiny.xml         | 11 ++--
 sw/airborne/ap_downlink.h                     |  2 +-
 sw/airborne/firmwares/fixedwing/main_ap.c     | 10 ++++
 sw/airborne/modules/ins/ins_ppzuavimu.c       |  5 +-
 sw/airborne/modules/ins/ins_xsens.c           | 50 +++++++++++++------
 sw/airborne/modules/ins/ins_xsens.h           |  2 -
 6 files changed, 56 insertions(+), 24 deletions(-)

diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
index 21d6c992732..12dd3481fa9 100644
--- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -9,8 +9,8 @@
   <servos>
     <servo name="THROTTLE" no="4" min="1000" neutral="1000" max="2000"/>
     <servo name="AILERON_LEFT" no="5" min="1000" neutral="1500" max="2000"/>
-    <servo name="AILERON_RIGHT" no="8" min="2000" neutral="1500" max="1000"/>
-    <servo name="ELEVATOR" no="1" min="2000" neutral="1500" max="1000"/>
+    <servo name="AILERON_RIGHT" no="1" min="2000" neutral="1500" max="1000"/>
+    <servo name="ELEVATOR" no="8" min="2000" neutral="1500" max="1000"/>
     <servo name="RUDDER" no="9" min="1100" neutral="1500" max="1900"/>
   </servos>
 
@@ -64,7 +64,7 @@
     <define name="GYRO_Q_SIGN" value="1"/>
     <define name="GYRO_R_SIGN" value="1"/>
 
-    <define name="ACCEL_X_NEUTRAL" value="0"/>
+    <define name="ACCEL_X_NEUTRAL" value="-14"/>
     <define name="ACCEL_Y_NEUTRAL" value="0"/>
     <define name="ACCEL_Z_NEUTRAL" value="0"/>
 
@@ -225,6 +225,7 @@
       <configure name="PERIODIC_FREQUENCY" value="120"/>
       <configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
       <configure name="AHRS_CORRECT_FREQUENCY" value="100"/>
+      <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
 
       <configure name="AHRS_ALIGNER_LED" value="3"/>
       <configure name="CPU_LED" value="3"/>
@@ -237,7 +238,9 @@
 
     <!-- Sensors -->
     <!--
-    <subsystem name="ahrs" type="ic"/>
+    <subsystem name="ahrs" type="ic">
+      <define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN" />
+    </subsystem>
     <subsystem name="imu" type="aspirin_i2c"/>
     -->
     <subsystem name="imu" type="ppzuav"/>
diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
index 256f0a1d68c..236283a1ff1 100644
--- a/sw/airborne/ap_downlink.h
+++ b/sw/airborne/ap_downlink.h
@@ -188,7 +188,7 @@
 #define PERIODIC_SEND_GPS(_chan) {                                      \
     static uint8_t i;                                                   \
     int16_t climb = -gps.ned_vel.z;                                     \
-    int16_t course = DegOfRad(gps.course / 10);                        \
+    int16_t course = DegOfRad(estimator_hspeed_dir * 10);                        \
     DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.lla_pos.alt, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \
     if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0;                                    \
     if (i >= gps.nb_channels * 2) i = 0;                                    \
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index e467ecdf7b0..2efc51b8962 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -410,6 +410,12 @@ static inline void attitude_loop( void ) {
 
 }
 
+#ifdef USE_IMU
+#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
+volatile uint8_t new_ins_attitude = 0;
+#endif
+#endif
+
 void periodic_task_ap( void ) {
 
   static uint8_t _60Hz = 0;
@@ -721,6 +727,10 @@ static inline void on_gyro_event( void ) {
     LED_OFF(AHRS_CPU_LED);
 #endif
 
+#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
+  new_ins_attitude = 1;
+#endif
+
 }
 
 static inline void on_mag_event(void)
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c
index 556b83cb2d3..c0921076161 100644
--- a/sw/airborne/modules/ins/ins_ppzuavimu.c
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.c
@@ -68,9 +68,10 @@ void imu_impl_init(void)
 #if PERIODIC_FREQUENCY == 60
   /* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */
   ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x04<<0);
+#  warning ITG3200 read at 50Hz
 #else
 #  if PERIODIC_FREQUENCY == 120
-#  warning ITG3200 read at 120Hz
+#  warning ITG3200 read at 100Hz
   /* set gyro range to 2000deg/s and low pass at 42Hz (< 120Hz/2) internal sampling at 1kHz */
   ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x03<<0);
 #  else
@@ -189,7 +190,7 @@ void imu_periodic( void )
 #if PERIODIC_FREQUENCY > 60
   });
 #endif
-  RunOnceEvery(10,ppzuavimu_module_downlink_raw());
+  //RunOnceEvery(10,ppzuavimu_module_downlink_raw());
 }
 
 void ppzuavimu_module_downlink_raw( void )
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index afb07a4aa6a..a99ec8d27a8 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -176,8 +176,6 @@ int32_t xsens_nanosec;
 int16_t xsens_year;
 int8_t xsens_month;
 int8_t xsens_day;
-float xsens_lat;
-float xsens_lon;
 
 static uint8_t xsens_id;
 static uint8_t xsens_status;
@@ -223,18 +221,35 @@ void ins_periodic_task( void ) {
 #include "estimator.h"
 
 void handle_ins_msg( void) {
+
+
+  // Send to Estimator (Control)
   EstimatorSetAtt(ins_phi, ins_psi, ins_theta);
   EstimatorSetRate(ins_p,ins_q);
+
+  // Position
+  float gps_east = gps.utm_pos.east / 100.;
+  float gps_north = gps.utm_pos.north / 100.;
+  gps_east -= nav_utm_east0;
+  gps_north -= nav_utm_north0;
+  EstimatorSetPosXY(gps_east, gps_north);
+
+  // Altitude and vertical speed
+  EstimatorSetAlt(-ins_z);
+
+  // Horizontal speed
+  float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
   if (gps.fix != GPS_FIX_3D)
-    gps.gspeed = 0;
+    fspeed = 0;
+  float fclimb = -ins_vz;
+  float fcourse = atan2f((float)ins_vy, (float)ins_vx);
+  EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
 
-  gps.course = (atan2f((float)ins_vx, (float)ins_vy))*1e7;
-  gps.ned_vel.z = (int16_t)(ins_vz * 100);
-  gps.gspeed = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy) * 100);
 
-  EstimatorSetAtt(ins_phi, ((float)gps.course / 1e7), ins_theta);
-  // EstimatorSetAlt(ins_z);
-  estimator_update_state_gps();
+  // Now also finish filling the gps struct for telemetry purposes
+  gps.gspeed = fspeed * 100.;
+  gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);
+  gps.course = fcourse * 1e7;
   // reset_gps_watchdog();
 }
 
@@ -302,15 +317,20 @@ void parse_ins_msg( void ) {
         ins_x = utm_f.east;
         ins_y = utm_f.north;
 
-        gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 100;
-        ins_z = -(INS_FORMAT)gps.hmsl / 1000.;
-        ins_vx = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset) / 100.;
-        ins_vy = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.;
+        // Altitude: FIXME Xsens gives ellipsoid height and not geoid height
+        ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 1000.;
+        gps.hmsl =  XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset);
+        gps.lla_pos.alt = gps.hmsl;
+        gps.utm_pos.alt = gps.hmsl;
+        ins_vx = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.;
+        ins_vy = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset) / 100.;
         ins_vz = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 100.;
-        gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 10;
+        gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset);
+        gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset);
+        gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset);
         gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100;
         gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100;
-        gps.pdop = 5;
+        gps.pdop = 5;  // FIXME Not output by XSens
 #endif
         offset += XSENS_DATA_RAWGPS_LENGTH;
       }
diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h
index 446faabad8f..c40df9336fe 100644
--- a/sw/airborne/modules/ins/ins_xsens.h
+++ b/sw/airborne/modules/ins/ins_xsens.h
@@ -38,8 +38,6 @@ extern int32_t xsens_nanosec;
 extern int16_t xsens_year;
 extern int8_t xsens_month;
 extern int8_t xsens_day;
-extern float xsens_lat;
-extern float xsens_lon;
 extern uint8_t xsens_msg_status;
 extern uint16_t xsens_time_stamp;
 

From f05c19c2312939dda3455e4032b8f0c9af32f376 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Wed, 11 May 2011 16:34:38 +0200
Subject: [PATCH 80/99] TUD Airframe

---
 conf/airframes/TU_Delft/skywalker.xml | 226 ++++++++++++++++++++++++++
 conf/radios/R6107SP_7ch.xml           |  54 ++++++
 2 files changed, 280 insertions(+)
 create mode 100644 conf/airframes/TU_Delft/skywalker.xml
 create mode 100644 conf/radios/R6107SP_7ch.xml

diff --git a/conf/airframes/TU_Delft/skywalker.xml b/conf/airframes/TU_Delft/skywalker.xml
new file mode 100644
index 00000000000..d738e886cf7
--- /dev/null
+++ b/conf/airframes/TU_Delft/skywalker.xml
@@ -0,0 +1,226 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
+<!--
+	YAPA + XSens + XBee
+-->
+
+<airframe name="Yapa v1">
+
+  <servos>
+    <servo name="THROTTLE" no="0" min="1100" neutral="1100" max="1900"/>
+    <servo name="AILERON_LEFT" no="2" min="1100" neutral="1500" max="1900"/>
+    <servo name="AILERON_RIGHT" no="6" min="1100" neutral="1500" max="1900"/>
+    <servo name="ELEVATOR" no="7" min="1900" neutral="1500" max="1100"/>
+    <servo name="RUDDER" no="3" min="1100" neutral="1500" max="1900"/>
+  </servos>
+
+  <commands>
+    <axis name="THROTTLE" failsafe_value="0"/>
+    <axis name="ROLL" failsafe_value="0"/>
+    <axis name="YAW" failsafe_value="0"/>
+    <axis name="PITCH" failsafe_value="0"/>
+    <axis name="BRAKE" failsafe_value="0"/>	<!-- both elerons up as butterfly brake ? -->
+  </commands>
+
+  <rc_commands>
+    <set command="THROTTLE" value="@THROTTLE"/>
+    <set command="ROLL" value="@ROLL"/>
+    <set command="YAW" value="@YAW"/>
+    <set command="PITCH" value="@PITCH"/>
+    <set command="BRAKE" value="@FLAPS"/>
+  </rc_commands>
+
+  <section name="SERVO_MIXER_GAINS">
+    <define name="AILERON_NEUTRAL" value="0.45f"/>
+
+    <define name="AILERON_RATE_UP" value="1.0f"/>
+    <define name="AILERON_RATE_DOWN" value="0.5f"/>
+
+    <define name="AILERON_RATE_UP_BRAKE" value="1.0f"/>
+    <define name="AILERON_RATE_DOWN_BRAKE" value="1.0f"/>
+
+    <define name="PITCH_GAIN" value="0.9f"/>
+
+    <define name="BRAKE_AILEVON" value="-0.7f"/>
+    <define name="BRAKE_PITCH" value="0.1f"/>
+    <define name="MAX_BRAKE_RATE" value="150"/>
+
+    <define name="LIMIT(X,XL,XH)" value="(    ((X)>(XH))   ?    (XH)    :    ( ((X)>(XL)) ? (X) : (XL) )    )"/>
+
+</section>
+
+  <command_laws>
+    <!-- Brake Rate Limiter -->
+    <let var="brake_value_nofilt" value="LIMIT(-@BRAKE, 0, MAX_PPRZ)"/>
+    <let var="test; \
+  	      static int16_t _var_brake_value = 0; \
+              _var_brake_value += LIMIT(_var_brake_value_nofilt - _var_brake_value,-MAX_BRAKE_RATE,MAX_BRAKE_RATE); \
+              int verwaarloos_deze_warning_CDW" value="0"/>
+
+    <!-- Differential Aileron Depending on Brake Value -->
+    <let var="aileron_up_rate" value="(AILERON_RATE_UP   * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_UP_BRAKE   * $brake_value)"/>
+    <let var="aileron_down_rate" value="(AILERON_RATE_DOWN * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_DOWN_BRAKE * $brake_value)"/>
+    <let var="aileron_up" value="(@ROLL  *  (((float)$aileron_up_rate) / ((float)MAX_PPRZ)))"/>
+    <let var="aileron_down" value="(@ROLL  * (((float)$aileron_down_rate) / ((float)MAX_PPRZ)))"/>
+    <let var="leftturn" value="(@ROLL >= 0? 1 : 0)"/>
+    <let var="rightturn" value="(1 - $leftturn)"/>
+    <set servo="AILERON_LEFT" value="($aileron_up * $leftturn) + ($aileron_down * $rightturn) - $brake_value*(BRAKE_AILEVON) - (MAX_PPRZ * AILERON_NEUTRAL)"/>
+    <set servo="AILERON_RIGHT" value="($aileron_up * $rightturn) + ($aileron_down * $leftturn) + $brake_value*(BRAKE_AILEVON) + (MAX_PPRZ *AILERON_NEUTRAL)"/>
+
+    <set servo="RUDDER" value="@YAW"/>
+    <set servo="THROTTLE" value="@THROTTLE"/>
+
+    <!-- Pitch with Brake-Trim Function -->
+    <set servo="ELEVATOR" value="@PITCH * PITCH_GAIN - BRAKE_PITCH * $brake_value"/>
+  </command_laws>
+
+  <section name="AUTO1" prefix="AUTO1_">
+    <define name="MAX_ROLL" value="0.7"/>
+    <define name="MAX_PITCH" value="0.7"/>
+  </section>
+
+  <section name="BAT">
+    <define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/>
+
+    <define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="10" unit="V"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+  </section>
+
+  <section name="MISC">
+    <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
+    <define name="CARROT" value="5." unit="s"/>
+    <define name="CONTROL_RATE" value="60" unit="Hz"/>
+    <define name="XBEE_INIT" value="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/>
+<!--    <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
+    <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
+
+    <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
+
+    <define name="GLIDE_AIRSPEED" value="10"/>
+    <define name="GLIDE_VSPEED" value="3."/>
+    <define name="GLIDE_PITCH" value="45" unit="deg"/>
+  </section>
+
+  <section name="VERTICAL CONTROL" prefix="V_CTL_">
+    <define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
+    <!-- outer loop proportional gain -->
+    <define name="ALTITUDE_PGAIN" value="-0.03"/>
+    <!-- outer loop saturation -->
+    <define name="ALTITUDE_MAX_CLIMB" value="2."/>
+
+    <!-- auto throttle inner loop -->
+    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
+    <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
+    <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
+    <define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
+    <define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
+    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
+    <define name="AUTO_THROTTLE_PGAIN" value="-0.01"/>
+    <define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
+    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
+
+    <define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
+  </section>
+
+  <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
+    <define name="COURSE_PGAIN" value="-1.20000004768"/>
+    <define name="COURSE_DGAIN" value="0.3"/>
+    <define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
+
+    <define name="ROLL_MAX_SETPOINT" value="0.75" unit="radians"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+
+    <define name="PITCH_PGAIN" value="-12000."/>
+    <define name="PITCH_DGAIN" value="1.5"/>
+
+    <define name="ELEVATOR_OF_ROLL" value="1000."/>
+
+    <define name="ROLL_SLEW" value="1."/>
+
+    <define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
+    <define name="ROLL_RATE_GAIN" value="0."/>
+  </section>
+
+  <section name="AGGRESSIVE" prefix="AGR_">
+    <define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
+    <define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
+    <define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
+    <define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
+    <define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
+    <define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
+    <define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
+    <define name="DESCENT_NAV_RATIO" value="1.0"/>
+  </section>
+
+  <section name="FAILSAFE" prefix="FAILSAFE_">
+    <define name="DEFAULT_THROTTLE" value="0.35" unit="%"/>
+    <define name="DEFAULT_ROLL" value="0.17" unit="rad"/>
+    <define name="DEFAULT_PITCH" value="0.08" unit="rad"/>
+
+    <define name="HOME_RADIUS" value="DEFAULT_CIRCLE_RADIUS" unit="m"/>
+    <define name="KILL_MODE_DISTANCE" value="(MAX_DIST_FROM_HOME*1.5)"/>
+    <define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
+  </section>
+
+  <section name="DIGITAL_CAMERA" prefix="DC_">
+    <define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
+    <define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
+  </section>
+
+
+  <modules>
+    <load name="ins_xsens_MTiG_fixedwing.xml">
+      <configure name="XSENS_UART_NR" value="0"/>
+    </load>
+
+    <load name="light.xml">
+      <define name="LIGHT_LED_STROBE" value="3"/>
+      <define name="LIGHT_LED_NAV" value="2"/>
+      <define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
+      <define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
+    </load>
+
+<!--    <load name="digital_cam_i2c.xml"/>  -->
+  <!--    <load name="ins_ppzuavimu.xml"/>  -->
+    <load name="digital_cam.xml" >
+      <define name="DC_SHUTTER_LED" value="2"/>
+    </load>
+
+  </modules>
+
+  <firmware name="fixedwing">
+
+    <target name="ap" board="tiny_2.11">
+      <define name="STRONG_WIND"/>
+      <define name="WIND_INFO"/>
+      <define name="WIND_INFO_RET"/>
+      <define name="LOITER_TRIM"/>
+      <define name="ALT_KALMAN"/>
+    </target>
+    <target name="sim" board="pc"/>
+
+    <subsystem name="radio_control" type="ppm"/>
+
+    <!-- Communication -->
+    <subsystem name="telemetry" type="xbee_api">
+      <configure name="MODEM_BAUD" value="B57600"/>
+    </subsystem>
+
+    <!-- Actuators -->
+    <subsystem name="control"/>
+    <!-- Sensors -->
+    <subsystem name="navigation"/>
+    <subsystem name="gps" type="xsens"/>
+
+    <subsystem name="i2c"/>
+
+</firmware>
+
+</airframe>
diff --git a/conf/radios/R6107SP_7ch.xml b/conf/radios/R6107SP_7ch.xml
new file mode 100644
index 00000000000..1886310cfab
--- /dev/null
+++ b/conf/radios/R6107SP_7ch.xml
@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+<!-- $Id$
+--
+-- (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.  
+-->
+
+<!--
+-- Attributes of root (Radio) tag :
+-- name: name of RC
+-- min: min width of a pulse to be considered as a data pulse
+-- max: max width of a pulse to be considered as a data pulse
+-- sync: min width of a pulse to be considered as a synchro pulse
+-- min, max and sync are expressed in micro-seconds
+-->
+
+<!-- 
+-- Attributes of channel tag :
+-- ctl: name of the command on the transmitter - only for displaying
+-- no: order in the PPM frame
+-- function: logical command
+-- averaged: channel filtered through several frames (for discrete commands)
+-- min: minimum pulse length (micro-seconds)
+-- max: maximum pulse length (micro-seconds)
+-- neutral: neutral pulse length (micro-seconds)
+Note: a command may be reversed by exchanging min and max values
+-->
+
+<!DOCTYPE radio SYSTEM "radio.dtd">
+<radio name="FASST TX Module" data_min="700" data_max="2300" sync_min="4000" sync_max="15000" pulse_type="NEGATIVE">
+  <channel ctl="A" function="ROLL" min="1100" neutral="1500" max="1900" average="0"/>     <!-- right stick left/right -->
+  <channel ctl="B" function="PITCH" min="1100" neutral="1500" max="1900" average="0"/>    <!-- left stick up/down -->
+  <channel ctl="C" function="THROTTLE" min="1100" neutral="1100" max="1900" average="0"/>	<!-- right stick up/down -->
+  <channel ctl="D" function="YAW" min="1900" neutral="1500" max="1100" average="0"/>	<!-- left stick left/right-->
+  <channel ctl="E" function="MODE" min="1100" neutral="1500" max="1900" average="0"/>     <!-- left switch -->
+  <channel ctl="F" function="EXTRA1" min="1100" neutral="1500" max="1900" average="0"/>     <!-- left switch -->
+  <channel ctl="G" function="FLAPS" min="1100" neutral="1500" max="1900" average="0"/>     <!-- left switch -->
+</radio>

From 9413418b6705269acb70216799e970ab942215f8 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 12 May 2011 08:57:39 +0200
Subject: [PATCH 81/99] 10deg World Geodetic Model

---
 sw/airborne/math/pprz_geodetic_wgs84.h | 71 ++++++++++++++++++++++++++
 1 file changed, 71 insertions(+)
 create mode 100644 sw/airborne/math/pprz_geodetic_wgs84.h

diff --git a/sw/airborne/math/pprz_geodetic_wgs84.h b/sw/airborne/math/pprz_geodetic_wgs84.h
new file mode 100644
index 00000000000..640bd7be309
--- /dev/null
+++ b/sw/airborne/math/pprz_geodetic_wgs84.h
@@ -0,0 +1,71 @@
+#ifndef PPRZ_GEODETIC_WGS84
+#define PPRZ_GEODETIC_WGS84
+
+/*
+
+Ten by Ten Degree WGS-84 Geoid Heights from -180 to +170 Degrees of Longitude
+
+Geoid height approximations in meters
+
+Source: 
+Defense Mapping Agency. 12 Jan 1987. GPS UE Relevant WGS-84 Data Base Package. Washington, DC: Defense Mapping Agency
+
+Link: 
+http://www.colorado.edu/geography/gcraft/notes/datum/geoid84.html
+
+rows are from -180 to +170 starting north +90 to south-90
+
+
+*/
+
+
+#define WGS84_H(x,y) ((float) pprz_geodetic_wgs84_int[(y)][(x)])
+
+#define WGS84_ELLIPSOID_TO_GEOID(_Lat,_Lon,_diff)	{		\
+	float x = (180.0f + DegOfRad(_Lon)) / 10.0f;			\
+	Bound(x,0.0f,35.99999f);					\
+	float y = (90.0f - DegOfRad(_Lat)) / 10.0f;			\
+	Bound(y,0.0f,17.99999f);					\
+	uint8_t ex1 = (uint8_t) x;					\
+	uint8_t ex2 = ex1 + 1;						\
+	if (ex2 >= 36) ex2 = 0;						\
+	uint8_t ey1 = (uint8_t) y;					\
+	uint8_t ey2 = ey1 + 1;						\
+	float lin_x = x - ((float) ex1);				\
+	float lin_y = y - ((float) ey1);				\
+	float h11 = (1.0f - lin_x) * (1.0f - lin_y) * WGS84_H(ex1,ey1);	\
+	float h12 = lin_x * (1.0f - lin_y) * WGS84_H(ex2,ey1);		\
+	float h21 = (1.0f - lin_x) * lin_y * WGS84_H(ex1,ey2);		\
+	float h22 = lin_x * lin_y * WGS84_H(ex2,ey2);			\
+	_diff = h11 + h12 + h21 + h22;					\
+}
+
+
+
+
+const int8_t pprz_geodetic_wgs84_int[19][36] = 
+{
+	{13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13},
+	{3,1,-2,-3,-3,-3,-1,3,1,5,9,11,19,27,31,34,33,34,33,34,28,23,17,13,9,4,4,1,-2,-2,0,2,3,2,1,1},
+	{2,2,1,-1,-3,-7,-14,-24,-27,-25,-19,3,24,37,47,60,61,58,51,43,29,20,12,5,-2,-10,-14,-12,-10,-14,-12,-6,-2,3,6,4},
+	{2,9,17,10,13,1,-14,-30,-39,-46,-42,-21,6,29,49,65,60,57,47,41,21,18,14,7,-3,-22,-29,-32,-32,-26,-15,-2,13,17,19,6},
+	{-8,8,8,1,-11,-19,-16,-18,-22,-35,-40,-26,-12,24,45,63,62,59,47,48,42,28,12,-10,-19,-33,-43,-42,-43,-29,-2,17,23,22,6,2},
+	{-12,-10,-13,-20,-31,-34,-21,-16,-26,-34,-33,-35,-26,2,33,59,52,51,52,48,35,40,33,-9,-28,-39,-48,-59,-50,-28,3,23,37,18,-1,-11},
+	{-7,-5,-8,-15,-28,-40,-42,-29,-22,-26,-32,-51,-40,-17,17,31,34,44,36,28,29,17,12,-20,-15,-40,-33,-34,-34,-28,7,29,43,20,4,-6},
+	{5,10,7,-7,-23,-39,-47,-34,-9,-10,-20,-45,-48,-32,-9,17,25,31,31,26,15,6,1,-29,-44,-61,-67,-59,-36,-11,21,39,49,39,22,10},
+	{13,12,11,2,-11,-28,-38,-29,-10,3,1,-11,-41,-42,-16,3,17,33,22,23,2,-3,-7,-36,-59,-90,-95,-63,-24,12,53,60,58,46,36,26},
+	{22,16,17,13,1,-12,-23,-20,-14,-3,14,10,-15,-27,-18,3,12,20,18,12,-13,-9,-28,-49,-62,-89,-102,-63,-9,33,58,73,74,63,50,32},
+	{36,22,11,6,-1,-8,-10,-8,-11,-9,1,32,4,-18,-13,-9,4,14,12,13,-2,-14,-25,-32,-38,-60,-75,-63,-26,0,35,52,68,76,64,52},
+	{51,27,10,0,-9,-11,-5,-2,-3,-1,9,35,20,-5,-6,-5,0,13,17,23,21,8,-9,-10,-11,-20,-40,-47,-45,-25,5,23,45,58,57,63},
+	{46,22,5,-2,-8,-13,-10,-7,-4,1,9,32,16,4,-8,4,12,15,22,27,34,29,14,15,15,7,-9,-25,-37,-39,-23,-14,15,33,34,45},
+	{21,6,1,-7,-12,-12,-12,-10,-7,-1,8,23,15,-2,-6,6,21,24,18,26,31,33,39,41,30,24,13,-2,-20,-32,-33,-27,-14,-2,5,20},
+	{-15,-18,-18,-16,-17,-15,-10,-10,-8,-2,6,14,13,3,3,10,20,27,25,26,34,39,45,45,38,39,28,13,-1,-15,-22,-22,-18,-15,-14,-10},
+	{-45,-43,-37,-32,-30,-26,-23,-22,-16,-10,-2,10,20,20,21,24,22,17,16,19,25,30,35,35,33,30,27,10,-2,-14,-23,-30,-33,-29,-35,-43},
+	{-61,-60,-61,-55,-49,-44,-38,-31,-25,-16,-6,1,4,5,4,2,6,12,16,16,17,21,20,26,26,22,16,10,-1,-16,-29,-36,-46,-55,-54,-59},
+	{-53,-54,-55,-52,-48,-42,-38,-38,-29,-26,-26,-24,-23,-21,-19,-16,-12,-8,-4,-1,1,4,4,6,5,4,2,-6,-15,-24,-33,-40,-48,-50,-53,-52},
+	{-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30}
+};
+
+
+
+#endif

From 032b5bd920723120a049badaf65874efe97815c3 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 12 May 2011 10:04:13 +0200
Subject: [PATCH 82/99] XSens now computes MSL

---
 sw/airborne/modules/ins/ins_xsens.c | 16 +++++++++++-----
 1 file changed, 11 insertions(+), 5 deletions(-)

diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index a99ec8d27a8..58c0e6bcb50 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -37,6 +37,7 @@
 
 #ifdef USE_GPS_XSENS
 #include "subsystems/gps.h"
+#include "math/pprz_geodetic_wgs84.h"
 #include "math/pprz_geodetic_float.h"
 #include "subsystems/navigation/common_nav.h" /* needed for nav_utm_zone0 */
 #endif
@@ -301,6 +302,7 @@ void parse_ins_msg( void ) {
         gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
         gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset));
         gps.lla_pos.lon = RadOfDeg(XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset));
+        gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset);
 
 
         /* Set the real UTM zone */
@@ -314,14 +316,18 @@ void parse_ins_msg( void ) {
         /* copy results of utm conversion */
         gps.utm_pos.east = utm_f.east*100;
         gps.utm_pos.north = utm_f.north*100;
+        gps.utm_pos.alt = gps.lla_pos.alt;
+
         ins_x = utm_f.east;
         ins_y = utm_f.north;
-
-        // Altitude: FIXME Xsens gives ellipsoid height and not geoid height
+        // Altitude: Xsens LLH gives ellipsoid height
         ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 1000.;
-        gps.hmsl =  XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset);
-        gps.lla_pos.alt = gps.hmsl;
-        gps.utm_pos.alt = gps.hmsl;
+
+	// Compute geoid (MSL) height
+        float hmsl;
+	WGS84_ELLIPSOID_TO_GEOID(lla_f.lat,lla_f.lon,hmsl);
+        gps.hmsl =  (hmsl * 1000.0f);
+
         ins_vx = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.;
         ins_vy = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset) / 100.;
         ins_vz = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 100.;

From 57a7d28b4a232381ad989a9b6a6f8789650bee84 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 12 May 2011 11:04:37 +0200
Subject: [PATCH 83/99] gps_course is stored in 1e-7 rad

---
 sw/airborne/ap_downlink.h | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
index 236283a1ff1..abfcdba69d3 100644
--- a/sw/airborne/ap_downlink.h
+++ b/sw/airborne/ap_downlink.h
@@ -142,8 +142,10 @@
 #include "subsystems/imu.h"
 #define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)}
 #define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)}
+#define PERIODIC_SEND_IMU_MAG_RAW(_chan) { DOWNLINK_SEND_IMU_MAG_RAW(_chan, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)}
 #define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)}
 #define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)}
+#define PERIODIC_SEND_IMU_MAG(_chan) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(_chan, &mag_float.x, &mag_float.y, &mag_float.z)}
 #else
 #define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {}
 #define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
@@ -184,11 +186,10 @@
 #define PERIODIC_SEND_GPS_SOL(_chan) DOWNLINK_SEND_GPS_SOL(_chan, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv)
 #endif
 
-//#define PERIODIC_SEND_GPS(_chan) gps_send() /* also sends svinfo */
 #define PERIODIC_SEND_GPS(_chan) {                                      \
     static uint8_t i;                                                   \
     int16_t climb = -gps.ned_vel.z;                                     \
-    int16_t course = DegOfRad(estimator_hspeed_dir * 10);                        \
+    int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));             \
     DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.lla_pos.alt, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \
     if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0;                                    \
     if (i >= gps.nb_channels * 2) i = 0;                                    \

From 8aabd2633277afe517675f0883e9525ab7247cad Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 12 May 2011 11:04:56 +0200
Subject: [PATCH 84/99] Airframe

---
 conf/airframes/TU_Delft/skywalker.xml | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/conf/airframes/TU_Delft/skywalker.xml b/conf/airframes/TU_Delft/skywalker.xml
index d738e886cf7..745fe4ae914 100644
--- a/conf/airframes/TU_Delft/skywalker.xml
+++ b/conf/airframes/TU_Delft/skywalker.xml
@@ -180,19 +180,19 @@
       <configure name="XSENS_UART_NR" value="0"/>
     </load>
 
-    <load name="light.xml">
+<!--    <load name="light.xml">
       <define name="LIGHT_LED_STROBE" value="3"/>
       <define name="LIGHT_LED_NAV" value="2"/>
       <define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
       <define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
     </load>
 
-<!--    <load name="digital_cam_i2c.xml"/>  -->
-  <!--    <load name="ins_ppzuavimu.xml"/>  -->
+    <load name="digital_cam_i2c.xml"/>  -->
+  <!--    <load name="ins_ppzuavimu.xml"/>  
     <load name="digital_cam.xml" >
       <define name="DC_SHUTTER_LED" value="2"/>
     </load>
-
+-->
   </modules>
 
   <firmware name="fixedwing">
@@ -219,7 +219,9 @@
     <subsystem name="navigation"/>
     <subsystem name="gps" type="xsens"/>
 
+<!--
     <subsystem name="i2c"/>
+-->
 
 </firmware>
 

From e346e983195b608b03c5749954b8dbc7a8bc715d Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 12 May 2011 11:10:23 +0200
Subject: [PATCH 85/99] Default Fixedwing IMU includes MAG when available and
 RAW data in tele_mode 2

---
 conf/telemetry/default_fixedwing_imu.xml | 18 ++++++++++++++++++
 sw/airborne/ap_downlink.h                |  8 +++++++-
 2 files changed, 25 insertions(+), 1 deletion(-)

diff --git a/conf/telemetry/default_fixedwing_imu.xml b/conf/telemetry/default_fixedwing_imu.xml
index c106da4e29d..5435f101128 100644
--- a/conf/telemetry/default_fixedwing_imu.xml
+++ b/conf/telemetry/default_fixedwing_imu.xml
@@ -28,6 +28,7 @@
       <message name="GPS_SOL"        period="2.0"/>
       <message name="IMU_ACCEL"      period=".8"/>
       <message name="IMU_GYRO"       period=".6"/>
+      <message name="IMU_MAG"        period="1.3"/>
     </mode>
     <mode name="minimal">
       <message name="ALIVE"          period="5"/>
@@ -50,6 +51,23 @@
       <message name="SURVEY"         period="2.1"/>
       <message name="GPS_SOL"        period="5.0"/>
     </mode>
+    <mode name="debug_imu">
+      <message name="ATTITUDE"       period="0.1"/>
+      <message name="ALIVE"          period="5"/>
+      <message name="GPS"            period="5.1"/>
+      <message name="ESTIMATOR"      period="5.3"/>
+      <message name="BAT"            period="10.1"/>
+      <message name="DESIRED"        period="10.2"/>
+      <message name="NAVIGATION"     period="5.4"/>
+      <message name="PPRZ_MODE"      period="5.5"/>
+      <message name="DOWNLINK"       period="5.7"/>
+      <message name="IMU_ACCEL"      period=".5"/>
+      <message name="IMU_GYRO"       period=".5"/>
+      <message name="IMU_MAG"        period=".5"/>
+      <message name="IMU_ACCEL_RAW"  period=".5"/>
+      <message name="IMU_GYRO_RAW"   period=".5"/>
+      <message name="IMU_MAG_RAW"    period=".5"/>
+    </mode>
     <mode name="extremal">
       <message name="ALIVE"          period="5"/>
       <message name="GPS"            period="5.1"/>
diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
index abfcdba69d3..8947b97a6f5 100644
--- a/sw/airborne/ap_downlink.h
+++ b/sw/airborne/ap_downlink.h
@@ -145,12 +145,18 @@
 #define PERIODIC_SEND_IMU_MAG_RAW(_chan) { DOWNLINK_SEND_IMU_MAG_RAW(_chan, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)}
 #define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)}
 #define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)}
-#define PERIODIC_SEND_IMU_MAG(_chan) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(_chan, &mag_float.x, &mag_float.y, &mag_float.z)}
+#  ifdef USE_MAGNETOMETER
+#    define PERIODIC_SEND_IMU_MAG(_chan) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(_chan, &mag_float.x, &mag_float.y, &mag_float.z)}
+#  else
+#    define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
+#  endif
 #else
 #define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {}
 #define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
+#define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
 #define PERIODIC_SEND_IMU_ACCEL(_chan) {}
 #define PERIODIC_SEND_IMU_GYRO(_chan) {}
+#define PERIODIC_SEND_IMU_MAG(_chan) {}
 #endif
 
 #ifdef IMU_ANALOG

From f4a736a545a41bf55cdb356a2ea279408a0bab12 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 12 May 2011 14:13:47 +0200
Subject: [PATCH 86/99] Added support for PPZUAV-9DOF-IMU

---
 .../PPZUAV/fixed-wing/ppzimu_tiny.xml         | 271 ++++++++++++++++++
 conf/autopilot/fixedwing.makefile             |   1 +
 .../fixedwing/attitude_dcm.makefile           |   1 +
 .../fixedwing/imu_aspirin_i2c.makefile        |   5 +
 .../subsystems/fixedwing/imu_ppzuav.makefile  |  22 ++
 conf/modules/ins_ppzuavimu.xml                |  24 ++
 conf/telemetry/default_fixedwing_imu.xml      |  18 ++
 sw/airborne/ap_downlink.h                     |   8 +
 sw/airborne/firmwares/fixedwing/main_ap.c     |  22 +-
 sw/airborne/modules/ins/ins_ppzuavimu.c       | 269 +++++++++++++++++
 sw/airborne/modules/ins/ins_ppzuavimu.h       |  57 ++++
 sw/airborne/peripherals/adxl345.h             |   2 +
 sw/airborne/peripherals/hmc5843.h             |   4 +-
 sw/airborne/subsystems/ahrs/ahrs_float_dcm.c  |   6 +
 sw/airborne/subsystems/ahrs/ahrs_float_dcm.h  |   2 +
 .../subsystems/ahrs/ahrs_float_utils.h        |   2 +
 sw/airborne/subsystems/ahrs/ahrs_int_utils.h  |   4 +-
 .../ahrs/ahrs_magnetic_field_model.h          |  10 +
 18 files changed, 720 insertions(+), 8 deletions(-)
 create mode 100644 conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
 create mode 100644 conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile
 create mode 100644 conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile
 create mode 100644 conf/modules/ins_ppzuavimu.xml
 create mode 100644 sw/airborne/modules/ins/ins_ppzuavimu.c
 create mode 100644 sw/airborne/modules/ins/ins_ppzuavimu.h
 create mode 100644 sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h

diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
new file mode 100644
index 00000000000..0fbca96a268
--- /dev/null
+++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -0,0 +1,271 @@
+<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
+
+<!--
+	YAPA + XSens + XBee
+-->
+
+<airframe name="Yapa v1">
+
+  <servos>
+    <servo name="THROTTLE" no="4" min="1000" neutral="1000" max="2000"/>
+    <servo name="AILERON_LEFT" no="5" min="1000" neutral="1500" max="2000"/>
+    <servo name="AILERON_RIGHT" no="1" min="2000" neutral="1500" max="1000"/>
+    <servo name="ELEVATOR" no="8" min="2000" neutral="1500" max="1000"/>
+    <servo name="RUDDER" no="9" min="1100" neutral="1500" max="1900"/>
+  </servos>
+
+  <commands>
+    <axis name="THROTTLE" failsafe_value="0"/>
+    <axis name="ROLL" failsafe_value="0"/>
+    <axis name="PITCH" failsafe_value="0"/>
+    <axis name="BRAKE" failsafe_value="0"/>	<!-- both elerons up as butterfly brake ? -->
+  </commands>
+
+  <rc_commands>
+    <set command="THROTTLE" value="@THROTTLE"/>
+    <set command="ROLL" value="@ROLL"/>
+    <set command="PITCH" value="@PITCH"/>
+    <set command="BRAKE" value="@YAW"/>
+  </rc_commands>
+
+  <command_laws>
+    <set servo="AILERON_LEFT" value="@ROLL"/>
+    <set servo="AILERON_RIGHT" value="@ROLL"/>
+    <set servo="THROTTLE" value="@THROTTLE"/>
+    <set servo="ELEVATOR" value="@PITCH"/>
+  </command_laws>
+
+  <!-- Local magnetic field -->
+  <section name="AHRS" prefix="AHRS_" >
+    <define name="H_X" value="0.51562740288882" />
+    <define name="H_Y" value="-0.05707735220832" />
+    <define name="H_Z" value="0.85490967783446" />
+  </section>
+
+  <section name="IMU" prefix="IMU_">
+    <!-- Calibration Neutral -->
+    <define name="GYRO_P_NEUTRAL" value="0"/>
+    <define name="GYRO_Q_NEUTRAL" value="0"/>
+    <define name="GYRO_R_NEUTRAL" value="0"/>
+
+    <!-- SENS = 14.375 LSB/(deg/sec) * 57.6 deg/rad = 828 LSB/rad/sec / 12bit FRAC: 4096 / 828 -->
+    <define name="GYRO_P_SENS" value="4.947" integer="16"/>
+    <define name="GYRO_Q_SENS" value="4.947" integer="16"/>
+    <define name="GYRO_R_SENS" value="4.947" integer="16"/>
+
+    <define name="GYRO_P_Q" value="0."/>
+    <define name="GYRO_P_R" value="0"/>
+    <define name="GYRO_Q_P" value="0."/>
+    <define name="GYRO_Q_R" value="0."/>
+    <define name="GYRO_R_P" value="0."/>
+    <define name="GYRO_R_Q" value="0."/>
+
+    <define name="GYRO_P_SIGN" value="1"/>
+    <define name="GYRO_Q_SIGN" value="1"/>
+    <define name="GYRO_R_SIGN" value="1"/>
+
+    <define name="ACCEL_X_NEUTRAL" value="-14"/>
+    <define name="ACCEL_Y_NEUTRAL" value="0"/>
+    <define name="ACCEL_Z_NEUTRAL" value="0"/>
+
+    <!-- SENS = 256 LSB/g @ 2.5V [X&Y: 265 LSB/g @ 3.3V] / 9.81 ms2/g = 26.095 LSB/ms2 / 10bit FRAC: 1024 / 26.095 for z and 1024 / 27.01 for X&Y -->
+    <define name="ACCEL_X_SENS" value="37.9" integer="16"/>
+    <define name="ACCEL_Y_SENS" value="37.9" integer="16"/>
+    <define name="ACCEL_Z_SENS" value="39.24" integer="16"/>
+
+    <define name="ACCEL_X_SIGN" value="1"/>
+    <define name="ACCEL_Y_SIGN" value="1"/>
+    <define name="ACCEL_Z_SIGN" value="1"/>
+
+    <define name="MAG_X_NEUTRAL" value="0"/>
+    <define name="MAG_Y_NEUTRAL" value="0"/>
+    <define name="MAG_Z_NEUTRAL" value="0"/>
+
+    <define name="MAG_X_SENS" value="1" integer="16"/>
+    <define name="MAG_Y_SENS" value="1" integer="16"/>
+    <define name="MAG_Z_SENS" value="1" integer="16"/>
+
+    <define name="MAG_X_SIGN" value="1"/>
+    <define name="MAG_Y_SIGN" value="1"/>
+    <define name="MAG_Z_SIGN" value="1"/>
+
+    <define name="BODY_TO_IMU_PHI" value="0"/>
+    <define name="BODY_TO_IMU_THETA" value="0"/>
+    <define name="BODY_TO_IMU_PSI" value="0"/>
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+  </section>
+
+  <section name="AUTO1" prefix="AUTO1_">
+    <define name="MAX_ROLL" value="0.7"/>
+    <define name="MAX_PITCH" value="0.7"/>
+  </section>
+
+  <section name="BAT">
+    <define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/>
+
+    <define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="10" unit="V"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
+  </section>
+
+  <section name="MISC">
+    <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
+    <define name="CARROT" value="5." unit="s"/>
+    <define name="CONTROL_RATE" value="60" unit="Hz"/>
+    <define name="XBEE_INIT" value="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/>
+<!--    <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
+    <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
+
+    <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
+
+    <define name="GLIDE_AIRSPEED" value="10"/>
+    <define name="GLIDE_VSPEED" value="3."/>
+    <define name="GLIDE_PITCH" value="45" unit="deg"/>
+  </section>
+
+  <section name="VERTICAL CONTROL" prefix="V_CTL_">
+    <define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
+    <!-- outer loop proportional gain -->
+    <define name="ALTITUDE_PGAIN" value="-0.03"/>
+    <!-- outer loop saturation -->
+    <define name="ALTITUDE_MAX_CLIMB" value="2."/>
+
+    <!-- auto throttle inner loop -->
+    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
+    <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
+    <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
+    <define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
+    <define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
+    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
+    <define name="AUTO_THROTTLE_PGAIN" value="-0.01"/>
+    <define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
+    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
+
+    <define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
+  </section>
+
+  <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
+    <define name="COURSE_PGAIN" value="-1.20000004768"/>
+    <define name="COURSE_DGAIN" value="0.3"/>
+    <define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
+
+    <define name="ROLL_MAX_SETPOINT" value="0.75" unit="radians"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+
+    <define name="PITCH_PGAIN" value="-12000."/>
+    <define name="PITCH_DGAIN" value="1.5"/>
+
+    <define name="ELEVATOR_OF_ROLL" value="1000."/>
+
+    <define name="ROLL_SLEW" value="1."/>
+
+    <define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
+    <define name="ROLL_RATE_GAIN" value="0."/>
+  </section>
+
+  <section name="AGGRESSIVE" prefix="AGR_">
+    <define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
+    <define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
+    <define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
+    <define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
+    <define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
+    <define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
+    <define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
+    <define name="DESCENT_NAV_RATIO" value="1.0"/>
+  </section>
+
+  <section name="FAILSAFE" prefix="FAILSAFE_">
+    <define name="DEFAULT_THROTTLE" value="0.35" unit="%"/>
+    <define name="DEFAULT_ROLL" value="0.17" unit="rad"/>
+    <define name="DEFAULT_PITCH" value="0.08" unit="rad"/>
+
+    <define name="HOME_RADIUS" value="DEFAULT_CIRCLE_RADIUS" unit="m"/>
+    <define name="KILL_MODE_DISTANCE" value="(MAX_DIST_FROM_HOME*1.5)"/>
+    <define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
+  </section>
+
+  <section name="DIGITAL_CAMERA" prefix="DC_">
+    <define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
+    <define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
+  </section>
+
+
+  <modules>
+<!--
+    <load name="light.xml">
+      <define name="LIGHT_LED_STROBE" value="3"/>
+      <define name="LIGHT_LED_NAV" value="2"/>
+      <define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
+      <define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
+    </load>
+    -->
+    <!--    <load name="digital_cam_i2c.xml"/>  -->
+
+ <!--     -->
+
+<!--
+    <load name="digital_cam.xml" >
+      <define name="DC_SHUTTER_LED" value="3"/>
+    </load>
+    <load name="ins_ppzuavimu.xml" />
+    -->
+  </modules>
+
+  <firmware name="fixedwing">
+
+    <target name="ap" board="tiny_2.11">
+      <define name="STRONG_WIND"/>
+      <define name="WIND_INFO"/>
+      <define name="WIND_INFO_RET"/>
+
+      <configure name="PERIODIC_FREQUENCY" value="120"/>
+      <configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
+      <configure name="AHRS_CORRECT_FREQUENCY" value="100"/>
+      <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
+
+      <configure name="AHRS_ALIGNER_LED" value="3"/>
+      <configure name="CPU_LED" value="3"/>
+    </target>
+    <target name="sim" board="pc"/>
+
+    <define name="AGR_CLIMB"/>
+    <define name="LOITER_TRIM"/>
+    <define name="ALT_KALMAN"/>
+
+    <!-- Sensors -->
+    <!--
+    <subsystem name="ahrs" type="ic">
+      <define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN" />
+    </subsystem>
+    <subsystem name="imu" type="aspirin_i2c"/>
+-->
+    
+    <subsystem name="imu" type="ppzuav"/>
+    <subsystem name="attitude" type="dcm">
+      <define name="USE_MAGNETOMETER" />
+    </subsystem>
+
+    <subsystem name="radio_control" type="ppm"/>
+
+    <!-- Communication -->
+    <subsystem name="telemetry" type="xbee_api">
+      <configure name="MODEM_BAUD" value="B57600"/>
+    </subsystem>
+
+    <!-- Actuators -->
+    <subsystem name="control"/>
+    <!-- Sensors -->
+    <subsystem name="navigation"/>
+    <subsystem name="gps" type="ublox_lea4p"/>
+
+    <subsystem name="i2c"/>
+
+</firmware>
+
+
+</airframe>
diff --git a/conf/autopilot/fixedwing.makefile b/conf/autopilot/fixedwing.makefile
index 139a7ca919b..3b0758e8bf7 100644
--- a/conf/autopilot/fixedwing.makefile
+++ b/conf/autopilot/fixedwing.makefile
@@ -15,6 +15,7 @@ SRC_FIXEDWING_TEST=$(SRC_FIXEDWING)/
 
 SRC_FIRMWARE=firmwares/fixedwing
 SRC_SUBSYSTEMS=subsystems
+SRC_MODULES=modules
 
 FIXEDWING_INC = -I$(SRC_FIRMWARE) -I$(SRC_FIXEDWING)
 
diff --git a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
index 91b1eb5ecd6..9aa26c0cdf3 100644
--- a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
+++ b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
@@ -10,6 +10,7 @@ ap.CFLAGS += -DUSE_AHRS
 ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs.c
 ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
 ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c
+ap.srcs   += math/pprz_trig_int.c
 
 ifdef AHRS_ALIGNER_LED
   ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
diff --git a/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile b/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile
new file mode 100644
index 00000000000..87d72683c7e
--- /dev/null
+++ b/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile
@@ -0,0 +1,5 @@
+
+
+include $(CFG_FIXEDWING)/imu_ppzuav.makefile
+
+ap.CFLAGS += -DASPIRIN_IMU
diff --git a/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile b/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile
new file mode 100644
index 00000000000..8f224a91668
--- /dev/null
+++ b/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile
@@ -0,0 +1,22 @@
+
+IMU_PPZUAVIMU_CFLAGS  = -DUSE_IMU
+IMU_PPZUAVIMU_CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_ppzuavimu.h\"
+
+IMU_PPZUAVIMU_SRCS    = $(SRC_SUBSYSTEMS)/imu.c             \
+                      $(SRC_MODULES)/ins/ins_ppzuavimu.c
+
+
+IMU_PPZUAVIMU_CFLAGS += -DUSE_I2C
+ifdef STM32
+	IMU_PPZUAVIMU_CFLAGS += -DUSE_I2C2
+	IMU_PPZUAVIMU_CFLAGS += -DPPZUAVIMU_I2C_DEVICE=i2c2
+else
+	IMU_PPZUAVIMU_CFLAGS += -DUSE_I2C0
+	IMU_PPZUAVIMU_CFLAGS += -DPPZUAVIMU_I2C_DEVICE=i2c0
+endif
+
+ap.CFLAGS += $(IMU_PPZUAVIMU_CFLAGS)
+ap.srcs   += $(IMU_PPZUAVIMU_SRCS)
+
+ap.CFLAGS += -DAHRS_MAG_UPDATE_YAW_ONLY
+
diff --git a/conf/modules/ins_ppzuavimu.xml b/conf/modules/ins_ppzuavimu.xml
new file mode 100644
index 00000000000..a2544c204df
--- /dev/null
+++ b/conf/modules/ins_ppzuavimu.xml
@@ -0,0 +1,24 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="ins_ppzuavimu" dir="ins">
+  <!-- <depend conflict="ins" -->
+  <header>
+    <file name="ins_ppzuavimu.h"/>
+  </header>
+
+  <!-- default imu stuff -->
+  <init fun="imu_impl_init()"/>
+  <periodic fun="imu_periodic()" freq="60"/>
+  <!-- ImuEvent called directly from main_ap -->
+
+  <!-- extras to become a usefull module -->
+  <periodic fun="ppzuavimu_module_downlink_raw()" freq="5"/>
+  <event fun="ppzuavimu_module_event()"/>
+
+  <makefile>
+    <file name="ins_ppzuavimu.c"/>
+    <define name="PPZUAVIMU_I2C_DEVICE" value="i2c0" />
+    <define name="USE_I2C" />
+    <define name="USE_I2C0" />
+  </makefile>
+</module>
diff --git a/conf/telemetry/default_fixedwing_imu.xml b/conf/telemetry/default_fixedwing_imu.xml
index c106da4e29d..5435f101128 100644
--- a/conf/telemetry/default_fixedwing_imu.xml
+++ b/conf/telemetry/default_fixedwing_imu.xml
@@ -28,6 +28,7 @@
       <message name="GPS_SOL"        period="2.0"/>
       <message name="IMU_ACCEL"      period=".8"/>
       <message name="IMU_GYRO"       period=".6"/>
+      <message name="IMU_MAG"        period="1.3"/>
     </mode>
     <mode name="minimal">
       <message name="ALIVE"          period="5"/>
@@ -50,6 +51,23 @@
       <message name="SURVEY"         period="2.1"/>
       <message name="GPS_SOL"        period="5.0"/>
     </mode>
+    <mode name="debug_imu">
+      <message name="ATTITUDE"       period="0.1"/>
+      <message name="ALIVE"          period="5"/>
+      <message name="GPS"            period="5.1"/>
+      <message name="ESTIMATOR"      period="5.3"/>
+      <message name="BAT"            period="10.1"/>
+      <message name="DESIRED"        period="10.2"/>
+      <message name="NAVIGATION"     period="5.4"/>
+      <message name="PPRZ_MODE"      period="5.5"/>
+      <message name="DOWNLINK"       period="5.7"/>
+      <message name="IMU_ACCEL"      period=".5"/>
+      <message name="IMU_GYRO"       period=".5"/>
+      <message name="IMU_MAG"        period=".5"/>
+      <message name="IMU_ACCEL_RAW"  period=".5"/>
+      <message name="IMU_GYRO_RAW"   period=".5"/>
+      <message name="IMU_MAG_RAW"    period=".5"/>
+    </mode>
     <mode name="extremal">
       <message name="ALIVE"          period="5"/>
       <message name="GPS"            period="5.1"/>
diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
index 041a6a0b667..8a0c7bc43f1 100644
--- a/sw/airborne/ap_downlink.h
+++ b/sw/airborne/ap_downlink.h
@@ -142,13 +142,21 @@
 #include "subsystems/imu.h"
 #define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)}
 #define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)}
+#define PERIODIC_SEND_IMU_MAG_RAW(_chan) { DOWNLINK_SEND_IMU_MAG_RAW(_chan, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)}
 #define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)}
 #define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)}
+#  ifdef USE_MAGNETOMETER
+#    define PERIODIC_SEND_IMU_MAG(_chan) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(_chan, &mag_float.x, &mag_float.y, &mag_float.z)}
+#  else
+#    define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
+#  endif
 #else
 #define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {}
 #define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
+#define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
 #define PERIODIC_SEND_IMU_ACCEL(_chan) {}
 #define PERIODIC_SEND_IMU_GYRO(_chan) {}
+#define PERIODIC_SEND_IMU_MAG(_chan) {}
 #endif
 
 #ifdef IMU_ANALOG
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index bc5d0a6c01e..9e79daacad7 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -74,7 +74,7 @@
 #ifdef USE_AHRS
 #include "subsystems/ahrs.h"
 #include "subsystems/ahrs/ahrs_aligner.h"
-#include "subsystems/ahrs/ahrs_float_dcm.h"
+#include AHRS_TYPE_H
 static inline void on_gyro_accel_event( void );
 static inline void on_accel_event( void );
 static inline void on_mag_event( void );
@@ -402,6 +402,12 @@ static inline void attitude_loop( void ) {
 
 }
 
+#ifdef USE_IMU
+#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
+volatile uint8_t new_ins_attitude = 0;
+#endif
+#endif
+
 void periodic_task_ap( void ) {
 
   static uint8_t _60Hz = 0;
@@ -731,15 +737,21 @@ static inline void on_gyro_accel_event( void ) {
     LED_OFF(AHRS_CPU_LED);
 #endif
 
+#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
+  new_ins_attitude = 1;
+#endif
+
 }
 
-static inline void on_mag_event(void) {
-  /*
+static inline void on_mag_event(void)
+{
+#ifdef IMU_MAG_X_SIGN
   ImuScaleMag(imu);
   if (ahrs.status == AHRS_RUNNING) {
     ahrs_update_mag();
-    ahrs_update_fw_estimator();
+//    ahrs_update_fw_estimator();
   }
-  */
+#endif
 }
 #endif // USE_AHRS
+
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c
new file mode 100644
index 00000000000..c0921076161
--- /dev/null
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.c
@@ -0,0 +1,269 @@
+/*
+ * 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 <math.h>
+#include "ins_ppzuavimu.h"
+#include "mcu_periph/i2c.h"
+#include "led.h"
+
+// Downlink
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+
+// Peripherials
+#define HMC5843_NO_IRQ
+#include "../../peripherals/itg3200.h"
+#include "../../peripherals/adxl345.h"
+#include "../../peripherals/hmc5843.h"
+
+// Results
+volatile bool_t mag_valid;
+volatile bool_t gyr_valid;
+volatile bool_t acc_valid;
+
+// Communication
+struct i2c_transaction ppzuavimu_hmc5843;
+struct i2c_transaction ppzuavimu_itg3200;
+struct i2c_transaction ppzuavimu_adxl345;
+
+// Standalone option: run module only
+#ifndef IMU_TYPE_H
+struct Imu imu;
+#endif
+
+#ifndef PERIODIC_FREQUENCY
+#define PERIODIC_FREQUENCY 60
+#endif
+
+void imu_impl_init(void)
+{
+  /////////////////////////////////////////////////////////////////////
+  // ITG3200
+  ppzuavimu_itg3200.type = I2CTransTx;
+  ppzuavimu_itg3200.slave_addr = ITG3200_ADDR;
+  ppzuavimu_itg3200.buf[0] = ITG3200_REG_DLPF_FS;
+#if PERIODIC_FREQUENCY == 60
+  /* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */
+  ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x04<<0);
+#  warning ITG3200 read at 50Hz
+#else
+#  if PERIODIC_FREQUENCY == 120
+#  warning ITG3200 read at 100Hz
+  /* set gyro range to 2000deg/s and low pass at 42Hz (< 120Hz/2) internal sampling at 1kHz */
+  ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x03<<0);
+#  else
+#  error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates
+#  endif
+#endif
+  ppzuavimu_itg3200.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
+    while(ppzuavimu_itg3200.status == I2CTransPending);
+
+  /* set sample rate to 66Hz: so at 60Hz there is always a new sample ready and you loose little */
+  ppzuavimu_itg3200.buf[0] = ITG3200_REG_SMPLRT_DIV;
+#if PERIODIC_FREQUENCY == 60
+  ppzuavimu_itg3200.buf[1] = 19;  // 50Hz
+#else
+  ppzuavimu_itg3200.buf[1] = 9;  // 100Hz
+#endif
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
+    while(ppzuavimu_itg3200.status == I2CTransPending);
+
+  /* switch to gyroX clock */
+  ppzuavimu_itg3200.buf[0] = ITG3200_REG_PWR_MGM;
+  ppzuavimu_itg3200.buf[1] = 0x01;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
+    while(ppzuavimu_itg3200.status == I2CTransPending);
+
+  /* no interrupts for now, but set data ready interrupt to enable reading status bits */
+  ppzuavimu_itg3200.buf[0] = ITG3200_REG_INT_CFG;
+  ppzuavimu_itg3200.buf[1] = 0x01;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200);
+    while(ppzuavimu_itg3200.status == I2CTransPending);
+
+  /////////////////////////////////////////////////////////////////////
+  // ADXL345
+
+  /* set data rate to 100Hz */
+  ppzuavimu_adxl345.slave_addr = ADXL345_ADDR;
+  ppzuavimu_adxl345.type = I2CTransTx;
+  ppzuavimu_adxl345.buf[0] = ADXL345_REG_BW_RATE;
+#if PERIODIC_FREQUENCY == 60
+  ppzuavimu_adxl345.buf[1] = 0x09;  // normal power and 50Hz sampling, 50Hz BW
+#else
+  ppzuavimu_adxl345.buf[1] = 0x0a;  // normal power and 100Hz sampling, 50Hz BW
+#endif
+  ppzuavimu_adxl345.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
+    while(ppzuavimu_adxl345.status == I2CTransPending);
+
+  /* switch to measurement mode */
+  ppzuavimu_adxl345.type = I2CTransTx;
+  ppzuavimu_adxl345.buf[0] = ADXL345_REG_POWER_CTL;
+  ppzuavimu_adxl345.buf[1] = 1<<3;
+  ppzuavimu_adxl345.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
+    while(ppzuavimu_adxl345.status == I2CTransPending);
+
+  /* Set range to 16g but keeping full resolution of 3.9 mV/g */
+  ppzuavimu_adxl345.type = I2CTransTx;
+  ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_FORMAT;
+  ppzuavimu_adxl345.buf[1] = 1<<3 | 0<<2 | 0x03;  // bit 3 is full resolution bit, bit 2 is left justify bit 0,1 are range: 00=2g 01=4g 10=8g 11=16g
+  ppzuavimu_adxl345.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
+    while(ppzuavimu_adxl345.status == I2CTransPending);
+
+  /////////////////////////////////////////////////////////////////////
+  // HMC5843
+  ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR;
+  ppzuavimu_hmc5843.type = I2CTransTx;
+  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA;  // set to rate to max speed: 50Hz no bias
+  ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2);
+  ppzuavimu_hmc5843.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
+    while(ppzuavimu_hmc5843.status == I2CTransPending);
+
+  ppzuavimu_hmc5843.type = I2CTransTx;
+  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGB;  // set to gain to 1 Gauss
+  ppzuavimu_hmc5843.buf[1] = 0x01<<5;
+  ppzuavimu_hmc5843.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
+    while(ppzuavimu_hmc5843.status == I2CTransPending);
+
+  ppzuavimu_hmc5843.type = I2CTransTx;
+  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_MODE;  // set to continuous mode
+  ppzuavimu_hmc5843.buf[1] = 0x00;
+  ppzuavimu_hmc5843.len_w = 2;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843);
+    while(ppzuavimu_hmc5843.status == I2CTransPending);
+
+}
+
+void imu_periodic( void )
+{
+  // Start reading the latest gyroscope data
+  ppzuavimu_itg3200.type = I2CTransTxRx;
+  ppzuavimu_itg3200.len_r = 9;
+  ppzuavimu_itg3200.len_w = 1;
+  ppzuavimu_itg3200.buf[0] = ITG3200_REG_INT_STATUS;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_itg3200);
+
+  // Start reading the latest accelerometer data
+  ppzuavimu_adxl345.type = I2CTransTxRx;
+  ppzuavimu_adxl345.len_r = 6;
+  ppzuavimu_adxl345.len_w = 1;
+  ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_X0;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_adxl345);
+
+  // Start reading the latest magnetometer data
+#if PERIODIC_FREQUENCY > 60
+  RunOnceEvery(2,{
+#endif
+  ppzuavimu_hmc5843.type = I2CTransTxRx;
+  ppzuavimu_hmc5843.len_r = 6;
+  ppzuavimu_hmc5843.len_w = 1;
+  ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM;
+  i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843);
+#if PERIODIC_FREQUENCY > 60
+  });
+#endif
+  //RunOnceEvery(10,ppzuavimu_module_downlink_raw());
+}
+
+void ppzuavimu_module_downlink_raw( void )
+{
+  DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);
+  DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);
+  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&imu.mag_unscaled.x,&imu.mag_unscaled.y,&imu.mag_unscaled.z);
+}
+
+void ppzuavimu_module_event( void )
+{
+  int32_t x, y, z;
+
+  // If the itg3200 I2C transaction has succeeded: convert the data
+  if (ppzuavimu_itg3200.status == I2CTransSuccess)
+  {
+#define ITG_STA_DAT_OFFSET 3
+    x = (int16_t) ((ppzuavimu_itg3200.buf[0+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[1+ITG_STA_DAT_OFFSET]);
+    y = (int16_t) ((ppzuavimu_itg3200.buf[2+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[3+ITG_STA_DAT_OFFSET]);
+    z = (int16_t) ((ppzuavimu_itg3200.buf[4+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[5+ITG_STA_DAT_OFFSET]);
+
+    // Is this is new data
+    if (ppzuavimu_itg3200.buf[0] & 0x01)
+    {
+      //LED_ON(3);
+      gyr_valid = TRUE;
+      //LED_OFF(3);
+    }
+    else
+    {
+    }
+
+    // Signs depend on the way sensors are soldered on the board: so they are hardcoded
+#ifdef ASPIRIN_IMU
+    RATES_ASSIGN(imu.gyro_unscaled, x, -y, -z);
+#else // PPZIMU
+    RATES_ASSIGN(imu.gyro_unscaled, -x, y, -z);
+#endif
+
+    ppzuavimu_itg3200.status = I2CTransDone;  // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data
+  }
+
+  // If the adxl345 I2C transaction has succeeded: convert the data
+  if (ppzuavimu_adxl345.status == I2CTransSuccess)
+  {
+    x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
+    y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
+    z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
+
+#ifdef ASPIRIN_IMU
+    VECT3_ASSIGN(imu.accel_unscaled, x, -y, -z);
+#else // PPZIMU
+    VECT3_ASSIGN(imu.accel_unscaled, -x, y, -z);
+#endif
+
+    acc_valid = TRUE;
+    ppzuavimu_adxl345.status = I2CTransDone;
+  }
+
+  // If the hmc5843 I2C transaction has succeeded: convert the data
+  if (ppzuavimu_hmc5843.status == I2CTransSuccess)
+  {
+    x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]);
+    y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
+    z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
+
+#ifdef ASPIRIN_IMU
+    VECT3_ASSIGN(imu.mag_unscaled, x, -y, -z);
+#else // PPZIMU
+    VECT3_ASSIGN(imu.mag_unscaled, -y, -x, -z);
+#endif
+
+    mag_valid = TRUE;
+    ppzuavimu_hmc5843.status = I2CTransDone;
+  }
+}
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.h b/sw/airborne/modules/ins/ins_ppzuavimu.h
new file mode 100644
index 00000000000..8ffe325d299
--- /dev/null
+++ b/sw/airborne/modules/ins/ins_ppzuavimu.h
@@ -0,0 +1,57 @@
+/*
+ * 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.
+ *
+ */
+
+#ifndef PPZUAVIMU_H
+#define PPZUAVIMU_H
+
+#include "std.h"
+#include "subsystems/imu.h"
+
+extern volatile bool_t gyr_valid;
+extern volatile bool_t acc_valid;
+extern volatile bool_t mag_valid;
+
+/* must be defined in order to be IMU code: declared in imu.h
+extern void imu_impl_init(void);
+extern void imu_periodic(void);
+*/
+
+#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) {   \
+    ppzuavimu_module_event();                \
+    if (gyr_valid) {                         \
+      gyr_valid = FALSE;                     \
+      _gyro_handler();                                  \
+    }                                                   \
+    if (acc_valid) {                         \
+      acc_valid = FALSE;                     \
+      _accel_handler();                                 \
+    }                                                   \
+    if (mag_valid) {                         \
+      mag_valid = FALSE;                     \
+      _mag_handler();                                 \
+    }                                                   \
+}
+
+/* Own Extra Functions */
+extern void ppzuavimu_module_event( void );
+extern void ppzuavimu_module_downlink_raw( void );
+
+
+#endif // PPZUAVIMU_H
diff --git a/sw/airborne/peripherals/adxl345.h b/sw/airborne/peripherals/adxl345.h
index de6fa023051..5e0b5559eff 100644
--- a/sw/airborne/peripherals/adxl345.h
+++ b/sw/airborne/peripherals/adxl345.h
@@ -1,6 +1,8 @@
 #ifndef ADXL345_H
 #define ADXL345_H
 
+#define ADXL345_ADDR            0xA6
+#define ADXL345_ADDR_ALT        0x3A
 
 #define ADXL345_REG_BW_RATE     0x2C
 #define ADXL345_REG_POWER_CTL   0x2D
diff --git a/sw/airborne/peripherals/hmc5843.h b/sw/airborne/peripherals/hmc5843.h
index 16a3d37351c..2601dae09d3 100644
--- a/sw/airborne/peripherals/hmc5843.h
+++ b/sw/airborne/peripherals/hmc5843.h
@@ -27,7 +27,6 @@
 #include "std.h"
 #include "mcu_periph/i2c.h"
 
-#include "peripherals/hmc5843_arch.h"
 
 struct Hmc5843 {
     struct i2c_transaction i2c_trans;
@@ -44,8 +43,11 @@ struct Hmc5843 {
 
 extern struct Hmc5843 hmc5843;
 
+#ifndef HMC5843_NO_IRQ
+#include "peripherals/hmc5843_arch.h"
 extern void hmc5843_arch_init( void );
 extern void hmc5843_arch_reset( void );
+#endif
 
 extern void hmc5843_init(void);
 extern void hmc5843_periodic(void);
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
index b7ebe07f6bb..4a3e9abce3f 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
@@ -213,6 +213,12 @@ void ahrs_update_accel(void)
 
   ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
 
+  // DCM filter uses g-force as positive
+  // accelerometer measures [0 0 -g] in a static case
+  accel_float.x = -accel_float.x;
+  accel_float.y = -accel_float.y;
+  accel_float.z = -accel_float.z;
+
 #ifdef USE_GPS
   if (gps_mode==3) {    //Remove centrifugal acceleration.
     accel_float.y += gps_speed_3d/100. * Omega[2];  // Centrifugal force on Acc_y = GPS_speed*GyroZ
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
index ea54f0c1d45..f731cf5a71d 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
@@ -53,7 +53,9 @@ void ahrs_update_fw_estimator(void);
 #define GRAVITY 9.81
 
 
+#ifndef OUTPUTMODE
 #define OUTPUTMODE 1
+#endif
 // Mode 0 = DCM integration without Ki gyro bias
 // Mode 1 = DCM integration with Kp and Ki
 // Mode 2 = direct accelerometer -> euler
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
index 03374c81498..69ac9c28c2a 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
@@ -1,6 +1,8 @@
 #ifndef AHRS_FLOAT_UTILS_H
 #define AHRS_FLOAT_UTILS_H
 
+#include "subsystems/ahrs/ahrs_magnetic_field_model.h"
+
 static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) {
   /* get phi and theta from accelerometer */
   struct FloatVect3 accelf;
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h
index 84918fcc620..73450f57916 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h
@@ -4,7 +4,7 @@
 //#include "../../test/pprz_algebra_print.h"
 #include "math/pprz_algebra_int.h"
 
-#include "generated/airframe.h"
+#include "subsystems/ahrs/ahrs_magnetic_field_model.h"
 
 static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) {
   //  DISPLAY_INT32_VECT3("# accel", (*accel));
@@ -17,7 +17,7 @@ static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, stru
   int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel->x, INT32_TRIG_FRAC);
   const float ftheta = atan2f(-cphi_ax, -accel->z);
   e->theta = ANGLE_BFP_OF_REAL(ftheta);
-    
+
   int32_t sphi;
   PPRZ_ITRIG_SIN(sphi, e->phi);
   int32_t stheta;
diff --git a/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h b/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h
new file mode 100644
index 00000000000..b548ea3b048
--- /dev/null
+++ b/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h
@@ -0,0 +1,10 @@
+#include "generated/airframe.h"
+
+#ifndef AHRS_H_X
+#ifdef AHRS_H_Y
+#error Either define both AHRS_H_X and AHRS_H_Y or none, but not half
+#endif
+#define AHRS_H_X 1
+#define AHRS_H_Y 0
+#endif
+

From 65f17b5ceda9c991fddf08b0299d8daf8d42f42b Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 12 May 2011 17:56:10 +0200
Subject: [PATCH 87/99] adding raw imu (slow rate) as default should help
 beginners in getting there stuff to work

---
 conf/telemetry/default_fixedwing_imu.xml | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/conf/telemetry/default_fixedwing_imu.xml b/conf/telemetry/default_fixedwing_imu.xml
index 5435f101128..181c1f5bef9 100644
--- a/conf/telemetry/default_fixedwing_imu.xml
+++ b/conf/telemetry/default_fixedwing_imu.xml
@@ -29,6 +29,9 @@
       <message name="IMU_ACCEL"      period=".8"/>
       <message name="IMU_GYRO"       period=".6"/>
       <message name="IMU_MAG"        period="1.3"/>
+      <message name="IMU_ACCEL_RAW"  period="5.2"/>
+      <message name="IMU_GYRO_RAW"   period="5.3"/>
+      <message name="IMU_MAG_RAW"    period="5.4"/>
     </mode>
     <mode name="minimal">
       <message name="ALIVE"          period="5"/>

From 9b02346532662f6cf84832242e511a094fdda52b Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 12 May 2011 19:39:40 +0200
Subject: [PATCH 88/99] gps-timeout fixed

---
 conf/airframes/TU_Delft/skywalker.xml | 73 ++++++++++++++-------------
 sw/airborne/modules/ins/ins_xsens.c   | 29 +++++++----
 2 files changed, 58 insertions(+), 44 deletions(-)

diff --git a/conf/airframes/TU_Delft/skywalker.xml b/conf/airframes/TU_Delft/skywalker.xml
index 745fe4ae914..a0f9529f277 100644
--- a/conf/airframes/TU_Delft/skywalker.xml
+++ b/conf/airframes/TU_Delft/skywalker.xml
@@ -8,8 +8,8 @@
 
   <servos>
     <servo name="THROTTLE" no="0" min="1100" neutral="1100" max="1900"/>
-    <servo name="AILERON_LEFT" no="2" min="1100" neutral="1500" max="1900"/>
-    <servo name="AILERON_RIGHT" no="6" min="1100" neutral="1500" max="1900"/>
+    <servo name="AILERON_LEFT" no="2" min="900" neutral="1500" max="2100"/>
+    <servo name="AILERON_RIGHT" no="6" min="900" neutral="1500" max="2100"/>
     <servo name="ELEVATOR" no="7" min="1900" neutral="1500" max="1100"/>
     <servo name="RUDDER" no="3" min="1100" neutral="1500" max="1900"/>
   </servos>
@@ -31,7 +31,7 @@
   </rc_commands>
 
   <section name="SERVO_MIXER_GAINS">
-    <define name="AILERON_NEUTRAL" value="0.45f"/>
+    <define name="AILERON_NEUTRAL" value="0.3f"/>
 
     <define name="AILERON_RATE_UP" value="1.0f"/>
     <define name="AILERON_RATE_DOWN" value="0.5f"/>
@@ -67,7 +67,7 @@
     <set servo="AILERON_LEFT" value="($aileron_up * $leftturn) + ($aileron_down * $rightturn) - $brake_value*(BRAKE_AILEVON) - (MAX_PPRZ * AILERON_NEUTRAL)"/>
     <set servo="AILERON_RIGHT" value="($aileron_up * $rightturn) + ($aileron_down * $leftturn) + $brake_value*(BRAKE_AILEVON) + (MAX_PPRZ *AILERON_NEUTRAL)"/>
 
-    <set servo="RUDDER" value="@YAW"/>
+    <set servo="RUDDER" value="@YAW + @ROLL * 0.3"/>
     <set servo="THROTTLE" value="@THROTTLE"/>
 
     <!-- Pitch with Brake-Trim Function -->
@@ -75,12 +75,12 @@
   </command_laws>
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="0.7"/>
-    <define name="MAX_PITCH" value="0.7"/>
+    <define name="MAX_ROLL" value="RadOfDeg(75)"/>
+    <define name="MAX_PITCH" value="RadOfDeg(45)"/>
   </section>
 
   <section name="BAT">
-    <define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/>
+<!--    <define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/> -->
 
     <define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
     <define name="CRITIC_BAT_LEVEL" value="10" unit="V"/>
@@ -110,54 +110,56 @@
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
     <define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
     <!-- outer loop proportional gain -->
-    <define name="ALTITUDE_PGAIN" value="-0.03"/>
+    <define name="ALTITUDE_PGAIN" value="-0.108000002801"/>
     <!-- outer loop saturation -->
     <define name="ALTITUDE_MAX_CLIMB" value="2."/>
 
     <!-- auto throttle inner loop -->
-    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
+    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.57800000906"/>
     <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
     <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
     <define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
     <define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
-    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
-    <define name="AUTO_THROTTLE_PGAIN" value="-0.01"/>
-    <define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
-    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
+    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.108999997377" unit="%/(m/s)"/>
+    <define name="AUTO_THROTTLE_PGAIN" value="0."/>
+    <define name="AUTO_THROTTLE_IGAIN" value="0."/>
+    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.324999988079"/>
 
     <define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
   </section>
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="-1.20000004768"/>
-    <define name="COURSE_DGAIN" value="0.3"/>
-    <define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
+    <define name="COURSE_DGAIN" value="0.300000011921"/>
+    <define name="COURSE_PRE_BANK_CORRECTION" value="1.01600003242"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.75" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.851999998093" unit="radians"/>
     <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
     <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
 
-    <define name="PITCH_PGAIN" value="-12000."/>
-    <define name="PITCH_DGAIN" value="1.5"/>
+    <define name="PITCH_PGAIN" value="-13476.5615234"/>
+    <define name="PITCH_DGAIN" value="7.73400020599"/>
 
-    <define name="ELEVATOR_OF_ROLL" value="1000."/>
+    <define name="ELEVATOR_OF_ROLL" value="3007.81298828"/>
 
     <define name="ROLL_SLEW" value="1."/>
 
-    <define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
-    <define name="ROLL_RATE_GAIN" value="0."/>
+    <define name="ROLL_ATTITUDE_GAIN" value="-11718.75"/>
+    <define name="ROLL_RATE_GAIN" value="-820.312011719"/>
   </section>
 
+<!--
   <section name="AGGRESSIVE" prefix="AGR_">
-    <define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
-    <define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
-    <define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
-    <define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
-    <define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
-    <define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
-    <define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
+    <define name="BLEND_START" value="20"/>
+    <define name="BLEND_END" value="10"/>
+    <define name="CLIMB_THROTTLE" value="1.00"/>
+    <define name="CLIMB_PITCH" value="0.3"/>
+    <define name="DESCENT_THROTTLE" value="0.1"/>
+    <define name="DESCENT_PITCH" value="-0.25"/>
+    <define name="CLIMB_NAV_RATIO" value="0.8"/>
     <define name="DESCENT_NAV_RATIO" value="1.0"/>
   </section>
+-->
 
   <section name="FAILSAFE" prefix="FAILSAFE_">
     <define name="DEFAULT_THROTTLE" value="0.35" unit="%"/>
@@ -180,19 +182,19 @@
       <configure name="XSENS_UART_NR" value="0"/>
     </load>
 
-<!--    <load name="light.xml">
+    <load name="light.xml">
       <define name="LIGHT_LED_STROBE" value="3"/>
       <define name="LIGHT_LED_NAV" value="2"/>
       <define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
       <define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
     </load>
 
-    <load name="digital_cam_i2c.xml"/>  -->
-  <!--    <load name="ins_ppzuavimu.xml"/>  
-    <load name="digital_cam.xml" >
+<!--    <load name="digital_cam_i2c.xml"/>  -->
+  <!--    <load name="ins_ppzuavimu.xml"/>  -->
+    <load name="digital_cam.xml">
       <define name="DC_SHUTTER_LED" value="2"/>
     </load>
--->
+
   </modules>
 
   <firmware name="fixedwing">
@@ -202,7 +204,10 @@
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
       <define name="LOITER_TRIM"/>
+<!--
       <define name="ALT_KALMAN"/>
+      <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
+-->
     </target>
     <target name="sim" board="pc"/>
 
@@ -219,9 +224,7 @@
     <subsystem name="navigation"/>
     <subsystem name="gps" type="xsens"/>
 
-<!--
     <subsystem name="i2c"/>
--->
 
 </firmware>
 
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index 58c0e6bcb50..c2335393ce2 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -32,6 +32,7 @@
 
 #include "generated/airframe.h"
 
+#include "sys_time.h"
 #include "downlink.h"
 #include "messages.h"
 
@@ -225,7 +226,7 @@ void handle_ins_msg( void) {
 
 
   // Send to Estimator (Control)
-  EstimatorSetAtt(ins_phi, ins_psi, ins_theta);
+  EstimatorSetAtt(ins_phi+ins_roll_neutral, ins_psi, ins_theta+ins_pitch_neutral);
   EstimatorSetRate(ins_p,ins_q);
 
   // Position
@@ -236,13 +237,17 @@ void handle_ins_msg( void) {
   EstimatorSetPosXY(gps_east, gps_north);
 
   // Altitude and vertical speed
-  EstimatorSetAlt(-ins_z);
+  float hmsl = gps.hmsl;
+  hmsl /= 1000.0f;
+  EstimatorSetAlt(hmsl);
 
   // Horizontal speed
   float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
   if (gps.fix != GPS_FIX_3D)
+  {
     fspeed = 0;
-  float fclimb = -ins_vz;
+  }
+  float fclimb = -ins_vz * ;
   float fcourse = atan2f((float)ins_vy, (float)ins_vx);
   EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
 
@@ -251,7 +256,6 @@ void handle_ins_msg( void) {
   gps.gspeed = fspeed * 100.;
   gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);
   gps.course = fcourse * 1e7;
-  // reset_gps_watchdog();
 }
 
 void parse_ins_msg( void ) {
@@ -270,6 +274,8 @@ void parse_ins_msg( void ) {
     gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
     gps.num_sv = 0;
 
+    gps.last_fix_time = cpu_time_sec;
+
     uint8_t i;
     // Do not write outside buffer
     for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
@@ -297,7 +303,10 @@ void parse_ins_msg( void ) {
       }
       if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
 #if defined(USE_GPS_XSENS_RAW_DATA) && defined(USE_GPS_XSENS)
-    LED_TOGGLE(3);
+#ifdef GPS_LED
+    LED_TOGGLE(GPS_LED);
+#endif
+        gps.last_fix_time = cpu_time_sec;
         gps.week = 0; // FIXME
         gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
         gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset));
@@ -326,11 +335,11 @@ void parse_ins_msg( void ) {
 	// Compute geoid (MSL) height
         float hmsl;
 	WGS84_ELLIPSOID_TO_GEOID(lla_f.lat,lla_f.lon,hmsl);
-        gps.hmsl =  (hmsl * 1000.0f);
+        gps.hmsl =  XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) - (hmsl * 1000.0f);
 
-        ins_vx = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.;
-        ins_vy = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset) / 100.;
-        ins_vz = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 100.;
+        ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset)) / 100.;
+        ins_vy = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset)) / 100.;
+        ins_vz = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset)) / 100.;
         gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset);
         gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset);
         gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset);
@@ -405,6 +414,8 @@ void parse_ins_msg( void ) {
       }
       if (XSENS_MASK_Position(xsens_output_mode)) {
 #if (!defined(USE_GPS_XSENS_RAW_DATA)) && defined(USE_GPS_XSENS)
+        gps.last_fix_time = cpu_time_sec;
+
         lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf,offset));
         lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf,offset));
         gps.lla_pos.lat = (int32_t)(lla_f.lat * 1e7);

From 126316aa760496cb06083e8026d435e248f74f03 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 12 May 2011 19:47:14 +0200
Subject: [PATCH 89/99] attempt to add magentometer in DCM filter: do not use
 in flight yet

---
 sw/airborne/subsystems/ahrs/ahrs_float_dcm.c | 70 ++++++++++++++++----
 1 file changed, 57 insertions(+), 13 deletions(-)

diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
index c7ad209c922..936fb8ff1ab 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
@@ -73,7 +73,8 @@ float Update_Matrix[3][3]    = {{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
 float Temporary_Matrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
 
 #ifdef USE_MAGNETOMETER
-float MAG_Heading;
+float MAG_Heading_X = 1;
+float MAG_Heading_Y = 0;
 #endif
 
 static inline void compute_body_orientation_and_rates(void);
@@ -230,17 +231,59 @@ void ahrs_update_accel(void)
   Drift_correction();
 }
 
-#ifdef USE_MAGNETOMETER
-#warning NO_PITCH_AND_ROLL_COMPENSATION_YET
-#endif
+
 void ahrs_update_mag(void)
 {
-  //FIXME: pitch and roll compensation please!!!
-  /*
-  struct Int32Vect3 ltp_mag;
-  INT32_RMAT_VMULT(expected_imu, ahrs.imu_to_ltp_rmat, imu.mag);
-   */
-  MAG_Heading = atan2(imu.mag.y, -imu.mag.x);
+#ifdef USE_MAGNETOMETER
+#warning MAGNETOMETER FEEDBACK NOT TESTED YET
+#endif
+
+
+  float cos_roll;
+  float sin_roll;
+  float cos_pitch;
+  float sin_pitch;
+
+  cos_roll = cos(ahrs_float.ltp_to_imu_euler.phi);
+  sin_roll = sin(ahrs_float.ltp_to_imu_euler.phi);
+  cos_pitch = cos(ahrs_float.ltp_to_imu_euler.theta);
+  sin_pitch = sin(ahrs_float.ltp_to_imu_euler.theta);
+
+
+  // Pitch&Roll Compensation:
+  MAG_Heading_X = imu.mag.x*cos_pitch+imu.mag.y*sin_roll*sin_pitch+imu.mag.z*cos_roll*sin_pitch;
+  MAG_Heading_Y = imu.mag.y*cos_roll-imu.mag.z*sin_roll;
+
+/*
+ *
+  // Magnetic Heading
+  Heading = atan2(-Head_Y,Head_X);
+
+  // Declination correction (if supplied)
+  if( declination != 0.0 )
+  {
+      Heading = Heading + declination;
+      if (Heading > M_PI)    // Angle normalization (-180 deg, 180 deg)
+          Heading -= (2.0 * M_PI);
+      else if (Heading < -M_PI)
+          Heading += (2.0 * M_PI);
+  }
+
+  // Optimization for external DCM use. Calculate normalized components
+  Heading_X = cos(Heading);
+  Heading_Y = sin(Heading);
+*/
+
+  struct FloatVect3 ltp_mag;
+
+  ltp_mag.x = MAG_Heading_X;
+  ltp_mag.y = MAG_Heading_Y;
+
+  // Downlink
+  RunOnceEvery(10,DOWNLINK_SEND_IMU_MAG(DefaultChannel, &ltp_mag.x, &ltp_mag.y, &ltp_mag.z));
+
+  // Magnetic Heading
+  // MAG_Heading = atan2(imu.mag.y, -imu.mag.x);
 }
 
 void Normalize(void)
@@ -374,9 +417,10 @@ void Drift_correction(void)
 
 #ifdef USE_MAGNETOMETER
   // We make the gyro YAW drift correction based on compass magnetic heading
-  float mag_heading_x = cos(MAG_Heading);
-  float mag_heading_y = sin(MAG_Heading);
-  errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x);  //Calculating YAW error
+//  float mag_heading_x = cos(MAG_Heading);
+//  float mag_heading_y = sin(MAG_Heading);
+  // 2D dot product
+  errorCourse=(DCM_Matrix[0][0]*MAG_Heading_Y) + (DCM_Matrix[1][0]*MAG_Heading_X);  //Calculating YAW error
   Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
 
   Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);

From 76ffcf4298f8aa23379e2826f18a52cfb6754400 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 12 May 2011 23:16:20 +0200
Subject: [PATCH 90/99] Kalman filter at 100Hz does not work

---
 conf/airframes/TU_Delft/skywalker.xml | 6 +-----
 sw/airborne/modules/ins/ins_xsens.c   | 2 +-
 2 files changed, 2 insertions(+), 6 deletions(-)

diff --git a/conf/airframes/TU_Delft/skywalker.xml b/conf/airframes/TU_Delft/skywalker.xml
index a0f9529f277..c5b9395843e 100644
--- a/conf/airframes/TU_Delft/skywalker.xml
+++ b/conf/airframes/TU_Delft/skywalker.xml
@@ -98,7 +98,7 @@
     <define name="CONTROL_RATE" value="60" unit="Hz"/>
     <define name="XBEE_INIT" value="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/>
 <!--    <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
-    <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
+<!--    <define name="ALT_KALMAN_ENABLED" value="TRUE"/> -->
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
@@ -204,10 +204,6 @@
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
       <define name="LOITER_TRIM"/>
-<!--
-      <define name="ALT_KALMAN"/>
-      <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
--->
     </target>
     <target name="sim" board="pc"/>
 
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index c2335393ce2..4e1e12fcccf 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -247,7 +247,7 @@ void handle_ins_msg( void) {
   {
     fspeed = 0;
   }
-  float fclimb = -ins_vz * ;
+  float fclimb = -ins_vz;
   float fcourse = atan2f((float)ins_vy, (float)ins_vx);
   EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
 

From 6f57c073e1357d10c2beb2a7673c4c78af739514 Mon Sep 17 00:00:00 2001
From: Allen Ibara <aibara@laptop.(none)>
Date: Thu, 12 May 2011 15:29:08 -0700
Subject: [PATCH 91/99] Update lisa_m to IC ahrs

---
 conf/airframes/esden/lisa_m_pwm.xml | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/conf/airframes/esden/lisa_m_pwm.xml b/conf/airframes/esden/lisa_m_pwm.xml
index bb9c4c53d37..9d87c99f6ab 100644
--- a/conf/airframes/esden/lisa_m_pwm.xml
+++ b/conf/airframes/esden/lisa_m_pwm.xml
@@ -177,6 +177,12 @@
    <define name="INV_M" value ="0.21"/>
   </section>
 
+  <section name="AHRS" prefix="AHRS_">
+    <define name="PROPAGATE_FREQUENCY" value="512"/>
+    <define name="H_X" value=" 0.51562740288882"/>
+    <define name="H_Y" value="-0.05707735220832"/>
+    <define name="H_Z" value=" 0.85490967783446"/>
+  </section>
 
  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
    <define name="PGAIN" value="-100"/>
@@ -219,7 +225,7 @@
 
     <subsystem name="imu"           type="aspirin"/>
     <subsystem name="gps"           type="ublox"/>
-    <subsystem name="ahrs"          type="cmpl"/>
+    <subsystem name="ahrs"          type="ic"/>
   </firmware>
 
 

From 22a442ea654c97bd75ede8fe9de9de3b9c469aa1 Mon Sep 17 00:00:00 2001
From: Allen <aibara@jobyenergy.com>
Date: Thu, 12 May 2011 16:44:59 -0700
Subject: [PATCH 92/99] Add airframe for JT with lisa/m

---
 conf/airframes/esden/jt_lisam.xml | 230 ++++++++++++++++++++++++++++++
 1 file changed, 230 insertions(+)
 create mode 100644 conf/airframes/esden/jt_lisam.xml

diff --git a/conf/airframes/esden/jt_lisam.xml b/conf/airframes/esden/jt_lisam.xml
new file mode 100644
index 00000000000..d47df390606
--- /dev/null
+++ b/conf/airframes/esden/jt_lisam.xml
@@ -0,0 +1,230 @@
+<!-- this is a synerani vehicle equiped with Lisa/M and generic china pwm motor controllers -->
+
+<airframe name="jt_lisam">
+
+  <servos min="0" neutral="0" max="0xff">
+    <servo name="A1"            no="0" min="1000" neutral="1000" max="2000"/>
+    <servo name="A2"            no="1" min="1000" neutral="1000" max="2000"/>
+    <servo name="B1"            no="2" min="1000" neutral="1000" max="2000"/>
+    <servo name="B2"            no="3" min="1000" neutral="1000" max="2000"/>
+    <servo name="AILEVON_LEFT"  no="4" min="1000" neutral="1500" max="2000"/>
+    <servo name="AILEVON_RIGHT" no="5" min="1000" neutral="1500" max="2000"/>
+  </servos>
+
+  <commands>
+    <axis name="PITCH"  failsafe_value="0"/>
+    <axis name="ROLL"   failsafe_value="0"/>
+    <axis name="YAW"    failsafe_value="0"/>
+    <axis name="THRUST" failsafe_value="0"/>
+  </commands>
+  
+  <!--
+  <command_laws>
+    <let var="aildderon"  value="@ROLL  * AILEVON_AILERON_RATE"/>
+    <let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
+    <set servo="MOTOR"         value="@THROTTLE"/>
+    <set servo="AILEVON_LEFT"  value="$elevator + $aileron"/>
+    <set servo="AILEVON_RIGHT" value="$elevator - $aileron"/>
+    <set servo="FRONT" value="0"/>
+    <set servo="BACK" value="1"/>
+    <set servo="LEFT" value="2"/>
+    <set servo="RIGHT" value="3"/>
+
+  </command_laws>
+  -->
+
+  <!-- for the sim -->
+  <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
+    <define name="NB" value="4"/>
+    <define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
+  </section>
+
+
+  <section name="SUPERVISION" prefix="SUPERVISION_">
+    <define name="MIN_MOTOR" value="1150"/>
+    <define name="MAX_MOTOR" value="2000"/>
+    <define name="TRIM_A" value="0"/>
+    <define name="TRIM_E" value="0"/>
+    <define name="TRIM_R" value="0"/>
+    <define name="NB_MOTOR" value="4"/>
+    <define name="SCALE" value="256"/>
+    <define name="ROLL_COEF"   value="{  256, -256,  256, -256 }"/>
+    <define name="PITCH_COEF"  value="{ -256, -256,  256,  256 }"/>
+    <define name="YAW_COEF"    value="{    0,    0,    0,    0 }"/>
+    <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
+  </section>
+
+  <include href="conf/airframes/esden/calib/aspirin_012.xml"/>
+
+  <section name="IMU" prefix="IMU_">
+    
+    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg( 180. )"/>
+    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(  90. )"/>
+    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(   0. )"/>
+
+  </section>
+
+ <section name="AUTOPILOT">
+   <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
+   <define name="MODE_AUTO1"  value="AP_MODE_ATTITUDE_RC_CLIMB"/>
+   <define name="MODE_AUTO2"  value="AP_MODE_HOVER_Z_HOLD"/>
+ </section>
+
+ <section name="BAT">
+   <define name="MILLIAMP_PER_PERCENT" value="0.86"/>
+   <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
+   <define name="BATTERY_SENS"   value="0.48" integer="16"/>
+ </section>
+
+
+  <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
+
+    <define name="SP_MAX_P" value="10000"/>
+    <define name="SP_MAX_Q" value="10000"/>
+    <define name="SP_MAX_R" value="10000"/>
+
+    <define name="GAIN_P" value="-400"/>
+    <define name="GAIN_Q" value="-400"/>
+    <define name="GAIN_R" value="-350"/>
+
+  </section>
+
+  <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
+
+    <!-- setpoints -->
+    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
+    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
+    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="DEADBAND_R"     value="250"/>
+
+    <!-- reference -->
+    <define name="REF_OMEGA_P"  value="RadOfDeg(600)"/>
+    <define name="REF_ZETA_P"   value="0.85"/>
+    <define name="REF_MAX_P"    value="RadOfDeg(400.)"/>
+    <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
+
+    <define name="REF_OMEGA_Q"  value="RadOfDeg(600)"/>
+    <define name="REF_ZETA_Q"   value="0.85"/>
+    <define name="REF_MAX_Q"    value="RadOfDeg(400.)"/>
+    <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
+
+    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_ZETA_R"   value="0.85"/>
+    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
+
+    <!-- feedback -->
+    <define name="PHI_PGAIN"  value="-1900"/>
+    <define name="PHI_DGAIN"  value="-380"/>
+    <define name="PHI_IGAIN"  value="-200"/>
+
+    <define name="THETA_PGAIN"  value="-1900"/>
+    <define name="THETA_DGAIN"  value="-380"/>
+    <define name="THETA_IGAIN"  value="-200"/>
+
+    <define name="PSI_PGAIN"  value="-2000"/>
+    <define name="PSI_DGAIN"  value="-400"/>
+    <define name="PSI_IGAIN"  value="-10"/>
+
+    <!-- feedforward -->
+    <define name="PHI_DDGAIN"   value=" 300"/>
+    <define name="THETA_DDGAIN" value=" 300"/>
+    <define name="PSI_DDGAIN"   value=" 300"/>
+
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="BARO_SENS" value="3.3" integer="16"/>
+  </section>
+
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
+   <define name="MIN_ERR_Z"   value="POS_BFP_OF_REAL(-10.)"/>
+   <define name="MAX_ERR_Z"   value="POS_BFP_OF_REAL( 10.)"/>
+   <define name="MIN_ERR_ZD"  value="SPEED_BFP_OF_REAL(-10.)"/>
+   <define name="MAX_ERR_ZD"  value="SPEED_BFP_OF_REAL( 10.)"/>
+   <define name="MAX_SUM_ERR" value="2000000"/>
+   <define name="HOVER_KP"    value="-150"/>
+   <define name="HOVER_KD"    value="-80"/>
+   <define name="HOVER_KI"    value="-20"/>
+   <!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
+   <define name="RC_CLIMB_COEF" value ="163"/>
+   <!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
+   <define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
+   <define name="INV_M" value ="0.21"/>
+  </section>
+
+  <section name="AHRS" prefix="AHRS_">
+    <define name="PROPAGATE_FREQUENCY" value="512"/>
+    <!-- magnetic field for Santa cruz from http://www.ngdc.noaa.gov/geomagmodels/IGRFWMM.jsp -->
+    <define name="H_X" value=" 0.47577"/>
+    <define name="H_Y" value=" 0.11811"/>
+    <define name="H_Z" value=" 0.87161"/>
+  </section>
+
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
+   <define name="PGAIN" value="-100"/>
+   <define name="DGAIN" value="-100"/>
+   <define name="IGAIN" value="-0"/>
+ </section>
+
+ <section name="MISC">
+   <define name="FACE_REINJ_1"  value="1024"/>
+ </section>
+
+ <section name="SIMULATOR" prefix="NPS_">
+   <define name="ACTUATOR_NAMES"  value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
+   <define name="INITIAL_CONDITITONS" value="&quot;reset00&quot;"/>
+   <define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_booz2_a1.h&quot;"/>
+ </section>
+
+<!--
+ <modules main_freq="512">
+   <load name="vehicle_interface_overo_link.xml"/>
+ </modules>
+-->
+
+  <firmware name="rotorcraft">
+    <target name="ap" board="lisa_m_1.0">
+<!--      <define name="BOOZ_START_DELAY" value="1"/> -->
+      <subsystem name="radio_control" type="spektrum"/>
+      <subsystem name="actuators"     type="pwm_supervision"/>
+      <subsystem name="telemetry"     type="transparent"/>
+      <define name="SERVO_HZ" value="400"/>
+      <define name="RADIO_MODE" value="RADIO_AUX2"/>
+      <define name="RADIO_KILL_SWITCH" value="RADIO_GEAR"/>
+      <define name="RADIO_CONTROL_SPEKTRUM_SIGNS" value="\{1,1,-1,1,-1,-1,-1,1,1,1,1,1\}"/>
+      <define name = "RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT" value = "UART5"/>
+      <define name = "OVERRIDE_UART5_IRQ_HANDLER"/>
+    </target>
+    <target name="sim" board="pc">
+      <subsystem name="fdm"           type="nps"/>
+      <subsystem name="radio_control" type="ppm"/>
+      <subsystem name="actuators"     type="mkk"/>
+    </target>
+
+    <subsystem name="imu"           type="aspirin"/>
+    <subsystem name="gps"           type="ublox"/>
+    <subsystem name="ahrs"          type="ic"/>
+  </firmware>
+
+
+  <firmware name="lisa_m_test_progs">
+    <target name="test_led"			board="lisa_m_1.0"/>
+    <target name="test_uart_lisam"			board="lisa_m_1.0"/>
+    <target name="test_servos"			board="lisa_m_1.0"/>
+    <target name="test_telemetry"		board="lisa_m_1.0"/>
+    <target name="test_imu_aspirin"			board="lisa_m_1.0"/>
+    <target name="test_rc_spektrum"		board="lisa_m_1.0"/>
+    <target name="test_baro"			board="lisa_m_1.0"/>
+    <!--<target name="test_imu"			board="lisa_m_1.0"/>
+    <target name="test_rc_ppm"			board="lisa_m_1.0"/>
+    <target name="test_adc"			board="lisa_m_1.0"/>
+    <target name="test_hmc5843"			board="lisa_m_1.0"/>
+    <target name="test_itg3200"			board="lisa_m_1.0"/>
+    <target name="test_adxl345"			board="lisa_m_1.0"/>
+    <target name="test_esc_mkk_simple"		board="lisa_m_1.0"/>
+    <target name="test_esc_asctecv1_simple"	board="lisa_m_1.0"/>
+    <target name="test_actuators_mkk"		board="lisa_m_1.0"/>
+    <target name="test_actuators_asctecv1"	board="lisa_m_1.0"/-->
+  </firmware>
+</airframe>

From afba2ad3503bc7ad0cf24d9288071594896882e7 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 13 May 2011 12:41:52 +0200
Subject: [PATCH 93/99] XSens gps-imu arm setting + declination +
 pulsepersecond + IMU telemetry

---
 conf/airframes/TU_Delft/skywalker.xml     | 35 +++++---
 conf/messages.xml                         |  9 ++-
 conf/modules/ins_xsens_MTiG_fixedwing.xml |  1 +
 conf/xsens_MTi-G.xml                      | 12 +++
 sw/airborne/ap_downlink.h                 | 36 ++++++---
 sw/airborne/modules/ins/ins_xsens.c       | 97 ++++++++++++++++++++---
 sw/airborne/modules/ins/ins_xsens.h       |  1 -
 7 files changed, 153 insertions(+), 38 deletions(-)

diff --git a/conf/airframes/TU_Delft/skywalker.xml b/conf/airframes/TU_Delft/skywalker.xml
index c5b9395843e..226be3e4272 100644
--- a/conf/airframes/TU_Delft/skywalker.xml
+++ b/conf/airframes/TU_Delft/skywalker.xml
@@ -19,7 +19,7 @@
     <axis name="ROLL" failsafe_value="0"/>
     <axis name="YAW" failsafe_value="0"/>
     <axis name="PITCH" failsafe_value="0"/>
-    <axis name="BRAKE" failsafe_value="0"/>	<!-- both elerons up as butterfly brake ? -->
+    <axis name="BRAKE" failsafe_value="9600"/>	<!-- both elerons up as butterfly brake ? -->
   </commands>
 
   <rc_commands>
@@ -43,7 +43,9 @@
 
     <define name="BRAKE_AILEVON" value="-0.7f"/>
     <define name="BRAKE_PITCH" value="0.1f"/>
-    <define name="MAX_BRAKE_RATE" value="150"/>
+    <define name="MAX_BRAKE_RATE" value="130"/>
+
+    <define name="RUDDER_OF_AILERON" value="0.3"/>
 
     <define name="LIMIT(X,XL,XH)" value="(    ((X)>(XH))   ?    (XH)    :    ( ((X)>(XL)) ? (X) : (XL) )    )"/>
 
@@ -67,13 +69,26 @@
     <set servo="AILERON_LEFT" value="($aileron_up * $leftturn) + ($aileron_down * $rightturn) - $brake_value*(BRAKE_AILEVON) - (MAX_PPRZ * AILERON_NEUTRAL)"/>
     <set servo="AILERON_RIGHT" value="($aileron_up * $rightturn) + ($aileron_down * $leftturn) + $brake_value*(BRAKE_AILEVON) + (MAX_PPRZ *AILERON_NEUTRAL)"/>
 
-    <set servo="RUDDER" value="@YAW + @ROLL * 0.3"/>
+    <set servo="RUDDER" value="@YAW + @ROLL * RUDDER_OF_AILERON"/>
     <set servo="THROTTLE" value="@THROTTLE"/>
 
     <!-- Pitch with Brake-Trim Function -->
     <set servo="ELEVATOR" value="@PITCH * PITCH_GAIN - BRAKE_PITCH * $brake_value"/>
   </command_laws>
 
+  <!-- Local magnetic field -->
+  <section name="AHRS" prefix="AHRS_" >
+    <define name="H_X" value="0.51562740288882" />
+    <define name="H_Y" value="-0.05707735220832" />
+    <define name="H_Z" value="0.85490967783446" />
+  </section>
+
+  <section name="XSENS">
+    <define name="GPS_IMU_LEVER_ARM_X" value="-0.285f" />
+    <define name="GPS_IMU_LEVER_ARM_Y" value="0.0f" />
+    <define name="GPS_IMU_LEVER_ARM_Z" value="0.0f" />
+  </section>
+
   <section name="AUTO1" prefix="AUTO1_">
     <define name="MAX_ROLL" value="RadOfDeg(75)"/>
     <define name="MAX_PITCH" value="RadOfDeg(45)"/>
@@ -93,7 +108,7 @@
   </section>
 
   <section name="MISC">
-    <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
+    <define name="NOMINAL_AIRSPEED" value="14." unit="m/s"/>
     <define name="CARROT" value="5." unit="s"/>
     <define name="CONTROL_RATE" value="60" unit="Hz"/>
     <define name="XBEE_INIT" value="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/>
@@ -148,18 +163,16 @@
     <define name="ROLL_RATE_GAIN" value="-820.312011719"/>
   </section>
 
-<!--
   <section name="AGGRESSIVE" prefix="AGR_">
-    <define name="BLEND_START" value="20"/>
-    <define name="BLEND_END" value="10"/>
+    <define name="BLEND_START" value="30"/>
+    <define name="BLEND_END" value="15"/>
     <define name="CLIMB_THROTTLE" value="1.00"/>
-    <define name="CLIMB_PITCH" value="0.3"/>
-    <define name="DESCENT_THROTTLE" value="0.1"/>
-    <define name="DESCENT_PITCH" value="-0.25"/>
+    <define name="CLIMB_PITCH" value="RadOfDeg(30)"/>
+    <define name="DESCENT_THROTTLE" value="0.0"/>
+    <define name="DESCENT_PITCH" value="RadOfDeg(-15)"/>
     <define name="CLIMB_NAV_RATIO" value="0.8"/>
     <define name="DESCENT_NAV_RATIO" value="1.0"/>
   </section>
--->
 
   <section name="FAILSAFE" prefix="FAILSAFE_">
     <define name="DEFAULT_THROTTLE" value="0.35" unit="%"/>
diff --git a/conf/messages.xml b/conf/messages.xml
index a25d2c6ebbb..fdd74237358 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1567,7 +1567,14 @@
     <field name="mz" type="int32" unit="adc"/>
   </message>
 
-  <!--206 is free -->
+  <message name="IMU_MAG_SETTINGS" id="206">
+    <field name="inclination" type="float" />
+    <field name="declination" type="float" />
+    <field name="hardiron_x" type="float" />
+    <field name="hardiron_y" type="float" />
+    <field name="hardiron_z" type="float" />
+  </message>
+
   <!--207 is free -->
   <!--208 is free -->
 
diff --git a/conf/modules/ins_xsens_MTiG_fixedwing.xml b/conf/modules/ins_xsens_MTiG_fixedwing.xml
index dcd3c87cb44..6ff09118062 100644
--- a/conf/modules/ins_xsens_MTiG_fixedwing.xml
+++ b/conf/modules/ins_xsens_MTiG_fixedwing.xml
@@ -11,6 +11,7 @@
   <event fun="InsEventCheckAndHandle(handle_ins_msg())"/>
   <makefile>
     <define name="AHRS_TYPE_H" value="\\\"modules/ins/ins_xsens.h\\\"" />
+    <define name="INS_MODULE_H" value="\\\"modules/ins/ins_xsens.h\\\"" />
     <define name="USE_UART$(XSENS_UART_NR)"/>
     <define name="INS_LINK" value="Uart$(XSENS_UART_NR)"/>
     <define name="UART$(XSENS_UART_NR)_BAUD" value="B230400"/>
diff --git a/conf/xsens_MTi-G.xml b/conf/xsens_MTi-G.xml
index 721c801aef9..63bf030e13a 100644
--- a/conf/xsens_MTi-G.xml
+++ b/conf/xsens_MTi-G.xml
@@ -123,6 +123,18 @@
   </message>
   <message name="SetMagneticDeclinationAck" ID="0x6B" to="host"/>
 
+  <message name="ReqLeverArmGps" ID="0x68" to="MT"/>
+  <message name="SetLeverArmGps" ID="0x68" to="MT" length="12">
+    <field name="x" format="R4" unit="m"/>
+    <field name="y" format="R4" unit="m"/>
+    <field name="z" format="R4" unit="m"/>
+  </message>
+  <message name="ReqLeverArmGpsAck" ID="0x69" to="host" length="12">
+    <field name="x" format="R4" unit="m"/>
+    <field name="y" format="R4" unit="m"/>
+    <field name="z" format="R4" unit="m"/>
+  </message>
+
   <message name="ReqGPSStatus" ID="0xA6" to="MT"/>
   <message name="GPSStatus" ID="0xA7" to="host">
     <field name="nch" format="U1"/>
diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
index 8947b97a6f5..34bedf56790 100644
--- a/sw/airborne/ap_downlink.h
+++ b/sw/airborne/ap_downlink.h
@@ -139,24 +139,34 @@
 #define PERIODIC_SEND_SEGMENT(_chan) if (nav_in_segment) { DOWNLINK_SEND_SEGMENT(_chan, &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); }
 
 #ifdef IMU_TYPE_H
-#include "subsystems/imu.h"
-#define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)}
-#define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)}
-#define PERIODIC_SEND_IMU_MAG_RAW(_chan) { DOWNLINK_SEND_IMU_MAG_RAW(_chan, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)}
-#define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)}
-#define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)}
+#  include "subsystems/imu.h"
+#  define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)}
+#  define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)}
+#  define PERIODIC_SEND_IMU_MAG_RAW(_chan) { DOWNLINK_SEND_IMU_MAG_RAW(_chan, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)}
+#  define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)}
+#  define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)}
 #  ifdef USE_MAGNETOMETER
 #    define PERIODIC_SEND_IMU_MAG(_chan) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(_chan, &mag_float.x, &mag_float.y, &mag_float.z)}
 #  else
-#    define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
+#    define PERIODIC_SEND_IMU_MAG(_chan) {}
 #  endif
 #else
-#define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {}
-#define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
-#define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
-#define PERIODIC_SEND_IMU_ACCEL(_chan) {}
-#define PERIODIC_SEND_IMU_GYRO(_chan) {}
-#define PERIODIC_SEND_IMU_MAG(_chan) {}
+#  ifdef INS_MODULE_H
+#  include "modules/ins/ins_module.h"
+#    define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {}
+#    define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
+#    define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
+#    define PERIODIC_SEND_IMU_GYRO(_chan) { DOWNLINK_SEND_IMU_GYRO(_chan, &ins_p, &ins_q, &ins_r)}
+#    define PERIODIC_SEND_IMU_ACCEL(_chan) { DOWNLINK_SEND_IMU_ACCEL(_chan, &ins_ax, &ins_ay, &ins_az)} 
+#    define PERIODIC_SEND_IMU_MAG(_chan) { DOWNLINK_SEND_IMU_MAG(_chan, &ins_mx, &ins_my, &ins_mz)}
+#  else
+#    define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {}
+#    define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
+#    define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
+#    define PERIODIC_SEND_IMU_ACCEL(_chan) {}
+#    define PERIODIC_SEND_IMU_GYRO(_chan) {}
+#    define PERIODIC_SEND_IMU_MAG(_chan) {}
+#  endif
 #endif
 
 #ifdef IMU_ANALOG
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index 4e1e12fcccf..c8be89d68c8 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -27,6 +27,7 @@
  */
 
 #include "ins_module.h"
+#include "ins_xsens.h"
 
 #include <inttypes.h>
 
@@ -165,11 +166,25 @@ uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD];
 #define GOT_DATA      5
 #define GOT_CHECKSUM  6
 
+// FIXME Debugging Only
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+
 uint8_t xsens_errorcode;
 uint8_t xsens_msg_status;
 uint16_t xsens_time_stamp;
 uint16_t xsens_output_mode;
 uint32_t xsens_output_settings;
+float xsens_declination = 0;
+float xsens_gps_arm_x = 0;
+float xsens_gps_arm_y = 0;
+float xsens_gps_arm_z = 0;
+
 
 int8_t xsens_hour;
 int8_t xsens_min;
@@ -189,9 +204,12 @@ uint8_t send_ck;
 struct LlaCoor_f lla_f;
 struct UtmCoor_f utm_f;
 
+volatile int xsens_configured = 0;
+
 void ins_init( void ) {
 
   xsens_status = UNINIT;
+  xsens_configured = 20;
 
   ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
   ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
@@ -200,23 +218,65 @@ void ins_init( void ) {
   xsens_time_stamp = 0;
   xsens_output_mode = XSENS_OUTPUT_MODE;
   xsens_output_settings = XSENS_OUTPUT_SETTINGS;
-  /* send mode and settings to MT */
-  XSENS_GoToConfig();
-  XSENS_SetOutputMode(xsens_output_mode);
-  XSENS_SetOutputSettings(xsens_output_settings);
-
-  uint8_t baud = 1;
-  XSENS_SetBaudrate(baud);
-  // Give pulses on SyncOut
-  XSENS_SetSyncOutSettings(0,0x0002);
-  // 1 pulse every 100 samples
-  // XSENS_SetSyncOutSettings(1,100);
-  //XSENS_GoToMeasurment();
 
   gps.nb_channels = 0;
 }
 
 void ins_periodic_task( void ) {
+  if (xsens_configured > 0)
+  {
+     switch (xsens_configured)
+     {
+	case 20:
+		/* send mode and settings to MT */
+		XSENS_GoToConfig();
+		XSENS_SetOutputMode(xsens_output_mode);
+		XSENS_SetOutputSettings(xsens_output_settings);
+		break;
+	case 18:
+		// Give pulses on SyncOut
+		XSENS_SetSyncOutSettings(0,0x0002);
+		break;
+	case 17:
+		// 1 pulse every 100 samples
+		XSENS_SetSyncOutSettings(1,100);
+		break;
+	case 2:
+		XSENS_ReqLeverArmGps();
+		break;
+
+	case 6:
+		XSENS_ReqMagneticDeclination();
+		break;
+
+	case 13:
+		#ifdef AHRS_H_X
+		#warning Sending XSens Magnetic Declination
+		xsens_declination = atan2(AHRS_H_Y, AHRS_H_X);
+		XSENS_SetMagneticDeclination(xsens_declination);
+		#endif
+		break;
+	case 12:
+		#ifdef GPS_IMU_LEVER_ARM_X
+		#warning Sending XSens GPS Arm
+		XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z);
+		#endif
+		break;
+	case 10:
+		{
+		uint8_t baud = 1;
+		XSENS_SetBaudrate(baud);
+		}
+		break;
+
+	case 1:
+		XSENS_GoToMeasurment();
+		break;
+     }
+     xsens_configured--;
+     return;
+  }
+
   RunOnceEvery(100,XSENS_ReqGPSStatus());
 }
 
@@ -256,6 +316,7 @@ void handle_ins_msg( void) {
   gps.gspeed = fspeed * 100.;
   gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);
   gps.course = fcourse * 1e7;
+
 }
 
 void parse_ins_msg( void ) {
@@ -266,6 +327,18 @@ void parse_ins_msg( void ) {
   else if (xsens_id == XSENS_ReqOutputSettings_ID) {
     xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens_msg_buf);
   }
+  else if (xsens_id == XSENS_ReqMagneticDeclinationAck_ID) {
+    xsens_declination = DegOfRad (XSENS_ReqMagneticDeclinationAck_declination(xsens_msg_buf) );
+
+    DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z);
+  }
+  else if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) {
+    xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf);
+    xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf);
+    xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf);
+
+    DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z);
+  }
   else if (xsens_id == XSENS_Error_ID) {
     xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
   }
diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h
index c40df9336fe..790e65bf5a5 100644
--- a/sw/airborne/modules/ins/ins_xsens.h
+++ b/sw/airborne/modules/ins/ins_xsens.h
@@ -42,5 +42,4 @@ extern uint8_t xsens_msg_status;
 extern uint16_t xsens_time_stamp;
 
 
-
 #endif

From 9862e3666eeeeae859db132b699b59271ce9a39e Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 13 May 2011 13:30:29 +0200
Subject: [PATCH 94/99] compile without USE_MAGNETOMETER

---
 sw/airborne/subsystems/ahrs/ahrs_float_dcm.c | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
index 173ca6a99c2..4438c4d0637 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
@@ -236,8 +236,6 @@ void ahrs_update_mag(void)
 {
 #ifdef USE_MAGNETOMETER
 #warning MAGNETOMETER FEEDBACK NOT TESTED YET
-#endif
-
 
   float cos_roll;
   float sin_roll;
@@ -284,6 +282,7 @@ void ahrs_update_mag(void)
 
   // Magnetic Heading
   // MAG_Heading = atan2(imu.mag.y, -imu.mag.x);
+#endif
 }
 
 void Normalize(void)

From ac6382a0790fb2d570891c928e90bfa231e33791 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 13 May 2011 21:08:18 +0200
Subject: [PATCH 95/99] AHRS provides attitude, not IMU

---
 sw/airborne/firmwares/fixedwing/main_ap.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 9e79daacad7..a12f4e873a9 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -402,7 +402,7 @@ static inline void attitude_loop( void ) {
 
 }
 
-#ifdef USE_IMU
+#ifdef USE_AHRS
 #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
 volatile uint8_t new_ins_attitude = 0;
 #endif

From 6a24f1ca289d2499ed4a2c34a28101013374c635 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 13 May 2011 21:09:20 +0200
Subject: [PATCH 96/99] GPS bugfix for LEA-4P with UTM only

---
 sw/airborne/subsystems/gps/gps_ubx.c | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c
index 0e2f9bd25a5..9939fa36a3a 100644
--- a/sw/airborne/subsystems/gps/gps_ubx.c
+++ b/sw/airborne/subsystems/gps/gps_ubx.c
@@ -155,6 +155,8 @@ void gps_ubx_read_message(void) {
       if (hem == UTM_HEM_SOUTH)
         gps.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */
       gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf)*10;
+      gps.hmsl = gps.utm_pos.alt;
+      gps.lla_pos.alt = gps.utm_pos.alt; // FIXME: with UTM only you do not receive ellipsoid altitude
       gps.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf);
 #endif
     }

From bf9f9d9eab3fde47ca9c8218336dfa40046356cf Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 13 May 2011 22:01:41 +0200
Subject: [PATCH 97/99] GPS fixed for 5H too

---
 sw/airborne/subsystems/gps/gps_ubx.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c
index 9939fa36a3a..2a23474045c 100644
--- a/sw/airborne/subsystems/gps/gps_ubx.c
+++ b/sw/airborne/subsystems/gps/gps_ubx.c
@@ -135,8 +135,8 @@ void gps_ubx_read_message(void) {
 #ifdef GPS_USE_LATLONG
       /* Computes from (lat, long) in the referenced UTM zone */
       struct LlaCoor_f lla_f;
-      lla_f.lat = (float) gps.lla_pos.lat * 1e7;
-      lla_f.lon = (float) gps.lla_pos.lat * 1e7;
+      lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
+      lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
       struct UtmCoor_f utm_f;
       utm_f.zone = nav_utm_zone0;
       /* convert to utm */

From 54a8ba26977ce4089ee2a7f0dc9e765131c6c74c Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Sat, 14 May 2011 18:24:17 +0200
Subject: [PATCH 98/99] Magnetics not good yet in DCM

---
 conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
index 12dd3481fa9..e2428d84fef 100644
--- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -245,7 +245,7 @@
     -->
     <subsystem name="imu" type="ppzuav"/>
     <subsystem name="attitude" type="dcm">
-      <define name="USE_MAGNETOMETER" />
+<!--      <define name="USE_MAGNETOMETER" /> -->
     </subsystem>
 
 
@@ -253,7 +253,7 @@
 
     <!-- Communication -->
     <subsystem name="telemetry" type="xbee_api">
-      <configure name="MODEM_BAUD" value="B57600"/>
+      <configure name="MODEM_BAUD" value="B9600"/>
     </subsystem>
 
     <!-- Actuators -->

From ebba3fbf1db8a53bb6ed6f7f43aa93efef27fd8a Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Tue, 17 May 2011 21:39:39 +0200
Subject: [PATCH 99/99] Superbug fix on GPS heading with nicely hidden
 fixedpoint overflow

---
 sw/airborne/subsystems/gps/gps_ubx.c | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c
index 2a23474045c..6a8d3fd61cc 100644
--- a/sw/airborne/subsystems/gps/gps_ubx.c
+++ b/sw/airborne/subsystems/gps/gps_ubx.c
@@ -166,7 +166,11 @@ void gps_ubx_read_message(void) {
       gps.ned_vel.x = UBX_NAV_VELNED_VEL_N(gps_ubx.msg_buf);
       gps.ned_vel.y = UBX_NAV_VELNED_VEL_E(gps_ubx.msg_buf);
       gps.ned_vel.z = UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf);
-      gps.course = RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf)*100);
+      // Ublox gives I4 heading in 1e-5 degrees, apparenty from 0 to 360 degrees (not -180 to 180)
+      // I4 max = 2^31 = 214 * 1e5 * 100 < 360 * 1e7: overflow on angles over 214 deg -> casted to -214 deg 
+      // solution: First to radians, and then scale to 1e-7 radians
+      // First x 10 for loosing less resolution, then to radians, then multiply x 10 again
+      gps.course = (RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf)*10)) * 10; 
       gps.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf);
     }
     else if (gps_ubx.msg_id == UBX_NAV_SVINFO_ID) {