[rotorcraft] big fat attempt to clean stabilization and guidance

- No more STABILIZATION_ATTITUDE_FLOAT_x in airframe file.
- Hopefully seamlessly switch between int and float.
- Also make it possible to not use a reference at all, e.g. for passthrough

conf/airframes/esden/gain_scheduling_example.xml

@@ -185,7 +185,6 @@
 
  <section name="MISC">
    <define name="FACE_REINJ_1"  value="1024"/>
-   <define name="REF_QUAT_INFINITESIMAL_STEP" value="TRUE"/>
  </section>
 
  <section name="SIMULATOR" prefix="NPS_">

conf/airframes/esden/qs_asp22.xml

@@ -192,7 +192,6 @@
  </section>
 
  <section name="MISC">
-   <define name="REF_QUAT_INFINITESIMAL_STEP" value="TRUE"/>
    <define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
 <!--    <define name="USE_REFERENCE_SYSTEM" value="FALSE"/> -->
    <define name="SWITCH_STICKS_FOR_RATE_CONTROL" value="TRUE"/>

conf/firmwares/subsystems/rotorcraft/stabilization_euler.makefile

@@ -1,12 +1,3 @@
-STAB_ATT_CFLAGS  = -DSTABILIZATION_ATTITUDE_TYPE_INT
+$(warning Warning: The stabilization euler subsystem has been renamed, please replace <subsystem name="stabilization" type="euler"/> with <subsystem name="stabilization" type="int_euler"/> in your airframe file.)
-STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_int.h\"
-STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_euler_int.h\"
-STAB_ATT_SRCS  = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_int.c
-STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_int.c
-STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c
 
-ap.CFLAGS += $(STAB_ATT_CFLAGS)
+include $(PAPARAZZI_SRC)/conf/firmwares/subsystems/rotorcraft/stabilization_int_euler.makefile
-ap.srcs += $(STAB_ATT_SRCS)
-
-nps.CFLAGS += $(STAB_ATT_CFLAGS)
-nps.srcs += $(STAB_ATT_SRCS)

conf/firmwares/subsystems/rotorcraft/stabilization_float_euler.makefile

@@ -1,6 +1,5 @@
 STAB_ATT_CFLAGS  = -DSTABILIZATION_ATTITUDE_TYPE_FLOAT
-STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_float.h\"
+STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_euler_float.h\"
-STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_euler_float.h\"
 STAB_ATT_SRCS  = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_float.c
 STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_float.c
 STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c

conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile

@@ -1,7 +1,6 @@
 STAB_ATT_CFLAGS  = -DSTABILIZATION_ATTITUDE_TYPE_FLOAT
 STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_QUAT
-STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_float.h\"
+STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_quat_float.h\"
-STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_quat_float.h\"
 STAB_ATT_SRCS  = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_float.c
 STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_float.c
 STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c

conf/firmwares/subsystems/rotorcraft/stabilization_int_euler.makefile

@@ -0,0 +1,11 @@
+STAB_ATT_CFLAGS  = -DSTABILIZATION_ATTITUDE_TYPE_INT
+STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_euler_int.h\"
+STAB_ATT_SRCS  = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_int.c
+STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_int.c
+STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c
+
+ap.CFLAGS += $(STAB_ATT_CFLAGS)
+ap.srcs += $(STAB_ATT_SRCS)
+
+nps.CFLAGS += $(STAB_ATT_CFLAGS)
+nps.srcs += $(STAB_ATT_SRCS)

conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile

@@ -1,7 +1,6 @@
 STAB_ATT_CFLAGS  = -DSTABILIZATION_ATTITUDE_TYPE_INT
 STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_QUAT
-STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_int.h\"
+STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_quat_int.h\"
-STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_quat_int.h\"
 STAB_ATT_SRCS  = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c
 STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c
 STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c

conf/firmwares/subsystems/rotorcraft/stabilization_passthrough.makefile

@@ -1,6 +1,5 @@
 STAB_ATT_CFLAGS  = -DSTABILIZATION_ATTITUDE_TYPE_INT
-STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_int.h\"
+STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_passthrough.h\"
-STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_euler_int.h\"
 STAB_ATT_SRCS  = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_passthrough.c
 STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c
 ap.CFLAGS += $(STAB_ATT_CFLAGS)

conf/settings/control/stabilization_att_int.xml

@@ -4,18 +4,18 @@
   <dl_settings>
 
     <dl_settings NAME="Att Loop">
-      <dl_setting var="stabilization_gains.p.x" min="1" step="1" max="8000"   module="stabilization/stabilization_attitude" shortname="pgain phi" param="STABILIZATION_ATTITUDE_PHI_PGAIN"/>
+      <dl_setting var="stabilization_gains.p.x" min="1" step="1" max="8000"   module="stabilization/stabilization_attitude_common_int" shortname="pgain phi" param="STABILIZATION_ATTITUDE_PHI_PGAIN"/>
-      <dl_setting var="stabilization_gains.d.x" min="1" step="1" max="4000"   module="stabilization/stabilization_attitude" shortname="dgain p" param="STABILIZATION_ATTITUDE_PHI_DGAIN"/>
+      <dl_setting var="stabilization_gains.d.x" min="1" step="1" max="4000"   module="stabilization/stabilization_attitude_common_int" shortname="dgain p" param="STABILIZATION_ATTITUDE_PHI_DGAIN"/>
-      <dl_setting var="stabilization_gains.i.x" min="0" step="1" max="800"    module="stabilization/stabilization_attitude" shortname="igain phi" handler="SetKiPhi"param="STABILIZATION_ATTITUDE_PHI_IGAIN" />
+      <dl_setting var="stabilization_gains.i.x" min="0" step="1" max="800"    module="stabilization/stabilization_attitude_common_int" shortname="igain phi" handler="SetKiPhi"param="STABILIZATION_ATTITUDE_PHI_IGAIN" />
-      <dl_setting var="stabilization_gains.dd.x" min="0" step="1" max="1000" module="stabilization/stabilization_attitude" shortname="ddgain p" param="STABILIZATION_ATTITUDE_PHI_DDGAIN"/>
+      <dl_setting var="stabilization_gains.dd.x" min="0" step="1" max="1000" module="stabilization/stabilization_attitude_common_int" shortname="ddgain p" param="STABILIZATION_ATTITUDE_PHI_DDGAIN"/>
-      <dl_setting var="stabilization_gains.p.y" min="1" step="1" max="8000"   module="stabilization/stabilization_attitude" shortname="pgain theta" param="STABILIZATION_ATTITUDE_THETA_PGAIN"/>
+      <dl_setting var="stabilization_gains.p.y" min="1" step="1" max="8000"   module="stabilization/stabilization_attitude_common_int" shortname="pgain theta" param="STABILIZATION_ATTITUDE_THETA_PGAIN"/>
-      <dl_setting var="stabilization_gains.d.y" min="1" step="1" max="4000"   module="stabilization/stabilization_attitude" shortname="dgain q" param="STABILIZATION_ATTITUDE_THETA_DGAIN"/>
+      <dl_setting var="stabilization_gains.d.y" min="1" step="1" max="4000"   module="stabilization/stabilization_attitude_common_int" shortname="dgain q" param="STABILIZATION_ATTITUDE_THETA_DGAIN"/>
-      <dl_setting var="stabilization_gains.i.y" min="0"  step="1" max="800"    module="stabilization/stabilization_attitude" shortname="igain theta" param="STABILIZATION_ATTITUDE_THETA_IGAIN"/>
+      <dl_setting var="stabilization_gains.i.y" min="0"  step="1" max="800"    module="stabilization/stabilization_attitude_common_int" shortname="igain theta" param="STABILIZATION_ATTITUDE_THETA_IGAIN"/>
-      <dl_setting var="stabilization_gains.dd.y" min="0"    step="1" max="1000"  module="stabilization/stabilization_attitude" shortname="ddgain q" param="STABILIZATION_ATTITUDE_THETA_DDGAIN"/>
+      <dl_setting var="stabilization_gains.dd.y" min="0"    step="1" max="1000"  module="stabilization/stabilization_attitude_common_int" shortname="ddgain q" param="STABILIZATION_ATTITUDE_THETA_DDGAIN"/>
-      <dl_setting var="stabilization_gains.p.z" min="1" step="1" max="4000"   module="stabilization/stabilization_attitude" shortname="pgain psi" param="STABILIZATION_ATTITUDE_PSI_PGAIN"/>
+      <dl_setting var="stabilization_gains.p.z" min="1" step="1" max="4000"   module="stabilization/stabilization_attitude_common_int" shortname="pgain psi" param="STABILIZATION_ATTITUDE_PSI_PGAIN"/>
-      <dl_setting var="stabilization_gains.d.z" min="1" step="1" max="4000"   module="stabilization/stabilization_attitude" shortname="dgain r" param="STABILIZATION_ATTITUDE_PSI_DGAIN"/>
+      <dl_setting var="stabilization_gains.d.z" min="1" step="1" max="4000"   module="stabilization/stabilization_attitude_common_int" shortname="dgain r" param="STABILIZATION_ATTITUDE_PSI_DGAIN"/>
-      <dl_setting var="stabilization_gains.i.z" min="0"  step="1" max="400"    module="stabilization/stabilization_attitude" shortname="igain psi" param="STABILIZATION_ATTITUDE_PSI_IGAIN"/>
+      <dl_setting var="stabilization_gains.i.z" min="0"  step="1" max="400"    module="stabilization/stabilization_attitude_common_int" shortname="igain psi" param="STABILIZATION_ATTITUDE_PSI_IGAIN"/>
-      <dl_setting var="stabilization_gains.dd.z" min="0" step="1" max="1000"  module="stabilization/stabilization_attitude" shortname="ddgain r" param="STABILIZATION_ATTITUDE_PSI_DDGAIN"/>
+      <dl_setting var="stabilization_gains.dd.z" min="0" step="1" max="1000"  module="stabilization/stabilization_attitude_common_int" shortname="ddgain r" param="STABILIZATION_ATTITUDE_PSI_DDGAIN"/>
     </dl_settings>
 
   </dl_settings>

sw/airborne/firmwares/rotorcraft/autopilot.c

@@ -132,8 +132,7 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
     switch (new_autopilot_mode) {
       case AP_MODE_FAILSAFE:
 #ifndef KILL_AS_FAILSAFE
-        stab_att_sp_euler.phi = 0;
+        stabilization_attitude_set_failsafe_setpoint();
-        stab_att_sp_euler.theta = 0;
         guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
         break;
 #endif

sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c

@@ -82,19 +82,13 @@ int32_t guidance_h_again;
 int32_t transition_percentage;
 int32_t transition_theta_offset;
 
+
 static void guidance_h_update_reference(void);
 static void guidance_h_traj_run(bool_t in_flight);
 static void guidance_h_hover_enter(void);
 static void guidance_h_nav_enter(void);
 static inline void transition_run(void);
 
-#define GuidanceHSetRef(_pos, _speed, _accel) { \
-    gh_set_ref(_pos, _speed, _accel);           \
-    VECT2_COPY(guidance_h_pos_ref,   _pos);     \
-    VECT2_COPY(guidance_h_speed_ref, _speed);   \
-    VECT2_COPY(guidance_h_accel_ref, _accel);   \
-  }
-
 
 void guidance_h_init(void) {
 
@@ -114,6 +108,20 @@ void guidance_h_init(void) {
 }
 
 
+static inline void reset_reference_from_current_position(void) {
+
+  VECT2_COPY(guidance_h_pos_ref, *stateGetPositionNed_i());
+  VECT2_COPY(guidance_h_speed_ref, *stateGetSpeedNed_i());
+  INT_VECT2_ZERO(guidance_h_accel_ref);
+  gh_set_ref(guidance_h_pos_ref, guidance_h_speed_ref, guidance_h_accel_ref);
+
+  /* reset attitude psi reference, set psi setpoint to current psi */
+  // FIXME
+  //reset_psi_ref_from_body();
+
+  INT_VECT2_ZERO(guidance_h_pos_err_sum);
+}
+
 void guidance_h_mode_changed(uint8_t new_mode) {
   if (new_mode == guidance_h_mode)
     return;
@@ -217,6 +225,9 @@ void guidance_h_run(bool_t  in_flight) {
       break;
 
     case GUIDANCE_H_MODE_HOVER:
+      if (!in_flight)
+        guidance_h_hover_enter();
+
       guidance_h_update_reference();
 
       /* set psi command */
@@ -228,17 +239,16 @@ void guidance_h_run(bool_t  in_flight) {
       break;
 
     case GUIDANCE_H_MODE_NAV:
-      if (!in_flight) guidance_h_nav_enter();
+      if (!in_flight)
+        guidance_h_nav_enter();
 
       if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
-        stab_att_sp_euler.phi = nav_roll;
+        struct Int32Eulers sp_euler_i;
-        stab_att_sp_euler.theta = nav_pitch;
+        sp_euler_i.phi = nav_roll;
+        sp_euler_i.theta = nav_pitch;
         /* FIXME: heading can't be set via attitude block yet, use current heading for now */
-        stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
+        sp_euler_i.psi = stateGetNedToBodyEulers_i()->psi;
-#ifdef STABILIZATION_ATTITUDE_TYPE_QUAT
+        stabilization_attitude_set_from_eulers_i(&sp_euler_i);
-        INT32_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler);
-        INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
-#endif
       }
       else {
         INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot);
@@ -256,9 +266,9 @@ void guidance_h_run(bool_t  in_flight) {
     default:
       break;
   }
-
 }
 
+
 static void guidance_h_update_reference(void) {
   /* compute reference even if usage temporarily disabled via guidance_h_use_ref */
 #if GUIDANCE_H_USE_REF
@@ -342,42 +352,28 @@ static void guidance_h_traj_run(bool_t in_flight) {
   guidance_h_command_body.phi += guidance_h_rc_sp.phi;
   guidance_h_command_body.theta += guidance_h_rc_sp.theta;
 
-  /* Set attitude setpoint in eulers and as quaternion */
+  /* Set attitude setpoint from pseudo-eulers */
-  EULERS_COPY(stab_att_sp_euler, guidance_h_command_body);
+  stabilization_attitude_set_from_eulers_i(&guidance_h_command_body);
-
-#ifdef STABILIZATION_ATTITUDE_TYPE_QUAT
-  INT32_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler);
-  INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
-#endif /* STABILIZATION_ATTITUDE_TYPE_QUAT */
-
 }
 
 static void guidance_h_hover_enter(void) {
 
+  /* set horizontal setpoint to current position */
   VECT2_COPY(guidance_h_pos_sp, *stateGetPositionNed_i());
 
+  reset_reference_from_current_position();
+
   guidance_h_rc_sp.psi = stateGetNedToBodyEulers_i()->psi;
-  reset_psi_ref_from_body();
-
-  INT_VECT2_ZERO(guidance_h_pos_err_sum);
-
 }
 
 static void guidance_h_nav_enter(void) {
 
+  /* horizontal position setpoint from navigation/flightplan */
   INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot);
-  struct Int32Vect2 pos,speed,zero;
-  INT_VECT2_ZERO(zero);
-  VECT2_COPY(pos, *stateGetPositionNed_i());
-  VECT2_COPY(speed, *stateGetSpeedNed_i());
-  GuidanceHSetRef(pos, speed, zero);
 
-  /* reset psi reference, set psi setpoint to current psi */
+  reset_reference_from_current_position();
-  reset_psi_ref_from_body();
+
   nav_heading = stateGetNedToBodyEulers_i()->psi;
-
-  INT_VECT2_ZERO(guidance_h_pos_err_sum);
-
 }
 
 static inline void transition_run(void) {

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h

@@ -19,6 +19,11 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/** @file firmwares/rotorcraft/stabilization_attitude.h
+ *  General attitude stabilization interface for rotorcrafts.
+ *  The actual implementation is automatically included.
+ */
+
 #ifndef STABILIZATION_ATTITUDE_H
 #define STABILIZATION_ATTITUDE_H
 
@@ -28,15 +33,9 @@
 extern void stabilization_attitude_init(void);
 extern void stabilization_attitude_read_rc(bool_t in_flight);
 extern void stabilization_attitude_enter(void);
+extern void stabilization_attitude_set_failsafe_setpoint(void);
+extern void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler);
 extern void stabilization_attitude_run(bool_t  in_flight);
 
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h"
-extern void stabilization_attitude_ref_init(void);
-extern void stabilization_attitude_ref_update(void);
-
-#define stabilization_attitude_SetKiPhi(_val) {	\
-    stabilization_gains.i.x = _val;             \
-    stabilization_att_sum_err.phi = 0;          \
-  }
 
 #endif /* STABILIZATION_ATTITUDE_H */

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h

@@ -19,20 +19,20 @@
  * Boston, MA 02111-1307, USA.
  */
 
-#ifndef STABILIZATION_ATTITUDE_FLOAT_H
+/**
-#define STABILIZATION_ATTITUDE_FLOAT_H
+ * @file stabilization_attitude_common_float.h
+ *
+ * Common data structures shared by euler and quaternion float implementations.
+ */
+
+
+#ifndef STABILIZATION_ATTITUDE_COMMON_FLOAT_H
+#define STABILIZATION_ATTITUDE_COMMON_FLOAT_H
 
 #include "math/pprz_algebra_float.h"
 
 #include "generated/airframe.h"
 
-#ifndef STABILIZATION_ATTITUDE_FLOAT_GAIN_NB
-#define STABILIZATION_ATTITUDE_FLOAT_GAIN_NB 1
-#endif
-
-#ifndef STABILIZATION_ATTITUDE_FLOAT_GAIN_IDX_DEFAULT
-#define STABILIZATION_ATTITUDE_FLOAT_GAIN_IDX_DEFAULT 0
-#endif
 
 struct FloatAttitudeGains {
   struct FloatVect3  p;
@@ -46,12 +46,9 @@ struct FloatAttitudeGains {
   struct FloatVect3  surface_i;
 };
 
-extern struct FloatAttitudeGains stabilization_gains[];
 extern struct FloatEulers stabilization_att_sum_err_eulers;
 
 extern float stabilization_att_fb_cmd[COMMANDS_NB];
 extern float stabilization_att_ff_cmd[COMMANDS_NB];
 
-void stabilization_attitude_gain_schedule(uint8_t idx);
+#endif /* STABILIZATION_ATTITUDE_COMMON_FLOAT_H */
-
-#endif /* STABILIZATION_ATTITUDE_FLOAT_H */

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h

@@ -19,8 +19,14 @@
  * Boston, MA 02111-1307, USA.
  */
 
-#ifndef STABILIZATION_ATTITUDE_INT_H
+/**
-#define STABILIZATION_ATTITUDE_INT_H
+ * @file stabilization_attitude_common_int.h
+ *
+ * Common data structures shared by euler and quaternion int implementations.
+ */
+
+#ifndef STABILIZATION_ATTITUDE_COMMON_INT_H
+#define STABILIZATION_ATTITUDE_COMMON_INT_H
 
 #include "math/pprz_algebra_int.h"
 
@@ -39,4 +45,12 @@ extern struct Int32Eulers stabilization_att_sum_err;
 extern int32_t stabilization_att_fb_cmd[COMMANDS_NB];
 extern int32_t stabilization_att_ff_cmd[COMMANDS_NB];
 
-#endif /* STABILIZATION_ATTITUDE_INT_H */
+// common so it can be used for downlink/debug
+extern struct Int32Eulers stab_att_sp_euler;  ///< with #INT32_ANGLE_FRAC
+
+#define stabilization_attitude_common_int_SetKiPhi(_val) {	\
+    stabilization_gains.i.x = _val;             \
+    stabilization_att_sum_err.phi = 0;          \
+  }
+
+#endif /* STABILIZATION_ATTITUDE_COMMON_INT_H */

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c

@@ -20,6 +20,7 @@
  */
 
 #include "firmwares/rotorcraft/stabilization.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
 
 #include "math/pprz_algebra_float.h"
 #include "state.h"
@@ -40,24 +41,24 @@ void stabilization_attitude_init(void) {
   stabilization_attitude_ref_init();
 
   VECT3_ASSIGN(stabilization_gains.p,
-               STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN,
+               STABILIZATION_ATTITUDE_PHI_PGAIN,
-               STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN,
+               STABILIZATION_ATTITUDE_THETA_PGAIN,
-               STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN);
+               STABILIZATION_ATTITUDE_PSI_PGAIN);
 
   VECT3_ASSIGN(stabilization_gains.d,
-               STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN,
+               STABILIZATION_ATTITUDE_PHI_DGAIN,
-               STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN,
+               STABILIZATION_ATTITUDE_THETA_DGAIN,
-               STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN);
+               STABILIZATION_ATTITUDE_PSI_DGAIN);
 
   VECT3_ASSIGN(stabilization_gains.i,
-               STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN,
+               STABILIZATION_ATTITUDE_PHI_IGAIN,
-               STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN,
+               STABILIZATION_ATTITUDE_THETA_IGAIN,
-               STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN);
+               STABILIZATION_ATTITUDE_PSI_IGAIN);
 
   VECT3_ASSIGN(stabilization_gains.dd,
-               STABILIZATION_ATTITUDE_FLOAT_PHI_DDGAIN,
+               STABILIZATION_ATTITUDE_PHI_DDGAIN,
-               STABILIZATION_ATTITUDE_FLOAT_THETA_DDGAIN,
+               STABILIZATION_ATTITUDE_THETA_DDGAIN,
-               STABILIZATION_ATTITUDE_FLOAT_PSI_DDGAIN);
+               STABILIZATION_ATTITUDE_PSI_DDGAIN);
 
   FLOAT_EULERS_ZERO( stabilization_att_sum_err );
 
@@ -65,17 +66,28 @@ void stabilization_attitude_init(void) {
 
 
 void stabilization_attitude_read_rc(bool_t in_flight) {
-
   stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight);
-
 }
 
 
 void stabilization_attitude_enter(void) {
 
-  STABILIZATION_ATTITUDE_FLOAT_RESET_PSI_REF(  stab_att_sp_euler );
+  /* reset psi setpoint to current psi angle */
-  FLOAT_EULERS_ZERO( stabilization_att_sum_err );
+  stab_att_sp_euler.psi = stabilization_attitude_get_heading_f();
 
+  stabilization_attitude_ref_enter();
+
+  FLOAT_EULERS_ZERO(stabilization_att_sum_err);
+}
+
+void stabilization_attitude_set_failsafe_setpoint(void) {
+  stab_att_sp_euler.phi = 0.0;
+  stab_att_sp_euler.theta = 0.0;
+  stab_att_sp_euler.psi = stateGetNedToBodyEulers_f()->psi;
+}
+
+void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
+  EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_euler);
 }
 
 
@@ -95,7 +107,7 @@ void stabilization_attitude_run(bool_t  in_flight) {
 
   /* Compute feedback                  */
   /* attitude error            */
-  struct FloatEulers att_float* = stateGetNedToBodyEulers_f();
+  struct FloatEulers *att_float = stateGetNedToBodyEulers_f();
   struct FloatEulers att_err;
   EULERS_DIFF(att_err, stab_att_ref_euler, *att_float);
   FLOAT_ANGLE_NORMALIZE(att_err.psi);

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h

@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
+ *
+ * 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 stabilization_attitude_euler_float.h
+ *
+ * Rotorcraft attitude stabilization in euler float version.
+ */
+
+#ifndef STABILIZATION_ATTITUDE_EULER_FLOAT_H
+#define STABILIZATION_ATTITUDE_EULER_FLOAT_H
+
+#include "math/pprz_algebra_float.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h"
+
+extern struct FloatAttitudeGains stabilization_gains;
+
+#endif /* STABILIZATION_ATTITUDE_EULER_FLOAT_H */

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c

@@ -47,6 +47,13 @@ struct Int32Eulers stabilization_att_sum_err;
 int32_t stabilization_att_fb_cmd[COMMANDS_NB];
 int32_t stabilization_att_ff_cmd[COMMANDS_NB];
 
+static inline void reset_psi_ref_from_body(void) {
+  //sp has been set from body using stabilization_attitude_get_yaw_i, use that value
+  stab_att_ref_euler.psi = stab_att_sp_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
+  stab_att_ref_rate.r = 0;
+  stab_att_ref_accel.r = 0;
+}
+
 void stabilization_attitude_init(void) {
 
   stabilization_attitude_ref_init();
@@ -79,18 +86,23 @@ void stabilization_attitude_init(void) {
 
 
 void stabilization_attitude_read_rc(bool_t in_flight) {
-
   stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight);
-
 }
 
-
 void stabilization_attitude_enter(void) {
-
   stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
   reset_psi_ref_from_body();
   INT_EULERS_ZERO( stabilization_att_sum_err );
+}
 
+void stabilization_attitude_set_failsafe_setpoint(void) {
+  stab_att_sp_euler.phi = 0;
+  stab_att_sp_euler.theta = 0;
+  stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
+}
+
+void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
+  memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers));
 }
 
 
@@ -101,7 +113,6 @@ void stabilization_attitude_enter(void) {
 
 void stabilization_attitude_run(bool_t  in_flight) {
 
-
   /* update reference */
   stabilization_attitude_ref_update();
 

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h

@@ -0,0 +1,28 @@
+/*
+ * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
+ *
+ * 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 STABILIZATION_ATTITUDE_EULER_INT_H
+#define STABILIZATION_ATTITUDE_EULER_INT_H
+
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h"
+
+#endif /* STABILIZATION_ATTITUDE_EULER_INT_H */

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c

@@ -32,27 +32,15 @@
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
 #include "paparazzi.h"
 
-#include "subsystems/radio_control.h"
 #include "generated/airframe.h"
 
 #define TRAJ_MAX_BANK (int32_t)ANGLE_BFP_OF_REAL(GUIDANCE_H_MAX_BANK)
 
 struct Int32Eulers stab_att_sp_euler;
 
-//STUB
-struct Int32Eulers stabilization_att_sum_err;
-struct Int32Eulers stab_att_ref_euler; ///< with #REF_ANGLE_FRAC
-struct Int32Rates  stab_att_ref_rate;  ///< with #REF_RATE_FRAC
-struct Int32Rates  stab_att_ref_accel; ///< with #REF_ACCEL_FRAC
-int32_t stabilization_att_fb_cmd[COMMANDS_NB];
-int32_t stabilization_att_ff_cmd[COMMANDS_NB];
 
 void stabilization_attitude_init(void) {
-  INT_EULERS_ZERO(stabilization_att_sum_err);
   INT_EULERS_ZERO(stab_att_sp_euler);
-  INT_EULERS_ZERO(stab_att_ref_euler);
-  INT_RATES_ZERO(stab_att_ref_rate);
-  INT_RATES_ZERO(stab_att_ref_accel);
 }
 
 
@@ -67,10 +55,11 @@ void stabilization_attitude_enter(void) {
 }
 
 void stabilization_attitude_run(bool_t  in_flight __attribute__ ((unused))) {
-  /* For roll an pitch we pass truough the desired angles as stabilization command */
+
-  EULERS_SMUL(stab_att_ref_euler, stab_att_sp_euler, MAX_PPRZ/TRAJ_MAX_BANK);
+  /* For roll and pitch we pass trough the desired angles as stabilization command */
-  stabilization_cmd[COMMAND_ROLL] = stab_att_ref_euler.phi;
+  const int32_t angle2cmd = (MAX_PPRZ/TRAJ_MAX_BANK);
-  stabilization_cmd[COMMAND_PITCH] = stab_att_ref_euler.theta;
+  stabilization_cmd[COMMAND_ROLL] = stab_att_sp_euler.phi * angle2cmd;
+  stabilization_cmd[COMMAND_PITCH] = stab_att_sp_euler.theta * angle2cmd;
 
   //TODO: Fix yaw with PID controller
   int32_t yaw_error = stateGetNedToBodyEulers_i()->psi - stab_att_sp_euler.psi;
@@ -83,10 +72,3 @@ void stabilization_attitude_run(bool_t  in_flight __attribute__ ((unused))) {
   BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
 }
 
-void stabilization_attitude_ref_init(void) {
-
-}
-
-void stabilization_attitude_ref_update(void) {
-
-}

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.h

@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
+ *
+ * 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 STABILIZATION_ATTITUDE_PASSTHROUGH_H
+#define STABILIZATION_ATTITUDE_PASSTHROUGH_H
+
+#include "math/pprz_algebra_int.h"
+
+#include "generated/airframe.h"
+
+
+extern struct Int32Eulers stab_att_sp_euler;  ///< with #INT32_ANGLE_FRAC
+
+
+#endif /* STABILIZATION_ATTITUDE_PASSTHROUGH_H */

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c

@@ -24,16 +24,15 @@
  */
 
 #include "firmwares/rotorcraft/stabilization.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
 
 #include <stdio.h>
 #include "math/pprz_algebra_float.h"
 #include "math/pprz_algebra_int.h"
 #include "state.h"
 #include "generated/airframe.h"
-#include "stabilization_attitude_float.h"
-#include "stabilization_attitude_rc_setpoint.h"
 
-struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_FLOAT_GAIN_NB];
+struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_GAIN_NB];
 
 struct FloatQuat stabilization_att_sum_err_quat;
 struct FloatEulers stabilization_att_sum_err_eulers;
@@ -43,27 +42,27 @@ struct FloatRates last_body_rate;
 float stabilization_att_fb_cmd[COMMANDS_NB];
 float stabilization_att_ff_cmd[COMMANDS_NB];
 
-static int gain_idx = STABILIZATION_ATTITUDE_FLOAT_GAIN_IDX_DEFAULT;
+static int gain_idx = STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT;
 
-static const float phi_pgain[] = STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN;
+static const float phi_pgain[] = STABILIZATION_ATTITUDE_PHI_PGAIN;
-static const float theta_pgain[] = STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN;
+static const float theta_pgain[] = STABILIZATION_ATTITUDE_THETA_PGAIN;
-static const float psi_pgain[] = STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN;
+static const float psi_pgain[] = STABILIZATION_ATTITUDE_PSI_PGAIN;
 
-static const float phi_dgain[] = STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN;
+static const float phi_dgain[] = STABILIZATION_ATTITUDE_PHI_DGAIN;
-static const float theta_dgain[] = STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN;
+static const float theta_dgain[] = STABILIZATION_ATTITUDE_THETA_DGAIN;
-static const float psi_dgain[] = STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN;
+static const float psi_dgain[] = STABILIZATION_ATTITUDE_PSI_DGAIN;
 
-static const float phi_igain[] = STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN;
+static const float phi_igain[] = STABILIZATION_ATTITUDE_PHI_IGAIN;
-static const float theta_igain[] = STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN;
+static const float theta_igain[] = STABILIZATION_ATTITUDE_THETA_IGAIN;
-static const float psi_igain[] = STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN;
+static const float psi_igain[] = STABILIZATION_ATTITUDE_PSI_IGAIN;
 
-static const float phi_ddgain[] = STABILIZATION_ATTITUDE_FLOAT_PHI_DDGAIN;
+static const float phi_ddgain[] = STABILIZATION_ATTITUDE_PHI_DDGAIN;
-static const float theta_ddgain[] = STABILIZATION_ATTITUDE_FLOAT_THETA_DDGAIN;
+static const float theta_ddgain[] = STABILIZATION_ATTITUDE_THETA_DDGAIN;
-static const float psi_ddgain[] = STABILIZATION_ATTITUDE_FLOAT_PSI_DDGAIN;
+static const float psi_ddgain[] = STABILIZATION_ATTITUDE_PSI_DDGAIN;
 
-static const float phi_dgain_d[] = STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN_D;
+static const float phi_dgain_d[] = STABILIZATION_ATTITUDE_PHI_DGAIN_D;
-static const float theta_dgain_d[] = STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN_D;
+static const float theta_dgain_d[] = STABILIZATION_ATTITUDE_THETA_DGAIN_D;
-static const float psi_dgain_d[] = STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN_D;
+static const float psi_dgain_d[] = STABILIZATION_ATTITUDE_PSI_DGAIN_D;
 
 
 #if defined COMMAND_ROLL_SURFACE && defined COMMAND_PITCH_SURFACE && defined COMMAND_YAW_SURFACE
@@ -71,21 +70,21 @@ static const float psi_dgain_d[] = STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN_D;
 #endif
 
 #ifdef HAS_SURFACE_COMMANDS
-static const float phi_pgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN_SURFACE;
+static const float phi_pgain_surface[] = STABILIZATION_ATTITUDE_PHI_PGAIN_SURFACE;
-static const float theta_pgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN_SURFACE;
+static const float theta_pgain_surface[] = STABILIZATION_ATTITUDE_THETA_PGAIN_SURFACE;
-static const float psi_pgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN_SURFACE;
+static const float psi_pgain_surface[] = STABILIZATION_ATTITUDE_PSI_PGAIN_SURFACE;
 
-static const float phi_dgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN_SURFACE;
+static const float phi_dgain_surface[] = STABILIZATION_ATTITUDE_PHI_DGAIN_SURFACE;
-static const float theta_dgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN_SURFACE;
+static const float theta_dgain_surface[] = STABILIZATION_ATTITUDE_THETA_DGAIN_SURFACE;
-static const float psi_dgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN_SURFACE;
+static const float psi_dgain_surface[] = STABILIZATION_ATTITUDE_PSI_DGAIN_SURFACE;
 
-static const float phi_igain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN_SURFACE;
+static const float phi_igain_surface[] = STABILIZATION_ATTITUDE_PHI_IGAIN_SURFACE;
-static const float theta_igain_surface[] = STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN_SURFACE;
+static const float theta_igain_surface[] = STABILIZATION_ATTITUDE_THETA_IGAIN_SURFACE;
-static const float psi_igain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN_SURFACE;
+static const float psi_igain_surface[] = STABILIZATION_ATTITUDE_PSI_IGAIN_SURFACE;
 
-static const float phi_ddgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PHI_DDGAIN_SURFACE;
+static const float phi_ddgain_surface[] = STABILIZATION_ATTITUDE_PHI_DDGAIN_SURFACE;
-static const float theta_ddgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_THETA_DDGAIN_SURFACE;
+static const float theta_ddgain_surface[] = STABILIZATION_ATTITUDE_THETA_DDGAIN_SURFACE;
-static const float psi_ddgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PSI_DDGAIN_SURFACE;
+static const float psi_ddgain_surface[] = STABILIZATION_ATTITUDE_PSI_DDGAIN_SURFACE;
 #endif
 
 #define IERROR_SCALE 1024
@@ -94,7 +93,7 @@ void stabilization_attitude_init(void) {
 
   stabilization_attitude_ref_init();
 
-  for (int i = 0; i < STABILIZATION_ATTITUDE_FLOAT_GAIN_NB; i++) {
+  for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
     VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
     VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
     VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
@@ -115,7 +114,7 @@ void stabilization_attitude_init(void) {
 
 void stabilization_attitude_gain_schedule(uint8_t idx)
 {
-  if (gain_idx >= STABILIZATION_ATTITUDE_FLOAT_GAIN_NB) {
+  if (gain_idx >= STABILIZATION_ATTITUDE_GAIN_NB) {
     // This could be bad -- Just say no.
     return;
   }
@@ -125,10 +124,8 @@ void stabilization_attitude_gain_schedule(uint8_t idx)
 
 void stabilization_attitude_enter(void) {
 
-  float heading = stabilization_attitude_get_heading_f();
-
   /* reset psi setpoint to current psi angle */
-  stab_att_sp_euler.psi = heading;
+  stab_att_sp_euler.psi = stabilization_attitude_get_heading_f();
 
   stabilization_attitude_ref_enter();
 
@@ -136,6 +133,21 @@ void stabilization_attitude_enter(void) {
   FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
 }
 
+void stabilization_attitude_set_failsafe_setpoint(void) {
+  /* set failsafe to zero roll/pitch and current heading */
+  float heading2 = stabilization_attitude_get_heading_f() / 2;
+  stab_att_sp_quat.qi = cosf(heading2);
+  stab_att_sp_quat.qx = 0.0;
+  stab_att_sp_quat.qy = 0.0;
+  stab_att_sp_quat.qz = sinf(heading2);
+}
+
+void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
+  EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_euler);
+  FLOAT_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler);
+  FLOAT_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
+}
+
 #ifndef GAIN_PRESCALER_FF
 #define GAIN_PRESCALER_FF 1
 #endif

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h

@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
+ *
+ * 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 stabilization_attitude_quat_float.h
+ *
+ * Rotorcraft attitude stabilization in quaternion float version.
+ */
+
+#ifndef STABILIZATION_ATTITUDE_QUAT_FLOAT_H
+#define STABILIZATION_ATTITUDE_QUAT_FLOAT_H
+
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h"
+
+#ifndef STABILIZATION_ATTITUDE_GAIN_NB
+#define STABILIZATION_ATTITUDE_GAIN_NB 1
+#endif
+
+#ifndef STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT
+#define STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT 0
+#endif
+
+extern struct FloatAttitudeGains stabilization_gains[];
+
+void stabilization_attitude_gain_schedule(uint8_t idx);
+
+#endif /* STABILIZATION_ATTITUDE_QUAT_FLOAT_H */

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c

@@ -74,10 +74,8 @@ void stabilization_attitude_init(void) {
 
 void stabilization_attitude_enter(void) {
 
-  int32_t heading = stabilization_attitude_get_heading_i();
-
   /* reset psi setpoint to current psi angle */
-  stab_att_sp_euler.psi = heading;
+  stab_att_sp_euler.psi = stabilization_attitude_get_heading_i();
 
   stabilization_attitude_ref_enter();
 
@@ -85,6 +83,22 @@ void stabilization_attitude_enter(void) {
   INT_EULERS_ZERO(stabilization_att_sum_err);
 }
 
+void stabilization_attitude_set_failsafe_setpoint(void) {
+  /* set failsafe to zero roll/pitch and current heading */
+  int32_t heading2 = stabilization_attitude_get_heading_i() / 2;
+  PPRZ_ITRIG_COS(stab_att_sp_quat.qi, heading2);
+  stab_att_sp_quat.qx = 0;
+  stab_att_sp_quat.qy = 0;
+  PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2);
+}
+
+void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
+  // copy euler setpoint for debugging
+  memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers));
+  INT32_QUAT_OF_EULERS(stab_att_sp_quat, *sp_euler);
+  INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
+}
+
 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
 #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
 

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h

@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
+ *
+ * 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 STABILIZATION_ATTITUDE_QUAT_INT_H
+#define STABILIZATION_ATTITUDE_QUAT_INT_H
+
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
+
+#include "math/pprz_algebra_int.h"
+
+extern struct Int32Quat   stab_att_sp_quat;   ///< with #INT32_QUAT_FRAC
+
+#endif /* STABILIZATION_ATTITUDE_QUAT_INT_H */

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c

@@ -28,21 +28,9 @@
 #include "subsystems/radio_control.h"
 #include "state.h"
 #include "firmwares/rotorcraft/guidance/guidance_h.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 #include "firmwares/rotorcraft/autopilot.h"
 
-#if defined STABILIZATION_ATTITUDE_TYPE_INT
-#define SP_MAX_PHI     (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI)
-#define SP_MAX_THETA   (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA)
-#define SP_MAX_R       (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R)
-#elif defined STABILIZATION_ATTITUDE_TYPE_FLOAT
-#define SP_MAX_PHI   STABILIZATION_ATTITUDE_SP_MAX_PHI
-#define SP_MAX_THETA STABILIZATION_ATTITUDE_SP_MAX_THETA
-#define SP_MAX_R     STABILIZATION_ATTITUDE_SP_MAX_R
-#else
-#error "STABILIZATION_ATTITUDE_TYPE not defined"
-#endif
-
 #ifndef RC_UPDATE_FREQ
 #define RC_UPDATE_FREQ 40
 #endif
@@ -96,13 +84,16 @@ float stabilization_attitude_get_heading_f(void) {
  * @param[out] sp         attitude setpoint as euler angles
  */
 void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight) {
+  const int32_t max_phi_scale = (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI) /  MAX_PPRZ;
+  const int32_t max_theta_scale = (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA) /  MAX_PPRZ;
+  const int32_t max_r_scale = (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R) /  MAX_PPRZ;
 
-  sp->phi = ((int32_t) radio_control.values[RADIO_ROLL]  * SP_MAX_PHI / MAX_PPRZ);
+  sp->phi = ((int32_t) radio_control.values[RADIO_ROLL]  * max_phi_scale);
-  sp->theta = ((int32_t) radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ);
+  sp->theta = ((int32_t) radio_control.values[RADIO_PITCH] * max_theta_scale);
 
   if (in_flight) {
     if (YAW_DEADBAND_EXCEEDED()) {
-      sp->psi += ((int32_t) radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
+      sp->psi += ((int32_t) radio_control.values[RADIO_YAW] * max_r_scale / RC_UPDATE_FREQ);
       INT32_ANGLE_NORMALIZE(sp->psi);
     }
     if (autopilot_mode == AP_MODE_FORWARD) {
@@ -121,10 +112,11 @@ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool
 #ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
     // Make sure the yaw setpoint does not differ too much from the real yaw
     // to prevent a sudden switch at 180 deg
+    const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
+
     int32_t heading = stabilization_attitude_get_heading_i();
 
     int32_t delta_psi = sp->psi - heading;
-    int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
     INT32_ANGLE_NORMALIZE(delta_psi);
     if (delta_psi > delta_limit){
       sp->psi = heading + delta_limit;
@@ -162,12 +154,12 @@ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool
 
 
 void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight) {
-  sp->phi = (radio_control.values[RADIO_ROLL]  * SP_MAX_PHI / MAX_PPRZ);
+  sp->phi = (radio_control.values[RADIO_ROLL]  * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ);
-  sp->theta = (radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ);
+  sp->theta = (radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ);
 
   if (in_flight) {
     if (YAW_DEADBAND_EXCEEDED()) {
-      sp->psi += (radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
+      sp->psi += (radio_control.values[RADIO_YAW] * STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
       FLOAT_ANGLE_NORMALIZE(sp->psi);
     }
     if (autopilot_mode == AP_MODE_FORWARD) {

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler.h

@@ -19,15 +19,12 @@
  * Boston, MA 02111-1307, USA.
  */
 
-/** @file firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h
+/** @file firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler.h
- *  Top level Rotorcraft attitude reference generation include.
+ *  Common rotorcraft attitude euler reference generation include.
- *  Automatically includes the actual implementation headers.
  */
 
-#ifndef STABILIZATION_ATTITUDE_REF_H
+#ifndef STABILIZATION_ATTITUDE_REF_EULER_H
-#define STABILIZATION_ATTITUDE_REF_H
+#define STABILIZATION_ATTITUDE_REF_EULER_H
-
-#include STABILIZATION_ATTITUDE_REF_TYPE_H
 
 #define SATURATE_SPEED_TRIM_ACCEL() {                   \
     if (stab_att_ref_rate.p >= REF_RATE_MAX_P) {		\
@@ -62,4 +59,5 @@
     }                                                   \
   }
 
-#endif /* STABILIZATION_ATTITUDE_REF_H */
+
+#endif /* STABILIZATION_ATTITUDE_REF_EULER_H */

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c

@@ -1,5 +1,5 @@
-#include "firmwares/rotorcraft/stabilization.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
-
+#include "generated/airframe.h"
 
 struct FloatEulers stab_att_sp_euler;
 struct FloatEulers stab_att_ref_euler;
@@ -21,25 +21,37 @@ void stabilization_attitude_ref_init(void) {
  */
 #define DT_UPDATE (1./PERIODIC_FREQUENCY)
 
-#define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_FLOAT_REF_MAX_PDOT
+#define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_REF_MAX_PDOT
-#define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_FLOAT_REF_MAX_QDOT
+#define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_QDOT
-#define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_FLOAT_REF_MAX_RDOT
+#define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_REF_MAX_RDOT
 
-#define REF_RATE_MAX_P STABILIZATION_ATTITUDE_FLOAT_REF_MAX_P
+#define REF_RATE_MAX_P STABILIZATION_ATTITUDE_REF_MAX_P
-#define REF_RATE_MAX_Q STABILIZATION_ATTITUDE_FLOAT_REF_MAX_Q
+#define REF_RATE_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_Q
-#define REF_RATE_MAX_R STABILIZATION_ATTITUDE_FLOAT_REF_MAX_R
+#define REF_RATE_MAX_R STABILIZATION_ATTITUDE_REF_MAX_R
 
-#define OMEGA_P   STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_P
+#define OMEGA_P   STABILIZATION_ATTITUDE_REF_OMEGA_P
-#define OMEGA_Q   STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_Q
+#define OMEGA_Q   STABILIZATION_ATTITUDE_REF_OMEGA_Q
-#define OMEGA_R   STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_R
+#define OMEGA_R   STABILIZATION_ATTITUDE_REF_OMEGA_R
 
-#define ZETA_P    STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_P
+#define ZETA_P    STABILIZATION_ATTITUDE_REF_ZETA_P
-#define ZETA_Q    STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_Q
+#define ZETA_Q    STABILIZATION_ATTITUDE_REF_ZETA_Q
-#define ZETA_R    STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_R
+#define ZETA_R    STABILIZATION_ATTITUDE_REF_ZETA_R
 
 
 #define USE_REF 1
 
+static inline void reset_psi_ref_from_body(void) {
+  //sp has been set from body using stabilization_attitude_get_yaw_f, use that value
+  stab_att_ref_euler.psi = stab_att_sp_euler.psi;
+  stab_att_ref_rate.r = 0;
+  stab_att_ref_accel.r = 0;
+}
+
+void stabilization_attitude_ref_enter()
+{
+  reset_psi_ref_from_body();
+}
+
 void stabilization_attitude_ref_update() {
 
 #if USE_REF

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h

@@ -22,20 +22,12 @@
 #ifndef STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H
 #define STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H
 
-#include "subsystems/radio_control.h"
 #include "math/pprz_algebra_float.h"
 
-
 #include "stabilization_attitude_ref_float.h"
+#include "stabilization_attitude_ref_euler.h"
 
-
+void stabilization_attitude_ref_enter(void);
-#define STABILIZATION_ATTITUDE_FLOAT_ADD_SP(_add_sp) {  \
-    struct FloatEulers add_sp_float;                    \
-    EULERS_FLOAT_OF_BFP(add_sp_float, (_add_sp));		\
-    EULERS_ADD(stabilization_att_sp,add_sp_float);		\
-    FLOAT_ANGLE_NORMALIZE(stabilization_att_sp.psi);	\
-  }
-
 
 
 #endif /* STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H */

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c

@@ -24,7 +24,8 @@
  *
  */
 
-#include "firmwares/rotorcraft/stabilization.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
+#include "generated/airframe.h"
 
 struct Int32Eulers stab_att_sp_euler;
 struct Int32Eulers stab_att_ref_euler;  ///< with #REF_ANGLE_FRAC

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h

@@ -28,6 +28,7 @@
 #define STABILIZATION_ATTITUDE_REF_EULER_INT_H
 
 #include "stabilization_attitude_ref_int.h"
+#include "stabilization_attitude_ref_euler.h"
 
 
 #endif /* STABILIZATION_ATTITUDE_REF_EULER_INT_H */

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h

@@ -18,6 +18,12 @@
  * the Free Software Foundation, 59 Temple Place - Suite 330,
  * Boston, MA 02111-1307, USA.
  */
+
+/** @file stabilization_attitude_ref_float.h
+ *  Rotorcraft attitude reference generation API.
+ *  Common to all floating-point reference generators (euler and quaternion)
+ */
+
 #ifndef STABILIZATION_ATTITUDE_REF_FLOAT_H
 #define STABILIZATION_ATTITUDE_REF_FLOAT_H
 
@@ -39,11 +45,7 @@ struct FloatRefModel {
 
 extern struct FloatRefModel stab_att_ref_model[];
 
-static inline void reset_psi_ref_from_body(void) {
+extern void stabilization_attitude_ref_init(void);
-//sp has been set from body using stabilization_attitude_get_yaw_f, use that value
+extern void stabilization_attitude_ref_update(void);
-  stab_att_ref_euler.psi = stab_att_sp_euler.psi;
-  stab_att_ref_rate.r = 0;
-  stab_att_ref_accel.r = 0;
-}
 
 #endif /* STABILIZATION_ATTITUDE_REF_FLOAT_H */

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h

@@ -31,8 +31,6 @@
 
 #include "state.h"
 
-extern struct Int32Eulers stab_att_sp_euler;  ///< with #INT32_ANGLE_FRAC
-extern struct Int32Quat   stab_att_sp_quat;   ///< with #INT32_QUAT_FRAC
 extern struct Int32Eulers stab_att_ref_euler; ///< with #REF_ANGLE_FRAC
 extern struct Int32Quat   stab_att_ref_quat;  ///< with #INT32_QUAT_FRAC
 extern struct Int32Rates  stab_att_ref_rate;  ///< with #REF_RATE_FRAC
@@ -56,12 +54,7 @@ extern struct Int32RefModel stab_att_ref_model;
     while (_a < -REF_ANGLE_PI)  _a += REF_ANGLE_TWO_PI; \
   }
 
-
+extern void stabilization_attitude_ref_init(void);
-static inline void reset_psi_ref_from_body(void) {
+extern void stabilization_attitude_ref_update(void);
-//sp has been set from body using stabilization_attitude_get_yaw_i, use that value
-  stab_att_ref_euler.psi = stab_att_sp_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
-  stab_att_ref_rate.r = 0;
-  stab_att_ref_accel.r = 0;
-}
 
 #endif /* STABILIZATION_ATTITUDE_REF_INT_H */

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c

@@ -33,9 +33,9 @@
 
 #include "stabilization_attitude_ref_float.h"
 
-#define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_FLOAT_REF_MAX_PDOT
+#define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_REF_MAX_PDOT
-#define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_FLOAT_REF_MAX_QDOT
+#define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_QDOT
-#define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_FLOAT_REF_MAX_RDOT
+#define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_REF_MAX_RDOT
 
 struct FloatEulers stab_att_sp_euler;
 struct FloatQuat   stab_att_sp_quat;
@@ -44,16 +44,23 @@ struct FloatQuat   stab_att_ref_quat;
 struct FloatRates  stab_att_ref_rate;
 struct FloatRates  stab_att_ref_accel;
 
-struct FloatRefModel stab_att_ref_model[STABILIZATION_ATTITUDE_FLOAT_GAIN_NB];
+struct FloatRefModel stab_att_ref_model[STABILIZATION_ATTITUDE_GAIN_NB];
 
-static int ref_idx = STABILIZATION_ATTITUDE_FLOAT_GAIN_IDX_DEFAULT;
+static int ref_idx = STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT;
 
-static const float omega_p[] = STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_P;
+static const float omega_p[] = STABILIZATION_ATTITUDE_REF_OMEGA_P;
-static const float zeta_p[] = STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_P;
+static const float zeta_p[] = STABILIZATION_ATTITUDE_REF_ZETA_P;
-static const float omega_q[] = STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_Q;
+static const float omega_q[] = STABILIZATION_ATTITUDE_REF_OMEGA_Q;
-static const float zeta_q[] = STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_Q;
+static const float zeta_q[] = STABILIZATION_ATTITUDE_REF_ZETA_Q;
-static const float omega_r[] = STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_R;
+static const float omega_r[] = STABILIZATION_ATTITUDE_REF_OMEGA_R;
-static const float zeta_r[] = STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_R;
+static const float zeta_r[] = STABILIZATION_ATTITUDE_REF_ZETA_R;
+
+static inline void reset_psi_ref_from_body(void) {
+  //sp has been set from body using stabilization_attitude_get_yaw_f, use that value
+  stab_att_ref_euler.psi = stab_att_sp_euler.psi;
+  stab_att_ref_rate.r = 0;
+  stab_att_ref_accel.r = 0;
+}
 
 static void update_ref_quat_from_eulers(void) {
   struct FloatRMat ref_rmat;
@@ -76,7 +83,7 @@ void stabilization_attitude_ref_init(void) {
   FLOAT_RATES_ZERO( stab_att_ref_rate);
   FLOAT_RATES_ZERO( stab_att_ref_accel);
 
-  for (int i = 0; i < STABILIZATION_ATTITUDE_FLOAT_GAIN_NB; i++) {
+  for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
     RATES_ASSIGN(stab_att_ref_model[i].omega, omega_p[i], omega_q[i], omega_r[i]);
     RATES_ASSIGN(stab_att_ref_model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]);
   }
@@ -102,14 +109,14 @@ void stabilization_attitude_ref_enter()
 #define DT_UPDATE (1./PERIODIC_FREQUENCY)
 
 // default to fast but less precise quaternion integration
-#ifndef STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_INFINITESIMAL_STEP
+#ifndef STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP
-#define STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_INFINITESIMAL_STEP TRUE
+#define STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP TRUE
 #endif
 
 void stabilization_attitude_ref_update() {
 
   /* integrate reference attitude            */
-#if STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_INFINITESIMAL_STEP
+#if STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP
   struct FloatQuat qdot;
   FLOAT_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat);
   QUAT_SMUL(qdot, qdot, DT_UPDATE);

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h

@@ -30,8 +30,6 @@
 #ifndef STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_FLOAT_H
 #define STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_FLOAT_H
 
-#include "firmwares/rotorcraft/stabilization.h"
-
 #include "subsystems/radio_control.h"
 #include "math/pprz_algebra_float.h"
 

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c

@@ -74,6 +74,12 @@ struct Int32RefModel stab_att_ref_model = {
   {STABILIZATION_ATTITUDE_REF_ZETA_P, STABILIZATION_ATTITUDE_REF_ZETA_Q, STABILIZATION_ATTITUDE_REF_ZETA_R}
 };
 
+static inline void reset_psi_ref_from_body(void) {
+  //sp has been set from body using stabilization_attitude_get_yaw_i, use that value
+  stab_att_ref_euler.psi = stab_att_sp_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
+  stab_att_ref_rate.r = 0;
+  stab_att_ref_accel.r = 0;
+}
 
 void stabilization_attitude_ref_init(void) {
 

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h

@@ -30,10 +30,8 @@
 #ifndef STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H
 #define STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H
 
-#include "firmwares/rotorcraft/stabilization.h"
 #include "stabilization_attitude_ref_int.h"
 
-
 void stabilization_attitude_ref_enter(void);
 
 #endif /* STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H */

sw/airborne/firmwares/rotorcraft/telemetry.h

@@ -223,6 +223,7 @@
                                   &stabilization_cmd[COMMAND_THRUST]); \
   }
 
+#ifdef STABILIZATION_ATTITUDE_REF_TYPE_H
 #ifdef STABILIZATION_ATTITUDE_TYPE_INT
 #define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) {       \
   struct Int32Rates* body_rate = stateGetBodyRates_i();   \
@@ -263,9 +264,9 @@
                           &stab_att_ref_accel.q, \
                           &stab_att_ref_accel.r); \
   }
-#endif /* STABILIZATION_ATTITUDE_TYPE_INT */
 
-#ifdef STABILIZATION_ATTITUDE_TYPE_FLOAT
+#elif defined STABILIZATION_ATTITUDE_TYPE_FLOAT
+
 #define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) {       \
   struct FloatRates* body_rate = stateGetBodyRates_f();   \
   struct FloatEulers* att = stateGetNedToBodyEulers_f();  \
@@ -308,6 +309,10 @@
   }
 
 #endif /* STABILIZATION_ATTITUDE_TYPE_FLOAT */
+#else /* STABILIZATION_ATTITUDE_REF_TYPE_H */
+#define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) {}
+#define PERIODIC_SEND_STAB_ATTITUDE_REF(_trans, _dev) {}
+#endif
 
 
 #include "subsystems/ahrs/ahrs_aligner.h"