Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 void update_ref_model_p(struct AttRefQuatInt *ref);
40 static void update_ref_model_q(struct AttRefQuatInt *ref);
41 static void update_ref_model_r(struct AttRefQuatInt *ref);
42 static void update_ref_model(struct AttRefQuatInt *ref);
43 
44 /*
45  *
46  * Implementation.
47  * Should not rely on any global variables, so these functions can be used like a lib.
48  *
49  */
51 {
52  INT_EULERS_ZERO(ref->euler);
53  int32_quat_identity(&ref->quat);
54  INT_RATES_ZERO(ref->rate);
55  INT_RATES_ZERO(ref->accel);
56 
63 
64  struct FloatRates omega0 = {STABILIZATION_ATTITUDE_REF_OMEGA_P,
65  STABILIZATION_ATTITUDE_REF_OMEGA_Q,
66  STABILIZATION_ATTITUDE_REF_OMEGA_R};
67  struct FloatRates zeta0 = {STABILIZATION_ATTITUDE_REF_ZETA_P,
68  STABILIZATION_ATTITUDE_REF_ZETA_Q,
69  STABILIZATION_ATTITUDE_REF_ZETA_R};
72 
73  /* calc the intermediate cached values */
75 }
76 
77 void attitude_ref_quat_int_enter(struct AttRefQuatInt *ref, struct Int32Quat *state_quat)
78 {
79  QUAT_COPY(ref->quat, *state_quat);
80 
81  /* set reference rate and acceleration to zero */
82  INT_RATES_ZERO(ref->rate);
83  INT_RATES_ZERO(ref->accel);
84 
85 }
86 
87 // CAUTION! Periodic frequency is assumed to be 512 Hz
88 // which is equal to >> 9
89 #define F_UPDATE_RES 9
90 
91 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
92 
98  float dt __attribute__((unused)))
99 {
100  /* integrate reference attitude */
101  const struct Int32Rates rate_ref_scaled = {
105  };
106  struct Int32Quat qdot;
107  int32_quat_derivative(&qdot, &rate_ref_scaled, &ref->quat);
108  qdot.qi = qdot.qi >> F_UPDATE_RES;
109  qdot.qx = qdot.qx >> F_UPDATE_RES;
110  qdot.qy = qdot.qy >> F_UPDATE_RES;
111  qdot.qz = qdot.qz >> F_UPDATE_RES;
112  QUAT_ADD(ref->quat, qdot);
113  int32_quat_normalize(&ref->quat);
114 
115  /* integrate reference rotational speeds
116  * delta rate = ref_accel * dt
117  * ref_rate = old_ref_rate + delta_rate
118  */
119  const struct Int32Rates delta_rate = {
120  ref->accel.p >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC),
121  ref->accel.q >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC),
122  ref->accel.r >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC)
123  };
124  RATES_ADD(ref->rate, delta_rate);
125 
126  /* compute reference angular accelerations */
127  struct Int32Quat err;
128  /* compute reference attitude error */
129  int32_quat_inv_comp(&err, sp_quat, &ref->quat);
130  /* wrap it in the shortest direction */
132 
133  /* propagate the 2nd order linear model : accel = -2*zeta*omega * rate - omega^2 * angle */
134 
135  const struct Int32Rates accel_rate = {
136  (-ref->model.two_zeta_omega.p * (ref->rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (TWO_ZETA_OMEGA_RES),
137  (-ref->model.two_zeta_omega.q * (ref->rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (TWO_ZETA_OMEGA_RES),
138  (-ref->model.two_zeta_omega.r * (ref->rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (TWO_ZETA_OMEGA_RES)
139  };
140 
141  /* since error quaternion contains the half-angles we get 2*omega^2*err */
142  const struct Int32Rates accel_angle = {
143  (-ref->model.two_omega2.p * (err.qx >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES),
144  (-ref->model.two_omega2.q * (err.qy >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES),
145  (-ref->model.two_omega2.r * (err.qz >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES)
146  };
147 
148  RATES_SUM(ref->accel, accel_rate, accel_angle);
149 
150 
151  /* saturate */
152  attitude_ref_int_saturate_naive(&ref->rate, &ref->accel, &ref->saturation);
153 
154  /* compute euler representation for debugging and telemetry */
155  int32_eulers_of_quat(&ref->euler, &ref->quat);
156 }
157 
158 
159 /*
160  * Recomputation of cached values.
161  *
162  */
164 {
165  ref->model.two_zeta_omega.p = BFP_OF_REAL((2 * ref->model.zeta.p * ref->model.omega.p), TWO_ZETA_OMEGA_RES);
166  ref->model.two_omega2.p = BFP_OF_REAL((2 * ref->model.omega.p * ref->model.omega.p), TWO_OMEGA_2_RES);
167 }
168 
170 {
171  ref->model.two_zeta_omega.q = BFP_OF_REAL((2 * ref->model.zeta.q * ref->model.omega.q), TWO_ZETA_OMEGA_RES);
172  ref->model.two_omega2.q = BFP_OF_REAL((2 * ref->model.omega.q * ref->model.omega.q), TWO_OMEGA_2_RES);
173 }
174 
176 {
177  ref->model.two_zeta_omega.r = BFP_OF_REAL((2 * ref->model.zeta.r * ref->model.omega.r), TWO_ZETA_OMEGA_RES);
178  ref->model.two_omega2.r = BFP_OF_REAL((2 * ref->model.omega.r * ref->model.omega.r), TWO_OMEGA_2_RES);
179 }
180 
181 static void update_ref_model(struct AttRefQuatInt *ref)
182 {
186 }
187 
188 
189 /*
190  * Setting handlers for changing the ref model parameters.
191  *
192  */
194 {
195  ref->model.omega.p = omega_p;
197 }
198 
200 {
201  ref->model.omega.q = omega_q;
203 }
204 
206 {
207  ref->model.omega.r = omega_r;
209 }
210 
212 {
216 }
217 
219 {
220  ref->model.zeta.p = zeta_p;
222 }
223 
225 {
226  ref->model.zeta.q = zeta_q;
228 }
229 
231 {
232  ref->model.zeta.r = zeta_r;
234 }
235 
237 {
241 }
242 
244 {
245  ref->saturation.max_rate.p = BFP_OF_REAL(max_p, REF_RATE_FRAC);
246 }
247 
249 {
250  ref->saturation.max_rate.q = BFP_OF_REAL(max_q, REF_RATE_FRAC);
251 }
252 
254 {
255  ref->saturation.max_rate.r = BFP_OF_REAL(max_r, REF_RATE_FRAC);
256 }
257 
259 {
260  ref->saturation.max_accel.p = BFP_OF_REAL(max_pdot, REF_ACCEL_FRAC);
261 }
262 
264 {
265  ref->saturation.max_accel.q = BFP_OF_REAL(max_qdot, REF_ACCEL_FRAC);
266 }
267 
269 {
270  ref->saturation.max_accel.r = BFP_OF_REAL(max_rdot, REF_ACCEL_FRAC);
271 }
272 
STABILIZATION_ATTITUDE_REF_MAX_QDOT
#define STABILIZATION_ATTITUDE_REF_MAX_QDOT
Definition: stabilization_attitude_ref_defaults.h:43
TWO_ZETA_OMEGA_RES
#define TWO_ZETA_OMEGA_RES
Definition: stabilization_attitude_ref_quat_int.c:35
STABILIZATION_ATTITUDE_REF_MAX_PDOT
#define STABILIZATION_ATTITUDE_REF_MAX_PDOT
Definition: stabilization_attitude_ref_defaults.h:40
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
omega_p
static const float omega_p[]
Definition: stabilization_attitude_ref_quat_float.c:42
INT_RATES_ZERO
#define INT_RATES_ZERO(_e)
Definition: pprz_algebra_int.h:575
F_UPDATE_RES
#define F_UPDATE_RES
Definition: stabilization_attitude_ref_quat_int.c:89
update_ref_model_p
static void update_ref_model_p(struct AttRefQuatInt *ref)
Definition: stabilization_attitude_ref_quat_int.c:163
attitude_ref_quat_int_enter
void attitude_ref_quat_int_enter(struct AttRefQuatInt *ref, struct Int32Quat *state_quat)
Definition: stabilization_attitude_ref_quat_int.c:77
Int32Quat
Rotation quaternion.
Definition: pprz_algebra_int.h:99
attitude_ref_quat_int_set_max_q
void attitude_ref_quat_int_set_max_q(struct AttRefQuatInt *ref, float max_q)
Definition: stabilization_attitude_ref_quat_int.c:248
INT32_RATE_FRAC
#define INT32_RATE_FRAC
Definition: pprz_algebra_int.h:117
INT_EULERS_ZERO
#define INT_EULERS_ZERO(_e)
Definition: pprz_algebra_int.h:549
REF_ACCEL_FRAC
#define REF_ACCEL_FRAC
Definition: stabilization_attitude_ref_int.h:32
stabilization_attitude_ref_quat_int.h
int32_quat_derivative
void int32_quat_derivative(struct Int32Quat *qd, const struct Int32Rates *r, struct Int32Quat *q)
Quaternion derivative from rotational velocity.
Definition: pprz_algebra_int.c:320
int32_quat_normalize
static void int32_quat_normalize(struct Int32Quat *q)
normalize a quaternion inplace
Definition: pprz_algebra_int.h:455
update_ref_model_q
static void update_ref_model_q(struct AttRefQuatInt *ref)
Definition: stabilization_attitude_ref_quat_int.c:169
int32_eulers_of_quat
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
Definition: pprz_algebra_int.c:523
int32_quat_inv_comp
void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions.
Definition: pprz_algebra_int.c:285
STABILIZATION_ATTITUDE_REF_MAX_RDOT
#define STABILIZATION_ATTITUDE_REF_MAX_RDOT
Definition: stabilization_attitude_ref_defaults.h:46
int32_quat_wrap_shortest
static void int32_quat_wrap_shortest(struct Int32Quat *q)
Definition: pprz_algebra_int.h:447
update_ref_model
static void update_ref_model(struct AttRefQuatInt *ref)
Definition: stabilization_attitude_ref_quat_int.c:181
attitude_ref_quat_int_set_omega
void attitude_ref_quat_int_set_omega(struct AttRefQuatInt *ref, struct FloatRates *omega)
Definition: stabilization_attitude_ref_quat_int.c:211
attitude_ref_quat_int_init
void attitude_ref_quat_int_init(struct AttRefQuatInt *ref)
Definition: stabilization_attitude_ref_quat_int.c:50
omega_q
static const float omega_q[]
Definition: stabilization_attitude_ref_quat_float.c:44
attitude_ref_quat_int_set_zeta_q
void attitude_ref_quat_int_set_zeta_q(struct AttRefQuatInt *ref, float zeta_q)
Definition: stabilization_attitude_ref_quat_int.c:224
stabilization_attitude_ref_defaults.h
STABILIZATION_ATTITUDE_REF_MAX_R
#define STABILIZATION_ATTITUDE_REF_MAX_R
Definition: stabilization_attitude_ref_defaults.h:36
RATES_SUM
#define RATES_SUM(_c, _a, _b)
Definition: pprz_algebra.h:358
attitude_ref_quat_int_set_zeta
void attitude_ref_quat_int_set_zeta(struct AttRefQuatInt *ref, struct FloatRates *zeta)
Definition: stabilization_attitude_ref_quat_int.c:236
Int32Quat::qz
int32_t qz
Definition: pprz_algebra_int.h:103
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
REF_RATE_FRAC
#define REF_RATE_FRAC
Definition: stabilization_attitude_ref_int.h:33
BFP_OF_REAL
#define BFP_OF_REAL(_vr, _frac)
Definition: pprz_algebra_int.h:205
RATES_ADD
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:344
Int32Quat::qi
int32_t qi
Definition: pprz_algebra_int.h:100
attitude_ref_quat_int_set_omega_r
void attitude_ref_quat_int_set_omega_r(struct AttRefQuatInt *ref, float omega_r)
Definition: stabilization_attitude_ref_quat_int.c:205
attitude_ref_quat_int_set_max_r
void attitude_ref_quat_int_set_max_r(struct AttRefQuatInt *ref, float max_r)
Definition: stabilization_attitude_ref_quat_int.c:253
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
attitude_ref_quat_int_set_omega_p
void attitude_ref_quat_int_set_omega_p(struct AttRefQuatInt *ref, float omega_p)
Definition: stabilization_attitude_ref_quat_int.c:193
attitude_ref_int_saturate_naive
static void attitude_ref_int_saturate_naive(struct Int32Rates *rate, struct Int32Rates *accel, struct Int32RefSat *sat)
Definition: attitude_ref_saturate_naive.h:71
attitude_ref_quat_int_set_max_pdot
void attitude_ref_quat_int_set_max_pdot(struct AttRefQuatInt *ref, float max_pdot)
Definition: stabilization_attitude_ref_quat_int.c:258
QUAT_COPY
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:596
Int32Quat::qy
int32_t qy
Definition: pprz_algebra_int.h:102
QUAT_ADD
#define QUAT_ADD(_qo, _qi)
Definition: pprz_algebra.h:619
AttRefQuatInt
Attitude reference models and state/output (quat int)
Definition: stabilization_attitude_ref_quat_int.h:49
omega_r
static const float omega_r[]
Definition: stabilization_attitude_ref_quat_float.c:46
attitude_ref_quat_int_set_max_rdot
void attitude_ref_quat_int_set_max_rdot(struct AttRefQuatInt *ref, float max_rdot)
Definition: stabilization_attitude_ref_quat_int.c:268
STABILIZATION_ATTITUDE_REF_MAX_P
#define STABILIZATION_ATTITUDE_REF_MAX_P
Definition: stabilization_attitude_ref_defaults.h:30
STABILIZATION_ATTITUDE_REF_MAX_Q
#define STABILIZATION_ATTITUDE_REF_MAX_Q
Definition: stabilization_attitude_ref_defaults.h:33
attitude_ref_quat_int_set_zeta_p
void attitude_ref_quat_int_set_zeta_p(struct AttRefQuatInt *ref, float zeta_p)
Definition: stabilization_attitude_ref_quat_int.c:218
zeta_q
static const float zeta_q[]
Definition: stabilization_attitude_ref_quat_float.c:45
ref
uint16_t ref[TCOUPLE_NB]
Definition: temp_tcouple_adc.c:49
OFFSET_AND_ROUND
#define OFFSET_AND_ROUND(_a, _b)
Definition: stabilization_attitude_ref_quat_int.c:91
zeta_r
static const float zeta_r[]
Definition: stabilization_attitude_ref_quat_float.c:47
attitude_ref_quat_int_set_max_qdot
void attitude_ref_quat_int_set_max_qdot(struct AttRefQuatInt *ref, float max_qdot)
Definition: stabilization_attitude_ref_quat_int.c:263
int32_quat_identity
static void int32_quat_identity(struct Int32Quat *q)
initialises a quaternion to identity
Definition: pprz_algebra_int.h:431
Int32Quat::qx
int32_t qx
Definition: pprz_algebra_int.h:101
attitude_ref_quat_int_set_omega_q
void attitude_ref_quat_int_set_omega_q(struct AttRefQuatInt *ref, float omega_q)
Definition: stabilization_attitude_ref_quat_int.c:199
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
attitude_ref_quat_int_set_zeta_r
void attitude_ref_quat_int_set_zeta_r(struct AttRefQuatInt *ref, float zeta_r)
Definition: stabilization_attitude_ref_quat_int.c:230
TWO_OMEGA_2_RES
#define TWO_OMEGA_2_RES
Definition: stabilization_attitude_ref_quat_int.c:36
attitude_ref_quat_int_set_max_p
void attitude_ref_quat_int_set_max_p(struct AttRefQuatInt *ref, float max_p)
Definition: stabilization_attitude_ref_quat_int.c:243
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
update_ref_model_r
static void update_ref_model_r(struct AttRefQuatInt *ref)
Definition: stabilization_attitude_ref_quat_int.c:175
INT32_QUAT_FRAC
#define INT32_QUAT_FRAC
Definition: pprz_algebra_int.h:95
zeta_p
static const float zeta_p[]
Definition: stabilization_attitude_ref_quat_float.c:43
attitude_ref_quat_int_update
void attitude_ref_quat_int_update(struct AttRefQuatInt *ref, struct Int32Quat *sp_quat, float dt)
Propagate reference.
Definition: stabilization_attitude_ref_quat_int.c:97