Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
stabilization_attitude_ref_quat_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 
30 #include "generated/airframe.h"
33 
34 
35 #define TWO_ZETA_OMEGA_RES 10
36 #define TWO_OMEGA_2_RES 7
37 
38 
39 static inline void reset_psi_ref(struct AttRefQuatInt *ref, int32_t psi);
40 static void update_ref_model_p(struct AttRefQuatInt *ref);
41 static void update_ref_model_q(struct AttRefQuatInt *ref);
42 static void update_ref_model_r(struct AttRefQuatInt *ref);
43 static void update_ref_model(struct AttRefQuatInt *ref);
44 
45 /*
46  *
47  * Implementation.
48  * Should not rely on any global variables, so these functions can be used like a lib.
49  *
50  */
52 {
53  INT_EULERS_ZERO(ref->euler);
55  INT_RATES_ZERO(ref->rate);
56  INT_RATES_ZERO(ref->accel);
57 
64 
65  struct FloatRates omega0 = {STABILIZATION_ATTITUDE_REF_OMEGA_P,
66  STABILIZATION_ATTITUDE_REF_OMEGA_Q,
67  STABILIZATION_ATTITUDE_REF_OMEGA_R};
68  struct FloatRates zeta0 = {STABILIZATION_ATTITUDE_REF_ZETA_P,
69  STABILIZATION_ATTITUDE_REF_ZETA_Q,
70  STABILIZATION_ATTITUDE_REF_ZETA_R};
71  attitude_ref_quat_int_set_omega(ref, &omega0);
72  attitude_ref_quat_int_set_zeta(ref, &zeta0);
73 
74  /* calc the intermediate cached values */
75  update_ref_model(ref);
76 }
77 
79 {
80  reset_psi_ref(ref, psi);
81 
82  int32_quat_of_eulers(&ref->quat, &ref->euler);
84 
85  /* set reference rate and acceleration to zero */
86  memset(&ref->accel, 0, sizeof(struct Int32Rates));
87  memset(&ref->rate, 0, sizeof(struct Int32Rates));
88 }
89 
90 // CAUTION! Periodic frequency is assumed to be 512 Hz
91 // which is equal to >> 9
92 #define F_UPDATE_RES 9
93 
94 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
95 
101  float dt __attribute__((unused)))
102 {
103  /* integrate reference attitude */
104  const struct Int32Rates rate_ref_scaled = {
108  };
109  struct Int32Quat qdot;
110  int32_quat_derivative(&qdot, &rate_ref_scaled, &ref->quat);
111  qdot.qi = qdot.qi >> F_UPDATE_RES;
112  qdot.qx = qdot.qx >> F_UPDATE_RES;
113  qdot.qy = qdot.qy >> F_UPDATE_RES;
114  qdot.qz = qdot.qz >> F_UPDATE_RES;
115  QUAT_ADD(ref->quat, qdot);
116  int32_quat_normalize(&ref->quat);
117 
118  /* integrate reference rotational speeds
119  * delta rate = ref_accel * dt
120  * ref_rate = old_ref_rate + delta_rate
121  */
122  const struct Int32Rates delta_rate = {
126  };
127  RATES_ADD(ref->rate, delta_rate);
128 
129  /* compute reference angular accelerations */
130  struct Int32Quat err;
131  /* compute reference attitude error */
132  int32_quat_inv_comp(&err, sp_quat, &ref->quat);
133  /* wrap it in the shortest direction */
135 
136  /* propagate the 2nd order linear model : accel = -2*zeta*omega * rate - omega^2 * angle */
137 
138  const struct Int32Rates accel_rate = {
142  };
143 
144  /* since error quaternion contains the half-angles we get 2*omega^2*err */
145  const struct Int32Rates accel_angle = {
146  (-ref->model.two_omega2.p * (err.qx >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES),
147  (-ref->model.two_omega2.q * (err.qy >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES),
149  };
150 
151  RATES_SUM(ref->accel, accel_rate, accel_angle);
152 
153 
154  /* saturate */
156 
157  /* compute euler representation for debugging and telemetry */
158  int32_eulers_of_quat(&ref->euler, &ref->quat);
159 }
160 
161 
162 static inline void reset_psi_ref(struct AttRefQuatInt *ref, int32_t psi)
163 {
164  ref->euler.psi = psi;
165  ref->rate.r = 0;
166  ref->accel.r = 0;
167 }
168 
169 
170 /*
171  * Recomputation of cached values.
172  *
173  */
174 static void update_ref_model_p(struct AttRefQuatInt *ref)
175 {
177  ref->model.two_omega2.p = BFP_OF_REAL((2 * ref->model.omega.p * ref->model.omega.p), TWO_OMEGA_2_RES);
178 }
179 
180 static void update_ref_model_q(struct AttRefQuatInt *ref)
181 {
183  ref->model.two_omega2.q = BFP_OF_REAL((2 * ref->model.omega.q * ref->model.omega.q), TWO_OMEGA_2_RES);
184 }
185 
186 static void update_ref_model_r(struct AttRefQuatInt *ref)
187 {
189  ref->model.two_omega2.r = BFP_OF_REAL((2 * ref->model.omega.r * ref->model.omega.r), TWO_OMEGA_2_RES);
190 }
191 
192 static void update_ref_model(struct AttRefQuatInt *ref)
193 {
194  update_ref_model_p(ref);
195  update_ref_model_q(ref);
196  update_ref_model_r(ref);
197 }
198 
199 
200 /*
201  * Setting handlers for changing the ref model parameters.
202  *
203  */
205 {
206  ref->model.omega.p = omega_p;
207  update_ref_model_p(ref);
208 }
209 
211 {
212  ref->model.omega.q = omega_q;
213  update_ref_model_q(ref);
214 }
215 
217 {
218  ref->model.omega.r = omega_r;
219  update_ref_model_r(ref);
220 }
221 
223 {
227 }
228 
230 {
231  ref->model.zeta.p = zeta_p;
232  update_ref_model_p(ref);
233 }
234 
236 {
237  ref->model.zeta.q = zeta_q;
238  update_ref_model_q(ref);
239 }
240 
242 {
243  ref->model.zeta.r = zeta_r;
244  update_ref_model_r(ref);
245 }
246 
248 {
252 }
253 
254 void attitude_ref_quat_int_set_max_p(struct AttRefQuatInt *ref, float max_p)
255 {
257 }
258 
259 void attitude_ref_quat_int_set_max_q(struct AttRefQuatInt *ref, float max_q)
260 {
262 }
263 
264 void attitude_ref_quat_int_set_max_r(struct AttRefQuatInt *ref, float max_r)
265 {
267 }
268 
269 void attitude_ref_quat_int_set_max_pdot(struct AttRefQuatInt *ref, float max_pdot)
270 {
271  ref->saturation.max_accel.p = BFP_OF_REAL(max_pdot, REF_ACCEL_FRAC);
272 }
273 
274 void attitude_ref_quat_int_set_max_qdot(struct AttRefQuatInt *ref, float max_qdot)
275 {
276  ref->saturation.max_accel.q = BFP_OF_REAL(max_qdot, REF_ACCEL_FRAC);
277 }
278 
279 void attitude_ref_quat_int_set_max_rdot(struct AttRefQuatInt *ref, float max_rdot)
280 {
281  ref->saturation.max_accel.r = BFP_OF_REAL(max_rdot, REF_ACCEL_FRAC);
282 }
283 
int32_t psi
in rad with INT32_ANGLE_FRAC
static void attitude_ref_int_saturate_naive(struct Int32Rates *rate, struct Int32Rates *accel, struct Int32RefSat *sat)
#define INT32_RATE_FRAC
angular rates
void attitude_ref_quat_int_init(struct AttRefQuatInt *ref)
struct Int32Rates rate
with REF_RATE_FRAC
static void int32_quat_normalize(struct Int32Quat *q)
normalize a quaternion inplace
void attitude_ref_quat_int_set_omega_r(struct AttRefQuatInt *ref, float omega_r)
void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions.
static const float zeta_p[]
void attitude_ref_quat_int_set_max_p(struct AttRefQuatInt *ref, float max_p)
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:344
void attitude_ref_quat_int_set_zeta_p(struct AttRefQuatInt *ref, float zeta_p)
void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e)
Quaternion from Euler angles.
#define INT_RATES_ZERO(_e)
float r
in rad/s
void int32_quat_derivative(struct Int32Quat *qd, const struct Int32Rates *r, struct Int32Quat *q)
Quaternion derivative from rotational velocity.
#define REF_ACCEL_FRAC
#define STABILIZATION_ATTITUDE_REF_MAX_QDOT
static void update_ref_model_r(struct AttRefQuatInt *ref)
void attitude_ref_quat_int_enter(struct AttRefQuatInt *ref, int32_t psi)
float q
in rad/s
float p
in rad/s
static const float omega_r[]
void attitude_ref_quat_int_set_omega_p(struct AttRefQuatInt *ref, float omega_p)
#define STABILIZATION_ATTITUDE_REF_MAX_RDOT
void attitude_ref_quat_int_set_max_qdot(struct AttRefQuatInt *ref, float max_qdot)
static const float zeta_r[]
uint16_t ref[TCOUPLE_NB]
#define BFP_OF_REAL(_vr, _frac)
int32_t r
in rad/s with INT32_RATE_FRAC
static const float zeta_q[]
void attitude_ref_quat_int_set_zeta_r(struct AttRefQuatInt *ref, float zeta_r)
static const float omega_q[]
void attitude_ref_quat_int_set_max_rdot(struct AttRefQuatInt *ref, float max_rdot)
#define REF_RATE_FRAC
static void reset_psi_ref(struct AttRefQuatInt *ref, int32_t psi)
static void update_ref_model_p(struct AttRefQuatInt *ref)
void attitude_ref_quat_int_set_max_q(struct AttRefQuatInt *ref, float max_q)
#define RATES_SUM(_c, _a, _b)
Definition: pprz_algebra.h:358
void attitude_ref_quat_int_update(struct AttRefQuatInt *ref, struct Int32Quat *sp_quat, float dt)
Propagate reference.
static void int32_quat_identity(struct Int32Quat *q)
initialises a quaternion to identity
static void update_ref_model_q(struct AttRefQuatInt *ref)
#define INT32_QUAT_FRAC
void attitude_ref_quat_int_set_omega_q(struct AttRefQuatInt *ref, float omega_q)
signed long int32_t
Definition: types.h:19
struct Int32Eulers euler
with INT32_ANGLE_FRAC
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
void attitude_ref_quat_int_set_omega(struct AttRefQuatInt *ref, struct FloatRates *omega)
static void update_ref_model(struct AttRefQuatInt *ref)
Default values for attitude reference saturation.
#define INT_EULERS_ZERO(_e)
Attitude reference models and state/output (quat int)
#define OFFSET_AND_ROUND(_a, _b)
void attitude_ref_quat_int_set_zeta(struct AttRefQuatInt *ref, struct FloatRates *zeta)
static void int32_quat_wrap_shortest(struct Int32Quat *q)
struct Int32Rates accel
with REF_ACCEL_FRAC
static const float omega_p[]
int32_t p
in rad/s with INT32_RATE_FRAC
#define STABILIZATION_ATTITUDE_REF_MAX_R
struct Int32Rates max_rate
void attitude_ref_quat_int_set_max_pdot(struct AttRefQuatInt *ref, float max_pdot)
struct Int32Rates max_accel
#define STABILIZATION_ATTITUDE_REF_MAX_Q
#define STABILIZATION_ATTITUDE_REF_MAX_PDOT
#define QUAT_ADD(_qo, _qi)
Definition: pprz_algebra.h:619
#define STABILIZATION_ATTITUDE_REF_MAX_P
void attitude_ref_quat_int_set_max_r(struct AttRefQuatInt *ref, float max_r)
int32_t q
in rad/s with INT32_RATE_FRAC
Rotorcraft attitude reference generation.
Rotation quaternion.
angular rates
void attitude_ref_quat_int_set_zeta_q(struct AttRefQuatInt *ref, float zeta_q)