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_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 
35 #define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_REF_MAX_PDOT
36 #define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_QDOT
37 #define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_REF_MAX_RDOT
38 
39 #define REF_RATE_MAX_P STABILIZATION_ATTITUDE_REF_MAX_P
40 #define REF_RATE_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_Q
41 #define REF_RATE_MAX_R STABILIZATION_ATTITUDE_REF_MAX_R
42 
49 
51 
53 
54 static const float omega_p[] = STABILIZATION_ATTITUDE_REF_OMEGA_P;
55 static const float zeta_p[] = STABILIZATION_ATTITUDE_REF_ZETA_P;
56 static const float omega_q[] = STABILIZATION_ATTITUDE_REF_OMEGA_Q;
57 static const float zeta_q[] = STABILIZATION_ATTITUDE_REF_ZETA_Q;
58 static const float omega_r[] = STABILIZATION_ATTITUDE_REF_OMEGA_R;
59 static const float zeta_r[] = STABILIZATION_ATTITUDE_REF_ZETA_R;
60 
62 
63 static inline void reset_psi_ref_from_body(void) {
64  //sp has been set from body using stabilization_attitude_get_yaw_f, use that value
66  stab_att_ref_rate.r = 0;
68 }
69 
70 static inline void update_ref_quat_from_eulers(void) {
71  struct FloatRMat ref_rmat;
75 }
76 
78  stab_att_ref_model[idx].omega.p = omega;
79  two_omega_squared[idx].p = 2 * omega * omega;
80 }
81 
83  stab_att_ref_model[idx].omega.q = omega;
84  two_omega_squared[idx].q = 2 * omega * omega;
85 }
86 
88  stab_att_ref_model[idx].omega.r = omega;
89  two_omega_squared[idx].r = 2 * omega * omega;
90 }
91 
94 }
95 
98 }
99 
102 }
103 
104 
106 
113 
114  for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
115  RATES_ASSIGN(stab_att_ref_model[i].omega, omega_p[i], omega_q[i], omega_r[i]);
116  RATES_ASSIGN(stab_att_ref_model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]);
118  }
119 
120 }
121 
123  ref_idx = idx;
124 }
125 
129 }
130 
131 /*
132  * Reference
133  */
134 #define DT_UPDATE (1./PERIODIC_FREQUENCY)
135 
136 // default to fast but less precise quaternion integration
137 #ifndef STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP
138 #define STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP TRUE
139 #endif
140 
142 
143  /* integrate reference attitude */
144 #if STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP
145  struct FloatQuat qdot;
147  QUAT_SMUL(qdot, qdot, DT_UPDATE);
149 #else // use finite step (involves trig)
150  struct FloatQuat delta_q;
152  /* compose new ref_quat by quaternion multiplication of delta rotation and current ref_quat */
153  struct FloatQuat new_ref_quat;
154  FLOAT_QUAT_COMP(new_ref_quat, stab_att_ref_quat, delta_q);
155  QUAT_COPY(stab_att_ref_quat, new_ref_quat);
156 #endif
158 
159  /* integrate reference rotational speeds */
160  struct FloatRates delta_rate;
162  RATES_ADD(stab_att_ref_rate, delta_rate);
163 
164  /* compute reference angular accelerations */
165  struct FloatQuat err;
166  /* compute reference attitude error */
168  /* wrap it in the shortest direction */
170  /* propagate the 2nd order linear model: xdotdot = -2*zeta*omega*xdot - omega^2*x */
171  /* since error quaternion contains the half-angles we get 2*omega^2*err */
173  - two_omega_squared[ref_idx].p * err.qx;
175  - two_omega_squared[ref_idx].q * err.qy;
177  - two_omega_squared[ref_idx].r * err.qz;
178 
179  /* saturate acceleration */
180  const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
181  const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
182  RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
183 
184  /* saturate angular speed and trim accel accordingly */
186 
187  /* compute ref_euler */
189 }
#define FLOAT_RATES_ZERO(_r)
void stabilization_attitude_ref_schedule(uint8_t idx)
rotation matrix
static const float zeta_p[]
void stabilization_attitude_ref_set_omega_p(float omega)
angular rates
Common rotorcraft attitude reference saturation include.
float psi
in radians
struct FloatEulers stab_att_ref_euler
with REF_ANGLE_FRAC
struct FloatQuat stab_att_ref_quat
with INT32_QUAT_FRAC
void stabilization_attitude_ref_set_omega_q(float omega)
#define SATURATE_SPEED_TRIM_ACCEL()
static void reset_psi_ref_from_body(void)
euler angles
struct FloatRates stab_att_ref_accel
with REF_ACCEL_FRAC
static const float omega_r[]
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:368
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
static const float zeta_r[]
float p
in rad/s
Roation quaternion.
struct FloatRefModel stab_att_ref_model[STABILIZATION_ATTITUDE_GAIN_NB]
static const float zeta_q[]
static const float omega_q[]
#define QUAT_SMUL(_qo, _qi, _s)
Definition: pprz_algebra.h:527
struct FloatQuat stab_att_sp_quat
with INT32_QUAT_FRAC
void stabilization_attitude_ref_idx_set_omega_q(uint8_t idx, float omega)
void stabilization_attitude_ref_enter(void)
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:333
struct FloatRates two_omega_squared[STABILIZATION_ATTITUDE_GAIN_NB]
#define FLOAT_QUAT_WRAP_SHORTEST(_q)
void stabilization_attitude_ref_idx_set_omega_r(uint8_t idx, float omega)
float r
in rad/s
#define FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c)
#define FLOAT_QUAT_NORMALIZE(_q)
#define FLOAT_EULERS_ZERO(_e)
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
static void update_ref_quat_from_eulers(void)
unsigned char uint8_t
Definition: types.h:14
#define FLOAT_QUAT_DERIVATIVE(_qd, _r, _q)
#define RATES_BOUND_BOX(_v, _v_min, _v_max)
Definition: pprz_algebra.h:396
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:512
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:319
#define FLOAT_EULERS_OF_QUAT(_e, _q)
#define FLOAT_QUAT_OF_RMAT(_q, _r)
static const float omega_p[]
#define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt)
#define FLOAT_RMAT_OF_EULERS(_rm, _e)
#define QUAT_ADD(_qo, _qi)
Definition: pprz_algebra.h:535
float q
in rad/s
#define STABILIZATION_ATTITUDE_GAIN_NB
Rotorcraft attitude reference generation.
void stabilization_attitude_ref_set_omega_r(float omega)
#define FLOAT_QUAT_ZERO(_q)
struct FloatRates stab_att_ref_rate
with REF_RATE_FRAC
#define STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT
void stabilization_attitude_ref_idx_set_omega_p(uint8_t idx, float omega)
void stabilization_attitude_ref_update(void)
void stabilization_attitude_ref_init(void)