Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 29 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); extern void stabilization_attitude_ref_update(void);
#define stabilization_attitude_SetKiPhi(_val) { \ #define stabilization_attitude_SetKiPhi(_val) { \
stabilization_gains.i.x = _val; \ stabilization_gains.i.x = _val; \
stabilization_att_sum_err.phi = 0; \ stabilization_att_sum_err.phi = 0; \
} }
#endif /* STABILIZATION_ATTITUDE_H */ #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" #include "generated/airframe.h"
struct FloatAttitudeGains { struct FloatAttitudeGains {
struct FloatVect3 p; struct FloatVect3 p;
struct FloatVect3 d; struct FloatVect3 d;
struct FloatVect3 dd; struct FloatVect3 dd;
struct FloatVect3 rates_d; struct FloatVect3 rates_d;
struct FloatVect3 i; struct FloatVect3 i;
struct FloatVect3 surface_p; struct FloatVect3 surface_p;
struct FloatVect3 surface_d; struct FloatVect3 surface_d;
struct FloatVect3 surface_dd; struct FloatVect3 surface_dd;
struct FloatVect3 surface_i; struct FloatVect3 surface_i;
}; };
extern struct FloatAttitudeGains stabilization_gains[]; 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" #include "generated/airframe.h"
struct Int32AttitudeGains { struct Int32AttitudeGains {
struct Int32Vect3 p; struct Int32Vect3 p;
struct Int32Vect3 d; struct Int32Vect3 d;
struct Int32Vect3 dd; struct Int32Vect3 dd;
struct Int32Vect3 i; struct Int32Vect3 i;
}; };
extern struct Int32AttitudeGains stabilization_gains; 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) void stabilization_attitude_gain_schedule(uint8_t idx)
{ {
if (gain_idx >= STABILIZATION_ATTITUDE_FLOAT_GAIN_NB) { if (gain_idx >= STABILIZATION_ATTITUDE_FLOAT_GAIN_NB) {
// This could be bad -- Just say no. // This could be bad -- Just say no.
return; return;
} }
gain_idx = idx; gain_idx = idx;
stabilization_attitude_ref_schedule(idx); stabilization_attitude_ref_schedule(idx);
} }
void stabilization_attitude_enter(void) { 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 #ifndef STABILIZATION_ATTITUDE_REF_H
#define 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) { \ if (stab_att_ref_rate.p >= REF_RATE_MAX_P) { \
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) \ if (stab_att_ref_accel.p > 0) \
stab_att_ref_accel.p = 0; \ stab_att_ref_accel.p = 0; \
} \ } \
else if (stab_att_ref_rate.p <= -REF_RATE_MAX_P) { \ else if (stab_att_ref_rate.p <= -REF_RATE_MAX_P) { \
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) \ if (stab_att_ref_accel.p < 0) \
stab_att_ref_accel.p = 0; \ stab_att_ref_accel.p = 0; \
} \ } \
if (stab_att_ref_rate.q >= REF_RATE_MAX_Q) { \ if (stab_att_ref_rate.q >= REF_RATE_MAX_Q) { \
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) \ if (stab_att_ref_accel.q > 0) \
stab_att_ref_accel.q = 0; \ stab_att_ref_accel.q = 0; \
} \ } \
else if (stab_att_ref_rate.q <= -REF_RATE_MAX_Q) { \ else if (stab_att_ref_rate.q <= -REF_RATE_MAX_Q) { \
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) \ if (stab_att_ref_accel.q < 0) \
stab_att_ref_accel.q = 0; \ stab_att_ref_accel.q = 0; \
} \ } \
if (stab_att_ref_rate.r >= REF_RATE_MAX_R) { \ if (stab_att_ref_rate.r >= REF_RATE_MAX_R) { \
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) \ if (stab_att_ref_accel.r > 0) \
stab_att_ref_accel.r = 0; \ stab_att_ref_accel.r = 0; \
} \ } \
else if (stab_att_ref_rate.r <= -REF_RATE_MAX_R) { \ else if (stab_att_ref_rate.r <= -REF_RATE_MAX_R) { \
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) \ if (stab_att_ref_accel.r < 0) \
stab_att_ref_accel.r = 0; \ stab_att_ref_accel.r = 0; \
} \ } \
} }
#endif /* STABILIZATION_ATTITUDE_REF_H */ #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 #if USE_REF
/* dumb integrate reference attitude */ /* dumb integrate reference attitude */
struct FloatRates delta_rate; struct FloatRates delta_rate;
RATES_SMUL(delta_rate, stab_att_ref_rate, DT_UPDATE); RATES_SMUL(delta_rate, stab_att_ref_rate, DT_UPDATE);
struct FloatEulers delta_angle; struct FloatEulers delta_angle;
EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r); EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r);
EULERS_ADD(stab_att_ref_euler, delta_angle ); EULERS_ADD(stab_att_ref_euler, delta_angle );
FLOAT_ANGLE_NORMALIZE(stab_att_ref_euler.psi); FLOAT_ANGLE_NORMALIZE(stab_att_ref_euler.psi);
/* integrate reference rotational speeds */ /* integrate reference rotational speeds */
struct FloatRates delta_accel; struct FloatRates delta_accel;
RATES_SMUL(delta_accel, stab_att_ref_accel, DT_UPDATE); RATES_SMUL(delta_accel, stab_att_ref_accel, DT_UPDATE);
RATES_ADD(stab_att_ref_rate, delta_accel); RATES_ADD(stab_att_ref_rate, delta_accel);
/* compute reference attitude error */ /* compute reference attitude error */
struct FloatEulers ref_err; struct FloatEulers ref_err;
EULERS_DIFF(ref_err, stab_att_ref_euler, stab_att_sp_euler); EULERS_DIFF(ref_err, stab_att_ref_euler, stab_att_sp_euler);
/* wrap it in the shortest direction */ /* wrap it in the shortest direction */
FLOAT_ANGLE_NORMALIZE(ref_err.psi); FLOAT_ANGLE_NORMALIZE(ref_err.psi);
/* compute reference angular accelerations */ /* 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.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.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; stab_att_ref_accel.r = -2.*ZETA_R*OMEGA_P*stab_att_ref_rate.r - OMEGA_R*OMEGA_R*ref_err.psi;
/* saturate acceleration */ /* saturate acceleration */
const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; 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 }; \ 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); RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
/* saturate speed and trim accel accordingly */ /* saturate speed and trim accel accordingly */
SATURATE_SPEED_TRIM_ACCEL(); SATURATE_SPEED_TRIM_ACCEL();
#else /* !USE_REF */ #else /* !USE_REF */
EULERS_COPY(stab_att_ref_euler, stabilization_att_sp); EULERS_COPY(stab_att_ref_euler, stabilization_att_sp);
FLOAT_RATES_ZERO(stab_att_ref_rate); FLOAT_RATES_ZERO(stab_att_ref_rate);
FLOAT_RATES_ZERO(stab_att_ref_accel); FLOAT_RATES_ZERO(stab_att_ref_accel);
#endif /* USE_REF */ #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 || \
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) { \ #define STABILIZATION_ATTITUDE_FLOAT_READ_RC(_sp, _inflight) { \
\ \
_sp.phi = \ _sp.phi = \
(-radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ); \ (-radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ); \
_sp.theta = \ _sp.theta = \
( radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ); \ ( radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ); \
if (_inflight) { \ if (_inflight) { \
if (YAW_DEADBAND_EXCEEDED()) { \ if (YAW_DEADBAND_EXCEEDED()) { \
_sp.psi += \ _sp.psi += \
(-radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); \ (-radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); \
FLOAT_ANGLE_NORMALIZE(_sp.psi); \ FLOAT_ANGLE_NORMALIZE(_sp.psi); \
} \ } \
} \ } \
else { /* if not flying, use current yaw as setpoint */ \ else { /* if not flying, use current yaw as setpoint */ \
_sp.psi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi); \ _sp.psi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi); \
} \ } \
} }
#define STABILIZATION_ATTITUDE_FLOAT_ADD_SP(_add_sp) { \ #define STABILIZATION_ATTITUDE_FLOAT_ADD_SP(_add_sp) { \
struct FloatEulers add_sp_float; \ struct FloatEulers add_sp_float; \
EULERS_FLOAT_OF_BFP(add_sp_float, (_add_sp)); \ EULERS_FLOAT_OF_BFP(add_sp_float, (_add_sp)); \
EULERS_ADD(stabilization_att_sp,add_sp_float); \ EULERS_ADD(stabilization_att_sp,add_sp_float); \
FLOAT_ANGLE_NORMALIZE(stabilization_att_sp.psi); \ 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 #if USE_ATTITUDE_REF
/* dumb integrate reference attitude */ /* dumb integrate reference attitude */
const struct Int32Eulers d_angle = { const struct Int32Eulers d_angle = {
stab_att_ref_rate.p >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC), 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.q >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC),
stab_att_ref_rate.r >> ( 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 ); EULERS_ADD(stab_att_ref_euler, d_angle );
ANGLE_REF_NORMALIZE(stab_att_ref_euler.psi); ANGLE_REF_NORMALIZE(stab_att_ref_euler.psi);
/* integrate reference rotational speeds */ /* integrate reference rotational speeds */
const struct Int32Rates d_rate = { const struct Int32Rates d_rate = {
stab_att_ref_accel.p >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), 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.q >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC),
stab_att_ref_accel.r >> ( 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); RATES_ADD(stab_att_ref_rate, d_rate);
/* compute reference attitude error */ /* compute reference attitude error */
struct Int32Eulers ref_err; struct Int32Eulers ref_err;
EULERS_DIFF(ref_err, stab_att_ref_euler, stab_att_sp_euler); EULERS_DIFF(ref_err, stab_att_ref_euler, stab_att_sp_euler);
/* wrap it in the shortest direction */ /* wrap it in the shortest direction */
ANGLE_REF_NORMALIZE(ref_err.psi); ANGLE_REF_NORMALIZE(ref_err.psi);
/* compute reference angular accelerations */ /* compute reference angular accelerations */
const struct Int32Rates accel_rate = { const struct Int32Rates accel_rate = {
((int32_t)(-2.*ZETA_OMEGA_P) * (stab_att_ref_rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) ((int32_t)(-2.*ZETA_OMEGA_P) * (stab_att_ref_rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
>> (ZETA_OMEGA_P_RES), >> (ZETA_OMEGA_P_RES),
((int32_t)(-2.*ZETA_OMEGA_Q) * (stab_att_ref_rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) ((int32_t)(-2.*ZETA_OMEGA_Q) * (stab_att_ref_rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
>> (ZETA_OMEGA_Q_RES), >> (ZETA_OMEGA_Q_RES),
((int32_t)(-2.*ZETA_OMEGA_R) * (stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) ((int32_t)(-2.*ZETA_OMEGA_R) * (stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
>> (ZETA_OMEGA_R_RES) }; >> (ZETA_OMEGA_R_RES) };
const struct Int32Rates accel_angle = { 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_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_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) }; ((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 */ /* saturate acceleration */
const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; 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 }; 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); RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
/* saturate speed and trim accel accordingly */ /* saturate speed and trim accel accordingly */
SATURATE_SPEED_TRIM_ACCEL(); SATURATE_SPEED_TRIM_ACCEL();
#else /* !USE_ATTITUDE_REF */ #else /* !USE_ATTITUDE_REF */
EULERS_COPY(stab_att_ref_euler, stab_att_sp_euler); EULERS_COPY(stab_att_ref_euler, stab_att_sp_euler);
INT_RATES_ZERO(stab_att_ref_rate); INT_RATES_ZERO(stab_att_ref_rate);
INT_RATES_ZERO(stab_att_ref_accel); INT_RATES_ZERO(stab_att_ref_accel);
#endif /* USE_ATTITUDE_REF */ #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 || \
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) { \ #define STABILIZATION_ATTITUDE_READ_RC(_sp, _inflight) { \
\ \
_sp.phi = \ _sp.phi = \
((int32_t)-radio_control.values[RADIO_ROLL] * (int32_t)SP_MAX_PHI / MAX_PPRZ) \ ((int32_t)-radio_control.values[RADIO_ROLL] * (int32_t)SP_MAX_PHI / MAX_PPRZ) \
<< (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \
_sp.theta = \ _sp.theta = \
((int32_t) radio_control.values[RADIO_PITCH] * (int32_t)SP_MAX_THETA / MAX_PPRZ) \ ((int32_t) radio_control.values[RADIO_PITCH] * (int32_t)SP_MAX_THETA / MAX_PPRZ) \
<< (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \
if (_inflight) { \ if (_inflight) { \
if (YAW_DEADBAND_EXCEEDED()) { \ if (YAW_DEADBAND_EXCEEDED()) { \
_sp.psi += \ _sp.psi += \
((int32_t)-radio_control.values[RADIO_YAW] * (int32_t)SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ) \ ((int32_t)-radio_control.values[RADIO_YAW] * (int32_t)SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ) \
<< (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \
ANGLE_REF_NORMALIZE(_sp.psi); \ ANGLE_REF_NORMALIZE(_sp.psi); \
} \ } \
} \ } \
else { /* if not flying, use current yaw as setpoint */ \ else { /* if not flying, use current yaw as setpoint */ \
_sp.psi = (ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); \ _sp.psi = (ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); \
} \ } \
} }
#define STABILIZATION_ATTITUDE_ADD_SP(_add_sp) { \ #define STABILIZATION_ATTITUDE_ADD_SP(_add_sp) { \
EULERS_ADD(stab_att_sp_euler,_add_sp); \ EULERS_ADD(stab_att_sp_euler,_add_sp); \
ANGLE_REF_NORMALIZE(stab_att_sp_euler.psi); \ 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); \ _sp.psi = ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \
stab_att_ref_euler.psi = _sp.psi; \ stab_att_ref_euler.psi = _sp.psi; \
stab_att_ref_rate.r = 0; \ 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 const float zeta_r[] = STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_R;
static void reset_psi_ref_from_body(void) { static void reset_psi_ref_from_body(void) {
stab_att_ref_euler.psi = ahrs_float.ltp_to_body_euler.psi; stab_att_ref_euler.psi = ahrs_float.ltp_to_body_euler.psi;
stab_att_ref_rate.r = 0; stab_att_ref_rate.r = 0;
stab_att_ref_accel.r = 0; stab_att_ref_accel.r = 0;
} }
static void update_ref_quat_from_eulers(void) { static void update_ref_quat_from_eulers(void) {
struct FloatRMat ref_rmat; struct FloatRMat ref_rmat;
#ifdef STICKS_RMAT312 #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 #else
FLOAT_RMAT_OF_EULERS_321(ref_rmat, stab_att_ref_euler); FLOAT_RMAT_OF_EULERS_321(ref_rmat, stab_att_ref_euler);
#endif #endif
FLOAT_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat); FLOAT_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat);
FLOAT_QUAT_WRAP_SHORTEST(stab_att_ref_quat); FLOAT_QUAT_WRAP_SHORTEST(stab_att_ref_quat);
} }
void stabilization_attitude_ref_init(void) { 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) { static void reset_psi_ref_from_body(void) {
stab_att_ref_euler.psi = ahrs.ltp_to_body_euler.psi; stab_att_ref_euler.psi = ahrs.ltp_to_body_euler.psi;
stab_att_ref_rate.r = 0; stab_att_ref_rate.r = 0;
stab_att_ref_accel.r = 0; stab_att_ref_accel.r = 0;
} }
static void update_ref_quat_from_eulers(void) { static void update_ref_quat_from_eulers(void) {
struct Int32RMat ref_rmat; struct Int32RMat ref_rmat;
#ifdef STICKS_RMAT312 #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 #else
INT32_RMAT_OF_EULERS_321(ref_rmat, stab_att_ref_euler); INT32_RMAT_OF_EULERS_321(ref_rmat, stab_att_ref_euler);
#endif #endif
INT32_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat); INT32_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat);
INT32_QUAT_WRAP_SHORTEST(stab_att_ref_quat); INT32_QUAT_WRAP_SHORTEST(stab_att_ref_quat);
} }
void stabilization_attitude_ref_init(void) { void stabilization_attitude_ref_init(void) {