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_euler_float.h
Go to the documentation of this file.
1 /*
2  * $Id: stabilization_attitude_ref_traj_euler.h 3796 2009-07-25 00:01:02Z poine $
3  *
4  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  */
23 
24 #ifndef STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H
25 #define STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H
26 
29 
30 
32 
33 /*
34  * Radio Control
35  */
36 #define SP_MAX_PHI STABILIZATION_ATTITUDE_FLOAT_SP_MAX_PHI
37 #define SP_MAX_THETA STABILIZATION_ATTITUDE_FLOAT_SP_MAX_THETA
38 #define SP_MAX_R STABILIZATION_ATTITUDE_FLOAT_SP_MAX_R
39 
40 
41 
42 
43 
44 #define RC_UPDATE_FREQ 40.
45 
46 #define YAW_DEADBAND_EXCEEDED() \
47  (radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_FLOAT_DEADBAND_R || \
48  radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_FLOAT_DEADBAND_R)
49 
50 #define STABILIZATION_ATTITUDE_FLOAT_READ_RC(_sp, _inflight) { \
51  \
52  _sp.phi = (radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ); \
53  _sp.theta = (radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ); \
54  if (_inflight) { \
55  if (YAW_DEADBAND_EXCEEDED()) { \
56  _sp.psi += \
57  (radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); \
58  FLOAT_ANGLE_NORMALIZE(_sp.psi); \
59  } \
60  } \
61  else { /* if not flying, use current yaw as setpoint */ \
62  _sp.psi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi); \
63  } \
64  }
65 
66 #define STABILIZATION_ATTITUDE_FLOAT_ADD_SP(_add_sp) { \
67  struct FloatEulers add_sp_float; \
68  EULERS_FLOAT_OF_BFP(add_sp_float, (_add_sp)); \
69  EULERS_ADD(stabilization_att_sp,add_sp_float); \
70  FLOAT_ANGLE_NORMALIZE(stabilization_att_sp.psi); \
71  }
72 
73 
74 
75 #endif /* STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H */
Paparazzi floating point algebra.