Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
stabilization_attitude_ref_euler_float.c
Go to the documentation of this file.
2 
3 
8 
10 
15 
16 }
17 
18 
19 /*
20  * Reference
21  */
22 #define DT_UPDATE (1./512.)
23 
24 #define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_FLOAT_REF_MAX_PDOT
25 #define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_FLOAT_REF_MAX_QDOT
26 #define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_FLOAT_REF_MAX_RDOT
27 
28 #define REF_RATE_MAX_P STABILIZATION_ATTITUDE_FLOAT_REF_MAX_P
29 #define REF_RATE_MAX_Q STABILIZATION_ATTITUDE_FLOAT_REF_MAX_Q
30 #define REF_RATE_MAX_R STABILIZATION_ATTITUDE_FLOAT_REF_MAX_R
31 
32 #define OMEGA_P STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_P
33 #define OMEGA_Q STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_Q
34 #define OMEGA_R STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_R
35 
36 #define ZETA_P STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_P
37 #define ZETA_Q STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_Q
38 #define ZETA_R STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_R
39 
40 
41 #define USE_REF 1
42 
44 
45 #if USE_REF
46 
47  /* dumb integrate reference attitude */
48  struct FloatRates delta_rate;
50  struct FloatEulers delta_angle;
51  EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r);
52  EULERS_ADD(stab_att_ref_euler, delta_angle );
54 
55  /* integrate reference rotational speeds */
56  struct FloatRates delta_accel;
58  RATES_ADD(stab_att_ref_rate, delta_accel);
59 
60  /* compute reference attitude error */
61  struct FloatEulers ref_err;
63  /* wrap it in the shortest direction */
64  FLOAT_ANGLE_NORMALIZE(ref_err.psi);
65 
66  /* compute reference angular accelerations */
70 
71  /* saturate acceleration */
72  const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
73  const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; \
74  RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
75 
76  /* saturate speed and trim accel accordingly */
78 
79 #else /* !USE_REF */
80  EULERS_COPY(stab_att_ref_euler, stabilization_att_sp);
83 #endif /* USE_REF */
84 
85 }
#define FLOAT_RATES_ZERO(_r)
#define EULERS_ASSIGN(_e, _phi, _theta, _psi)
Definition: pprz_algebra.h:242
#define EULERS_ADD(_a, _b)
Definition: pprz_algebra.h:249
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:263
angular rates
#define SATURATE_SPEED_TRIM_ACCEL()
float psi
in radians
float theta
in radians
euler angles
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:345
float p
in rad/s^2
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
struct FloatEulers stab_att_ref_euler
with REF_ANGLE_FRAC
float phi
in radians
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:310
#define FLOAT_ANGLE_NORMALIZE(_a)
void stabilization_attitude_ref_init(void)
void stabilization_attitude_ref_update()
angular rates
float r
in rad/s^2
#define FLOAT_EULERS_ZERO(_e)
#define RATES_BOUND_BOX(_v, _v_min, _v_max)
Definition: pprz_algebra.h:373
General stabilization interface for rotorcrafts.
float q
in rad/s^2
struct FloatRates stab_att_ref_rate
with REF_RATE_FRAC
#define EULERS_COPY(_a, _b)
Definition: pprz_algebra.h:236
struct FloatRates stab_att_ref_accel
with REF_ACCEL_FRAC