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.c
Go to the documentation of this file.
1 /*
2  * Paparazzi autopilot $Id$
3  *
4  * Copyright (C) 2004-2010 The Paparazzi Team
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 #include <inttypes.h>
30 #include <math.h>
31 
32 #include "estimator.h"
33 #include "mcu_periph/uart.h"
34 #include "ap_downlink.h"
35 #include "subsystems/gps.h"
36 #include "subsystems/nav.h"
37 #ifdef EXTRA_DOWNLINK_DEVICE
38 #include "core/extra_pprz_dl.h"
39 #endif
40 
41 /* position in meters */
45 
47 
48 /* attitude in radian */
52 
53 /* rates in radians per second */
57 
58 /* flight time in seconds */
60 /* flight time in seconds */
62 
63 /* horizontal speed in module and dir */
66 
67 /* wind */
71 
72 #define NORM_RAD_ANGLE2(x) { \
73  while (x > 2 * M_PI) x -= 2 * M_PI; \
74  while (x < 0 ) x += 2 * M_PI; \
75  }
76 
77 
78 #define EstimatorSetSpeedCart(vx, vy, vz) { \
79  estimator_vx = vx; \
80  estimator_vy = vy; \
81  estimator_vz = vz; \
82 }
83 
84 
85 void estimator_init( void ) {
86 
87  EstimatorSetPosXY(0., 0.);
88  EstimatorSetAlt(0.);
89 
90  EstimatorSetAtt (0., 0., 0);
91 
92  EstimatorSetSpeedPol ( 0., 0., 0.);
93 
94  EstimatorSetRate(0., 0., 0.);
95 
96 #ifdef USE_AOA
97  EstimatorSetAOA( 0. );
98 #endif
99 
101 
102  // FIXME? Set initial airspeed to zero if USE_AIRSPEED ?
103  EstimatorSetAirspeed( NOMINAL_AIRSPEED );
104 }
105 
106 
108 
109 #ifdef ALT_KALMAN
110 
111 #ifndef ALT_KALMAN_ENABLED
112 #define ALT_KALMAN_ENABLED FALSE
113 #endif
114 
115 #define GPS_SIGMA2 1.
116 #define GPS_DT 0.25
117 #define GPS_R 2.
118 
119 #define BARO_DT 0.1
120 
121 static float p[2][2];
122 
123 void alt_kalman_reset( void ) {
124  p[0][0] = 1.;
125  p[0][1] = 0.;
126  p[1][0] = 0.;
127  p[1][1] = 1.;
128 }
129 
130 void alt_kalman_init( void ) {
131  alt_kalman_enabled = ALT_KALMAN_ENABLED;
132  alt_kalman_reset();
133 }
134 
135 void alt_kalman(float gps_z) {
136  float DT;
137  float R;
138  float SIGMA2;
139 
140 #if USE_BARO_MS5534A
141  if (alt_baro_enabled) {
142  DT = BARO_DT;
143  R = baro_MS5534A_r;
144  SIGMA2 = baro_MS5534A_sigma2;
145  } else
146 #elif USE_BARO_ETS
147  if (baro_ets_enabled) {
148  DT = BARO_ETS_DT;
149  R = baro_ets_r;
150  SIGMA2 = baro_ets_sigma2;
151  } else
152 #elif USE_BARO_MS5611
153  if (baro_ms5611_enabled) {
154  DT = BARO_MS5611_DT;
155  R = baro_ms5611_r;
156  SIGMA2 = baro_ms5611_sigma2;
157  } else
158 #elif USE_BARO_BMP
159  if (baro_bmp_enabled) {
160  DT = BARO_BMP_DT;
161  R = baro_bmp_r;
162  SIGMA2 = baro_bmp_sigma2;
163  } else
164 #endif
165  {
166  DT = GPS_DT;
167  R = GPS_R;
168  SIGMA2 = GPS_SIGMA2;
169  }
170 
171  float q[2][2];
172  q[0][0] = DT*DT*DT*DT/4.;
173  q[0][1] = DT*DT*DT/2.;
174  q[1][0] = DT*DT*DT/2.;
175  q[1][1] = DT*DT;
176 
177 
178  /* predict */
180  p[0][0] = p[0][0]+p[1][0]*DT+DT*(p[0][1]+p[1][1]*DT) + SIGMA2*q[0][0];
181  p[0][1] = p[0][1]+p[1][1]*DT + SIGMA2*q[0][1];
182  p[1][0] = p[1][0]+p[1][1]*DT + SIGMA2*q[1][0];
183  p[1][1] = p[1][1] + SIGMA2*q[1][1];
184 
185  /* error estimate */
186  float e = p[0][0] + R;
187 
188  if (fabs(e) > 1e-5) {
189  float k_0 = p[0][0] / e;
190  float k_1 = p[1][0] / e;
191  e = gps_z - estimator_z;
192 
193  /* correction */
194  estimator_z += k_0 * e;
195  estimator_z_dot += k_1 * e;
196 
197  p[1][0] = -p[0][0]*k_1+p[1][0];
198  p[1][1] = -p[0][1]*k_1+p[1][1];
199  p[0][0] = p[0][0] * (1-k_0);
200  p[0][1] = p[0][1] * (1-k_0);
201  }
202 
203 #ifdef DEBUG_ALT_KALMAN
204  DOWNLINK_SEND_ALT_KALMAN(DefaultChannel,DefaultDevice,&(p[0][0]),&(p[0][1]),&(p[1][0]), &(p[1][1]));
205 #endif
206 }
207 
208 #endif // ALT_KALMAN
209 
211  float gps_east = gps.utm_pos.east / 100.;
212  float gps_north = gps.utm_pos.north / 100.;
213 
214  /* Relative position to reference */
215  gps_east -= nav_utm_east0;
216  gps_north -= nav_utm_north0;
217 
218  EstimatorSetPosXY(gps_east, gps_north);
219 #if !USE_BARO_BMP && !USE_BARO_ETS && !USE_BARO_MS5534A && !USE_BARO_MS5611
220  float falt = gps.hmsl / 1000.;
221  EstimatorSetAlt(falt);
222 #endif
223  float fspeed = gps.gspeed / 100.;
224  float fclimb = -gps.ned_vel.z / 100.;
225  float fcourse = gps.course / 1e7;
226  EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
227 
228  // Heading estimation now in ahrs_infrared
229 
230 }
unsigned short uint16_t
Definition: types.h:16
bool_t baro_ms5611_enabled
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:68
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
float baro_ms5611_sigma2
float estimator_theta
pitch angle in rad, + = up
Definition: estimator.c:51
float estimator_q
Definition: estimator.c:55
float estimator_hspeed_mod
module of horizontal ground speed in m/s
Definition: estimator.c:64
float estimator_r
Definition: estimator.c:56
int32_t course
GPS heading in rad*1e7 (CW/north)
Definition: gps.h:71
uint16_t estimator_flight_time
flight time in seconds.
Definition: estimator.c:59
float estimator_z_dot
Definition: estimator.c:46
float estimator_phi
roll angle in rad, + = right
Definition: estimator.c:49
#define EstimatorSetAtt(phi, psi, theta)
Definition: estimator.h:135
float estimator_y
north position in meters
Definition: estimator.c:43
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:66
void estimator_update_state_gps(void)
Definition: estimator.c:210
#define EstimatorSetAlt(z)
Definition: estimator.h:123
float baro_ets_r
Definition: baro_ets.c:71
bool_t alt_kalman_enabled
Definition: estimator.c:107
float estimator_p
Definition: estimator.c:54
#define EstimatorSetAirspeed(airspeed)
Definition: estimator.h:132
float estimator_hspeed_dir
direction of horizontal ground speed in rad (CW/North)
Definition: estimator.c:65
bool_t baro_bmp_enabled
Definition: baro_bmp.c:70
#define BARO_ETS_DT
Definition: baro_ets.h:44
#define EstimatorSetAOA(AOA)
Definition: estimator.h:133
int32_t nav_utm_north0
Definition: common_nav.c:42
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:69
float estimator_AOA
angle of attack in rad
Definition: estimator.c:70
float wind_east
Definition: estimator.c:68
Device independent GPS code (interface)
float estimator_airspeed
m/s
Definition: estimator.c:69
bool_t alt_baro_enabled
Definition: sim_baro.c:7
#define EstimatorSetSpeedPol(vhmod, vhdir, vz)
Definition: estimator.h:124
float baro_MS5534A_r
Definition: sim_baro.c:13
float estimator_x
east position in meters
Definition: estimator.c:42
float baro_bmp_sigma2
Definition: baro_bmp.c:72
float estimator_t
Definition: estimator.c:61
int32_t north
in centimeters
float baro_bmp_r
Definition: baro_bmp.c:71
#define BARO_BMP_DT
Definition: baro_bmp.h:37
bool_t baro_ets_enabled
Definition: baro_ets.c:70
#define BARO_MS5611_DT
int32_t east
in centimeters
#define EstimatorSetPosXY(x, y)
Definition: estimator.h:122
float estimator_z
altitude above MSL in meters
Definition: estimator.c:44
float baro_ets_sigma2
Definition: baro_ets.c:72
#define R
Definition: agl_vfilter.c:22
int32_t nav_utm_east0
Definition: common_nav.c:41
float estimator_psi
heading in rad, CW, 0 = N
Definition: estimator.c:50
State estimation, fusioning sensors.
float wind_north
Definition: estimator.c:68
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:65
float baro_MS5534A_sigma2
Definition: sim_baro.c:14
struct GpsState gps
global GPS state
Definition: gps.c:31
void estimator_init(void)
Definition: estimator.c:85
#define EstimatorSetRate(p, q, r)
Definition: estimator.h:138
float baro_ms5611_r