Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ins_alt_float.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2004-2012 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
28 
29 #include "subsystems/abi.h"
30 
31 #include <inttypes.h>
32 #include <math.h>
33 
34 #include "state.h"
35 #include "subsystems/gps.h"
37 
38 #include "generated/airframe.h"
39 #include "generated/modules.h"
40 
41 #ifdef DEBUG_ALT_KALMAN
42 #include "mcu_periph/uart.h"
44 #endif
45 
46 #if defined ALT_KALMAN || defined ALT_KALMAN_ENABLED
47 #warning Please remove the obsolete ALT_KALMAN and ALT_KALMAN_ENABLED defines from your airframe file.
48 #endif
49 
50 
52 
53 #if USE_BAROMETER
55 #include "math/pprz_isa.h"
56 
57 PRINT_CONFIG_MSG("USE_BAROMETER is TRUE: Using baro for altitude estimation.")
58 
59 // Baro event on ABI
60 #ifndef INS_BARO_ID
61 #if USE_BARO_BOARD
62 #define INS_BARO_ID BARO_BOARD_SENDER_ID
63 #else
64 #define INS_BARO_ID ABI_BROADCAST
65 #endif
66 #endif
69 static void baro_cb(uint8_t sender_id, const float *pressure);
70 #endif /* USE_BAROMETER */
71 
72 static void alt_kalman_reset(void);
73 static void alt_kalman_init(void);
74 static void alt_kalman(float);
75 
76 void ins_init(void) {
77 
80 
81  stateSetPositionUtm_f(&utm0);
82 
84 
85 #if USE_BAROMETER
86  ins_impl.qfe = 0.0f;
88  ins_impl.baro_alt = 0.0f;
89  // Bind to BARO_ABS message
90  AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
91 #endif
93 
94  alt_kalman(0.0f);
95 
97 }
98 
99 
102  struct UtmCoor_f utm;
103 #ifdef GPS_USE_LATLONG
104  /* Recompute UTM coordinates in this zone */
105  struct LlaCoor_f lla;
107  utm.zone = (gps.lla_pos.lon/1e7 + 180) / 6 + 1;
108  utm_of_lla_f(&utm, &lla);
109 #else
110  utm.zone = gps.utm_pos.zone;
111  utm.east = gps.utm_pos.east / 100.0f;
112  utm.north = gps.utm_pos.north / 100.0f;
113 #endif
114  // ground_alt
115  utm.alt = gps.hmsl /1000.0f;
116 
117  // reset state UTM ref
119 
120  // reset filter flag
122 }
123 
125  struct UtmCoor_f utm = state.utm_origin_f;
126  // ground_alt
127  utm.alt = gps.hmsl / 1000.0f;
128  // reset state UTM ref
130  // reset filter flag
132 }
133 
134 
135 #if USE_BAROMETER
136 static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) {
137  if (!ins_impl.baro_initialized) {
138  ins_impl.qfe = *pressure;
140  }
141  if (ins_impl.reset_alt_ref) {
144  ins_impl.alt_dot = 0.0f;
145  ins_impl.qfe = *pressure;
147  }
148  else { /* not realigning, so normal update with baro measurement */
150  /* run the filter */
152  /* set new altitude, just copy old horizontal position */
153  struct UtmCoor_f utm;
155  utm.alt = ins_impl.alt;
156  stateSetPositionUtm_f(&utm);
157  struct NedCoor_f ned_vel;
158  memcpy(&ned_vel, stateGetSpeedNed_f(), sizeof(struct NedCoor_f));
159  ned_vel.z = -ins_impl.alt_dot;
160  stateSetSpeedNed_f(&ned_vel);
161  }
162 }
163 #endif
164 
165 
166 void ins_update_gps(void) {
167 #if USE_GPS
168  struct UtmCoor_f utm;
169  utm.east = gps.utm_pos.east / 100.0f;
170  utm.north = gps.utm_pos.north / 100.0f;
171  utm.zone = nav_utm_zone0;
172 
173 #if !USE_BAROMETER
174  float falt = gps.hmsl / 1000.0f;
175  if (ins_impl.reset_alt_ref) {
177  ins_impl.alt = falt;
178  ins_impl.alt_dot = 0.0f;
180  }
181  else {
182  alt_kalman(falt);
183  ins_impl.alt_dot = -gps.ned_vel.z / 100.0f;
184  }
185 #endif
186  utm.alt = ins_impl.alt;
187  // set position
188  stateSetPositionUtm_f(&utm);
189 
190  struct NedCoor_f ned_vel = {
191  gps.ned_vel.x / 100.0f,
192  gps.ned_vel.y / 100.0f,
194  };
195  // set velocity
196  stateSetSpeedNed_f(&ned_vel);
197 
198 #endif
199 }
200 
201 
202 #ifndef GPS_DT
203 #define GPS_DT 0.25
204 #endif
205 #define GPS_SIGMA2 1.
206 #define GPS_R 2.
207 
208 static float p[2][2];
209 
210 static void alt_kalman_reset(void) {
211  p[0][0] = 1.0f;
212  p[0][1] = 0.0f;
213  p[1][0] = 0.0f;
214  p[1][1] = 1.0f;
215 }
216 
217 static void alt_kalman_init(void) {
219 }
220 
221 static void alt_kalman(float z_meas) {
222  float DT = GPS_DT;
223  float R = GPS_R;
224  float SIGMA2 = GPS_SIGMA2;
225 
226 #if USE_BAROMETER
227 #ifdef SITL
228  // stupid hack for nps, we need to get rid of all these DTs
229 #ifndef BARO_SIM_DT
230 #define BARO_SIM_DT (1./50.)
231 #endif
232  DT = BARO_SIM_DT;
233  R = 0.5;
234  SIGMA2 = 0.1;
235 #elif USE_BARO_BOARD
236 #ifndef BARO_PERIODIC_FREQUENCY
237 #define BARO_PERIODIC_FREQUENCY 50
238 #endif
239  DT = (1./BARO_PERIODIC_FREQUENCY);
240  R = 0.5;
241  SIGMA2 = 0.1;
242 #elif USE_BARO_MS5534A
243  if (alt_baro_enabled) {
244  DT = 0.1;
245  R = baro_MS5534A_r;
246  SIGMA2 = baro_MS5534A_sigma2;
247  }
248 #elif USE_BARO_ETS
249  if (baro_ets_enabled) {
250  DT = BARO_ETS_DT;
251  R = baro_ets_r;
252  SIGMA2 = baro_ets_sigma2;
253  }
254 #elif USE_BARO_MS5611
255  if (baro_ms5611_enabled) {
256  DT = BARO_MS5611_DT;
257  R = baro_ms5611_r;
258  SIGMA2 = baro_ms5611_sigma2;
259  }
260 #elif USE_BARO_AMSYS
261  if (baro_amsys_enabled) {
262  DT = BARO_AMSYS_DT;
263  R = baro_amsys_r;
264  SIGMA2 = baro_amsys_sigma2;
265  }
266 #elif USE_BARO_BMP
267  if (baro_bmp_enabled) {
268  DT = BARO_BMP_DT;
269  R = baro_bmp_r;
270  SIGMA2 = baro_bmp_sigma2;
271  }
272 #endif
273 #endif // USE_BAROMETER
274 
275  float q[2][2];
276  q[0][0] = DT*DT*DT*DT/4.;
277  q[0][1] = DT*DT*DT/2.;
278  q[1][0] = DT*DT*DT/2.;
279  q[1][1] = DT*DT;
280 
281 
282  /* predict */
283  ins_impl.alt += ins_impl.alt_dot * DT;
284  p[0][0] = p[0][0]+p[1][0]*DT+DT*(p[0][1]+p[1][1]*DT) + SIGMA2*q[0][0];
285  p[0][1] = p[0][1]+p[1][1]*DT + SIGMA2*q[0][1];
286  p[1][0] = p[1][0]+p[1][1]*DT + SIGMA2*q[1][0];
287  p[1][1] = p[1][1] + SIGMA2*q[1][1];
288 
289  /* error estimate */
290  float e = p[0][0] + R;
291 
292  if (fabs(e) > 1e-5) {
293  float k_0 = p[0][0] / e;
294  float k_1 = p[1][0] / e;
295  e = z_meas - ins_impl.alt;
296 
297  /* correction */
298  ins_impl.alt += k_0 * e;
299  ins_impl.alt_dot += k_1 * e;
300 
301  p[1][0] = -p[0][0]*k_1+p[1][0];
302  p[1][1] = -p[0][1]*k_1+p[1][1];
303  p[0][0] = p[0][0] * (1-k_0);
304  p[0][1] = p[0][1] * (1-k_0);
305  }
306 
307 #ifdef DEBUG_ALT_KALMAN
308  DOWNLINK_SEND_ALT_KALMAN(DefaultChannel,DefaultDevice,&(p[0][0]),&(p[0][1]),&(p[1][0]), &(p[1][1]));
309 #endif
310 }
311 
float alt_dot
estimated vertical speed in m/s (positive-up)
Definition: ins_alt_float.h:39
struct Ins ins
global INS state
Definition: ins.c:36
bool_t baro_ms5611_enabled
#define UTM_COPY(_u1, _u2)
Definition: pprz_geodetic.h:30
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
void ins_reset_local_origin(void)
Reset the geographic reference to the current GPS fix.
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:69
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
float baro_ms5611_sigma2
#define INS_BARO_ID
Definition: ins_alt_float.c:64
static void alt_kalman(float)
Common barometric sensor implementation.
void ins_update_gps(void)
Update INS state with GPS measurements.
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:40
uint8_t zone
UTM zone number.
bool_t reset_alt_ref
flag to request reset of altitude reference to current alt
Definition: ins_alt_float.h:41
static void baro_cb(uint8_t sender_id, const float *pressure)
enum InsStatus status
status of the INS
Definition: ins.h:47
float baro_MS5534A_r
Definition: baro_MS5534A.c:47
abi_event baro_ev
Definition: ins_alt_float.c:68
Main include for ABI (AirBorneInterface).
#define GPS_R
int32_t x
North.
uint8_t nav_utm_zone0
Definition: common_nav.c:44
static float pprz_isa_height_of_pressure(float pressure, float ref)
Get relative altitude from pressure (using simplified equation).
Definition: pprz_isa.h:73
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
void ins_init(void)
INS initialization.
Definition: ins_alt_float.c:76
static void alt_kalman_init(void)
float baro_ets_r
Definition: baro_ets.c:108
vector in Latitude, Longitude and Altitude
bool_t baro_bmp_enabled
Definition: baro_bmp.c:54
#define FALSE
Definition: imu_chimu.h:141
float baro_amsys_r
Definition: baro_amsys.c:78
#define BARO_ETS_DT
new measurement every baro_ets_read_periodic
Definition: baro_ets.h:49
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:555
int32_t z
Down.
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:754
bool_t alt_baro_enabled
Definition: baro_MS5534A.c:45
uint8_t zone
UTM zone number.
int32_t nav_utm_north0
Definition: common_nav.c:43
vector in North East Down coordinates Units: meters
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:833
Ins implementation state (altitude, float)
Definition: ins_alt_float.h:37
float north
in meters
Device independent GPS code (interface)
Paparazzi atmospheric pressure convertion utilities.
float z
in meters
position in UTM coordinates Units: meters
int32_t y
East.
float baro_bmp_sigma2
Definition: baro_bmp.c:56
int32_t north
in centimeters
float baro_bmp_r
Definition: baro_bmp.c:55
float baro_amsys_sigma2
Definition: baro_amsys.c:79
float baro_MS5534A_sigma2
Definition: baro_MS5534A.c:48
#define BARO_AMSYS_DT
new measurement every baro_amsys_read_periodic
Definition: baro_amsys.h:32
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:228
#define BARO_BMP_DT
new measurement every 3rd baro_bmp_periodic
Definition: baro_bmp.h:39
#define TRUE
Definition: imu_chimu.h:144
bool_t baro_amsys_enabled
Definition: baro_amsys.c:77
bool_t baro_ets_enabled
Definition: baro_ets.c:107
#define GPS_SIGMA2
#define BARO_MS5611_DT
new measurement with every baro_ms5611_read() call
int32_t east
in centimeters
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:651
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
float baro_ets_sigma2
Definition: baro_ets.c:109
int32_t lon
in degrees*1e7
int32_t nav_utm_east0
Definition: common_nav.c:42
#define LLA_FLOAT_OF_BFP(_o, _i)
bool_t baro_initialized
Definition: ins_alt_float.h:46
static float p[2][2]
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:66
Filters altitude and climb rate for fixedwings.
#define BARO_PERIODIC_FREQUENCY
Definition: main.c:101
float east
in meters
static void alt_kalman_reset(void)
float baro_alt
Definition: ins_alt_float.h:45
struct InsAltFloat ins_impl
global INS state
Definition: ins_alt_float.c:51
float alt
in meters above WGS84 reference ellipsoid
void ins_reset_altitude_ref(void)
INS altitude reference reset.
#define BARO_SIM_DT
new measurement every baro_sim_periodic
Definition: baro_sim.h:32
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:459
struct GpsState gps
global GPS state
Definition: gps.c:41
float alt
estimated altitude above MSL in meters
Definition: ins_alt_float.h:38
struct State state
Definition: state.c:36
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
#define GPS_DT
float baro_ms5611_r
Event structure to store callbacks in a linked list.
Definition: abi_common.h:58