Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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 #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 "subsystems/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
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, 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 
92 static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f);
94 
95 static void alt_kalman_reset(void);
96 static void alt_kalman_init(void);
97 static void alt_kalman(float z_meas, float dt);
98 
99 void ins_alt_float_update_gps(struct GpsState *gps_s);
100 
102 {
103 #if USE_INS_NAV_INIT
107 
108  stateSetPositionUtm_f(&utm0);
109 #else
111 #endif
112 
113  // set initial body to imu to 0
114  struct Int32Eulers b2i0 = { 0, 0, 0 };
116 
117  alt_kalman_init();
118 
119 #if USE_BAROMETER
120  ins_altf.qfe = 0.0f;
121  ins_altf.baro_initialized = false;
122  ins_altf.baro_alt = 0.0f;
123 #endif
124  ins_altf.reset_alt_ref = false;
125 
126  // why do we have this here?
127  alt_kalman(0.0f, 0.1);
128 
129 #if USE_BAROMETER
130  // Bind to BARO_ABS message
131  AbiBindMsgBARO_ABS(INS_ALT_BARO_ID, &baro_ev, baro_cb);
132 #endif
133  AbiBindMsgGPS(INS_ALT_GPS_ID, &gps_ev, gps_cb);
134  AbiBindMsgIMU_ACCEL_INT32(INS_ALT_IMU_ID, &accel_ev, accel_cb);
135  AbiBindMsgBODY_TO_IMU_QUAT(INS_ALT_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
136 }
137 
140 {
141  // get utm pos
142  struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
143 
144  // reset state UTM ref
146 
148 
149  // reset filter flag
150  ins_altf.reset_alt_ref = true;
151 }
152 
154 {
155  struct UtmCoor_f utm = state.utm_origin_f;
156  // ground_alt
157  utm.alt = gps.hmsl / 1000.0f;
158  // reset state UTM ref
160  // reset filter flag
161  ins_altf.reset_alt_ref = true;
162 }
163 
164 #if USE_BAROMETER
165 void ins_alt_float_update_baro(float pressure)
166 {
167  // timestamp in usec when last callback was received
168  static uint32_t last_ts = 0;
169  // current timestamp
170  uint32_t now_ts = get_sys_time_usec();
171  // dt between this and last callback in seconds
172  float dt = (float)(now_ts - last_ts) / 1e6;
173  last_ts = now_ts;
174 
175  // bound dt (assume baro freq 1Hz-500Hz
176  Bound(dt, 0.002, 1.0)
177 
178  if (!ins_altf.baro_initialized) {
179  ins_altf.qfe = pressure;
180  ins_altf.baro_initialized = true;
181  }
182  if (ins_altf.reset_alt_ref) {
183  ins_altf.reset_alt_ref = false;
185  ins_altf.alt_dot = 0.0f;
186  ins_altf.qfe = pressure;
188  } else { /* not realigning, so normal update with baro measurement */
190  /* run the filter */
192  /* set new altitude, just copy old horizontal position */
193  struct UtmCoor_f utm;
195  utm.alt = ins_altf.alt;
196  stateSetPositionUtm_f(&utm);
197  struct NedCoor_f ned_vel;
198  ned_vel = *stateGetSpeedNed_f();
199  ned_vel.z = -ins_altf.alt_dot;
200  stateSetSpeedNed_f(&ned_vel);
201  }
202 }
203 #else
204 void ins_alt_float_update_baro(float pressure __attribute__((unused)))
205 {
206 }
207 #endif
208 
209 
210 void ins_alt_float_update_gps(struct GpsState *gps_s __attribute__((unused)))
211 {
212 #if USE_GPS
213  if (gps_s->fix < GPS_FIX_3D) {
214  return;
215  }
216 
219  }
220 
221  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
222 
223 #if !USE_BAROMETER
224 #ifdef GPS_DT
225  const float dt = GPS_DT;
226 #else
227  // timestamp in usec when last callback was received
228  static uint32_t last_ts = 0;
229  // current timestamp
230  uint32_t now_ts = get_sys_time_usec();
231  // dt between this and last callback in seconds
232  float dt = (float)(now_ts - last_ts) / 1e6;
233  last_ts = now_ts;
234 
235  // bound dt (assume GPS freq between 0.5Hz and 50Hz)
236  Bound(dt, 0.02, 2)
237 #endif
238 
239  if (ins_altf.reset_alt_ref) {
240  ins_altf.reset_alt_ref = false;
241  ins_altf.alt = utm.alt;
242  ins_altf.alt_dot = 0.0f;
244  } else {
245  alt_kalman(utm.alt, dt);
246  ins_altf.alt_dot = -gps_s->ned_vel.z / 100.0f;
247  }
248 #endif
249  utm.alt = ins_altf.alt;
250  // set position
251  stateSetPositionUtm_f(&utm);
252 
253  struct NedCoor_f ned_vel = {
254  gps_s->ned_vel.x / 100.0f,
255  gps_s->ned_vel.y / 100.0f,
257  };
258  // set velocity
259  stateSetSpeedNed_f(&ned_vel);
260 
261 #endif
262 }
263 
264 
265 #define GPS_SIGMA2 1.
266 #define GPS_R 2.
267 
268 static float p[2][2];
269 
270 static void alt_kalman_reset(void)
271 {
272  p[0][0] = 1.0f;
273  p[0][1] = 0.0f;
274  p[1][0] = 0.0f;
275  p[1][1] = 1.0f;
276 }
277 
278 static void alt_kalman_init(void)
279 {
281 }
282 
283 static void alt_kalman(float z_meas, float dt)
284 {
285  float R = GPS_R;
286  float SIGMA2 = GPS_SIGMA2;
287 
288 #if USE_BAROMETER
289 #ifdef SITL
290  R = 0.5;
291  SIGMA2 = 0.1;
292 #elif USE_BARO_MS5534A
293  if (alt_baro_enabled) {
294  R = baro_MS5534A_r;
295  SIGMA2 = baro_MS5534A_sigma2;
296  }
297 #elif USE_BARO_ETS
298  if (baro_ets_enabled) {
299  R = baro_ets_r;
300  SIGMA2 = baro_ets_sigma2;
301  }
302 #elif USE_BARO_MS5611
303  if (baro_ms5611_enabled) {
304  R = baro_ms5611_r;
305  SIGMA2 = baro_ms5611_sigma2;
306  }
307 #elif USE_BARO_AMSYS
308  if (baro_amsys_enabled) {
309  R = baro_amsys_r;
310  SIGMA2 = baro_amsys_sigma2;
311  }
312 #elif USE_BARO_BMP
313  if (baro_bmp_enabled) {
314  R = baro_bmp_r;
315  SIGMA2 = baro_bmp_sigma2;
316  }
317 #endif
318 #endif // USE_BAROMETER
319 
320  float q[2][2];
321  q[0][0] = dt * dt * dt * dt / 4.;
322  q[0][1] = dt * dt * dt / 2.;
323  q[1][0] = dt * dt * dt / 2.;
324  q[1][1] = dt * dt;
325 
326 
327  /* predict */
328  ins_altf.alt += ins_altf.alt_dot * dt;
329  p[0][0] = p[0][0] + p[1][0] * dt + dt * (p[0][1] + p[1][1] * dt) + SIGMA2 * q[0][0];
330  p[0][1] = p[0][1] + p[1][1] * dt + SIGMA2 * q[0][1];
331  p[1][0] = p[1][0] + p[1][1] * dt + SIGMA2 * q[1][0];
332  p[1][1] = p[1][1] + SIGMA2 * q[1][1];
333 
334  /* error estimate */
335  float e = p[0][0] + R;
336 
337  if (fabs(e) > 1e-5) {
338  float k_0 = p[0][0] / e;
339  float k_1 = p[1][0] / e;
340  e = z_meas - ins_altf.alt;
341 
342  /* correction */
343  ins_altf.alt += k_0 * e;
344  ins_altf.alt_dot += k_1 * e;
345 
346  p[1][0] = -p[0][0] * k_1 + p[1][0];
347  p[1][1] = -p[0][1] * k_1 + p[1][1];
348  p[0][0] = p[0][0] * (1 - k_0);
349  p[0][1] = p[0][1] * (1 - k_0);
350  }
351 
352 #ifdef DEBUG_ALT_KALMAN
353  DOWNLINK_SEND_ALT_KALMAN(DefaultChannel, DefaultDevice, &(p[0][0]), &(p[0][1]), &(p[1][0]), &(p[1][1]));
354 #endif
355 }
356 
357 #if USE_BAROMETER
358 static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
359 {
360  ins_alt_float_update_baro(pressure);
361 }
362 #endif
363 
364 static void gps_cb(uint8_t sender_id __attribute__((unused)),
365  uint32_t stamp __attribute__((unused)),
366  struct GpsState *gps_s)
367 {
369 }
370 
371 static void accel_cb(uint8_t sender_id __attribute__((unused)),
372  uint32_t stamp __attribute__((unused)),
373  struct Int32Vect3 *accel)
374 {
375  // untilt accel and remove gravity
376  struct Int32Vect3 accel_body, accel_ned;
377  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&body_to_imu);
378  int32_rmat_transp_vmult(&accel_body, body_to_imu_rmat, accel);
379  struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
380  int32_rmat_transp_vmult(&accel_ned, ned_to_body_rmat, &accel_body);
381  accel_ned.z += ACCEL_BFP_OF_REAL(9.81);
382  stateSetAccelNed_i((struct NedCoor_i *)&accel_ned);
383 }
384 
385 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
386  struct FloatQuat *q_b2i_f)
387 {
389 }
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
void ins_reset_local_origin(void)
Reset the geographic reference to the current GPS fix.
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
float baro_ms5611_sigma2
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
Common barometric sensor implementation.
static abi_event accel_ev
Definition: ins_alt_float.c:88
static void baro_cb(uint8_t sender_id, float pressure)
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
static abi_event body_to_imu_ev
Definition: ins_alt_float.c:91
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:40
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
float baro_MS5534A_r
Definition: baro_MS5534A.c:47
abi_event baro_ev
Definition: ins_alt_float.c:71
Main include for ABI (AirBorneInterface).
#define GPS_R
uint8_t nav_utm_zone0
Definition: common_nav.c:44
static void alt_kalman_init(void)
position in UTM coordinates Units: meters
float alt
estimated altitude above MSL in meters
Definition: ins_alt_float.h:38
float baro_ets_r
Definition: baro_ets.c:108
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
void ins_alt_float_update_baro(float pressure)
Ins implementation state (altitude, float)
Definition: ins_alt_float.h:37
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
Roation quaternion.
float baro_amsys_r
Definition: baro_amsys.c:78
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:582
static void stateSetAccelNed_i(struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
Definition: state.h:986
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
static void orientationSetEulers_i(struct OrientationReps *orientation, struct Int32Eulers *eulers)
Set vehicle body attitude from euler angles (int).
static void alt_kalman(float z_meas, float dt)
struct InsAltFloat ins_altf
Definition: ins_alt_float.c:53
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:88
int32_t nav_utm_north0
Definition: common_nav.c:43
vector in North East Down coordinates Units: meters
void ins_alt_float_update_gps(struct GpsState *gps_s)
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
Architecture independent timing functions.
data structure for GPS information
Definition: gps.h:81
void ins_alt_float_init(void)
#define INS_ALT_IMU_ID
Definition: ins_alt_float.c:86
euler angles
Device independent GPS code (interface)
Paparazzi atmospheric pressure conversion utilities.
bool alt_baro_enabled
Definition: baro_MS5534A.c:45
bool reset_alt_ref
flag to request reset of altitude reference to current alt
Definition: ins_alt_float.h:41
unsigned long uint32_t
Definition: types.h:18
bool baro_amsys_enabled
Definition: baro_amsys.c:77
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
bool baro_initialized
Definition: ins_alt_float.h:47
bool baro_bmp_enabled
Definition: baro_bmp.c:54
uint32_t last_ts
Definition: bluegiga.c:131
float baro_bmp_sigma2
Definition: baro_bmp.c:56
static abi_event gps_ev
Definition: ins_alt_float.c:82
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
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
bool baro_ms5611_enabled
#define GPS_SIGMA2
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
unsigned char uint8_t
Definition: types.h:14
float baro_alt
Definition: ins_alt_float.h:46
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:393
API to get/set the generic vehicle states.
float baro_ets_sigma2
Definition: baro_ets.c:109
vector in North East Down coordinates
#define INS_ALT_BARO_ID
Definition: ins_alt_float.c:66
int32_t nav_utm_east0
Definition: common_nav.c:42
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
bool baro_ets_enabled
Definition: baro_ets.c:107
float alt_dot
estimated vertical speed in m/s (positive-up)
Definition: ins_alt_float.h:39
static float p[2][2]
rotation matrix
static struct OrientationReps body_to_imu
Definition: ins_alt_float.c:93
Filters altitude and climb rate for fixedwings.
static float pprz_isa_height_of_pressure(float pressure, float ref_p)
Get relative altitude from pressure (using simplified equation).
Definition: pprz_isa.h:95
#define ACCEL_BFP_OF_REAL(_af)
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
static void alt_kalman_reset(void)
void ins_reset_altitude_ref(void)
INS altitude reference reset.
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:477
struct GpsState gps
global GPS state
Definition: gps.c:75
struct State state
Definition: state.c:36
#define INS_ALT_GPS_ID
ABI binding for gps data.
Definition: ins_alt_float.c:79
bool origin_initialized
TRUE if UTM origin was initialized.
Definition: ins_alt_float.h:42
float x
in meters
float baro_ms5611_r
#define UTM_COPY(_u1, _u2)
Definition: pprz_geodetic.h:66