Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
26
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#include "generated/flight_plan.h"
43
44#ifdef DEBUG_ALT_KALMAN
45#include "mcu_periph/uart.h"
47#endif
48
49#ifndef USE_INS_NAV_INIT
50#define USE_INS_NAV_INIT TRUE
51PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
52#endif
53
55
56#if USE_BAROMETER
57#include "math/pprz_isa.h"
58
59PRINT_CONFIG_MSG("USE_BAROMETER is TRUE: Using baro for altitude estimation.")
60
61// Baro event on ABI
62#ifndef INS_ALT_BARO_ID
63#define INS_ALT_BARO_ID AIR_DATA_SENDER_ID
64#endif
66
68static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure);
69#endif /* USE_BAROMETER */
70
74#ifndef INS_ALT_GPS_ID
75#define INS_ALT_GPS_ID GPS_MULTI_ID
76#endif
79static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
80
81#ifndef INS_ALT_IMU_ID
82#define INS_ALT_IMU_ID ABI_BROADCAST
83#endif
85static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
86
88static void reset_cb(uint8_t sender_id, uint8_t flag);
89
90static void alt_kalman_reset(void);
91static void alt_kalman_init(void);
92static void alt_kalman(float z_meas, float dt);
93
94void ins_alt_float_update_gps(struct GpsState *gps_s);
95
97{
98#if USE_INS_NAV_INIT
99 struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, GROUND_ALT, nav_utm_zone0 };
100 stateSetLocalUtmOrigin_f(MODULE_INS_ALT_FLOAT_ID, &utm0);
101 ins_altf.origin_initialized = true;
102
103 stateSetPositionUtm_f(MODULE_INS_ALT_FLOAT_ID, &utm0);
104#else
105 ins_altf.origin_initialized = false;
106#endif
107
109
110#if USE_BAROMETER
111 ins_altf.qfe = 0.0f;
112 ins_altf.baro_initialized = false;
113 ins_altf.baro_alt = 0.0f;
114#endif
115 ins_altf.reset_alt_ref = false;
116
117 // why do we have this here?
118 alt_kalman(0.0f, 0.1);
119
120#if USE_BAROMETER
121 // Bind to BARO_ABS message
122 AbiBindMsgBARO_ABS(INS_ALT_BARO_ID, &baro_ev, baro_cb);
123#endif
124 AbiBindMsgGPS(INS_ALT_GPS_ID, &gps_ev, gps_cb);
125 AbiBindMsgIMU_ACCEL(INS_ALT_IMU_ID, &accel_ev, accel_cb);
126 AbiBindMsgINS_RESET(ABI_BROADCAST, &reset_ev, reset_cb);
127}
128
129static void reset_ref(void)
130{
131 // get utm pos
132 struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
133 // reset state UTM ref
134 stateSetLocalUtmOrigin_f(MODULE_INS_ALT_FLOAT_ID, &utm);
135 ins_altf.origin_initialized = true;
136 // reset filter flag
137 ins_altf.reset_alt_ref = true;
138}
139
140static void reset_vertical_ref(void)
141{
142 struct UtmCoor_f utm = *stateGetUtmOrigin_f();
143 // ground_alt
144 utm.alt = gps.hmsl / 1000.0f;
145 // reset state UTM ref
146 stateSetLocalUtmOrigin_f(MODULE_INS_ALT_FLOAT_ID, &utm);
147 // reset filter flag
148 ins_altf.reset_alt_ref = true;
149}
150
151static void reset_cb(uint8_t sender_id UNUSED, uint8_t flag)
152{
153 switch (flag) {
154 case INS_RESET_REF:
155 reset_ref();
156 break;
159 break;
160 default:
161 // unsupported cases
162 break;
163 }
164}
165
166#if USE_BAROMETER
167void ins_alt_float_update_baro(float pressure)
168{
169 // timestamp in usec when last callback was received
170 static uint32_t last_ts = 0;
171 // current timestamp
172 uint32_t now_ts = get_sys_time_usec();
173 // dt between this and last callback in seconds
174 float dt = (float)(now_ts - last_ts) / 1e6;
175 last_ts = now_ts;
176
177 // bound dt (assume baro freq 1Hz-500Hz
178 Bound(dt, 0.002, 1.0)
179
180 if (!ins_altf.baro_initialized) {
181 ins_altf.qfe = pressure;
182 ins_altf.baro_initialized = true;
183 }
184 if (ins_altf.reset_alt_ref) {
185 ins_altf.reset_alt_ref = false;
186 ins_altf.alt = ground_alt;
187 ins_altf.alt_dot = 0.0f;
188 ins_altf.qfe = pressure;
190 } else { /* not realigning, so normal update with baro measurement */
191 ins_altf.baro_alt = ground_alt + pprz_isa_height_of_pressure(pressure, ins_altf.qfe);
192 /* run the filter */
193 alt_kalman(ins_altf.baro_alt, dt);
194 /* set new altitude, just copy old horizontal position */
195 struct UtmCoor_f utm;
197 utm.alt = ins_altf.alt;
198 stateSetPositionUtm_f(MODULE_INS_ALT_FLOAT_ID, &utm);
199 struct NedCoor_f ned_vel;
200 ned_vel = *stateGetSpeedNed_f();
201 ned_vel.z = -ins_altf.alt_dot;
202 stateSetSpeedNed_f(MODULE_INS_ALT_FLOAT_ID, &ned_vel);
203 }
204}
205#else
206void ins_alt_float_update_baro(float pressure __attribute__((unused)))
207{
208}
209#endif
210
211
212void ins_alt_float_update_gps(struct GpsState *gps_s __attribute__((unused)))
213{
214#if USE_GPS
215 if (gps_s->fix < GPS_FIX_3D) {
216 return;
217 }
218
219 if (!ins_altf.origin_initialized) {
220 reset_ref();
221 }
222
223 struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
224 struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s);
225
226#if !USE_BAROMETER
227#ifdef GPS_DT
228 const float dt = GPS_DT;
229#else
230 // timestamp in usec when last callback was received
231 static uint32_t last_ts = 0;
232 // current timestamp
233 uint32_t now_ts = get_sys_time_usec();
234 // dt between this and last callback in seconds
235 float dt = (float)(now_ts - last_ts) / 1e6;
236 last_ts = now_ts;
237
238 // bound dt (assume GPS freq between 0.5Hz and 50Hz)
239 Bound(dt, 0.02, 2)
240#endif
241
242 if (ins_altf.reset_alt_ref) {
243 ins_altf.reset_alt_ref = false;
244 ins_altf.alt = utm.alt;
245 ins_altf.alt_dot = 0.0f;
247 } else {
248 alt_kalman(utm.alt, dt);
249 ins_altf.alt_dot = -ned_vel.z;
250 }
251#endif // !USE_BAROMETER
252
253 utm.alt = ins_altf.alt;
254 // set position
255 stateSetPositionUtm_f(MODULE_INS_ALT_FLOAT_ID, &utm);
256
257 ned_vel.z = -ins_altf.alt_dot; // vz (down) from filter
258 // set velocity
259 stateSetSpeedNed_f(MODULE_INS_ALT_FLOAT_ID, &ned_vel);
260
261#endif
262}
263
264
265#define GPS_SIGMA2 1.
266#define GPS_R 2.
267
268static float p[2][2];
269
270static 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
278static void alt_kalman_init(void)
279{
281}
282
283static 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
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
358static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
359{
361}
362#endif
363
364static void gps_cb(uint8_t sender_id __attribute__((unused)),
365 uint32_t stamp __attribute__((unused)),
366 struct GpsState *gps_s)
367{
369}
370
371static 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_ned;
377 stateSetAccelBody_i(MODULE_INS_ALT_FLOAT_ID, accel);
378 struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
379 int32_rmat_transp_vmult(&accel_ned, ned_to_body_rmat, accel);
380 accel_ned.z += ACCEL_BFP_OF_REAL(9.81);
381 struct NedCoor_i accel_coord;
382 VECT3_COPY(accel_coord, accel_ned);
383 stateSetAccelNed_i(MODULE_INS_ALT_FLOAT_ID, &accel_coord);
384}
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
#define ABI_BROADCAST
Broadcast address.
Definition abi_common.h:80
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static abi_event gps_ev
float baro_MS5534A_sigma2
bool alt_baro_enabled
float baro_MS5534A_r
float baro_amsys_r
Definition baro_amsys.c:79
float baro_amsys_sigma2
Definition baro_amsys.c:80
bool baro_amsys_enabled
Definition baro_amsys.c:78
bool baro_bmp_enabled
Definition baro_bmp.c:51
float baro_bmp_r
Definition baro_bmp.c:52
float baro_bmp_sigma2
Definition baro_bmp.c:53
float baro_ets_r
Definition baro_ets.c:101
bool baro_ets_enabled
Definition baro_ets.c:100
float baro_ets_sigma2
Definition baro_ets.c:102
float baro_ms5611_sigma2
bool baro_ms5611_enabled
float baro_ms5611_r
#define UNUSED(x)
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
int32_t nav_utm_east0
Definition common_nav.c:48
uint8_t nav_utm_zone0
Definition common_nav.c:50
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition common_nav.c:46
int32_t nav_utm_north0
Definition common_nav.c:49
static abi_event accel_ev
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
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:591
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:558
struct GpsState gps
global GPS state
Definition gps.c:74
Device independent GPS code (interface).
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
data structure for GPS information
Definition gps.h:90
#define VECT3_COPY(_a, _b)
#define ACCEL_BFP_OF_REAL(_af)
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
rotation matrix
#define UTM_COPY(_u1, _u2)
vector in North East Down coordinates
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
static void stateSetAccelNed_i(uint16_t id, struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
Definition state.h:1146
static void stateSetAccelBody_i(uint16_t id, struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition state.h:1186
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition state.h:1301
static void stateSetPositionUtm_f(uint16_t id, struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition state.h:709
static struct UtmCoor_f * stateGetUtmOrigin_f(void)
Get the coordinate UTM frame origin (int).
Definition state.h:587
static void stateSetLocalUtmOrigin_f(uint16_t id, struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition state.h:552
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition state.h:832
static void stateSetSpeedNed_f(uint16_t id, struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition state.h:958
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition state.h:1060
#define INS_RESET_VERTICAL_REF
Definition ins.h:37
#define INS_RESET_REF
flags for INS reset
Definition ins.h:36
static void reset_vertical_ref(void)
#define GPS_SIGMA2
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static void reset_cb(uint8_t sender_id, uint8_t flag)
#define INS_ALT_IMU_ID
static void reset_ref(void)
#define INS_ALT_GPS_ID
ABI binding for gps data.
static void alt_kalman_init(void)
static abi_event reset_ev
#define GPS_R
abi_event baro_ev
static void alt_kalman(float z_meas, float dt)
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
static float p[2][2]
struct InsAltFloat ins_altf
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
#define INS_ALT_BARO_ID
void ins_alt_float_update_gps(struct GpsState *gps_s)
void ins_alt_float_init(void)
static void alt_kalman_reset(void)
void ins_alt_float_update_baro(float pressure)
Filters altitude and climb rate for fixedwings.
Ins implementation state (altitude, float).
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
Fixedwing Navigation library.
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
float z
in meters
vector in North East Down coordinates Units: meters
position in UTM coordinates Units: meters
Paparazzi atmospheric pressure conversion utilities.
API to get/set the generic vehicle states.
Architecture independent timing functions.
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.