Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
stabilization_attitude_ref_euler_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 
28 #include "generated/airframe.h"
29 
32 
33 static inline void reset_psi_ref(struct AttRefEulerFloat *ref, float psi);
34 
35 /*
36  *
37  * Implementation.
38  * Should not rely on any global variables, so these functions can be used like a lib.
39  *
40  */
42 {
43  FLOAT_EULERS_ZERO(ref->euler);
44  FLOAT_RATES_ZERO(ref->rate);
45  FLOAT_RATES_ZERO(ref->accel);
46 
47  ref->model.omega.p = STABILIZATION_ATTITUDE_REF_OMEGA_P;
48  ref->model.omega.q = STABILIZATION_ATTITUDE_REF_OMEGA_Q;
49  ref->model.omega.r = STABILIZATION_ATTITUDE_REF_OMEGA_R;
50  ref->model.zeta.p = STABILIZATION_ATTITUDE_REF_ZETA_P;
51  ref->model.zeta.q = STABILIZATION_ATTITUDE_REF_ZETA_Q;
52  ref->model.zeta.r = STABILIZATION_ATTITUDE_REF_ZETA_R;
53 
54  ref->saturation.max_rate.p = STABILIZATION_ATTITUDE_REF_MAX_P;
55  ref->saturation.max_rate.q = STABILIZATION_ATTITUDE_REF_MAX_Q;
56  ref->saturation.max_rate.r = STABILIZATION_ATTITUDE_REF_MAX_R;
57  ref->saturation.max_accel.p = STABILIZATION_ATTITUDE_REF_MAX_PDOT;
58  ref->saturation.max_accel.q = STABILIZATION_ATTITUDE_REF_MAX_QDOT;
59  ref->saturation.max_accel.r = STABILIZATION_ATTITUDE_REF_MAX_RDOT;
60 }
61 
63 {
64  reset_psi_ref(ref, psi);
65 }
66 
67 void attitude_ref_euler_float_update(struct AttRefEulerFloat *ref, struct FloatEulers *sp_eulers, float dt)
68 {
69 
70  /* dumb integrate reference attitude */
71  struct FloatRates delta_rate;
72  RATES_SMUL(delta_rate, ref->rate, dt);
73  struct FloatEulers delta_angle;
74  EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r);
75  EULERS_ADD(ref->euler, delta_angle);
76  FLOAT_ANGLE_NORMALIZE(ref->euler.psi);
77 
78  /* integrate reference rotational speeds */
79  struct FloatRates delta_accel;
80  RATES_SMUL(delta_accel, ref->accel, dt);
81  RATES_ADD(ref->rate, delta_accel);
82 
83  /* compute reference attitude error */
84  struct FloatEulers ref_err;
85  EULERS_DIFF(ref_err, ref->euler, *sp_eulers);
86  /* wrap it in the shortest direction */
87  FLOAT_ANGLE_NORMALIZE(ref_err.psi);
88 
89  /* compute reference angular accelerations -2*zeta*omega*rate - omega*omega*ref_err */
90  ref->accel.p = -2. * ref->model.zeta.p * ref->model.omega.p * ref->rate.p -
91  ref->model.omega.p * ref->model.omega.p * ref_err.phi;
92  ref->accel.q = -2. * ref->model.zeta.q * ref->model.omega.p * ref->rate.q -
93  ref->model.omega.q * ref->model.omega.q * ref_err.theta;
94  ref->accel.r = -2. * ref->model.zeta.r * ref->model.omega.p * ref->rate.r -
95  ref->model.omega.r * ref->model.omega.r * ref_err.psi;
96 
97  /* saturate acceleration */
98  attitude_ref_float_saturate_naive(&ref->rate, &ref->accel, &ref->saturation);
99 }
100 
101 /*
102  *
103  * Local helper functions.
104  *
105  */
106 static inline void reset_psi_ref(struct AttRefEulerFloat *ref, float psi)
107 {
108  ref->euler.psi = psi;
109  ref->rate.r = 0;
110  ref->accel.r = 0;
111 }
static void attitude_ref_float_saturate_naive(struct FloatRates *rate, struct FloatRates *accel, struct FloatRefSat *sat)
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
float psi
in radians
#define FLOAT_ANGLE_NORMALIZE(_a)
#define FLOAT_EULERS_ZERO(_e)
#define FLOAT_RATES_ZERO(_r)
euler angles
angular rates
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:379
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:295
#define EULERS_ADD(_a, _b)
Definition: pprz_algebra.h:281
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:344
#define EULERS_ASSIGN(_e, _phi, _theta, _psi)
Definition: pprz_algebra.h:274
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 void reset_psi_ref(struct AttRefEulerFloat *ref, float psi)
void attitude_ref_euler_float_enter(struct AttRefEulerFloat *ref, float psi)
void attitude_ref_euler_float_update(struct AttRefEulerFloat *ref, struct FloatEulers *sp_eulers, float dt)
void attitude_ref_euler_float_init(struct AttRefEulerFloat *ref)
Attitude reference state/output (euler float)
uint16_t ref[TCOUPLE_NB]