diff --git a/conf/airframes/booz2_a1.xml b/conf/airframes/booz2_a1.xml
index fc6330c900..72db7873d9 100644
--- a/conf/airframes/booz2_a1.xml
+++ b/conf/airframes/booz2_a1.xml
@@ -91,21 +91,48 @@
 
   <section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
 
+    <!-- setpoints -->
     <define name="SP_MAX_PHI"     value="3000"/>
     <define name="SP_MAX_THETA"   value="3000"/>
     <define name="SP_MAX_R"       value="5500"/>
     <define name="DEADBAND_R"     value="250"/>
 
-    <define name="PHI_THETA_PGAIN"  value="-400"/>
-    <define name="PHI_THETA_DGAIN"  value="-300"/>
-    <define name="PHI_THETA_DDGAIN" value=" 300"/>
-    <define name="PHI_THETA_IGAIN"  value="-100"/>
+    <!-- reference -->
+    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_ZETA_P"   value="0.85"/>
+    <define name="REF_MAX_P"    value="BFP_OF_REAL(5,RATE_REF_RES)"/>
+    <define name="REF_MAX_PDOT" value="BFP_OF_REAL(128,ACCEL_REF_RES)"/>
+
+    <define name="REF_OMEGA_Q" value="RadOfDeg(800)"/>
+    <define name="REF_ZETA_Q"  value="0.85"/>
+    <define name="REF_MAX_Q"    value="BFP_OF_REAL(5,RATE_REF_RES)"/>
+    <define name="REF_MAX_QDOT" value="BFP_OF_REAL(128,ACCEL_REF_RES)"/>
+
+    <define name="REF_OMEGA_R" value="RadOfDeg(500)"/>
+    <define name="REF_ZETA_R"  value="0.85"/>
+    <define name="REF_MAX_R"    value="BFP_OF_REAL(3,RATE_REF_RES)"/>
+    <define name="REF_MAX_RDOT" value="BFP_OF_REAL(32,ACCEL_REF_RES)"/>
+
+    <!-- feedback -->
+    <define name="PHI_PGAIN"  value="-400"/>
+    <define name="PHI_DGAIN"  value="-300"/>
+    <define name="PHI_IGAIN"  value="-100"/>
+
+    <define name="THETA_PGAIN"  value="-400"/>
+    <define name="THETA_DGAIN"  value="-300"/>
+    <define name="THETA_IGAIN"  value="-100"/>
 
     <define name="PSI_PGAIN"  value="-380"/>
     <define name="PSI_DGAIN"  value="-320"/>
-    <define name="PSI_DDGAIN" value=" 300"/>
     <define name="PSI_IGAIN"  value="-75"/>
 
+    <!-- feedforward -->
+    <define name="PHI_DDGAIN"   value=" 300"/>
+    <define name="THETA_DDGAIN" value=" 300"/>
+    <define name="PSI_DDGAIN"   value=" 300"/>
+
+
+
   </section>
 
   <section name="INS" prefix="BOOZ_INS_">
diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler.c b/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler.c
index ebc332129f..2249ba3985 100644
--- a/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler.c
+++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler.c
@@ -25,6 +25,8 @@
 #include "booz_ahrs.h"
 #include "booz_radio_control.h"
 
+
+
 #include "airframe.h"
 
 struct Int32Eulers booz_stabilization_att_sp;
@@ -54,24 +56,25 @@ void booz_stabilization_attitude_init(void) {
   INT_VECT3_ZERO(booz_stabilization_accel_ref);
 
   VECT3_ASSIGN(booz_stabilization_pgain,
-	       BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_PGAIN,
-	       BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_PGAIN,
+	       BOOZ_STABILIZATION_ATTITUDE_PHI_PGAIN,
+	       BOOZ_STABILIZATION_ATTITUDE_THETA_PGAIN,
 	       BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN);
   
   VECT3_ASSIGN(booz_stabilization_dgain,
-	       BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DGAIN,
-	       BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DGAIN,
+	       BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN,
+	       BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN,
 	       BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN);
   
+  VECT3_ASSIGN(booz_stabilization_igain,
+	       BOOZ_STABILIZATION_ATTITUDE_PHI_IGAIN,
+	       BOOZ_STABILIZATION_ATTITUDE_THETA_IGAIN,
+	       BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN);
+
   VECT3_ASSIGN(booz_stabilization_ddgain,
-	       BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DDGAIN,
-	       BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DDGAIN,
+	       BOOZ_STABILIZATION_ATTITUDE_PHI_DDGAIN,
+	       BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN,
 	       BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN);
   
-  VECT3_ASSIGN(booz_stabilization_igain,
-	       BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_IGAIN,
-	       BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_IGAIN,
-	       BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN);
 
   INT_EULERS_ZERO( booz_stabilization_att_sum_err );
 
@@ -104,9 +107,9 @@ void booz_stabilization_attitude_run(bool_t  in_flight) {
 
   /* compute attitude error            */
   const struct Int32Eulers att_ref_scaled = {
-    OFFSET_AND_ROUND2(booz_stabilization_att_ref.phi,   (ANGLE_REF_RES - INT32_ANGLE_FRAC)),
-    OFFSET_AND_ROUND2(booz_stabilization_att_ref.theta, (ANGLE_REF_RES - INT32_ANGLE_FRAC)),
-    OFFSET_AND_ROUND2(booz_stabilization_att_ref.psi,   (ANGLE_REF_RES - INT32_ANGLE_FRAC)) };
+    OFFSET_AND_ROUND(booz_stabilization_att_ref.phi,   (ANGLE_REF_RES - INT32_ANGLE_FRAC)),
+    OFFSET_AND_ROUND(booz_stabilization_att_ref.theta, (ANGLE_REF_RES - INT32_ANGLE_FRAC)),
+    OFFSET_AND_ROUND(booz_stabilization_att_ref.psi,   (ANGLE_REF_RES - INT32_ANGLE_FRAC)) };
   struct Int32Eulers att_err;
   EULERS_DIFF(att_err, booz_ahrs.ltp_to_body_euler, att_ref_scaled);
   INT32_ANGLE_NORMALIZE(att_err.psi);
@@ -122,9 +125,9 @@ void booz_stabilization_attitude_run(bool_t  in_flight) {
   
   /* compute rate error                */
   const struct Int32Rates rate_ref_scaled = {
-    OFFSET_AND_ROUND2(booz_stabilization_rate_ref.x, (RATE_REF_RES - INT32_RATE_FRAC)),
-    OFFSET_AND_ROUND2(booz_stabilization_rate_ref.y, (RATE_REF_RES - INT32_RATE_FRAC)),
-    OFFSET_AND_ROUND2(booz_stabilization_rate_ref.z, (RATE_REF_RES - INT32_RATE_FRAC)) };
+    OFFSET_AND_ROUND(booz_stabilization_rate_ref.x, (RATE_REF_RES - INT32_RATE_FRAC)),
+    OFFSET_AND_ROUND(booz_stabilization_rate_ref.y, (RATE_REF_RES - INT32_RATE_FRAC)),
+    OFFSET_AND_ROUND(booz_stabilization_rate_ref.z, (RATE_REF_RES - INT32_RATE_FRAC)) };
   struct Int32Rates rate_err;
   RATES_DIFF(rate_err, booz_ahrs.body_rate, rate_ref_scaled);
 
@@ -133,7 +136,7 @@ void booz_stabilization_attitude_run(bool_t  in_flight) {
   booz_stabilization_att_fb_cmd[COMMAND_ROLL] = 
     booz_stabilization_pgain.x    * att_err.phi +
     booz_stabilization_dgain.x    * rate_err.p +
-    OFFSET_AND_ROUND((booz_stabilization_igain.x  * booz_stabilization_att_sum_err.phi), 10);
+    OFFSET_AND_ROUND2((booz_stabilization_igain.x  * booz_stabilization_att_sum_err.phi), 10);
 
   booz_stabilization_att_ff_cmd[COMMAND_ROLL] = 
     OFFSET_AND_ROUND(booz_stabilization_ddgain.x * booz_stabilization_accel_ref.x, 5);
diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_traj_euler.h b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_traj_euler.h
index eb0fe09a47..9718fcfa85 100644
--- a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_traj_euler.h
+++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_traj_euler.h
@@ -35,12 +35,14 @@ extern struct Int32Vect3  booz_stabilization_accel_ref;
 #define F_UPDATE   (1<<F_UPDATE_RES)
 
 #define ACCEL_REF_RES 12
-#define ACCEL_REF_MAX_PQ BFP_OF_REAL(128,ACCEL_REF_RES)
-#define ACCEL_REF_MAX_R  BFP_OF_REAL( 32,ACCEL_REF_RES)
+#define ACCEL_REF_MAX_P BOOZ_STABILIZATION_ATTITUDE_REF_MAX_PDOT
+#define ACCEL_REF_MAX_Q BOOZ_STABILIZATION_ATTITUDE_REF_MAX_QDOT
+#define ACCEL_REF_MAX_R BOOZ_STABILIZATION_ATTITUDE_REF_MAX_RDOT
 
 #define RATE_REF_RES  16
-#define RATE_REF_MAX_PQ BFP_OF_REAL(5,RATE_REF_RES)
-#define RATE_REF_MAX_R  BFP_OF_REAL(3,RATE_REF_RES)
+#define RATE_REF_MAX_P BOOZ_STABILIZATION_ATTITUDE_REF_MAX_P
+#define RATE_REF_MAX_Q BOOZ_STABILIZATION_ATTITUDE_REF_MAX_Q
+#define RATE_REF_MAX_R BOOZ_STABILIZATION_ATTITUDE_REF_MAX_R
 
 #define ANGLE_REF_RES 20
 #define PI_ANGLE_REF      BFP_OF_REAL(3.1415926535897932384626433832795029, ANGLE_REF_RES)
@@ -51,14 +53,23 @@ extern struct Int32Vect3  booz_stabilization_accel_ref;
   }
 
 
-#define OMEGA_PQ   RadOfDeg(800)
-#define ZETA_PQ    0.85
-#define ZETA_OMEGA_PQ_RES 10
-#define ZETA_OMEGA_PQ BFP_OF_REAL((ZETA_PQ*OMEGA_PQ), ZETA_OMEGA_PQ_RES)
-#define OMEGA_2_PQ_RES 7
-#define OMEGA_2_PQ    BFP_OF_REAL((OMEGA_PQ*OMEGA_PQ), OMEGA_2_PQ_RES)
-#define OMEGA_R   RadOfDeg(500)
-#define ZETA_R    0.85
+#define OMEGA_P   BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_P
+#define ZETA_P    BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_P
+#define ZETA_OMEGA_P_RES 10
+#define ZETA_OMEGA_P BFP_OF_REAL((ZETA_P*OMEGA_P), ZETA_OMEGA_P_RES)
+#define OMEGA_2_P_RES 7
+#define OMEGA_2_P    BFP_OF_REAL((OMEGA_P*OMEGA_P), OMEGA_2_P_RES)
+
+#define OMEGA_Q   BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_Q
+#define ZETA_Q    BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_Q
+#define ZETA_OMEGA_Q_RES 10
+#define ZETA_OMEGA_Q BFP_OF_REAL((ZETA_Q*OMEGA_Q), ZETA_OMEGA_Q_RES)
+#define OMEGA_2_Q_RES 7
+#define OMEGA_2_Q    BFP_OF_REAL((OMEGA_Q*OMEGA_Q), OMEGA_2_Q_RES)
+
+
+#define OMEGA_R   BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_R
+#define ZETA_R    BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_R
 #define ZETA_OMEGA_R_RES 10
 #define ZETA_OMEGA_R BFP_OF_REAL((ZETA_R*OMEGA_R), ZETA_OMEGA_R_RES)
 #define OMEGA_2_R_RES 7
@@ -92,43 +103,43 @@ extern struct Int32Vect3  booz_stabilization_accel_ref;
     									\
     /* compute reference angular accelerations */			\
     const struct Int32Vect3 accel_rate = {				\
-      ((int32_t)(-2.*ZETA_OMEGA_PQ)* (booz_stabilization_rate_ref.x >> (RATE_REF_RES - ACCEL_REF_RES))) \
-      >> (ZETA_OMEGA_PQ_RES),						\
-      ((int32_t)(-2.*ZETA_OMEGA_PQ)* (booz_stabilization_rate_ref.y >> (RATE_REF_RES - ACCEL_REF_RES))) \
-      >> (ZETA_OMEGA_PQ_RES),						\
+      ((int32_t)(-2.*ZETA_OMEGA_P) * (booz_stabilization_rate_ref.x >> (RATE_REF_RES - ACCEL_REF_RES))) \
+      >> (ZETA_OMEGA_P_RES),						\
+      ((int32_t)(-2.*ZETA_OMEGA_Q) * (booz_stabilization_rate_ref.y >> (RATE_REF_RES - ACCEL_REF_RES))) \
+      >> (ZETA_OMEGA_Q_RES),						\
       ((int32_t)(-2.*ZETA_OMEGA_R) * (booz_stabilization_rate_ref.z >> (RATE_REF_RES - ACCEL_REF_RES))) \
       >> (ZETA_OMEGA_R_RES) };						\
     									\
     const struct Int32Vect3 accel_angle = {				\
-      ((int32_t)(-OMEGA_2_PQ)* (ref_err.phi   >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_PQ_RES), \
-      ((int32_t)(-OMEGA_2_PQ)* (ref_err.theta >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_PQ_RES), \
-      ((int32_t)(-OMEGA_2_R )* (ref_err.psi   >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_R_RES ) }; \
+      ((int32_t)(-OMEGA_2_P)* (ref_err.phi   >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_P_RES), \
+      ((int32_t)(-OMEGA_2_Q)* (ref_err.theta >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_Q_RES), \
+      ((int32_t)(-OMEGA_2_R)* (ref_err.psi   >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_R_RES) }; \
     									\
     VECT3_SUM(booz_stabilization_accel_ref, accel_rate, accel_angle);	\
 									\
     /*	saturate acceleration */					\
-    const struct Int32Vect3 MIN_ACCEL = { -ACCEL_REF_MAX_PQ, -ACCEL_REF_MAX_PQ, -ACCEL_REF_MAX_R }; \
-    const struct Int32Vect3 MAX_ACCEL = {  ACCEL_REF_MAX_PQ,  ACCEL_REF_MAX_PQ,  ACCEL_REF_MAX_R }; \
+    const struct Int32Vect3 MIN_ACCEL = { -ACCEL_REF_MAX_P, -ACCEL_REF_MAX_Q, -ACCEL_REF_MAX_R }; \
+    const struct Int32Vect3 MAX_ACCEL = {  ACCEL_REF_MAX_P,  ACCEL_REF_MAX_Q,  ACCEL_REF_MAX_R }; \
     VECT3_BOUND_BOX(booz_stabilization_accel_ref, MIN_ACCEL, MAX_ACCEL); \
     									\
     /* saturate speed and trim accel accordingly */			\
-    if (booz_stabilization_rate_ref.x >= RATE_REF_MAX_PQ) {		\
-      booz_stabilization_rate_ref.x = RATE_REF_MAX_PQ;			\
+    if (booz_stabilization_rate_ref.x >= RATE_REF_MAX_P) {		\
+      booz_stabilization_rate_ref.x = RATE_REF_MAX_P;			\
       if (booz_stabilization_accel_ref.x > 0)				\
 	booz_stabilization_accel_ref.x = 0;				\
     }									\
-    else if (booz_stabilization_rate_ref.x <= -RATE_REF_MAX_PQ) {	\
-      booz_stabilization_rate_ref.x = -RATE_REF_MAX_PQ;			\
+    else if (booz_stabilization_rate_ref.x <= -RATE_REF_MAX_P) {	\
+      booz_stabilization_rate_ref.x = -RATE_REF_MAX_P;			\
       if (booz_stabilization_accel_ref.x < 0)				\
 	booz_stabilization_accel_ref.x = 0;				\
     }									\
-    if (booz_stabilization_rate_ref.y >= RATE_REF_MAX_PQ) {		\
-      booz_stabilization_rate_ref.y = RATE_REF_MAX_PQ;			\
+    if (booz_stabilization_rate_ref.y >= RATE_REF_MAX_Q) {		\
+      booz_stabilization_rate_ref.y = RATE_REF_MAX_Q;			\
       if (booz_stabilization_accel_ref.y > 0)				\
 	booz_stabilization_accel_ref.y = 0;				\
     }									\
-    else if (booz_stabilization_rate_ref.y <= -RATE_REF_MAX_PQ) {	\
-      booz_stabilization_rate_ref.y = -RATE_REF_MAX_PQ;			\
+    else if (booz_stabilization_rate_ref.y <= -RATE_REF_MAX_Q) {	\
+      booz_stabilization_rate_ref.y = -RATE_REF_MAX_Q;			\
       if (booz_stabilization_accel_ref.y < 0)				\
 	booz_stabilization_accel_ref.y = 0;				\
     }									\