Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
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 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);
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};
70  attitude_ref_quat_int_set_omega(ref, &omega0);
71  attitude_ref_quat_int_set_zeta(ref, &zeta0);
72 
73  /* calc the intermediate cached values */
74  update_ref_model(ref);
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 = {
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 = {
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),
146  };
147 
148  RATES_SUM(ref->accel, accel_rate, accel_angle);
149 
150 
151  /* saturate */
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  */
163 static void update_ref_model_p(struct AttRefQuatInt *ref)
164 {
166  ref->model.two_omega2.p = BFP_OF_REAL((2 * ref->model.omega.p * ref->model.omega.p), TWO_OMEGA_2_RES);
167 }
168 
169 static void update_ref_model_q(struct AttRefQuatInt *ref)
170 {
172  ref->model.two_omega2.q = BFP_OF_REAL((2 * ref->model.omega.q * ref->model.omega.q), TWO_OMEGA_2_RES);
173 }
174 
175 static void update_ref_model_r(struct AttRefQuatInt *ref)
176 {
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 {
183  update_ref_model_p(ref);
184  update_ref_model_q(ref);
185  update_ref_model_r(ref);
186 }
187 
188 
189 /*
190  * Setting handlers for changing the ref model parameters.
191  *
192  */
194 {
195  ref->model.omega.p = omega_p;
196  update_ref_model_p(ref);
197 }
198 
200 {
201  ref->model.omega.q = omega_q;
202  update_ref_model_q(ref);
203 }
204 
206 {
207  ref->model.omega.r = omega_r;
208  update_ref_model_r(ref);
209 }
210 
212 {
216 }
217 
219 {
220  ref->model.zeta.p = zeta_p;
221  update_ref_model_p(ref);
222 }
223 
225 {
226  ref->model.zeta.q = zeta_q;
227  update_ref_model_q(ref);
228 }
229 
231 {
232  ref->model.zeta.r = zeta_r;
233  update_ref_model_r(ref);
234 }
235 
237 {
241 }
242 
243 void attitude_ref_quat_int_set_max_p(struct AttRefQuatInt *ref, float max_p)
244 {
246 }
247 
248 void attitude_ref_quat_int_set_max_q(struct AttRefQuatInt *ref, float max_q)
249 {
251 }
252 
253 void attitude_ref_quat_int_set_max_r(struct AttRefQuatInt *ref, float max_r)
254 {
256 }
257 
258 void attitude_ref_quat_int_set_max_pdot(struct AttRefQuatInt *ref, float max_pdot)
259 {
260  ref->saturation.max_accel.p = BFP_OF_REAL(max_pdot, REF_ACCEL_FRAC);
261 }
262 
263 void attitude_ref_quat_int_set_max_qdot(struct AttRefQuatInt *ref, float max_qdot)
264 {
265  ref->saturation.max_accel.q = BFP_OF_REAL(max_qdot, REF_ACCEL_FRAC);
266 }
267 
268 void attitude_ref_quat_int_set_max_rdot(struct AttRefQuatInt *ref, float max_rdot)
269 {
270  ref->saturation.max_accel.r = BFP_OF_REAL(max_rdot, REF_ACCEL_FRAC);
271 }
272 
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)
#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)
float q
in rad/s
float p
in rad/s
void attitude_ref_quat_int_enter(struct AttRefQuatInt *ref, struct Int32Quat *state_quat)
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[]
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:596
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 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)
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)