Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
estimator.h
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin
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 
29 #ifndef ESTIMATOR_H
30 #define ESTIMATOR_H
31 
32 #include <inttypes.h>
33 
34 #include "std.h"
35 #ifdef BARO_MS5534A
36 #include "baro_MS5534A.h"
37 #endif
38 
39 #if USE_BARO_ETS
41 #endif
42 
43 #if USE_BARO_BMP
45 #endif
46 
47 /* position in meters, ENU frame, relative to reference */
48 extern float estimator_x;
49 extern float estimator_y;
50 extern float estimator_z;
51 
52 /* attitude in radians */
53 extern float estimator_phi;
54 extern float estimator_psi;
55 extern float estimator_theta;
56 
57 /* speed in meters per second */
58 extern float estimator_z_dot;
59 
60 /* rates in radians per second */
61 extern float estimator_p;
62 extern float estimator_q;
63 extern float estimator_r;
64 
67 extern float estimator_t;
68 
69 /* horizontal ground speed in module and dir (m/s, rad (CW/North)) */
70 extern float estimator_hspeed_mod;
71 extern float estimator_hspeed_dir;
72 
73 /* Wind and airspeed estimation sent by the GCS */
74 extern float wind_east, wind_north; /* m/s */
75 extern float estimator_airspeed;
76 
77 extern float estimator_AOA;
78 
79 void estimator_init( void );
80 
81 void estimator_update_state_gps( void );
82 
83 extern bool_t alt_kalman_enabled;
84 #ifdef ALT_KALMAN
85 extern void alt_kalman_reset( void );
86 extern void alt_kalman_init( void );
87 extern void alt_kalman( float );
88 #endif
89 
90 
91 #define GetPosX() (estimator_x)
92 #define GetPosY() (estimator_y)
93 #define GetPosAlt() (estimator_z)
94 
95 
96 #ifdef ALT_KALMAN
97 #define EstimatorSetPosXY(x, y) { estimator_x = x; estimator_y = y; }
98 
99 #if USE_BARO_MS5534A || USE_BARO_ETS || USE_BARO_BMP
100 /* Kalman filter cannot be disabled in this mode (no z_dot) */
101 #define EstimatorSetAlt(z) alt_kalman(z)
102 #else /* USE_BARO_x */
103 #define EstimatorSetAlt(z) { \
104  if (!alt_kalman_enabled) { \
105  estimator_z = z; \
106  } else { \
107  alt_kalman(z); \
108  } \
109 }
110 #endif /* ! USE_BARO_x */
111 
112 #define EstimatorSetSpeedPol(vhmod, vhdir, vz) { \
113  estimator_hspeed_mod = vhmod; \
114  estimator_hspeed_dir = vhdir; \
115  if (!alt_kalman_enabled) estimator_z_dot = vz; \
116 }
117 #else /* ALT_KALMAN */
118 #define EstimatorSetPosXY(x, y) { estimator_x = x; estimator_y = y; }
119 #define EstimatorSetAlt(z) { estimator_z = z; }
120 #define EstimatorSetSpeedPol(vhmod, vhdir, vz) { \
121  estimator_hspeed_mod = vhmod; \
122  estimator_hspeed_dir = vhdir; \
123  estimator_z_dot = vz; \
124 }
125 
126 #endif
127 
128 #define EstimatorSetAirspeed(airspeed) { estimator_airspeed = airspeed; }
129 #define EstimatorSetAOA(AOA) { estimator_AOA = AOA; }
130 
131 #define EstimatorSetAtt(phi, psi, theta) { estimator_phi = phi; estimator_psi = psi; estimator_theta = theta; }
132 #define EstimatorSetPhiPsi(phi, psi) { estimator_phi = phi; estimator_psi = psi; }
133 
134 #define EstimatorSetRate(p, q, r) { estimator_p = p; estimator_q = q; estimator_r = r; }
135 
136 
137 #endif /* ESTIMATOR_H */
float estimator_y
north position in meters
Definition: estimator.c:43
unsigned short uint16_t
Definition: types.h:16
bool_t alt_kalman_enabled
Definition: estimator.c:107
float estimator_hspeed_dir
direction of horizontal ground speed in rad (CW/North)
Definition: estimator.c:65
float estimator_t
Definition: estimator.c:61
float estimator_psi
heading in rad, CW, 0 = N
Definition: estimator.c:50
float wind_north
Definition: estimator.c:68
uint16_t estimator_flight_time
flight time in seconds.
Definition: estimator.c:59
float estimator_x
east position in meters
Definition: estimator.c:42
float estimator_hspeed_mod
module of horizontal ground speed in m/s
Definition: estimator.c:64
void estimator_update_state_gps(void)
Definition: estimator.c:204
float estimator_phi
roll angle in rad, + = right
Definition: estimator.c:49
float estimator_theta
pitch angle in rad, + = up
Definition: estimator.c:51
float estimator_q
Definition: estimator.c:55
float estimator_r
Definition: estimator.c:56
float estimator_p
Definition: estimator.c:54
float estimator_z_dot
Definition: estimator.c:46
float estimator_airspeed
m/s
Definition: estimator.c:69
void estimator_init(void)
Definition: estimator.c:85
float estimator_AOA
angle of attack in rad
Definition: estimator.c:70
float wind_east
Definition: estimator.c:68
float estimator_z
altitude above MSL in meters
Definition: estimator.c:44