diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c b/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c index be860d51ff..3870c69706 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c +++ b/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 diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.c index a4a5a4ceb4..18c7c4b1a3 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.c +++ b/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; } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c index 4cd62b90a9..3c1c0d2ffa 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c +++ b/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; diff --git a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c index 6217fbfeac..95bff9abf9 100644 --- a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c +++ b/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(); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c index e069b53536..013d630921 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c +++ b/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(); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c index eeb5cceb9d..1f6df65ccb 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c +++ b/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); diff --git a/sw/airborne/math/pprz_orientation_conversion.c b/sw/airborne/math/pprz_orientation_conversion.c index e825130ff3..97aa855b3d 100644 --- a/sw/airborne/math/pprz_orientation_conversion.c +++ b/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); +} /** @}*/ /** @}*/ diff --git a/sw/airborne/math/pprz_orientation_conversion.h b/sw/airborne/math/pprz_orientation_conversion.h index 5c5d3fadbe..cdaa45b099 100644 --- a/sw/airborne/math/pprz_orientation_conversion.h +++ b/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 diff --git a/sw/airborne/modules/ctrl/eff_scheduling_cyfoam.c b/sw/airborne/modules/ctrl/eff_scheduling_cyfoam.c index 6b0566e555..7eb3751e27 100644 --- a/sw/airborne/modules/ctrl/eff_scheduling_cyfoam.c +++ b/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.); diff --git a/sw/airborne/modules/ctrl/eff_scheduling_falcon.c b/sw/airborne/modules/ctrl/eff_scheduling_falcon.c index d3fa515ae6..011399e4c2 100644 --- a/sw/airborne/modules/ctrl/eff_scheduling_falcon.c +++ b/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) { diff --git a/sw/airborne/modules/ctrl/eff_scheduling_nederdrone.c b/sw/airborne/modules/ctrl/eff_scheduling_nederdrone.c index 6421f4f4ab..6f18bc7716 100644 --- a/sw/airborne/modules/ctrl/eff_scheduling_nederdrone.c +++ b/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) { diff --git a/sw/airborne/modules/ctrl/eff_scheduling_rotwing.c b/sw/airborne/modules/ctrl/eff_scheduling_rotwing.c index 73245d024f..2f240f48a6 100644 --- a/sw/airborne/modules/ctrl/eff_scheduling_rotwing.c +++ b/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(); diff --git a/sw/airborne/modules/ctrl/eff_scheduling_rotwing_V2.c b/sw/airborne/modules/ctrl/eff_scheduling_rotwing_V2.c index 918b76bfd1..6dfc5a8c64 100644 --- a/sw/airborne/modules/ctrl/eff_scheduling_rotwing_V2.c +++ b/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; diff --git a/sw/airborne/modules/sensors/aoa_t4.c b/sw/airborne/modules/sensors/aoa_t4.c index d5339c29d4..3b96f0625c 100644 --- a/sw/airborne/modules/sensors/aoa_t4.c +++ b/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) { diff --git a/sw/airborne/state.h b/sw/airborne/state.h index 19ca094150..8dfd9bfb2f 100644 --- a/sw/airborne/state.h +++ b/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); +} /** @}*/