Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 /*
50  *
51  * Implementation.
52  * Should not rely on any global variables, so these functions can be used like a lib.
53  *
54  */
56 {
57  FLOAT_EULERS_ZERO(ref->euler);
58  float_quat_identity(&ref->quat);
59  FLOAT_RATES_ZERO(ref->rate);
60  FLOAT_RATES_ZERO(ref->accel);
61 
62  ref->saturation.max_rate.p = STABILIZATION_ATTITUDE_REF_MAX_P;
63  ref->saturation.max_rate.q = STABILIZATION_ATTITUDE_REF_MAX_Q;
64  ref->saturation.max_rate.r = STABILIZATION_ATTITUDE_REF_MAX_R;
65  ref->saturation.max_accel.p = STABILIZATION_ATTITUDE_REF_MAX_PDOT;
66  ref->saturation.max_accel.q = STABILIZATION_ATTITUDE_REF_MAX_QDOT;
67  ref->saturation.max_accel.r = STABILIZATION_ATTITUDE_REF_MAX_RDOT;
68 
69  for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
70  RATES_ASSIGN(ref->model[i].omega, omega_p[i], omega_q[i], omega_r[i]);
71  RATES_ASSIGN(ref->model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]);
72  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]);
73  }
74 
75  ref->cur_idx = 0;
76 }
77 
79 {
80  QUAT_COPY(ref->quat, *state_quat);
81 
82  /* set reference rate and acceleration to zero */
83  FLOAT_RATES_ZERO(ref->rate);
84  FLOAT_RATES_ZERO(ref->accel);
85 }
86 
87 
88 void attitude_ref_quat_float_update(struct AttRefQuatFloat *ref, struct FloatQuat *sp_quat, float dt)
89 {
90 
91  /* integrate reference attitude */
92 #if STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP
93  struct FloatQuat qdot;
94  float_quat_derivative(&qdot, &ref->rate, &ref->quat);
95  QUAT_SMUL(qdot, qdot, dt);
96  QUAT_ADD(ref->quat, qdot);
97 #else // use finite step (involves trig)
98  struct FloatQuat delta_q;
99  float_quat_differential(&delta_q, &ref->rate, dt);
100  /* compose new ref_quat by quaternion multiplication of delta rotation and current ref_quat */
101  struct FloatQuat new_ref_quat;
102  float_quat_comp(&new_ref_quat, &ref->quat, &delta_q);
103  QUAT_COPY(ref->quat, new_ref_quat);
104 #endif
105  float_quat_normalize(&ref->quat);
106 
107  /* integrate reference rotational speeds */
108  struct FloatRates delta_rate;
109  RATES_SMUL(delta_rate, ref->accel, dt);
110  RATES_ADD(ref->rate, delta_rate);
111 
112  /* compute reference angular accelerations */
113  struct FloatQuat err;
114  /* compute reference attitude error */
115  float_quat_inv_comp(&err, sp_quat, &ref->quat);
116  /* wrap it in the shortest direction */
118  /* propagate the 2nd order linear model: xdotdot = -2*zeta*omega*xdot - omega^2*x */
119  /* since error quaternion contains the half-angles we get 2*omega^2*err */
120  ref->accel.p = -2.*ref->model[ref->cur_idx].zeta.p * ref->model[ref->cur_idx].omega.p * ref->rate.p -
121  ref->model[ref->cur_idx].two_omega2.p * err.qx;
122  ref->accel.q = -2.*ref->model[ref->cur_idx].zeta.q * ref->model[ref->cur_idx].omega.q * ref->rate.q -
123  ref->model[ref->cur_idx].two_omega2.q * err.qy;
124  ref->accel.r = -2.*ref->model[ref->cur_idx].zeta.r * ref->model[ref->cur_idx].omega.r * ref->rate.r -
125  ref->model[ref->cur_idx].two_omega2.r * err.qz;
126 
127  /* saturate */
128  attitude_ref_float_saturate_naive(&ref->rate, &ref->accel, &ref->saturation);
129 
130  /* compute ref_euler */
131  float_eulers_of_quat(&ref->euler, &ref->quat);
132 }
133 
134 
135 /*
136  *
137  * Setting of the reference model parameters
138  *
139  */
141 {
142  ref->model[idx].omega.p = omega;
143  ref->model[idx].two_omega2.p = 2 * omega * omega;
144 }
145 
147 {
148  ref->model[idx].omega.q = omega;
149  ref->model[idx].two_omega2.q = 2 * omega * omega;
150 }
151 
153 {
154  ref->model[idx].omega.r = omega;
155  ref->model[idx].two_omega2.r = 2 * omega * omega;
156 }
157 
159 {
161 }
162 
164 {
166 }
167 
169 {
171 }
172 
173 
174 /*
175  * schedule a different model
176  */
178 {
179  ref->cur_idx = idx;
180 }
static void attitude_ref_float_saturate_naive(struct FloatRates *rate, struct FloatRates *accel, struct FloatRefSat *sat)
static void float_quat_normalize(struct FloatQuat *q)
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions.
#define FLOAT_EULERS_ZERO(_e)
static void float_quat_wrap_shortest(struct FloatQuat *q)
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
#define FLOAT_RATES_ZERO(_r)
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt)
Delta rotation quaternion with constant angular rates.
void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity.
Roation quaternion.
angular rates
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:330
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:379
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:596
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:344
#define QUAT_SMUL(_qo, _qi, _s)
Definition: pprz_algebra.h:611
#define QUAT_ADD(_qo, _qi)
Definition: pprz_algebra.h:619
static uint32_t idx
#define STABILIZATION_ATTITUDE_GAIN_NB
Default values for attitude reference saturation.
#define STABILIZATION_ATTITUDE_REF_MAX_P
#define STABILIZATION_ATTITUDE_REF_MAX_PDOT
#define STABILIZATION_ATTITUDE_REF_MAX_QDOT
#define STABILIZATION_ATTITUDE_REF_MAX_Q
#define STABILIZATION_ATTITUDE_REF_MAX_RDOT
#define STABILIZATION_ATTITUDE_REF_MAX_R
static const float omega_r[]
void attitude_ref_quat_float_set_omega_q(struct AttRefQuatFloat *ref, float omega)
static const float zeta_r[]
void attitude_ref_quat_float_update(struct AttRefQuatFloat *ref, struct FloatQuat *sp_quat, float dt)
void attitude_ref_quat_float_idx_set_omega_p(struct AttRefQuatFloat *ref, uint8_t idx, float omega)
void attitude_ref_quat_float_enter(struct AttRefQuatFloat *ref, struct FloatQuat *state_quat)
static const float omega_q[]
static const float omega_p[]
void attitude_ref_quat_float_set_omega_r(struct AttRefQuatFloat *ref, float omega)
void attitude_ref_quat_float_idx_set_omega_q(struct AttRefQuatFloat *ref, uint8_t idx, float omega)
static const float zeta_q[]
void attitude_ref_quat_float_idx_set_omega_r(struct AttRefQuatFloat *ref, uint8_t idx, float omega)
void attitude_ref_quat_float_set_omega_p(struct AttRefQuatFloat *ref, float omega)
void attitude_ref_quat_float_init(struct AttRefQuatFloat *ref)
static const float zeta_p[]
void attitude_ref_quat_float_schedule(struct AttRefQuatFloat *ref, uint8_t idx)
Rotorcraft attitude reference generation.
Attitude reference models and state/output (float)
uint16_t ref[TCOUPLE_NB]
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98