Add euler zxy get (#3695)

Add get Euler ZXY and correct RC attitude reset

sw/airborne/firmwares/rotorcraft/autopilot_firmware.c

@@ -156,7 +156,7 @@ static void send_fp(struct transport_tx *trans, struct link_device *dev)
   struct EnuCoor_i *pos = stateGetPositionEnu_i();
 #if GUIDANCE_INDI_HYBRID
   struct FloatEulers eulers_zxy;
-  float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
+  eulers_zxy = *stateGetNedToBodyEulersZxy_f();
   struct Int32Eulers att;
   EULERS_BFP_OF_REAL(att, eulers_zxy);
 #else

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

@@ -491,7 +491,7 @@ void guidance_h_run_enter(void)
 {
   /*Obtain eulers with zxy rotation order*/
   struct FloatEulers eulers_zxy;
-  float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
+  eulers_zxy = *stateGetNedToBodyEulersZxy_f();
   nav.heading = eulers_zxy.psi;
 }
 

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

@@ -390,7 +390,7 @@ void guidance_indi_init(void)
 void guidance_indi_enter(void)
 {
   /*Obtain eulers with zxy rotation order*/
-  float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
+  eulers_zxy = *stateGetNedToBodyEulersZxy_f();
   nav.heading = eulers_zxy.psi;
 
   thrust_in = stabilization.cmd[COMMAND_THRUST];
@@ -443,7 +443,7 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
   sp_accel = *accel_sp;
 
   /* Obtain eulers with zxy rotation order */
-  float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
+  eulers_zxy = *stateGetNedToBodyEulersZxy_f();
   struct FloatEulers eulers_filtered;
   float_eulers_of_quat_zxy(&eulers_filtered, &quat_filt.quat);
 
@@ -623,7 +623,7 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
 {
   struct FloatVect3 accel_sp = { 0.f, 0.f, 0.f };
 
-  float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
+  eulers_zxy = *stateGetNedToBodyEulersZxy_f();
 
   //for rc control horizontal, rotate from body axes to NED
   float psi = eulers_zxy.psi;

sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c

@@ -1628,7 +1628,7 @@ void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h,
 void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v)
 {
   // At beginnig of the loop: (1) Register Attitude, (2) Initialize gains of RM and EC, (3) Calculate Normalization of Actuators Signals, (4) Propagate Actuator Model, (5) Update effectiveness matrix
-  float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
+  eulers_zxy = *stateGetNedToBodyEulersZxy_f();
   init_controller();
   calc_normalization();
   get_act_state_oneloop();

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

@@ -199,7 +199,11 @@ void stabilization_attitude_read_rc_setpoint_earth_bound(struct AttitudeRCInput
 /// reset to current state
 void stabilization_attitude_reset_rc_setpoint(struct AttitudeRCInput *rc_sp)
 {
+#if USE_EARTH_BOUND_RC_SETPOINT
+  rc_sp->rc_eulers = *stateGetNedToBodyEulersZxy_f();
+#else
   rc_sp->rc_eulers = *stateGetNedToBodyEulers_f();
+#endif
   rc_sp->rc_quat = *stateGetNedToBodyQuat_f();
 }
 

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

@@ -390,7 +390,7 @@ static void send_att_full_indi(struct transport_tx *trans, struct link_device *d
   struct FloatRates *body_rates = stateGetBodyRates_f();
   struct FloatEulers att, att_sp;
 #if GUIDANCE_INDI_HYBRID
-  float_eulers_of_quat_zxy(&att, stateGetNedToBodyQuat_f());
+  att = *stateGetNedToBodyEulersZxy_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);

sw/airborne/math/pprz_orientation_conversion.c

@@ -194,5 +194,20 @@ void orientationCalcEulers_f(struct OrientationReps *orientation)
   /* set bit to indicate this representation is computed */
   SetBit(orientation->status, ORREP_EULER_F);
 }
+
+void orientationCalcEulersZxy_f(struct OrientationReps *orientation)
+{
+  if (bit_is_set(orientation->status, ORREP_EULER_ZXY_F)) {
+    return;
+  }
+
+  if (!bit_is_set(orientation->status, ORREP_QUAT_F)) {
+    orientationCalcQuat_f(orientation);
+  }
+  float_eulers_of_quat_zxy(&(orientation->eulers_zxy_f), &(orientation->quat_f));
+
+  /* set bit to indicate this representation is computed */
+  SetBit(orientation->status, ORREP_EULER_ZXY_F);
+}
 /** @}*/
 /** @}*/

sw/airborne/math/pprz_orientation_conversion.h

@@ -72,6 +72,7 @@ extern "C" {
 #define ORREP_QUAT_F  3  ///< Quaternion (float)
 #define ORREP_EULER_F 4  ///< zyx Euler (float)
 #define ORREP_RMAT_F  5  ///< Rotation Matrix (float)
+#define ORREP_EULER_ZXY_F 6  ///< zxy Euler (float)
 
 /*
  * @brief Struct with euler/rmat/quaternion orientation representations in BFP int and float
@@ -114,6 +115,12 @@ struct OrientationReps {
    */
   struct FloatEulers eulers_f;
 
+  /**
+   * Orientation in zxy euler angles.
+   * Units: rad
+   */
+  struct FloatEulers eulers_zxy_f;
+
   /**
    * Orientation rotation matrix.
    * Units: rad
@@ -128,6 +135,7 @@ extern void orientationCalcEulers_i(struct OrientationReps *orientation);
 extern void orientationCalcQuat_f(struct OrientationReps *orientation);
 extern void orientationCalcRMat_f(struct OrientationReps *orientation);
 extern void orientationCalcEulers_f(struct OrientationReps *orientation);
+extern void orientationCalcEulersZxy_f(struct OrientationReps *orientation);
 
 
 /*********************** validity test functions ******************/
@@ -248,6 +256,15 @@ static inline struct FloatEulers *orientationGetEulers_f(struct OrientationReps
   return &orientation->eulers_f;
 }
 
+/// Get orientation as ZXY euler angles (float).
+static inline struct FloatEulers *orientationGetEulersZxy_f(struct OrientationReps *orientation)
+{
+  if (!bit_is_set(orientation->status, ORREP_EULER_ZXY_F)) {
+    orientationCalcEulersZxy_f(orientation);
+  }
+  return &orientation->eulers_zxy_f;
+}
+
 #ifdef __cplusplus
 } /* extern "C" */
 #endif

sw/airborne/modules/ctrl/eff_scheduling_cyfoam.c

@@ -96,7 +96,7 @@ static void eff_scheduling_periodic_b(void)
   float airspeed = stateGetAirspeed_f();
   struct FloatEulers eulers_zxy;
   if(airspeed < 6.0) {
-    float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
+    eulers_zxy = *stateGetNedToBodyEulersZxy_f();
     float pitch_interp = DegOfRad(eulers_zxy.theta);
     Bound(pitch_interp, -60.0, -30.0);
     float ratio = (pitch_interp + 30.0)/(-30.);

sw/airborne/modules/ctrl/eff_scheduling_falcon.c

@@ -63,7 +63,7 @@ void eff_scheduling_falcon_periodic(void)
   if (airspeed > EFF_SCHEDULING_FALCON_LOW_AIRSPEED) {
     airspeed -= EFF_SCHEDULING_FALCON_LOW_AIRSPEED; //offset for start eff at zero!
     struct FloatEulers eulers_zxy;
-    float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
+    eulers_zxy = *stateGetNedToBodyEulersZxy_f();
 
     float pitch_ratio = 0.0f;
     if (eulers_zxy.theta > -M_PI_4) {

sw/airborne/modules/ctrl/eff_scheduling_nederdrone.c

@@ -133,7 +133,7 @@ void schdule_control_effectiveness(void) {
 
   float ratio = 0.0;
   struct FloatEulers eulers_zxy;
-  float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
+  eulers_zxy = *stateGetNedToBodyEulersZxy_f();
 
   // Ratio is only based on pitch now, as the pitot tube is often not mounted.
   if (use_scheduling == 1) {

sw/airborne/modules/ctrl/eff_scheduling_rotwing.c

@@ -495,9 +495,6 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, fl
   wls_guid_p.Wv[0] = Wv_original[0] * (1.0f + fixed_wing_percentile *
                                          AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT); // stall n low hover motor_off (weight 16x more important than vertical weight)
 
-  struct FloatEulers eulers_zxy;
-  float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
-
   float du_min_thrust_z = ((MAX_PPRZ - actuator_state_filt_vect[0]) * g1g2[3][0] + (MAX_PPRZ -
                            actuator_state_filt_vect[1]) * g1g2[3][1] + (MAX_PPRZ - actuator_state_filt_vect[2]) * g1g2[3][2] +
                            (MAX_PPRZ - actuator_state_filt_vect[3]) * g1g2[3][3]) * rotwing_state_hover_motors_running();

sw/airborne/modules/ctrl/eff_scheduling_rotwing_V2.c

@@ -209,7 +209,7 @@ void init_RW_Model(void)
 /*Update the attitude*/
 void  update_attitude(void)
 {
-  float_eulers_of_quat_zxy(&eulers_zxy_RW_EFF, stateGetNedToBodyQuat_f());
+  eulers_zxy_RW_EFF = *stateGetNedToBodyEulersZxy_f();
   RW.att.phi    = eulers_zxy_RW_EFF.phi;
   RW.att.theta  = eulers_zxy_RW_EFF.theta;
   RW.att.psi    = eulers_zxy_RW_EFF.psi;

sw/airborne/modules/sensors/aoa_t4.c

@@ -339,7 +339,7 @@ void aoa_t4_update(void)
     aoa_t4.angle = a_rad;
 
 #if AOA_T4_USE_COMPENSATION
-    float_eulers_of_quat_zxy(&eulers_t4, stateGetNedToBodyQuat_f());
+    eulers_t4 = *stateGetNedToBodyEulersZxy_f();
     // First order fn
     if (eulers_t4.theta >= 0)
     {

sw/airborne/state.h

@@ -1315,6 +1315,12 @@ static inline struct FloatEulers *stateGetNedToBodyEulers_f(void)
 {
   return orientationGetEulers_f(&state.ned_to_body_orientation);
 }
+
+/// Get vehicle body attitude ZXY euler angles (float).
+static inline struct FloatEulers *stateGetNedToBodyEulersZxy_f(void)
+{
+  return orientationGetEulersZxy_f(&state.ned_to_body_orientation);
+}
 /** @}*/