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_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 
30 #include "generated/airframe.h"
31 
36 
38 
43 
44 }
45 
46 
47 /*
48  * Reference
49  */
50 #define DT_UPDATE (1./PERIODIC_FREQUENCY)
51 
52 #define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_REF_MAX_PDOT
53 #define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_QDOT
54 #define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_REF_MAX_RDOT
55 
56 #define REF_RATE_MAX_P STABILIZATION_ATTITUDE_REF_MAX_P
57 #define REF_RATE_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_Q
58 #define REF_RATE_MAX_R STABILIZATION_ATTITUDE_REF_MAX_R
59 
60 #define OMEGA_P STABILIZATION_ATTITUDE_REF_OMEGA_P
61 #define OMEGA_Q STABILIZATION_ATTITUDE_REF_OMEGA_Q
62 #define OMEGA_R STABILIZATION_ATTITUDE_REF_OMEGA_R
63 
64 #define ZETA_P STABILIZATION_ATTITUDE_REF_ZETA_P
65 #define ZETA_Q STABILIZATION_ATTITUDE_REF_ZETA_Q
66 #define ZETA_R STABILIZATION_ATTITUDE_REF_ZETA_R
67 
68 
69 #define USE_REF 1
70 
71 static inline void reset_psi_ref_from_body(void) {
72  //sp has been set from body using stabilization_attitude_get_yaw_f, use that value
74  stab_att_ref_rate.r = 0;
76 }
77 
79 {
81 }
82 
84 
85 #if USE_REF
86 
87  /* dumb integrate reference attitude */
88  struct FloatRates delta_rate;
90  struct FloatEulers delta_angle;
91  EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r);
92  EULERS_ADD(stab_att_ref_euler, delta_angle );
94 
95  /* integrate reference rotational speeds */
96  struct FloatRates delta_accel;
98  RATES_ADD(stab_att_ref_rate, delta_accel);
99 
100  /* compute reference attitude error */
101  struct FloatEulers ref_err;
103  /* wrap it in the shortest direction */
104  FLOAT_ANGLE_NORMALIZE(ref_err.psi);
105 
106  /* compute reference angular accelerations */
110 
111  /* saturate acceleration */
112  const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
113  const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
114  RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
115 
116  /* saturate speed and trim accel accordingly */
118 
119 #else /* !USE_REF */
120  EULERS_COPY(stab_att_ref_euler, stabilization_att_sp);
123 #endif /* USE_REF */
124 
125 }
#define FLOAT_RATES_ZERO(_r)
#define EULERS_ASSIGN(_e, _phi, _theta, _psi)
Definition: pprz_algebra.h:263
#define EULERS_ADD(_a, _b)
Definition: pprz_algebra.h:270
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:284
angular rates
Common rotorcraft attitude reference saturation include.
float psi
in radians
void stabilization_attitude_ref_enter()
#define SATURATE_SPEED_TRIM_ACCEL()
float theta
in radians
euler angles
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:368
float p
in rad/s
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
static void reset_psi_ref_from_body(void)
struct FloatEulers stab_att_ref_euler
with REF_ANGLE_FRAC
float phi
in radians
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:333
#define FLOAT_ANGLE_NORMALIZE(_a)
void stabilization_attitude_ref_init(void)
void stabilization_attitude_ref_update()
float r
in rad/s
#define FLOAT_EULERS_ZERO(_e)
#define RATES_BOUND_BOX(_v, _v_min, _v_max)
Definition: pprz_algebra.h:396
float q
in rad/s
struct FloatRates stab_att_ref_rate
with REF_RATE_FRAC
#define EULERS_COPY(_a, _b)
Definition: pprz_algebra.h:257
struct FloatRates stab_att_ref_accel
with REF_ACCEL_FRAC