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
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 #if USE_BARO_MS5611
49 #endif
50 
51 /* position in meters, ENU frame, relative to reference */
52 extern float estimator_x;
53 extern float estimator_y;
54 extern float estimator_z;
55 
56 /* attitude in radians */
57 extern float estimator_phi;
58 extern float estimator_psi;
59 extern float estimator_theta;
60 
61 /* speed in meters per second */
62 extern float estimator_z_dot;
63 
64 /* rates in radians per second */
65 extern float estimator_p;
66 extern float estimator_q;
67 extern float estimator_r;
68 
71 extern float estimator_t;
72 
73 /* horizontal ground speed in module and dir (m/s, rad (CW/North)) */
74 extern float estimator_hspeed_mod;
75 extern float estimator_hspeed_dir;
76 
77 /* Wind and airspeed estimation sent by the GCS */
78 extern float wind_east, wind_north; /* m/s */
79 extern float estimator_airspeed;
80 
81 extern float estimator_AOA;
82 
83 void estimator_init( void );
84 
85 void estimator_update_state_gps( void );
86 
87 extern bool_t alt_kalman_enabled;
88 #ifdef ALT_KALMAN
89 extern void alt_kalman_reset( void );
90 extern void alt_kalman_init( void );
91 extern void alt_kalman( float );
92 #endif
93 
94 
95 #define GetPosX() (estimator_x)
96 #define GetPosY() (estimator_y)
97 #define GetPosAlt() (estimator_z)
98 
99 
100 #ifdef ALT_KALMAN
101 #define EstimatorSetPosXY(x, y) { estimator_x = x; estimator_y = y; }
102 
103 #if USE_BARO_MS5534A || USE_BARO_ETS || USE_BARO_BMP || USE_BARO_MS5611
104 /* Kalman filter cannot be disabled in this mode (no z_dot) */
105 #define EstimatorSetAlt(z) alt_kalman(z)
106 #else /* USE_BARO_x */
107 #define EstimatorSetAlt(z) { \
108  if (!alt_kalman_enabled) { \
109  estimator_z = z; \
110  } else { \
111  alt_kalman(z); \
112  } \
113 }
114 #endif /* ! USE_BARO_x */
115 
116 #define EstimatorSetSpeedPol(vhmod, vhdir, vz) { \
117  estimator_hspeed_mod = vhmod; \
118  estimator_hspeed_dir = vhdir; \
119  if (!alt_kalman_enabled) estimator_z_dot = vz; \
120 }
121 #else /* ALT_KALMAN */
122 #define EstimatorSetPosXY(x, y) { estimator_x = x; estimator_y = y; }
123 #define EstimatorSetAlt(z) { estimator_z = z; }
124 #define EstimatorSetSpeedPol(vhmod, vhdir, vz) { \
125  estimator_hspeed_mod = vhmod; \
126  estimator_hspeed_dir = vhdir; \
127  estimator_z_dot = vz; \
128 }
129 
130 #endif
131 
132 #define EstimatorSetAirspeed(airspeed) { estimator_airspeed = airspeed; }
133 #define EstimatorSetAOA(AOA) { estimator_AOA = AOA; }
134 
135 #define EstimatorSetAtt(phi, psi, theta) { estimator_phi = phi; estimator_psi = psi; estimator_theta = theta; }
136 #define EstimatorSetPhiPsi(phi, psi) { estimator_phi = phi; estimator_psi = psi; }
137 
138 #define EstimatorSetRate(p, q, r) { estimator_p = p; estimator_q = q; estimator_r = r; }
139 
140 
141 #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:210
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