Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 #define REF_ACCEL_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_PDOT, REF_ACCEL_FRAC)
35 #define REF_ACCEL_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_QDOT, REF_ACCEL_FRAC)
36 #define REF_ACCEL_MAX_R BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_RDOT, REF_ACCEL_FRAC)
37 
38 #define REF_RATE_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_P, REF_RATE_FRAC)
39 #define REF_RATE_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_Q, REF_RATE_FRAC)
40 #define REF_RATE_MAX_R BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_R, REF_RATE_FRAC)
41 
42 
49 
51  {STABILIZATION_ATTITUDE_REF_OMEGA_P, STABILIZATION_ATTITUDE_REF_OMEGA_Q, STABILIZATION_ATTITUDE_REF_OMEGA_R},
52  {STABILIZATION_ATTITUDE_REF_ZETA_P, STABILIZATION_ATTITUDE_REF_ZETA_Q, STABILIZATION_ATTITUDE_REF_ZETA_R}
53 };
54 
55 #define TWO_ZETA_OMEGA_RES 10
56 #define TWO_OMEGA_2_RES 7
57 static struct Int32Rates two_zeta_omega;
58 static struct Int32Rates two_omega_2;
59 
60 static void update_ref_model_p(void) {
61  two_zeta_omega.p = BFP_OF_REAL((2*stab_att_ref_model.zeta.p * stab_att_ref_model.omega.p), TWO_ZETA_OMEGA_RES);
62  two_omega_2.p = BFP_OF_REAL((2*stab_att_ref_model.omega.p * stab_att_ref_model.omega.p), TWO_OMEGA_2_RES);
63 }
64 
65 static void update_ref_model_q(void) {
66  two_zeta_omega.q = BFP_OF_REAL((2*stab_att_ref_model.zeta.q * stab_att_ref_model.omega.q), TWO_ZETA_OMEGA_RES);
67  two_omega_2.q = BFP_OF_REAL((2*stab_att_ref_model.omega.q * stab_att_ref_model.omega.q), TWO_OMEGA_2_RES);
68 }
69 
70 static void update_ref_model_r(void) {
71  two_zeta_omega.r = BFP_OF_REAL((2*stab_att_ref_model.zeta.r * stab_att_ref_model.omega.r), TWO_ZETA_OMEGA_RES);
72  two_omega_2.r = BFP_OF_REAL((2*stab_att_ref_model.omega.r * stab_att_ref_model.omega.r), TWO_OMEGA_2_RES);
73 }
74 
75 static void update_ref_model(void) {
79 }
80 
81 
83  stab_att_ref_model.omega.p = omega_p;
85 }
86 
88  stab_att_ref_model.omega.q = omega_q;
90 }
91 
93  stab_att_ref_model.omega.r = omega_r;
95 }
96 
101 }
102 
104  stab_att_ref_model.zeta.p = zeta_p;
106 }
107 
109  stab_att_ref_model.zeta.q = zeta_q;
111 }
112 
114  stab_att_ref_model.zeta.r = zeta_r;
116 }
117 
122 }
123 
124 
125 static inline void reset_psi_ref_from_body(void) {
126  //sp has been set from body using stabilization_attitude_get_yaw_i, use that value
128  stab_att_ref_rate.r = 0;
129  stab_att_ref_accel.r = 0;
130 }
131 
133 
140 
142 
143 }
144 
146 {
148 
149  /* convert reference attitude with REF_ANGLE_FRAC to eulers with normal INT32_ANGLE_FRAC */
150  struct Int32Eulers ref_eul;
154 
155  /* set reference rate and acceleration to zero */
156  memset(&stab_att_ref_accel, 0, sizeof(struct Int32Rates));
157  memset(&stab_att_ref_rate, 0, sizeof(struct Int32Rates));
158 }
159 
160 /*
161  * Reference
162  */
163 #define DT_UPDATE (1./PERIODIC_FREQUENCY)
164 // CAUTION! Periodic frequency is assumed to be 512 Hz
165 // which is equal to >> 9
166 #define F_UPDATE_RES 9
167 
168 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
169 
170 
172 
173  /* integrate reference attitude */
174  const struct Int32Rates rate_ref_scaled = {
178  struct Int32Quat qdot;
179  INT32_QUAT_DERIVATIVE(qdot, rate_ref_scaled, stab_att_ref_quat);
180  //QUAT_SMUL(qdot, qdot, DT_UPDATE);
181  qdot.qi = qdot.qi >> F_UPDATE_RES;
182  qdot.qx = qdot.qx >> F_UPDATE_RES;
183  qdot.qy = qdot.qy >> F_UPDATE_RES;
184  qdot.qz = qdot.qz >> F_UPDATE_RES;
187 
188  /* integrate reference rotational speeds
189  * delta rate = ref_accel * dt
190  * ref_rate = old_ref_rate + delta_rate
191  */
192  const struct Int32Rates delta_rate = {
196  RATES_ADD(stab_att_ref_rate, delta_rate);
197 
198  /* compute reference angular accelerations */
199  struct Int32Quat err;
200  /* compute reference attitude error */
202  /* wrap it in the shortest direction */
204 
205  /* propagate the 2nd order linear model : accel = -2*zeta*omega * rate - omega^2 * angle */
206 
207  const struct Int32Rates accel_rate = {
211 
212  /* since error quaternion contains the half-angles we get 2*omega^2*err */
213  const struct Int32Rates accel_angle = {
217 
218  RATES_SUM(stab_att_ref_accel, accel_rate, accel_angle);
219 
220 
221  /* saturate acceleration */
222  const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
223  const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
224  RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
225 
226  /* saturate angular speed and trim accel accordingly */
228 
229 
230  /* compute ref_euler for debugging and telemetry */
231  struct Int32Eulers ref_eul;
234 }
void stabilization_attitude_ref_set_zeta_p(float zeta_p)
#define INT32_QUAT_FRAC
static const float zeta_p[]
int32_t p
in rad/s with INT32_RATE_FRAC
#define INT32_QUAT_ZERO(_q)
angular rates
struct Int32Rates stab_att_ref_rate
with REF_RATE_FRAC
Rotation quaternion.
Common rotorcraft attitude reference saturation include.
struct Int32Quat stab_att_ref_quat
with INT32_QUAT_FRAC
#define INT32_QUAT_NORMALIZE(q)
struct Int32Eulers stab_att_ref_euler
with REF_ANGLE_FRAC
#define INT32_QUAT_DERIVATIVE(_qd, _r, _q)
#define INT32_EULERS_LSHIFT(_o, _i, _r)
static void update_ref_model_r(void)
#define INT32_EULERS_RSHIFT(_o, _i, _r)
#define INT_RATES_ZERO(_e)
#define REF_ACCEL_FRAC
static void update_ref_model_q(void)
#define INT_EULERS_ZERO(_e)
#define INT32_RATE_FRAC
#define SATURATE_SPEED_TRIM_ACCEL()
void stabilization_attitude_ref_set_zeta(struct FloatRates *zeta)
static const float omega_r[]
void stabilization_attitude_ref_set_omega_r(float omega_r)
void stabilization_attitude_ref_set_zeta_q(float zeta_q)
static const float zeta_r[]
static void reset_psi_ref_from_body(void)
float p
in rad/s
#define INT32_QUAT_OF_EULERS(_q, _e)
static const float zeta_q[]
static struct Int32Rates two_zeta_omega
static const float omega_q[]
void stabilization_attitude_ref_enter(void)
#define INT32_ANGLE_FRAC
void stabilization_attitude_ref_set_omega_p(float omega_p)
#define REF_RATE_FRAC
void stabilization_attitude_ref_set_zeta_r(float zeta_r)
void stabilization_attitude_ref_set_omega(struct FloatRates *omega)
static struct Int32Rates two_omega_2
void stabilization_attitude_ref_set_omega_q(float omega_q)
#define BFP_OF_REAL(_vr, _frac)
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:333
#define REF_ANGLE_FRAC
angular rates
struct FloatRefModel stab_att_ref_model
float r
in rad/s
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
void stabilization_attitude_ref_init(void)
void stabilization_attitude_ref_update(void)
#define RATES_BOUND_BOX(_v, _v_min, _v_max)
Definition: pprz_algebra.h:396
#define OFFSET_AND_ROUND(_a, _b)
static void update_ref_model(void)
int32_t psi
in rad with INT32_ANGLE_FRAC
static const float omega_p[]
#define INT32_QUAT_INV_COMP(_b2c, _a2b, _a2c)
int32_t q
in rad/s with INT32_RATE_FRAC
#define QUAT_ADD(_qo, _qi)
Definition: pprz_algebra.h:535
float q
in rad/s
static void update_ref_model_p(void)
int32_t r
in rad/s with INT32_RATE_FRAC
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
#define INT32_EULERS_OF_QUAT(_e, _q)
struct Int32Rates stab_att_ref_accel
with REF_ACCEL_FRAC
Rotorcraft attitude reference generation.
#define INT32_QUAT_WRAP_SHORTEST(q)
#define RATES_SUM(_c, _a, _b)
Definition: pprz_algebra.h:347
euler angles