Paparazzi UAS  v5.15_devel-81-gd13dafb
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_float.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"
31 
34 
36 #ifndef STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP
37 #define STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP TRUE
38 #endif
39 
40 
41 /* parameters used for initialization */
42 static const float omega_p[] = STABILIZATION_ATTITUDE_REF_OMEGA_P;
43 static const float zeta_p[] = STABILIZATION_ATTITUDE_REF_ZETA_P;
44 static const float omega_q[] = STABILIZATION_ATTITUDE_REF_OMEGA_Q;
45 static const float zeta_q[] = STABILIZATION_ATTITUDE_REF_ZETA_Q;
46 static const float omega_r[] = STABILIZATION_ATTITUDE_REF_OMEGA_R;
47 static const float zeta_r[] = STABILIZATION_ATTITUDE_REF_ZETA_R;
48 
49 static inline void reset_psi_ref(struct AttRefQuatFloat *ref, float psi);
50 
51 /*
52  *
53  * Implementation.
54  * Should not rely on any global variables, so these functions can be used like a lib.
55  *
56  */
58 {
61  FLOAT_RATES_ZERO(ref->rate);
62  FLOAT_RATES_ZERO(ref->accel);
63 
70 
71  for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
72  RATES_ASSIGN(ref->model[i].omega, omega_p[i], omega_q[i], omega_r[i]);
73  RATES_ASSIGN(ref->model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]);
74  RATES_ASSIGN(ref->model[i].two_omega2, 2 * omega_p[i]*omega_p[i], 2 * omega_q[i]*omega_q[i], 2 * omega_r[i]*omega_r[i]);
75  }
76 
77  ref->cur_idx = 0;
78 }
79 
81 {
82  reset_psi_ref(ref, psi);
83 }
84 
85 
86 void attitude_ref_quat_float_update(struct AttRefQuatFloat *ref, struct FloatQuat *sp_quat, float dt)
87 {
88 
89  /* integrate reference attitude */
90 #if STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP
91  struct FloatQuat qdot;
92  float_quat_derivative(&qdot, &ref->rate, &ref->quat);
93  QUAT_SMUL(qdot, qdot, dt);
94  QUAT_ADD(ref->quat, qdot);
95 #else // use finite step (involves trig)
96  struct FloatQuat delta_q;
97  float_quat_differential(&delta_q, &ref->rate, dt);
98  /* compose new ref_quat by quaternion multiplication of delta rotation and current ref_quat */
99  struct FloatQuat new_ref_quat;
100  float_quat_comp(&new_ref_quat, &ref->quat, &delta_q);
101  QUAT_COPY(ref->quat, new_ref_quat);
102 #endif
103  float_quat_normalize(&ref->quat);
104 
105  /* integrate reference rotational speeds */
106  struct FloatRates delta_rate;
107  RATES_SMUL(delta_rate, ref->accel, dt);
108  RATES_ADD(ref->rate, delta_rate);
109 
110  /* compute reference angular accelerations */
111  struct FloatQuat err;
112  /* compute reference attitude error */
113  float_quat_inv_comp(&err, sp_quat, &ref->quat);
114  /* wrap it in the shortest direction */
116  /* propagate the 2nd order linear model: xdotdot = -2*zeta*omega*xdot - omega^2*x */
117  /* since error quaternion contains the half-angles we get 2*omega^2*err */
118  ref->accel.p = -2.*ref->model[ref->cur_idx].zeta.p * ref->model[ref->cur_idx].omega.p * ref->rate.p -
119  ref->model[ref->cur_idx].two_omega2.p * err.qx;
120  ref->accel.q = -2.*ref->model[ref->cur_idx].zeta.q * ref->model[ref->cur_idx].omega.q * ref->rate.q -
121  ref->model[ref->cur_idx].two_omega2.q * err.qy;
122  ref->accel.r = -2.*ref->model[ref->cur_idx].zeta.r * ref->model[ref->cur_idx].omega.r * ref->rate.r -
123  ref->model[ref->cur_idx].two_omega2.r * err.qz;
124 
125  /* saturate */
127 
128  /* compute ref_euler */
129  float_eulers_of_quat(&ref->euler, &ref->quat);
130 }
131 
132 
133 
134 /*
135  *
136  * Local helper functions.
137  *
138  */
139 static inline void reset_psi_ref(struct AttRefQuatFloat *ref, float psi)
140 {
141  ref->euler.psi = psi;
142  ref->rate.r = 0;
143  ref->accel.r = 0;
144 
145  // recalc quat from eulers
146  float_quat_of_eulers(&ref->quat, &ref->euler);
148 }
149 
150 
151 /*
152  *
153  * Setting of the reference model parameters
154  *
155  */
157 {
158  ref->model[idx].omega.p = omega;
159  ref->model[idx].two_omega2.p = 2 * omega * omega;
160 }
161 
163 {
164  ref->model[idx].omega.q = omega;
165  ref->model[idx].two_omega2.q = 2 * omega * omega;
166 }
167 
169 {
170  ref->model[idx].omega.r = omega;
171  ref->model[idx].two_omega2.r = 2 * omega * omega;
172 }
173 
175 {
177 }
178 
180 {
182 }
183 
185 {
187 }
188 
189 
190 /*
191  * schedule a different model
192  */
194 {
195  ref->cur_idx = idx;
196 }
void attitude_ref_quat_float_set_omega_p(struct AttRefQuatFloat *ref, float omega)
Attitude reference models and state/output (float)
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
quat of euler roation 'ZYX'
struct FloatRefModel model[STABILIZATION_ATTITUDE_GAIN_NB]
static uint32_t idx
static const float zeta_p[]
void attitude_ref_quat_float_enter(struct AttRefQuatFloat *ref, float psi)
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:344
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
#define FLOAT_EULERS_ZERO(_e)
#define QUAT_SMUL(_qo, _qi, _s)
Definition: pprz_algebra.h:611
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
float r
in rad/s
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:330
#define FLOAT_RATES_ZERO(_r)
float psi
in radians
#define STABILIZATION_ATTITUDE_REF_MAX_QDOT
void attitude_ref_quat_float_idx_set_omega_q(struct AttRefQuatFloat *ref, uint8_t idx, float omega)
float q
in rad/s
float p
in rad/s
void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions.
struct FloatRates two_omega2
cached value of 2*omega*omega
static const float omega_r[]
#define STABILIZATION_ATTITUDE_REF_MAX_RDOT
Roation quaternion.
static const float zeta_r[]
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:596
static void attitude_ref_float_saturate_naive(struct FloatRates *rate, struct FloatRates *accel, struct FloatRefSat *sat)
uint16_t ref[TCOUPLE_NB]
void attitude_ref_quat_float_idx_set_omega_r(struct AttRefQuatFloat *ref, uint8_t idx, float omega)
struct FloatRates max_rate
static const float zeta_q[]
static const float omega_q[]
void attitude_ref_quat_float_idx_set_omega_p(struct AttRefQuatFloat *ref, uint8_t idx, float omega)
void attitude_ref_quat_float_update(struct AttRefQuatFloat *ref, struct FloatQuat *sp_quat, float dt)
void attitude_ref_quat_float_schedule(struct AttRefQuatFloat *ref, uint8_t idx)
static void float_quat_normalize(struct FloatQuat *q)
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:379
void attitude_ref_quat_float_set_omega_r(struct AttRefQuatFloat *ref, float omega)
void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity.
Default values for attitude reference saturation.
unsigned char uint8_t
Definition: types.h:14
static void float_quat_wrap_shortest(struct FloatQuat *q)
struct FloatRates max_accel
static const float omega_p[]
static void reset_psi_ref(struct AttRefQuatFloat *ref, float psi)
#define STABILIZATION_ATTITUDE_REF_MAX_R
#define STABILIZATION_ATTITUDE_REF_MAX_Q
#define STABILIZATION_ATTITUDE_REF_MAX_PDOT
#define QUAT_ADD(_qo, _qi)
Definition: pprz_algebra.h:619
void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt)
Delta rotation quaternion with constant angular rates.
#define STABILIZATION_ATTITUDE_REF_MAX_P
Rotorcraft attitude reference generation.
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
void attitude_ref_quat_float_set_omega_q(struct AttRefQuatFloat *ref, float omega)
angular rates
void attitude_ref_quat_float_init(struct AttRefQuatFloat *ref)
#define STABILIZATION_ATTITUDE_GAIN_NB