diff --git a/conf/airframes/booz2_a1.xml b/conf/airframes/booz2_a1.xml
index 495305f034..d96be00ff0 100644
--- a/conf/airframes/booz2_a1.xml
+++ b/conf/airframes/booz2_a1.xml
@@ -35,37 +35,33 @@
     <define name="GYRO_Y_NEUTRAL" value="33417"/>
     <define name="GYRO_Z_NEUTRAL" value="32809"/>
 
-    <define name="GYRO_X_SENS" value="( 4000.)"/>
-    <define name="GYRO_Y_SENS" value="(-4000.)"/>
-    <define name="GYRO_Z_SENS" value="(-4000.)"/>
+    <define name="GYRO_X_SENS" value=" 1.01" integer="16"/>
+    <define name="GYRO_Y_SENS" value="-1.01" integer="16"/>
+    <define name="GYRO_Z_SENS" value="-1.01" integer="16"/>
   
     <define name="ACCEL_X_CHAN" value="3"/>
     <define name="ACCEL_Y_CHAN" value="5"/>
     <define name="ACCEL_Z_CHAN" value="6"/>    
 
-    <define name="ACCEL_X_NEUTRAL" value="32068"/>
-    <define name="ACCEL_Y_NEUTRAL" value="33731"/>
-    <define name="ACCEL_Z_NEUTRAL" value="32439"/>
-
-    <define name="ACCEL_X_SENS" value="( -410)"/><!-- 409.58772 -->
-    <define name="ACCEL_Y_SENS" value="( -414)"/><!-- 414.00958 -->
-    <define name="ACCEL_Z_SENS" value="( -409)"/><!-- 408.84626 -->
+    <define name="ACCEL_X_SENS" value="-2.50411474" integer="16"/>
+    <define name="ACCEL_Y_SENS" value="-2.48126183" integer="16"/>
+    <define name="ACCEL_Z_SENS" value="-2.51396167" integer="16"/>
 
+    <define name="ACCEL_X_NEUTRAL" value="32081"/>
+    <define name="ACCEL_Y_NEUTRAL" value="33738"/>
+    <define name="ACCEL_Z_NEUTRAL" value="32441"/>
 
     <define name="MAG_X_CHAN" value="4"/>
     <define name="MAG_Y_CHAN" value="0"/>
     <define name="MAG_Z_CHAN" value="2"/>
-
     
     <define name="MAG_X_NEUTRAL" value="2358"/>
     <define name="MAG_Y_NEUTRAL" value="2362"/> 
     <define name="MAG_Z_NEUTRAL" value="2119"/>
 
-    <define name="MAG_X_SENS" value="-414."/><!-- 414.51787 -->
-    <define name="MAG_Y_SENS" value=" 401."/><!-- 401.40112 -->
-    <define name="MAG_Z_SENS" value="-417."/><!-- 417.2869  -->
-
-    <define name="MAG_Z_SENS_FOO" value="-0.0024124412" integer="16"/>
+    <define name="MAG_X_SENS" value="-4.94075530" integer="16"/>
+    <define name="MAG_Y_SENS" value="5.10207664" integer="16"/>
+    <define name="MAG_Z_SENS" value="-4.90788848" integer="16"/>
 
   </section>
 
@@ -96,15 +92,15 @@
 
     <define name="PSI_PGAIN"  value="-380"/>
     <define name="PSI_DGAIN"  value="-320"/>
-    <define name="PSI_DDGAIN" value="0"/>
+    <define name="PSI_DDGAIN" value=" 300"/>
     <define name="PSI_IGAIN"  value="-75"/>
 
   </section>
 
   <section name="FILTER" prefix="FILTER_">
-   <define name="ALIGNMENT_DPHI" value="BOOZ_ANGLE_I_OF_F(RadOfDeg(4.))"/>
+   <define name="ALIGNMENT_DPHI"   value="BOOZ_ANGLE_I_OF_F(RadOfDeg(4.))"/>
    <define name="ALIGNMENT_DTHETA" value="BOOZ_ANGLE_I_OF_F(RadOfDeg(3.))"/>
-   <define name="ALIGNMENT_DPSI" value="BOOZ_ANGLE_I_OF_F(0.)"/>
+   <define name="ALIGNMENT_DPSI"   value="BOOZ_ANGLE_I_OF_F(0.)"/>
   </section>
 
 
diff --git a/conf/autopilot/booz2_autopilot.makefile b/conf/autopilot/booz2_autopilot.makefile
index 3f038d0490..121e1844c6 100644
--- a/conf/autopilot/booz2_autopilot.makefile
+++ b/conf/autopilot/booz2_autopilot.makefile
@@ -116,6 +116,8 @@ ap.srcs += $(BOOZ_PRIV)/booz2_stabilization_rate.c
 ap.srcs += $(BOOZ_PRIV)/booz2_stabilization_attitude.c
 ap.srcs += $(BOOZ_PRIV)/booz_supervision_int.c
 
+ap.CFLAGS += -DHS_YAW
+
 # Joystick
 ap.CFLAGS += -DUSE_DATALINK_FMS -DFMS_USE_COMMANDS_RAW
 ap.srcs += $(BOOZ_PRIV)/booz2_fms.c
\ No newline at end of file
diff --git a/conf/messages.xml b/conf/messages.xml
index 017aa79a64..9f77173dff 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -933,12 +933,12 @@
  </message>
 
  <message name="BOOZ2_FILTER" ID="134">   
-   <field name="phi"              type="int32"/>
-   <field name="theta"            type="int32"/>
-   <field name="psi"              type="int32"/>
-   <field name="measure_phi"      type="int32"/>
-   <field name="measure_theta"    type="int32"/>
-   <field name="measure_psi"      type="int32"/>
+   <field name="phi"              type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
+   <field name="theta"            type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
+   <field name="psi"              type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
+   <field name="measure_phi"      type="int32" alt_unit="degres" alt_unit_coef="0.0000273"/>
+   <field name="measure_theta"    type="int32" alt_unit="degres" alt_unit_coef="0.0000273"/>
+   <field name="measure_psi"      type="int32" alt_unit="degres" alt_unit_coef="0.0000273"/>
    <field name="corrected_phi"    type="int32"/>
    <field name="corrected_theta"  type="int32"/>
    <field name="corrected_psi"    type="int32"/>
@@ -1037,11 +1037,10 @@
       <field name="p_ref"     type="int32" alt_unit="degres/s" alt_unit_coef="0.0008743"/>   
       <field name="pd_ref"    type="int32" alt_unit="degres/s2" alt_unit_coef="0.0139882"/>
       <field name="m_phi"     type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>   
-      <field name="m_p"   type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
+      <field name="m_p"       type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
       <field name="sum_err_phi"   type="int32"/>   
       <field name="delta_a" type="int32"/>
-    </message>
-
+ </message>
 
 
  <message name="BOOZ2_CMD" ID="143">
diff --git a/conf/settings/settings_booz2.xml b/conf/settings/settings_booz2.xml
index fd59125336..2c7c930066 100644
--- a/conf/settings/settings_booz2.xml
+++ b/conf/settings/settings_booz2.xml
@@ -25,6 +25,9 @@
       <dl_setting var="booz_stabilization_igain.x" min="-300" step="1" max="0" module="booz2_stabilization_attitude" shortname="igain phi"/>
       <dl_setting var="booz_stabilization_igain.y" min="-300" step="1" max="0" module="booz2_stabilization_attitude" shortname="igain theta"/>
       <dl_setting var="booz_stabilization_igain.z" min="-300" step="1" max="0" module="booz2_stabilization_attitude" shortname="igain psi"/>
+      <dl_setting var="booz_stabilization_ddgain.x" min="0" step="1" max="500" module="booz2_stabilization_attitude" shortname="ddgain p"/>
+      <dl_setting var="booz_stabilization_ddgain.y" min="0" step="1" max="500" module="booz2_stabilization_attitude" shortname="ddgain q"/>
+      <dl_setting var="booz_stabilization_ddgain.z" min="0" step="1" max="500" module="booz2_stabilization_attitude" shortname="ddgain r"/>
     </dl_settings>
 
     <dl_settings NAME="Vert Loop">