Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros 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 
27 #include "generated/airframe.h"
29 #include "subsystems/ahrs.h"
30 
32 
33 #if USE_SETPOINTS_WITH_TRANSITIONS
35 #endif
36 
37 #define REF_ACCEL_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_PDOT, REF_ACCEL_FRAC)
38 #define REF_ACCEL_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_QDOT, REF_ACCEL_FRAC)
39 #define REF_ACCEL_MAX_R BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_RDOT, REF_ACCEL_FRAC)
40 
41 #define REF_RATE_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_P, REF_RATE_FRAC)
42 #define REF_RATE_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_Q, REF_RATE_FRAC)
43 #define REF_RATE_MAX_R BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_R, REF_RATE_FRAC)
44 
45 #define OMEGA_P STABILIZATION_ATTITUDE_REF_OMEGA_P
46 #define ZETA_P STABILIZATION_ATTITUDE_REF_ZETA_P
47 #define ZETA_OMEGA_P_RES 10
48 #define ZETA_OMEGA_P BFP_OF_REAL((ZETA_P*OMEGA_P), ZETA_OMEGA_P_RES)
49 #define OMEGA_2_P_RES 7
50 #define OMEGA_2_P BFP_OF_REAL((OMEGA_P*OMEGA_P), OMEGA_2_P_RES)
51 
52 #define OMEGA_Q STABILIZATION_ATTITUDE_REF_OMEGA_Q
53 #define ZETA_Q STABILIZATION_ATTITUDE_REF_ZETA_Q
54 #define ZETA_OMEGA_Q_RES 10
55 #define ZETA_OMEGA_Q BFP_OF_REAL((ZETA_Q*OMEGA_Q), ZETA_OMEGA_Q_RES)
56 #define OMEGA_2_Q_RES 7
57 #define OMEGA_2_Q BFP_OF_REAL((OMEGA_Q*OMEGA_Q), OMEGA_2_Q_RES)
58 
59 #define OMEGA_R STABILIZATION_ATTITUDE_REF_OMEGA_R
60 #define ZETA_R STABILIZATION_ATTITUDE_REF_ZETA_R
61 #define ZETA_OMEGA_R_RES 10
62 #define ZETA_OMEGA_R BFP_OF_REAL((ZETA_R*OMEGA_R), ZETA_OMEGA_R_RES)
63 #define OMEGA_2_R_RES 7
64 #define OMEGA_2_R BFP_OF_REAL((OMEGA_R*OMEGA_R), OMEGA_2_R_RES)
65 
66 
73 
75  {STABILIZATION_ATTITUDE_REF_OMEGA_P, STABILIZATION_ATTITUDE_REF_OMEGA_Q, STABILIZATION_ATTITUDE_REF_OMEGA_R},
76  {STABILIZATION_ATTITUDE_REF_ZETA_P, STABILIZATION_ATTITUDE_REF_ZETA_Q, STABILIZATION_ATTITUDE_REF_ZETA_R}
77 };
78 
79 
81 
88 
89  /*
90  for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
91  RATES_ASSIGN(stab_att_ref_model[i].omega, omega_p[i], omega_q[i], omega_r[i]);
92  RATES_ASSIGN(stab_att_ref_model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]);
93  }
94  */
95 
96 }
97 
99 {
101 
102 #if USE_SETPOINTS_WITH_TRANSITIONS
104  memcpy(&stab_att_ref_quat, &stab_att_sp_quat, sizeof(struct Int32Quat));
105 #else
106  /* convert reference attitude with REF_ANGLE_FRAC to eulers with normal INT32_ANGLE_FRAC */
107  struct Int32Eulers ref_eul;
111 #endif
112 
113  /* set reference rate and acceleration to zero */
114  memset(&stab_att_ref_accel, 0, sizeof(struct Int32Rates));
115  memset(&stab_att_ref_rate, 0, sizeof(struct Int32Rates));
116 }
117 
118 /*
119  * Reference
120  */
121 #define DT_UPDATE (1./PERIODIC_FREQUENCY)
122 #define F_UPDATE_RES 9
123 
125 
126  /* integrate reference attitude */
127  struct Int32Quat qdot;
129  //QUAT_SMUL(qdot, qdot, RATE_BFP_OF_REAL(DT_UPDATE));
130  QUAT_SMUL(qdot, qdot, 4);
131  QUAT_SMUL(qdot, qdot, DT_UPDATE);
134 
135  /* integrate reference rotational speeds */
136  const struct Int32Rates delta_rate = {
140 
141  //RATES_SMUL(delta_rate, stab_att_ref_accel, RATE_BFP_OF_REAL(DT_UPDATE));
142  //RATES_SMUL(delta_rate, stab_att_ref_accel, DT_UPDATE);
143  RATES_ADD(stab_att_ref_rate, delta_rate);
144 
145  /* compute reference angular accelerations */
146  struct Int32Quat err;
147  /* compute reference attitude error */
149  /* wrap it in the shortest direction */
151  /* propagate the 2nd order linear model */
152 
153  const struct Int32Rates accel_rate = {
157 
158  const struct Int32Rates accel_angle = {
161  ((int32_t)(-OMEGA_2_R)* (err.qz >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) };
162 
163  RATES_SUM(stab_att_ref_accel, accel_rate, accel_angle);
164 
165 
166  /*
167  stab_att_ref_accel.p = -2.*stab_att_ref_model.zeta.p*stab_att_ref_model.omega.p*stab_att_ref_rate.p
168  - stab_att_ref_model.omega.p*stab_att_ref_model.omega.p*err.qx;
169  stab_att_ref_accel.q = -2.*stab_att_ref_model.zeta.q*stab_att_ref_model.omega.q*stab_att_ref_rate.q
170  - stab_att_ref_model.omega.q*stab_att_ref_model.omega.q*err.qy;
171  stab_att_ref_accel.r = -2.*stab_att_ref_model.zeta.r*stab_att_ref_model.omega.r*stab_att_ref_rate.r
172  - stab_att_ref_model.omega.r*stab_att_ref_model.omega.r*err.qz;
173  */
174 
175  /* saturate acceleration */
176  //const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
177  //const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
178  //RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
179 
180 
181  /* compute ref_euler for debugging and telemetry */
182  struct Int32Eulers ref_eul;
185 }
static void reset_psi_ref_from_body(void)
Attitude and Heading Reference System interface.
void stabilization_attitude_ref_enter()
int32_t p
in rad/s^2 with INT32_RATE_FRAC
#define INT32_QUAT_ZERO(_q)
struct Int32Rates stab_att_ref_rate
with REF_RATE_FRAC
void stabilization_attitude_sp_enter(void)
Rotation quaternion.
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)
#define INT32_EULERS_RSHIFT(_o, _i, _r)
#define INT_RATES_ZERO(_e)
#define REF_ACCEL_FRAC
#define INT_EULERS_ZERO(_e)
struct Int32RefModel stab_att_ref_model
#define INT32_QUAT_OF_EULERS(_q, _e)
#define QUAT_SMUL(_qo, _qi, _s)
Definition: pprz_algebra.h:493
#define INT32_ANGLE_FRAC
#define REF_RATE_FRAC
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:310
#define REF_ANGLE_FRAC
angular rates
signed long int32_t
Definition: types.h:19
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
void stabilization_attitude_ref_init(void)
General stabilization interface for rotorcrafts.
#define INT32_QUAT_INV_COMP(_b2c, _a2b, _a2c)
int32_t q
in rad/s^2 with INT32_RATE_FRAC
#define QUAT_ADD(_qo, _qi)
Definition: pprz_algebra.h:500
int32_t r
in rad/s^2 with INT32_RATE_FRAC
void stabilization_attitude_ref_update()
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
Rotorcraft attitude reference generation API.
#define INT32_EULERS_OF_QUAT(_e, _q)
struct Int32Rates stab_att_ref_accel
with REF_ACCEL_FRAC
#define INT32_QUAT_WRAP_SHORTEST(q)
#define RATES_SUM(_c, _a, _b)
Definition: pprz_algebra.h:324
euler angles