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.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_BMP
153  if (baro_bmp_enabled) {
154  DT = BARO_BMP_DT;
155  R = baro_bmp_r;
156  SIGMA2 = baro_bmp_sigma2;
157  } else
158 #endif
159  {
160  DT = GPS_DT;
161  R = GPS_R;
162  SIGMA2 = GPS_SIGMA2;
163  }
164 
165  float q[2][2];
166  q[0][0] = DT*DT*DT*DT/4.;
167  q[0][1] = DT*DT*DT/2.;
168  q[1][0] = DT*DT*DT/2.;
169  q[1][1] = DT*DT;
170 
171 
172  /* predict */
174  p[0][0] = p[0][0]+p[1][0]*DT+DT*(p[0][1]+p[1][1]*DT) + SIGMA2*q[0][0];
175  p[0][1] = p[0][1]+p[1][1]*DT + SIGMA2*q[0][1];
176  p[1][0] = p[1][0]+p[1][1]*DT + SIGMA2*q[1][0];
177  p[1][1] = p[1][1] + SIGMA2*q[1][1];
178 
179  /* error estimate */
180  float e = p[0][0] + R;
181 
182  if (fabs(e) > 1e-5) {
183  float k_0 = p[0][0] / e;
184  float k_1 = p[1][0] / e;
185  e = gps_z - estimator_z;
186 
187  /* correction */
188  estimator_z += k_0 * e;
189  estimator_z_dot += k_1 * e;
190 
191  p[1][0] = -p[0][0]*k_1+p[1][0];
192  p[1][1] = -p[0][1]*k_1+p[1][1];
193  p[0][0] = p[0][0] * (1-k_0);
194  p[0][1] = p[0][1] * (1-k_0);
195  }
196 
197 #ifdef DEBUG_ALT_KALMAN
198  DOWNLINK_SEND_ALT_KALMAN(DefaultChannel,DefaultDevice,&(p[0][0]),&(p[0][1]),&(p[1][0]), &(p[1][1]));
199 #endif
200 }
201 
202 #endif // ALT_KALMAN
203 
205  float gps_east = gps.utm_pos.east / 100.;
206  float gps_north = gps.utm_pos.north / 100.;
207 
208  /* Relative position to reference */
209  gps_east -= nav_utm_east0;
210  gps_north -= nav_utm_north0;
211 
212  EstimatorSetPosXY(gps_east, gps_north);
213 #if !USE_BARO_BMP && !USE_BARO_ETS && !USE_BARO_MS5534A
214  float falt = gps.hmsl / 1000.;
215  EstimatorSetAlt(falt);
216 #endif
217  float fspeed = gps.gspeed / 100.;
218  float fclimb = -gps.ned_vel.z / 100.;
219  float fcourse = gps.course / 1e7;
220  EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
221 
222  // Heading estimation now in ahrs_infrared
223 
224 }
unsigned short uint16_t
Definition: types.h:16
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:68
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
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:131
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:204
#define EstimatorSetAlt(z)
Definition: estimator.h:119
float baro_ets_r
Definition: baro_ets.c:69
bool_t alt_kalman_enabled
Definition: estimator.c:107
float estimator_p
Definition: estimator.c:54
#define EstimatorSetAirspeed(airspeed)
Definition: estimator.h:128
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:129
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:120
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:68
int32_t east
in centimeters
#define EstimatorSetPosXY(x, y)
Definition: estimator.h:118
float estimator_z
altitude above MSL in meters
Definition: estimator.c:44
float baro_ets_sigma2
Definition: baro_ets.c:70
#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:134