Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
stabilization_attitude_ref_euler_float.c
Go to the documentation of this file.
2 #include "generated/airframe.h"
3 
8 
10 
15 
16 }
17 
18 
19 /*
20  * Reference
21  */
22 #define DT_UPDATE (1./PERIODIC_FREQUENCY)
23 
24 #define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_REF_MAX_PDOT
25 #define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_QDOT
26 #define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_REF_MAX_RDOT
27 
28 #define REF_RATE_MAX_P STABILIZATION_ATTITUDE_REF_MAX_P
29 #define REF_RATE_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_Q
30 #define REF_RATE_MAX_R STABILIZATION_ATTITUDE_REF_MAX_R
31 
32 #define OMEGA_P STABILIZATION_ATTITUDE_REF_OMEGA_P
33 #define OMEGA_Q STABILIZATION_ATTITUDE_REF_OMEGA_Q
34 #define OMEGA_R STABILIZATION_ATTITUDE_REF_OMEGA_R
35 
36 #define ZETA_P STABILIZATION_ATTITUDE_REF_ZETA_P
37 #define ZETA_Q STABILIZATION_ATTITUDE_REF_ZETA_Q
38 #define ZETA_R STABILIZATION_ATTITUDE_REF_ZETA_R
39 
40 
41 #define USE_REF 1
42 
43 static inline void reset_psi_ref_from_body(void) {
44  //sp has been set from body using stabilization_attitude_get_yaw_f, use that value
46  stab_att_ref_rate.r = 0;
48 }
49 
51 {
53 }
54 
56 
57 #if USE_REF
58 
59  /* dumb integrate reference attitude */
60  struct FloatRates delta_rate;
62  struct FloatEulers delta_angle;
63  EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r);
64  EULERS_ADD(stab_att_ref_euler, delta_angle );
66 
67  /* integrate reference rotational speeds */
68  struct FloatRates delta_accel;
70  RATES_ADD(stab_att_ref_rate, delta_accel);
71 
72  /* compute reference attitude error */
73  struct FloatEulers ref_err;
75  /* wrap it in the shortest direction */
76  FLOAT_ANGLE_NORMALIZE(ref_err.psi);
77 
78  /* compute reference angular accelerations */
82 
83  /* saturate acceleration */
84  const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
85  const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; \
86  RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
87 
88  /* saturate speed and trim accel accordingly */
90 
91 #else /* !USE_REF */
92  EULERS_COPY(stab_att_ref_euler, stabilization_att_sp);
95 #endif /* USE_REF */
96 
97 }
#define FLOAT_RATES_ZERO(_r)
#define EULERS_ASSIGN(_e, _phi, _theta, _psi)
Definition: pprz_algebra.h:240
#define EULERS_ADD(_a, _b)
Definition: pprz_algebra.h:247
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:261
General attitude stabilization interface for rotorcrafts.
angular rates
float psi
in radians
void stabilization_attitude_ref_enter()
float theta
in radians
euler angles
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:343
float p
in rad/s^2
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
static void reset_psi_ref_from_body(void)
struct FloatEulers stab_att_ref_euler
with REF_ANGLE_FRAC
float phi
in radians
#define SATURATE_SPEED_TRIM_ACCEL()
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:308
#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:371
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:234
struct FloatRates stab_att_ref_accel
with REF_ACCEL_FRAC