From ccfc57ef1560bf1268940cf480a7fce8a1f53b04 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Mon, 17 Jun 2024 14:25:09 +0200
Subject: [PATCH] [stabilization] fix reporting of attitude setpoint in hybrid
 mode (#3309)

also add stab message to flight recorder
---
 conf/telemetry/default_rotorcraft.xml            |  9 +++++----
 .../stabilization/stabilization_indi.c           | 16 ++++++----------
 2 files changed, 11 insertions(+), 14 deletions(-)

diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml
index e8bc580192..22a44f3c35 100644
--- a/conf/telemetry/default_rotorcraft.xml
+++ b/conf/telemetry/default_rotorcraft.xml
@@ -37,8 +37,8 @@
       <message name="INS_EKF2"                 period=".25"/>
       <message name="WIND_INFO_RET"            period="1."/>
       <message name="AHRS_REF_QUAT"            period="0.5"/>
-      <message name="STAB_ATTITUDE"      period=".25"/>
-      <message name="EFF_MAT_G"            period="2.0"/>
+      <message name="STAB_ATTITUDE"            period=".25"/>
+      <message name="EFF_MAT_G"                period="2.0"/>
     </mode>
 
     <mode name="ppm">
@@ -111,8 +111,8 @@
       <message name="STAB_ATTITUDE_REF_INT"   period=".03"/>
       <message name="STAB_ATTITUDE_FLOAT"     period=".03"/>
       <message name="STAB_ATTITUDE_REF_FLOAT" period=".03"/>
-      <message name="STAB_ATTITUDE"      period=".25"/>
-      <message name="EFF_MAT_G"            period="2.0"/>
+      <message name="STAB_ATTITUDE"           period=".25"/>
+      <message name="EFF_MAT_G"               period="2.0"/>
     </mode>
 
     <mode name="vert_loop" key_press="v">
@@ -206,6 +206,7 @@
       <message name="IMU_MAG_SCALED"           period=".2"/>
       <message name="LIDAR"                    period="0.05"/>
       <message name="ESC"                      period="0.05"/>
+      <message name="STAB_ATTITUDE"            period=".1"/>
     </mode>
   </process>
 
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
index c7033a294a..45c10a94f8 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
@@ -212,7 +212,6 @@ struct FloatRates angular_rate_ref = {0., 0., 0.};
 float angular_acceleration[3] = {0., 0., 0.};
 float actuator_state[INDI_NUM_ACT];
 float indi_u[INDI_NUM_ACT];
-float rate_vect_prev[3] = {0., 0., 0.};
 
 float q_filt = 0.0;
 float r_filt = 0.0;
@@ -329,13 +328,15 @@ static void send_att_full_indi(struct transport_tx *trans, struct link_device *d
 {
   float zero = 0.0;
   struct FloatRates *body_rates = stateGetBodyRates_f();
-  struct FloatEulers att_sp;
-  EULERS_FLOAT_OF_BFP(att_sp, stab_att_sp_euler);
+  struct FloatEulers att, att_sp;
 #if GUIDANCE_INDI_HYBRID
-  struct FloatEulers att;
   float_eulers_of_quat_zxy(&att, stateGetNedToBodyQuat_f());
+  struct FloatQuat stab_att_sp_quat_f;
+  QUAT_FLOAT_OF_BFP(stab_att_sp_quat_f, stab_att_sp_quat);
+  float_eulers_of_quat_zxy(&att_sp, &stab_att_sp_quat_f);
 #else
-  struct FloatEulers att = *stateGetNedToBodyEulers_f();
+  att = *stateGetNedToBodyEulers_f();
+  EULERS_FLOAT_OF_BFP(att_sp, stab_att_sp_euler);
 #endif
   pprz_msg_send_STAB_ATTITUDE(trans, dev, AC_ID,
                                       &att.phi, &att.theta, &att.psi,           // att
@@ -434,11 +435,6 @@ void stabilization_indi_init(void)
  */
 void stabilization_indi_enter(void)
 {
-  /* reset psi setpoint to current psi angle */
-  stab_att_sp_euler.psi = stabilization_attitude_get_heading_i();
-
-  float_vect_zero(rate_vect_prev, 3);
-
   float_vect_zero(du_estimation, INDI_NUM_ACT);
   float_vect_zero(ddu_estimation, INDI_NUM_ACT);
 }