Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/core/abi.h"
30 #include "state.h"
31 
32 #include <inttypes.h>
33 #include <math.h>
34 
35 #include "state.h"
36 #include "mcu_periph/sys_time.h"
37 #include "modules/gps/gps.h"
39 
40 #include "generated/airframe.h"
41 #include "generated/modules.h"
42 
43 #ifdef DEBUG_ALT_KALMAN
44 #include "mcu_periph/uart.h"
46 #endif
47 
48 #ifndef USE_INS_NAV_INIT
49 #define USE_INS_NAV_INIT TRUE
50 PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
51 #endif
52 
54 
55 #if USE_BAROMETER
56 #include "modules/sensors/baro.h"
57 #include "math/pprz_isa.h"
58 
59 PRINT_CONFIG_MSG("USE_BAROMETER is TRUE: Using baro for altitude estimation.")
60 
61 // Baro event on ABI
62 #ifndef INS_ALT_BARO_ID
63 #if USE_BARO_BOARD
64 #define INS_ALT_BARO_ID BARO_BOARD_SENDER_ID
65 #else
66 #define INS_ALT_BARO_ID ABI_BROADCAST
67 #endif
68 #endif
69 PRINT_CONFIG_VAR(INS_ALT_BARO_ID)
70 
72 static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure);
73 #endif /* USE_BAROMETER */
74 
78 #ifndef INS_ALT_GPS_ID
79 #define INS_ALT_GPS_ID GPS_MULTI_ID
80 #endif
81 PRINT_CONFIG_VAR(INS_ALT_GPS_ID)
83 static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
84 
85 #ifndef INS_ALT_IMU_ID
86 #define INS_ALT_IMU_ID ABI_BROADCAST
87 #endif
89 static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
90 
91 static void alt_kalman_reset(void);
92 static void alt_kalman_init(void);
93 static void alt_kalman(float z_meas, float dt);
94 
95 void ins_alt_float_update_gps(struct GpsState *gps_s);
96 
98 {
99 #if USE_INS_NAV_INIT
103 
104  stateSetPositionUtm_f(&utm0);
105 #else
107 #endif
108 
109  alt_kalman_init();
110 
111 #if USE_BAROMETER
112  ins_altf.qfe = 0.0f;
113  ins_altf.baro_initialized = false;
114  ins_altf.baro_alt = 0.0f;
115 #endif
116  ins_altf.reset_alt_ref = false;
117 
118  // why do we have this here?
119  alt_kalman(0.0f, 0.1);
120 
121 #if USE_BAROMETER
122  // Bind to BARO_ABS message
123  AbiBindMsgBARO_ABS(INS_ALT_BARO_ID, &baro_ev, baro_cb);
124 #endif
125  AbiBindMsgGPS(INS_ALT_GPS_ID, &gps_ev, gps_cb);
126  AbiBindMsgIMU_ACCEL(INS_ALT_IMU_ID, &accel_ev, accel_cb);
127 }
128 
131 {
132  // get utm pos
133  struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
134 
135  // reset state UTM ref
137 
139 
140  // reset filter flag
141  ins_altf.reset_alt_ref = true;
142 }
143 
145 {
146  struct UtmCoor_f utm = state.utm_origin_f;
147  // ground_alt
148  utm.alt = gps.hmsl / 1000.0f;
149  // reset state UTM ref
151  // reset filter flag
152  ins_altf.reset_alt_ref = true;
153 }
154 
155 #if USE_BAROMETER
156 void ins_alt_float_update_baro(float pressure)
157 {
158  // timestamp in usec when last callback was received
159  static uint32_t last_ts = 0;
160  // current timestamp
161  uint32_t now_ts = get_sys_time_usec();
162  // dt between this and last callback in seconds
163  float dt = (float)(now_ts - last_ts) / 1e6;
164  last_ts = now_ts;
165 
166  // bound dt (assume baro freq 1Hz-500Hz
167  Bound(dt, 0.002, 1.0)
168 
169  if (!ins_altf.baro_initialized) {
170  ins_altf.qfe = pressure;
171  ins_altf.baro_initialized = true;
172  }
173  if (ins_altf.reset_alt_ref) {
174  ins_altf.reset_alt_ref = false;
176  ins_altf.alt_dot = 0.0f;
177  ins_altf.qfe = pressure;
179  } else { /* not realigning, so normal update with baro measurement */
181  /* run the filter */
183  /* set new altitude, just copy old horizontal position */
184  struct UtmCoor_f utm;
186  utm.alt = ins_altf.alt;
187  stateSetPositionUtm_f(&utm);
188  struct NedCoor_f ned_vel;
189  ned_vel = *stateGetSpeedNed_f();
190  ned_vel.z = -ins_altf.alt_dot;
191  stateSetSpeedNed_f(&ned_vel);
192  }
193 }
194 #else
195 void ins_alt_float_update_baro(float pressure __attribute__((unused)))
196 {
197 }
198 #endif
199 
200 
201 void ins_alt_float_update_gps(struct GpsState *gps_s __attribute__((unused)))
202 {
203 #if USE_GPS
204  if (gps_s->fix < GPS_FIX_3D) {
205  return;
206  }
207 
210  }
211 
212  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
213  struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s);
214 
215 #if !USE_BAROMETER
216 #ifdef GPS_DT
217  const float dt = GPS_DT;
218 #else
219  // timestamp in usec when last callback was received
220  static uint32_t last_ts = 0;
221  // current timestamp
222  uint32_t now_ts = get_sys_time_usec();
223  // dt between this and last callback in seconds
224  float dt = (float)(now_ts - last_ts) / 1e6;
225  last_ts = now_ts;
226 
227  // bound dt (assume GPS freq between 0.5Hz and 50Hz)
228  Bound(dt, 0.02, 2)
229 #endif
230 
231  if (ins_altf.reset_alt_ref) {
232  ins_altf.reset_alt_ref = false;
233  ins_altf.alt = utm.alt;
234  ins_altf.alt_dot = 0.0f;
236  } else {
237  alt_kalman(utm.alt, dt);
238  ins_altf.alt_dot = -ned_vel.z;
239  }
240 #endif // !USE_BAROMETER
241 
242  utm.alt = ins_altf.alt;
243  // set position
244  stateSetPositionUtm_f(&utm);
245 
246  ned_vel.z = -ins_altf.alt_dot; // vz (down) from filter
247  // set velocity
248  stateSetSpeedNed_f(&ned_vel);
249 
250 #endif
251 }
252 
253 
254 #define GPS_SIGMA2 1.
255 #define GPS_R 2.
256 
257 static float p[2][2];
258 
259 static void alt_kalman_reset(void)
260 {
261  p[0][0] = 1.0f;
262  p[0][1] = 0.0f;
263  p[1][0] = 0.0f;
264  p[1][1] = 1.0f;
265 }
266 
267 static void alt_kalman_init(void)
268 {
270 }
271 
272 static void alt_kalman(float z_meas, float dt)
273 {
274  float R = GPS_R;
275  float SIGMA2 = GPS_SIGMA2;
276 
277 #if USE_BAROMETER
278 #ifdef SITL
279  R = 0.5;
280  SIGMA2 = 0.1;
281 #elif USE_BARO_MS5534A
282  if (alt_baro_enabled) {
283  R = baro_MS5534A_r;
284  SIGMA2 = baro_MS5534A_sigma2;
285  }
286 #elif USE_BARO_ETS
287  if (baro_ets_enabled) {
288  R = baro_ets_r;
289  SIGMA2 = baro_ets_sigma2;
290  }
291 #elif USE_BARO_MS5611
292  if (baro_ms5611_enabled) {
293  R = baro_ms5611_r;
294  SIGMA2 = baro_ms5611_sigma2;
295  }
296 #elif USE_BARO_AMSYS
297  if (baro_amsys_enabled) {
298  R = baro_amsys_r;
299  SIGMA2 = baro_amsys_sigma2;
300  }
301 #elif USE_BARO_BMP
302  if (baro_bmp_enabled) {
303  R = baro_bmp_r;
304  SIGMA2 = baro_bmp_sigma2;
305  }
306 #endif
307 #endif // USE_BAROMETER
308 
309  float q[2][2];
310  q[0][0] = dt * dt * dt * dt / 4.;
311  q[0][1] = dt * dt * dt / 2.;
312  q[1][0] = dt * dt * dt / 2.;
313  q[1][1] = dt * dt;
314 
315 
316  /* predict */
317  ins_altf.alt += ins_altf.alt_dot * dt;
318  p[0][0] = p[0][0] + p[1][0] * dt + dt * (p[0][1] + p[1][1] * dt) + SIGMA2 * q[0][0];
319  p[0][1] = p[0][1] + p[1][1] * dt + SIGMA2 * q[0][1];
320  p[1][0] = p[1][0] + p[1][1] * dt + SIGMA2 * q[1][0];
321  p[1][1] = p[1][1] + SIGMA2 * q[1][1];
322 
323  /* error estimate */
324  float e = p[0][0] + R;
325 
326  if (fabs(e) > 1e-5) {
327  float k_0 = p[0][0] / e;
328  float k_1 = p[1][0] / e;
329  e = z_meas - ins_altf.alt;
330 
331  /* correction */
332  ins_altf.alt += k_0 * e;
333  ins_altf.alt_dot += k_1 * e;
334 
335  p[1][0] = -p[0][0] * k_1 + p[1][0];
336  p[1][1] = -p[0][1] * k_1 + p[1][1];
337  p[0][0] = p[0][0] * (1 - k_0);
338  p[0][1] = p[0][1] * (1 - k_0);
339  }
340 
341 #ifdef DEBUG_ALT_KALMAN
342  DOWNLINK_SEND_ALT_KALMAN(DefaultChannel, DefaultDevice, &(p[0][0]), &(p[0][1]), &(p[1][0]), &(p[1][1]));
343 #endif
344 }
345 
346 #if USE_BAROMETER
347 static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
348 {
349  ins_alt_float_update_baro(pressure);
350 }
351 #endif
352 
353 static void gps_cb(uint8_t sender_id __attribute__((unused)),
354  uint32_t stamp __attribute__((unused)),
355  struct GpsState *gps_s)
356 {
358 }
359 
360 static void accel_cb(uint8_t sender_id __attribute__((unused)),
361  uint32_t stamp __attribute__((unused)),
362  struct Int32Vect3 *accel)
363 {
364  // untilt accel and remove gravity
365  struct Int32Vect3 accel_ned;
366  stateSetAccelBody_i(accel);
367  struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
368  int32_rmat_transp_vmult(&accel_ned, ned_to_body_rmat, accel);
369  accel_ned.z += ACCEL_BFP_OF_REAL(9.81);
370  stateSetAccelNed_i((struct NedCoor_i *)&accel_ned);
371 }
stateSetAccelNed_i
static void stateSetAccelNed_i(struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
Definition: state.h:986
InsAltFloat::reset_alt_ref
bool reset_alt_ref
flag to request reset of altitude reference to current alt
Definition: ins_alt_float.h:41
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
NedCoor_f
vector in North East Down coordinates Units: meters
Definition: pprz_geodetic_float.h:63
alt_kalman_reset
static void alt_kalman_reset(void)
Definition: ins_alt_float.c:259
baro_ets_sigma2
float baro_ets_sigma2
Definition: baro_ets.c:109
baro_amsys_r
float baro_amsys_r
Definition: baro_amsys.c:78
GPS_R
#define GPS_R
Definition: ins_alt_float.c:255
ins_reset_local_origin
void ins_reset_local_origin(void)
Reset the geographic reference to the current GPS fix.
Definition: ins_alt_float.c:130
baro_ms5611_r
float baro_ms5611_r
Definition: baro_ms5611_i2c.c:62
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
baro_amsys_enabled
bool baro_amsys_enabled
Definition: baro_amsys.c:77
gps_cb
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_alt_float.c:353
accel_cb
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ins_alt_float.c:360
stateSetSpeedNed_f
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
InsAltFloat::qfe
float qfe
Definition: ins_alt_float.h:45
NedCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:66
abi.h
baro_ms5611_sigma2
float baro_ms5611_sigma2
Definition: baro_ms5611_i2c.c:63
nav_utm_east0
int32_t nav_utm_east0
Definition: common_nav.c:43
accel_ev
static abi_event accel_ev
Definition: ins_alt_float.c:88
baro_MS5534A_r
float baro_MS5534A_r
Definition: baro_MS5534A.c:47
baro_ev
abi_event baro_ev
Definition: ins_alt_float.c:71
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
GpsState
data structure for GPS information
Definition: gps.h:90
InsAltFloat::baro_alt
float baro_alt
Definition: ins_alt_float.h:46
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
baro_ets_r
float baro_ets_r
Definition: baro_ets.c:108
ned_vel_float_from_gps
struct NedCoor_f ned_vel_float_from_gps(struct GpsState *gps_s)
Get GPS ned velocity (float) Converted on the fly if not available.
Definition: gps.c:506
baro_bmp_r
float baro_bmp_r
Definition: baro_bmp.c:55
baro_bmp_sigma2
float baro_bmp_sigma2
Definition: baro_bmp.c:56
UTM_COPY
#define UTM_COPY(_u1, _u2)
Definition: pprz_geodetic.h:71
stateSetPositionUtm_f
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:582
stateGetPositionUtm_f
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
ins_altf
struct InsAltFloat ins_altf
Definition: ins_alt_float.c:53
InsAltFloat::alt_dot
float alt_dot
estimated vertical speed in m/s (positive-up)
Definition: ins_alt_float.h:39
stateSetAccelBody_i
static void stateSetAccelBody_i(struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition: state.h:855
nav_utm_north0
int32_t nav_utm_north0
Definition: common_nav.c:44
ins_alt_float_update_gps
void ins_alt_float_update_gps(struct GpsState *gps_s)
Definition: ins_alt_float.c:201
ins_alt_float_update_baro
void ins_alt_float_update_baro(float pressure)
Definition: ins_alt_float.c:156
ins_alt_float_init
void ins_alt_float_init(void)
Definition: ins_alt_float.c:97
INS_ALT_IMU_ID
#define INS_ALT_IMU_ID
Definition: ins_alt_float.c:86
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:75
stateGetNedToBodyRMat_i
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
InsAltFloat
Ins implementation state (altitude, float)
Definition: ins_alt_float.h:37
stateSetLocalUtmOrigin_f
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:477
alt_kalman_init
static void alt_kalman_init(void)
Definition: ins_alt_float.c:267
stateGetSpeedNed_f
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
last_ts
uint32_t last_ts
Definition: bluegiga.c:131
ins_alt_float.h
gps.h
Device independent GPS code (interface)
int32_rmat_transp_vmult
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
Definition: pprz_algebra_int.c:117
gps_ev
static abi_event gps_ev
Definition: ins_alt_float.c:82
UtmCoor_f::alt
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
Definition: pprz_geodetic_float.h:84
baro_MS5534A_sigma2
float baro_MS5534A_sigma2
Definition: baro_MS5534A.c:48
NedCoor_i
vector in North East Down coordinates
Definition: pprz_geodetic_int.h:68
InsAltFloat::origin_initialized
bool origin_initialized
TRUE if UTM origin was initialized.
Definition: ins_alt_float.h:42
Int32Vect3
Definition: pprz_algebra_int.h:88
sys_time.h
Architecture independent timing functions.
GPS_SIGMA2
#define GPS_SIGMA2
Definition: ins_alt_float.c:254
ACCEL_BFP_OF_REAL
#define ACCEL_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:221
alt_kalman
static void alt_kalman(float z_meas, float dt)
Definition: ins_alt_float.c:272
alt_baro_enabled
bool alt_baro_enabled
Definition: baro_MS5534A.c:45
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
pprz_isa_height_of_pressure
static float pprz_isa_height_of_pressure(float pressure, float ref_p)
Get relative altitude from pressure (using simplified equation).
Definition: pprz_isa.h:102
baro.h
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
utm_float_from_gps
struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
Convenience functions to get utm position from GPS state.
Definition: gps.c:539
InsAltFloat::alt
float alt
estimated altitude above MSL in meters
Definition: ins_alt_float.h:38
nav.h
ground_alt
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:41
baro_amsys_sigma2
float baro_amsys_sigma2
Definition: baro_amsys.c:79
UtmCoor_f
position in UTM coordinates Units: meters
Definition: pprz_geodetic_float.h:81
nav_utm_zone0
uint8_t nav_utm_zone0
Definition: common_nav.c:45
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:97
baro_bmp_enabled
bool baro_bmp_enabled
Definition: baro_bmp.c:54
INS_ALT_GPS_ID
#define INS_ALT_GPS_ID
ABI binding for gps data.
Definition: ins_alt_float.c:79
State::utm_origin_f
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
InsAltFloat::baro_initialized
bool baro_initialized
Definition: ins_alt_float.h:47
state.h
INS_ALT_BARO_ID
#define INS_ALT_BARO_ID
Definition: ins_alt_float.c:66
ins_reset_altitude_ref
void ins_reset_altitude_ref(void)
INS altitude reference reset.
Definition: ins_alt_float.c:144
state
struct State state
Definition: state.c:36
gps
struct GpsState gps
global GPS state
Definition: gps.c:69
GPS_FIX_3D
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:42
baro_ets_enabled
bool baro_ets_enabled
Definition: baro_ets.c:107
p
static float p[2][2]
Definition: ins_alt_float.c:257
pprz_isa.h
Paparazzi atmospheric pressure conversion utilities.
baro_cb
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition: ins_alt_float.c:347
baro_ms5611_enabled
bool baro_ms5611_enabled
Definition: baro_ms5611_i2c.c:60