diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h index 6aab4c07f7..f4b1259353 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h @@ -39,8 +39,8 @@ 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; \ + stabilization_gains.i.x = _val; \ + stabilization_att_sum_err.phi = 0; \ } #endif /* STABILIZATION_ATTITUDE_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h index 0c78592ae3..22ad878da5 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h @@ -29,15 +29,15 @@ #include "generated/airframe.h" struct FloatAttitudeGains { - struct FloatVect3 p; - struct FloatVect3 d; - struct FloatVect3 dd; - struct FloatVect3 rates_d; - struct FloatVect3 i; - struct FloatVect3 surface_p; - struct FloatVect3 surface_d; - struct FloatVect3 surface_dd; - struct FloatVect3 surface_i; + struct FloatVect3 p; + struct FloatVect3 d; + struct FloatVect3 dd; + struct FloatVect3 rates_d; + struct FloatVect3 i; + struct FloatVect3 surface_p; + struct FloatVect3 surface_d; + struct FloatVect3 surface_dd; + struct FloatVect3 surface_i; }; extern struct FloatAttitudeGains stabilization_gains[]; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h index 3b86387356..02b761183a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h @@ -29,10 +29,10 @@ #include "generated/airframe.h" struct Int32AttitudeGains { - struct Int32Vect3 p; - struct Int32Vect3 d; - struct Int32Vect3 dd; - struct Int32Vect3 i; + struct Int32Vect3 p; + struct Int32Vect3 d; + struct Int32Vect3 dd; + struct Int32Vect3 i; }; extern struct Int32AttitudeGains stabilization_gains; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index 36f6dd9df7..4ca17813fc 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -103,12 +103,12 @@ void stabilization_attitude_init(void) { void stabilization_attitude_gain_schedule(uint8_t idx) { - if (gain_idx >= STABILIZATION_ATTITUDE_FLOAT_GAIN_NB) { - // This could be bad -- Just say no. - return; - } - gain_idx = idx; - stabilization_attitude_ref_schedule(idx); + if (gain_idx >= STABILIZATION_ATTITUDE_FLOAT_GAIN_NB) { + // This could be bad -- Just say no. + return; + } + gain_idx = idx; + stabilization_attitude_ref_schedule(idx); } void stabilization_attitude_enter(void) { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h index fbf43e2a20..0a7fb78b7e 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h @@ -1,37 +1,37 @@ #ifndef STABILIZATION_ATTITUDE_REF_H #define STABILIZATION_ATTITUDE_REF_H -#define SATURATE_SPEED_TRIM_ACCEL() { \ +#define SATURATE_SPEED_TRIM_ACCEL() { \ if (stab_att_ref_rate.p >= REF_RATE_MAX_P) { \ - stab_att_ref_rate.p = REF_RATE_MAX_P; \ - if (stab_att_ref_accel.p > 0) \ - stab_att_ref_accel.p = 0; \ - } \ + stab_att_ref_rate.p = REF_RATE_MAX_P; \ + if (stab_att_ref_accel.p > 0) \ + stab_att_ref_accel.p = 0; \ + } \ else if (stab_att_ref_rate.p <= -REF_RATE_MAX_P) { \ - stab_att_ref_rate.p = -REF_RATE_MAX_P; \ - if (stab_att_ref_accel.p < 0) \ - stab_att_ref_accel.p = 0; \ - } \ + stab_att_ref_rate.p = -REF_RATE_MAX_P; \ + if (stab_att_ref_accel.p < 0) \ + stab_att_ref_accel.p = 0; \ + } \ if (stab_att_ref_rate.q >= REF_RATE_MAX_Q) { \ - stab_att_ref_rate.q = REF_RATE_MAX_Q; \ - if (stab_att_ref_accel.q > 0) \ - stab_att_ref_accel.q = 0; \ - } \ + stab_att_ref_rate.q = REF_RATE_MAX_Q; \ + if (stab_att_ref_accel.q > 0) \ + stab_att_ref_accel.q = 0; \ + } \ else if (stab_att_ref_rate.q <= -REF_RATE_MAX_Q) { \ - stab_att_ref_rate.q = -REF_RATE_MAX_Q; \ - if (stab_att_ref_accel.q < 0) \ - stab_att_ref_accel.q = 0; \ - } \ + stab_att_ref_rate.q = -REF_RATE_MAX_Q; \ + if (stab_att_ref_accel.q < 0) \ + stab_att_ref_accel.q = 0; \ + } \ if (stab_att_ref_rate.r >= REF_RATE_MAX_R) { \ - stab_att_ref_rate.r = REF_RATE_MAX_R; \ - if (stab_att_ref_accel.r > 0) \ - stab_att_ref_accel.r = 0; \ - } \ + stab_att_ref_rate.r = REF_RATE_MAX_R; \ + if (stab_att_ref_accel.r > 0) \ + stab_att_ref_accel.r = 0; \ + } \ else if (stab_att_ref_rate.r <= -REF_RATE_MAX_R) { \ - stab_att_ref_rate.r = -REF_RATE_MAX_R; \ - if (stab_att_ref_accel.r < 0) \ - stab_att_ref_accel.r = 0; \ - } \ + stab_att_ref_rate.r = -REF_RATE_MAX_R; \ + if (stab_att_ref_accel.r < 0) \ + stab_att_ref_accel.r = 0; \ + } \ } #endif /* STABILIZATION_ATTITUDE_REF_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c index 3e52134344..9030fdacc8 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c @@ -44,42 +44,42 @@ void stabilization_attitude_ref_update() { #if USE_REF - /* dumb integrate reference attitude */ - struct FloatRates delta_rate; - RATES_SMUL(delta_rate, stab_att_ref_rate, DT_UPDATE); - struct FloatEulers delta_angle; - EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r); - EULERS_ADD(stab_att_ref_euler, delta_angle ); - FLOAT_ANGLE_NORMALIZE(stab_att_ref_euler.psi); + /* dumb integrate reference attitude */ + struct FloatRates delta_rate; + RATES_SMUL(delta_rate, stab_att_ref_rate, DT_UPDATE); + struct FloatEulers delta_angle; + EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r); + EULERS_ADD(stab_att_ref_euler, delta_angle ); + FLOAT_ANGLE_NORMALIZE(stab_att_ref_euler.psi); - /* integrate reference rotational speeds */ - struct FloatRates delta_accel; - RATES_SMUL(delta_accel, stab_att_ref_accel, DT_UPDATE); - RATES_ADD(stab_att_ref_rate, delta_accel); + /* integrate reference rotational speeds */ + struct FloatRates delta_accel; + RATES_SMUL(delta_accel, stab_att_ref_accel, DT_UPDATE); + RATES_ADD(stab_att_ref_rate, delta_accel); - /* compute reference attitude error */ - struct FloatEulers ref_err; - EULERS_DIFF(ref_err, stab_att_ref_euler, stab_att_sp_euler); - /* wrap it in the shortest direction */ - FLOAT_ANGLE_NORMALIZE(ref_err.psi); + /* compute reference attitude error */ + struct FloatEulers ref_err; + EULERS_DIFF(ref_err, stab_att_ref_euler, stab_att_sp_euler); + /* wrap it in the shortest direction */ + FLOAT_ANGLE_NORMALIZE(ref_err.psi); - /* compute reference angular accelerations */ - stab_att_ref_accel.p = -2.*ZETA_P*OMEGA_P*stab_att_ref_rate.p - OMEGA_P*OMEGA_P*ref_err.phi; - stab_att_ref_accel.q = -2.*ZETA_Q*OMEGA_P*stab_att_ref_rate.q - OMEGA_Q*OMEGA_Q*ref_err.theta; - stab_att_ref_accel.r = -2.*ZETA_R*OMEGA_P*stab_att_ref_rate.r - OMEGA_R*OMEGA_R*ref_err.psi; + /* compute reference angular accelerations */ + stab_att_ref_accel.p = -2.*ZETA_P*OMEGA_P*stab_att_ref_rate.p - OMEGA_P*OMEGA_P*ref_err.phi; + stab_att_ref_accel.q = -2.*ZETA_Q*OMEGA_P*stab_att_ref_rate.q - OMEGA_Q*OMEGA_Q*ref_err.theta; + stab_att_ref_accel.r = -2.*ZETA_R*OMEGA_P*stab_att_ref_rate.r - OMEGA_R*OMEGA_R*ref_err.psi; - /* saturate acceleration */ - const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; - const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; \ - RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); + /* saturate acceleration */ + const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; + const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; \ + RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); - /* saturate speed and trim accel accordingly */ - SATURATE_SPEED_TRIM_ACCEL(); + /* saturate speed and trim accel accordingly */ + SATURATE_SPEED_TRIM_ACCEL(); #else /* !USE_REF */ - EULERS_COPY(stab_att_ref_euler, stabilization_att_sp); - FLOAT_RATES_ZERO(stab_att_ref_rate); - FLOAT_RATES_ZERO(stab_att_ref_accel); + EULERS_COPY(stab_att_ref_euler, stabilization_att_sp); + FLOAT_RATES_ZERO(stab_att_ref_rate); + FLOAT_RATES_ZERO(stab_att_ref_accel); #endif /* USE_REF */ } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h index 3808c9aaea..27f952a42d 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h @@ -47,26 +47,26 @@ (radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_FLOAT_DEADBAND_R || \ radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_FLOAT_DEADBAND_R) -#define STABILIZATION_ATTITUDE_FLOAT_READ_RC(_sp, _inflight) { \ - \ - _sp.phi = \ - (-radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ); \ - _sp.theta = \ - ( radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ); \ - if (_inflight) { \ - if (YAW_DEADBAND_EXCEEDED()) { \ - _sp.psi += \ - (-radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); \ - FLOAT_ANGLE_NORMALIZE(_sp.psi); \ - } \ - } \ - else { /* if not flying, use current yaw as setpoint */ \ - _sp.psi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi); \ - } \ +#define STABILIZATION_ATTITUDE_FLOAT_READ_RC(_sp, _inflight) { \ + \ + _sp.phi = \ + (-radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ); \ + _sp.theta = \ + ( radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ); \ + if (_inflight) { \ + if (YAW_DEADBAND_EXCEEDED()) { \ + _sp.psi += \ + (-radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); \ + FLOAT_ANGLE_NORMALIZE(_sp.psi); \ + } \ + } \ + else { /* if not flying, use current yaw as setpoint */ \ + _sp.psi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi); \ + } \ } -#define STABILIZATION_ATTITUDE_FLOAT_ADD_SP(_add_sp) { \ - struct FloatEulers add_sp_float; \ +#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); \ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c index 879c6cbc13..075fc7f67d 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c @@ -77,55 +77,55 @@ void stabilization_attitude_ref_update() { #if USE_ATTITUDE_REF - /* dumb integrate reference attitude */ - const struct Int32Eulers d_angle = { - stab_att_ref_rate.p >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC), - stab_att_ref_rate.q >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC), - stab_att_ref_rate.r >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC)}; - EULERS_ADD(stab_att_ref_euler, d_angle ); - ANGLE_REF_NORMALIZE(stab_att_ref_euler.psi); + /* dumb integrate reference attitude */ + const struct Int32Eulers d_angle = { + stab_att_ref_rate.p >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC), + stab_att_ref_rate.q >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC), + stab_att_ref_rate.r >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC)}; + EULERS_ADD(stab_att_ref_euler, d_angle ); + ANGLE_REF_NORMALIZE(stab_att_ref_euler.psi); - /* integrate reference rotational speeds */ - const struct Int32Rates d_rate = { - stab_att_ref_accel.p >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), - stab_att_ref_accel.q >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), - stab_att_ref_accel.r >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC)}; - RATES_ADD(stab_att_ref_rate, d_rate); + /* integrate reference rotational speeds */ + const struct Int32Rates d_rate = { + stab_att_ref_accel.p >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), + stab_att_ref_accel.q >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), + stab_att_ref_accel.r >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC)}; + RATES_ADD(stab_att_ref_rate, d_rate); - /* compute reference attitude error */ - struct Int32Eulers ref_err; - EULERS_DIFF(ref_err, stab_att_ref_euler, stab_att_sp_euler); - /* wrap it in the shortest direction */ - ANGLE_REF_NORMALIZE(ref_err.psi); + /* compute reference attitude error */ + struct Int32Eulers ref_err; + EULERS_DIFF(ref_err, stab_att_ref_euler, stab_att_sp_euler); + /* wrap it in the shortest direction */ + ANGLE_REF_NORMALIZE(ref_err.psi); - /* compute reference angular accelerations */ - const struct Int32Rates accel_rate = { - ((int32_t)(-2.*ZETA_OMEGA_P) * (stab_att_ref_rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) - >> (ZETA_OMEGA_P_RES), - ((int32_t)(-2.*ZETA_OMEGA_Q) * (stab_att_ref_rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) - >> (ZETA_OMEGA_Q_RES), - ((int32_t)(-2.*ZETA_OMEGA_R) * (stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) - >> (ZETA_OMEGA_R_RES) }; + /* compute reference angular accelerations */ + const struct Int32Rates accel_rate = { + ((int32_t)(-2.*ZETA_OMEGA_P) * (stab_att_ref_rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) + >> (ZETA_OMEGA_P_RES), + ((int32_t)(-2.*ZETA_OMEGA_Q) * (stab_att_ref_rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) + >> (ZETA_OMEGA_Q_RES), + ((int32_t)(-2.*ZETA_OMEGA_R) * (stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) + >> (ZETA_OMEGA_R_RES) }; - const struct Int32Rates accel_angle = { - ((int32_t)(-OMEGA_2_P)* (ref_err.phi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES), - ((int32_t)(-OMEGA_2_Q)* (ref_err.theta >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES), - ((int32_t)(-OMEGA_2_R)* (ref_err.psi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) }; + const struct Int32Rates accel_angle = { + ((int32_t)(-OMEGA_2_P)* (ref_err.phi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES), + ((int32_t)(-OMEGA_2_Q)* (ref_err.theta >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES), + ((int32_t)(-OMEGA_2_R)* (ref_err.psi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) }; - RATES_SUM(stab_att_ref_accel, accel_rate, accel_angle); + RATES_SUM(stab_att_ref_accel, accel_rate, accel_angle); - /* saturate acceleration */ - const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; - const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; - RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); + /* saturate acceleration */ + const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; + const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; + RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); - /* saturate speed and trim accel accordingly */ - SATURATE_SPEED_TRIM_ACCEL(); + /* saturate speed and trim accel accordingly */ + SATURATE_SPEED_TRIM_ACCEL(); #else /* !USE_ATTITUDE_REF */ - EULERS_COPY(stab_att_ref_euler, stab_att_sp_euler); - INT_RATES_ZERO(stab_att_ref_rate); - INT_RATES_ZERO(stab_att_ref_accel); + EULERS_COPY(stab_att_ref_euler, stab_att_sp_euler); + INT_RATES_ZERO(stab_att_ref_rate); + INT_RATES_ZERO(stab_att_ref_accel); #endif /* USE_ATTITUDE_REF */ } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h index ba2977be87..b511ea7162 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h @@ -58,36 +58,36 @@ (radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_DEADBAND_R || \ radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R) -#define STABILIZATION_ATTITUDE_READ_RC(_sp, _inflight) { \ - \ - _sp.phi = \ +#define STABILIZATION_ATTITUDE_READ_RC(_sp, _inflight) { \ + \ + _sp.phi = \ ((int32_t)-radio_control.values[RADIO_ROLL] * (int32_t)SP_MAX_PHI / MAX_PPRZ) \ - << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ - _sp.theta = \ + << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ + _sp.theta = \ ((int32_t) radio_control.values[RADIO_PITCH] * (int32_t)SP_MAX_THETA / MAX_PPRZ) \ - << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ - if (_inflight) { \ - if (YAW_DEADBAND_EXCEEDED()) { \ - _sp.psi += \ - ((int32_t)-radio_control.values[RADIO_YAW] * (int32_t)SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ) \ - << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ - ANGLE_REF_NORMALIZE(_sp.psi); \ - } \ - } \ - else { /* if not flying, use current yaw as setpoint */ \ - _sp.psi = (ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); \ - } \ + << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ + if (_inflight) { \ + if (YAW_DEADBAND_EXCEEDED()) { \ + _sp.psi += \ + ((int32_t)-radio_control.values[RADIO_YAW] * (int32_t)SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ) \ + << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ + ANGLE_REF_NORMALIZE(_sp.psi); \ + } \ + } \ + else { /* if not flying, use current yaw as setpoint */ \ + _sp.psi = (ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); \ + } \ } #define STABILIZATION_ATTITUDE_ADD_SP(_add_sp) { \ - EULERS_ADD(stab_att_sp_euler,_add_sp); \ - ANGLE_REF_NORMALIZE(stab_att_sp_euler.psi); \ -} + EULERS_ADD(stab_att_sp_euler,_add_sp); \ + ANGLE_REF_NORMALIZE(stab_att_sp_euler.psi); \ + } -#define STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) { \ +#define STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) { \ _sp.psi = ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ - stab_att_ref_euler.psi = _sp.psi; \ - stab_att_ref_rate.r = 0; \ + stab_att_ref_euler.psi = _sp.psi; \ + stab_att_ref_rate.r = 0; \ } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c index c6478d5b40..cb28d79409 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c @@ -56,21 +56,21 @@ static const float omega_r[] = STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_R; static const float zeta_r[] = STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_R; static void reset_psi_ref_from_body(void) { - stab_att_ref_euler.psi = ahrs_float.ltp_to_body_euler.psi; - stab_att_ref_rate.r = 0; - stab_att_ref_accel.r = 0; + stab_att_ref_euler.psi = ahrs_float.ltp_to_body_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; + struct FloatRMat ref_rmat; #ifdef STICKS_RMAT312 - FLOAT_RMAT_OF_EULERS_312(ref_rmat, stab_att_ref_euler); + FLOAT_RMAT_OF_EULERS_312(ref_rmat, stab_att_ref_euler); #else - FLOAT_RMAT_OF_EULERS_321(ref_rmat, stab_att_ref_euler); + FLOAT_RMAT_OF_EULERS_321(ref_rmat, stab_att_ref_euler); #endif - FLOAT_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat); - FLOAT_QUAT_WRAP_SHORTEST(stab_att_ref_quat); + FLOAT_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat); + FLOAT_QUAT_WRAP_SHORTEST(stab_att_ref_quat); } void stabilization_attitude_ref_init(void) { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c index bb83dd82c4..17aaa88d7c 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c @@ -85,21 +85,21 @@ static const float zeta_r[] = STABILIZATION_ATTITUDE_REF_ZETA_R; */ static void reset_psi_ref_from_body(void) { - stab_att_ref_euler.psi = ahrs.ltp_to_body_euler.psi; - stab_att_ref_rate.r = 0; - stab_att_ref_accel.r = 0; + stab_att_ref_euler.psi = ahrs.ltp_to_body_euler.psi; + stab_att_ref_rate.r = 0; + stab_att_ref_accel.r = 0; } static void update_ref_quat_from_eulers(void) { - struct Int32RMat ref_rmat; + struct Int32RMat ref_rmat; #ifdef STICKS_RMAT312 - INT32_RMAT_OF_EULERS_312(ref_rmat, stab_att_ref_euler); + INT32_RMAT_OF_EULERS_312(ref_rmat, stab_att_ref_euler); #else - INT32_RMAT_OF_EULERS_321(ref_rmat, stab_att_ref_euler); + INT32_RMAT_OF_EULERS_321(ref_rmat, stab_att_ref_euler); #endif - INT32_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat); - INT32_QUAT_WRAP_SHORTEST(stab_att_ref_quat); + INT32_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat); + INT32_QUAT_WRAP_SHORTEST(stab_att_ref_quat); } void stabilization_attitude_ref_init(void) {