Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

indentation only

Browse Source
This commit is contained in:
Felix Ruess
2012-02-27 15:26:33 +01:00
parent 81d88ed344
commit 3e8d174486
11 changed files with 172 additions and 172 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
+2 -2
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h
+9 -9
View File
@@ -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[];
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h
+4 -4
View File
@@ -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;
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+6 -6
View File
@@ -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) {
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h
+25 -25
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c
+29 -29
View File
@@ -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 */
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h
+18 -18
View File
@@ -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); \
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c
+40 -40
View File
@@ -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 */
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h
+23 -23
View File
@@ -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; \
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
+8 -8
View File
@@ -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) {
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
+8 -8
View File
@@ -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) {