Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
stabilization_attitude_ref_euler_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 
28 #include "generated/airframe.h"
29 
34 
36 
41 
42 }
43 
44 #define F_UPDATE_RES 9
45 #define F_UPDATE (1<<F_UPDATE_RES)
46 
47 #define REF_ACCEL_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_PDOT, REF_ACCEL_FRAC)
48 #define REF_ACCEL_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_QDOT, REF_ACCEL_FRAC)
49 #define REF_ACCEL_MAX_R BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_RDOT, REF_ACCEL_FRAC)
50 
51 #define REF_RATE_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_P, REF_RATE_FRAC)
52 #define REF_RATE_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_Q, REF_RATE_FRAC)
53 #define REF_RATE_MAX_R BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_R, REF_RATE_FRAC)
54 
55 #define OMEGA_P STABILIZATION_ATTITUDE_REF_OMEGA_P
56 #define ZETA_P STABILIZATION_ATTITUDE_REF_ZETA_P
57 #define ZETA_OMEGA_P_RES 10
58 #define ZETA_OMEGA_P BFP_OF_REAL((ZETA_P*OMEGA_P), ZETA_OMEGA_P_RES)
59 #define OMEGA_2_P_RES 7
60 #define OMEGA_2_P BFP_OF_REAL((OMEGA_P*OMEGA_P), OMEGA_2_P_RES)
61 
62 #define OMEGA_Q STABILIZATION_ATTITUDE_REF_OMEGA_Q
63 #define ZETA_Q STABILIZATION_ATTITUDE_REF_ZETA_Q
64 #define ZETA_OMEGA_Q_RES 10
65 #define ZETA_OMEGA_Q BFP_OF_REAL((ZETA_Q*OMEGA_Q), ZETA_OMEGA_Q_RES)
66 #define OMEGA_2_Q_RES 7
67 #define OMEGA_2_Q BFP_OF_REAL((OMEGA_Q*OMEGA_Q), OMEGA_2_Q_RES)
68 
69 #define OMEGA_R STABILIZATION_ATTITUDE_REF_OMEGA_R
70 #define ZETA_R STABILIZATION_ATTITUDE_REF_ZETA_R
71 #define ZETA_OMEGA_R_RES 10
72 #define ZETA_OMEGA_R BFP_OF_REAL((ZETA_R*OMEGA_R), ZETA_OMEGA_R_RES)
73 #define OMEGA_2_R_RES 7
74 #define OMEGA_2_R BFP_OF_REAL((OMEGA_R*OMEGA_R), OMEGA_2_R_RES)
75 
76 
78 #ifndef USE_ATTITUDE_REF
79 #define USE_ATTITUDE_REF 1
80 #endif
81 
83 
84 #if USE_ATTITUDE_REF
85 
86  /* dumb integrate reference attitude */
87  const struct Int32Eulers d_angle = {
91  EULERS_ADD(stab_att_ref_euler, d_angle );
93 
94  /* integrate reference rotational speeds */
95  const struct Int32Rates d_rate = {
100 
101  /* attitude setpoint with REF_ANGLE_FRAC */
102  struct Int32Eulers sp_ref;
104 
105  /* compute reference attitude error */
106  struct Int32Eulers ref_err;
107  EULERS_DIFF(ref_err, stab_att_ref_euler, sp_ref);
108  /* wrap it in the shortest direction */
109  ANGLE_REF_NORMALIZE(ref_err.psi);
110 
111  /* compute reference angular accelerations */
112  const struct Int32Rates accel_rate = {
114  >> (ZETA_OMEGA_P_RES),
116  >> (ZETA_OMEGA_Q_RES),
118  >> (ZETA_OMEGA_R_RES) };
119 
120  const struct Int32Rates accel_angle = {
121  ((int32_t)(-OMEGA_2_P)* (ref_err.phi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES),
122  ((int32_t)(-OMEGA_2_Q)* (ref_err.theta >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES),
123  ((int32_t)(-OMEGA_2_R)* (ref_err.psi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) };
124 
125  RATES_SUM(stab_att_ref_accel, accel_rate, accel_angle);
126 
127  /* saturate acceleration */
128  const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
129  const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
130  RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
131 
132  /* saturate speed and trim accel accordingly */
134 
135 #else /* !USE_ATTITUDE_REF */
139 #endif /* USE_ATTITUDE_REF */
140 
141 }
void stabilization_attitude_ref_init(void)
int32_t phi
in rad with INT32_ANGLE_FRAC
#define EULERS_ADD(_a, _b)
Definition: pprz_algebra.h:247
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:261
int32_t p
in rad/s^2 with INT32_RATE_FRAC
General attitude stabilization interface for rotorcrafts.
struct Int32Eulers stab_att_ref_euler
with REF_ANGLE_FRAC
#define INT32_EULERS_LSHIFT(_o, _i, _r)
#define INT_RATES_ZERO(_e)
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
#define REF_ACCEL_FRAC
#define INT_EULERS_ZERO(_e)
#define INT32_ANGLE_FRAC
#define REF_RATE_FRAC
#define SATURATE_SPEED_TRIM_ACCEL()
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:308
#define REF_ANGLE_FRAC
angular rates
struct Int32Rates stab_att_ref_rate
with REF_RATE_FRAC
signed long int32_t
Definition: types.h:19
void stabilization_attitude_ref_update()
#define RATES_BOUND_BOX(_v, _v_min, _v_max)
Definition: pprz_algebra.h:371
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t q
in rad/s^2 with INT32_RATE_FRAC
int32_t r
in rad/s^2 with INT32_RATE_FRAC
#define ANGLE_REF_NORMALIZE(_a)
struct Int32Rates stab_att_ref_accel
with REF_ACCEL_FRAC
#define RATES_SUM(_c, _a, _b)
Definition: pprz_algebra.h:322
euler angles