Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
stabilization_attitude_ref_euler_int.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
28 #include "generated/airframe.h"
29 
30 
31 #define F_UPDATE_RES 9
32 #define F_UPDATE (1<<F_UPDATE_RES)
33 
34 #define REF_ACCEL_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_PDOT, REF_ACCEL_FRAC)
35 #define REF_ACCEL_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_QDOT, REF_ACCEL_FRAC)
36 #define REF_ACCEL_MAX_R BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_RDOT, REF_ACCEL_FRAC)
37 
38 #define REF_RATE_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_P, REF_RATE_FRAC)
39 #define REF_RATE_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_Q, REF_RATE_FRAC)
40 #define REF_RATE_MAX_R BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_R, REF_RATE_FRAC)
41 
42 #define OMEGA_P STABILIZATION_ATTITUDE_REF_OMEGA_P
43 #define ZETA_P STABILIZATION_ATTITUDE_REF_ZETA_P
44 #define ZETA_OMEGA_P_RES 10
45 #define ZETA_OMEGA_P BFP_OF_REAL((ZETA_P*OMEGA_P), ZETA_OMEGA_P_RES)
46 #define OMEGA_2_P_RES 7
47 #define OMEGA_2_P BFP_OF_REAL((OMEGA_P*OMEGA_P), OMEGA_2_P_RES)
48 
49 #define OMEGA_Q STABILIZATION_ATTITUDE_REF_OMEGA_Q
50 #define ZETA_Q STABILIZATION_ATTITUDE_REF_ZETA_Q
51 #define ZETA_OMEGA_Q_RES 10
52 #define ZETA_OMEGA_Q BFP_OF_REAL((ZETA_Q*OMEGA_Q), ZETA_OMEGA_Q_RES)
53 #define OMEGA_2_Q_RES 7
54 #define OMEGA_2_Q BFP_OF_REAL((OMEGA_Q*OMEGA_Q), OMEGA_2_Q_RES)
55 
56 #define OMEGA_R STABILIZATION_ATTITUDE_REF_OMEGA_R
57 #define ZETA_R STABILIZATION_ATTITUDE_REF_ZETA_R
58 #define ZETA_OMEGA_R_RES 10
59 #define ZETA_OMEGA_R BFP_OF_REAL((ZETA_R*OMEGA_R), ZETA_OMEGA_R_RES)
60 #define OMEGA_2_R_RES 7
61 #define OMEGA_2_R BFP_OF_REAL((OMEGA_R*OMEGA_R), OMEGA_2_R_RES)
62 
63 
64 #define REF_ANGLE_PI BFP_OF_REAL(3.1415926535897932384626433832795029, REF_ANGLE_FRAC)
65 #define REF_ANGLE_TWO_PI BFP_OF_REAL(2.*3.1415926535897932384626433832795029, REF_ANGLE_FRAC)
66 #define ANGLE_REF_NORMALIZE(_a) { \
67  while (_a > REF_ANGLE_PI) _a -= REF_ANGLE_TWO_PI; \
68  while (_a < -REF_ANGLE_PI) _a += REF_ANGLE_TWO_PI; \
69  }
70 
71 
72 
74 {
75  INT_EULERS_ZERO(ref->euler);
76  INT_RATES_ZERO(ref->rate);
77  INT_RATES_ZERO(ref->accel);
78 }
79 
85 {
86 
87  /* dumb integrate reference attitude */
88  const struct Int32Eulers d_angle = {
92  };
93  EULERS_ADD(ref->euler, d_angle);
94  ANGLE_REF_NORMALIZE(ref->euler.psi);
95 
96  /* integrate reference rotational speeds */
97  const struct Int32Rates d_rate = {
98  ref->accel.p >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC),
99  ref->accel.q >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC),
100  ref->accel.r >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC)
101  };
102  RATES_ADD(ref->rate, d_rate);
103 
104  /* attitude setpoint with REF_ANGLE_FRAC */
105  struct Int32Eulers sp_ref;
106  INT32_EULERS_LSHIFT(sp_ref, *sp_euler, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
107 
108  /* compute reference attitude error */
109  struct Int32Eulers ref_err;
110  EULERS_DIFF(ref_err, ref->euler, sp_ref);
111  /* wrap it in the shortest direction */
112  ANGLE_REF_NORMALIZE(ref_err.psi);
113 
114  /* compute reference angular accelerations */
115  const struct Int32Rates accel_rate = {
116  ((int32_t)(-2.*ZETA_OMEGA_P) * (ref->rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
117  >> (ZETA_OMEGA_P_RES),
118  ((int32_t)(-2.*ZETA_OMEGA_Q) * (ref->rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
119  >> (ZETA_OMEGA_Q_RES),
120  ((int32_t)(-2.*ZETA_OMEGA_R) * (ref->rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
121  >> (ZETA_OMEGA_R_RES)
122  };
123 
124  const struct Int32Rates accel_angle = {
125  ((int32_t)(-OMEGA_2_P) * (ref_err.phi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES),
126  ((int32_t)(-OMEGA_2_Q) * (ref_err.theta >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES),
127  ((int32_t)(-OMEGA_2_R) * (ref_err.psi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES)
128  };
129 
130  RATES_SUM(ref->accel, accel_rate, accel_angle);
131 
132  /* saturate acceleration */
133  const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
134  const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
135  RATES_BOUND_BOX(ref->accel, MIN_ACCEL, MAX_ACCEL);
136 
137  /* saturate speed and trim accel accordingly */
138 }
#define RATES_BOUND_BOX(_v, _v_min, _v_max)
Definition: pprz_algebra.h:407
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:295
#define EULERS_ADD(_a, _b)
Definition: pprz_algebra.h:281
#define RATES_SUM(_c, _a, _b)
Definition: pprz_algebra.h:358
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:344
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
#define INT_RATES_ZERO(_e)
#define INT32_ANGLE_FRAC
#define INT32_EULERS_LSHIFT(_o, _i, _r)
#define INT_EULERS_ZERO(_e)
euler angles
angular rates
void attitude_ref_euler_int_update(struct AttRefEulerInt *ref, struct Int32Eulers *sp_euler)
Propagate reference model.
void attitude_ref_euler_int_init(struct AttRefEulerInt *ref)
#define ANGLE_REF_NORMALIZE(_a)
Rotorcraft attitude reference generation (euler int version)
Attitude reference models and state/output (euler int)
#define REF_RATE_FRAC
#define REF_ACCEL_FRAC
#define REF_ANGLE_FRAC
uint16_t ref[TCOUPLE_NB]
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83