Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
ins_mekf_wind_wrapper.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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, see
18 * <http://www.gnu.org/licenses/>.
19 */
20
31#if USE_AHRS_ALIGNER
33#endif
34#include "modules/ins/ins.h"
35#include "modules/core/abi.h"
36#include "math/pprz_isa.h"
37#include "state.h"
38
39#include "generated/airframe.h"
40#include "generated/flight_plan.h"
41
42#if FIXEDWING_FIRMWARE
44#endif
45
46#ifndef INS_MEKF_WIND_FILTER_ID
47#define INS_MEKF_WIND_FILTER_ID 3
48#endif
49
51
55
57static void set_state_from_ins(void);
58
60#if LOG_MEKF_WIND
61#ifndef SITL
63#define PrintLog sdLogWriteLog
64#define LogFileIsOpen() (pprzLogFile != -1)
65#else // SITL: print in a file
66#include <stdio.h>
67#define PrintLog fprintf
68#define LogFileIsOpen() (pprzLogFile != NULL)
69static FILE* pprzLogFile = NULL;
70#endif
71#endif
72
74#if PERIODIC_TELEMETRY
76#include "mcu_periph/sys_time.h"
77
78static void send_euler(struct transport_tx *trans, struct link_device *dev)
79{
81 struct FloatQuat quat = ins_mekf_wind_get_quat();
86 &ltp_to_imu_euler.theta,
88 &id);
89}
90
91static void send_wind(struct transport_tx *trans, struct link_device *dev)
92{
94 struct EnuCoor_f wind_enu;
97 uint8_t flags = 7; // 3D wind + airspeed
99 &wind_enu.x, &wind_enu.y, &wind_enu.z, &va);
100}
101
102static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
103{
104 uint8_t mde = 3; // OK
105 uint16_t val = 0;
107 mde = 2; // ALIGN
109 mde = 1; // INIT
110 }
112 /* set lost if no new gyro measurements for 50ms */
113 if (t_diff > 50000) {
114 mde = 5; // IMU_LOST
115 }
118}
119
120static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
121{
122 struct FloatEulers eulers;
123 struct FloatQuat quat = ins_mekf_wind_get_quat();
124 float_eulers_of_quat(&eulers, &quat);
125 //struct FloatRates rates = ins_mekf_wind_get_body_rates();
127 struct NedCoor_f speed = ins_mekf_wind_get_speed_ned();
128 //struct NedCoor_f accel = ins_mekf_wind_get_accel_ned();
131 float airspeed = ins_mekf_wind_get_airspeed_norm();
133 AC_ID,
134 &quat.qi,
135 &eulers.phi,
136 &eulers.theta,
137 &eulers.psi,
138 &speed.x,
139 &speed.y,
140 &speed.z,
141 &pos.x,
142 &pos.y,
143 &pos.z,
144 &rb.p,
145 &rb.q,
146 &rb.r,
147 &ab.x,
148 &ab.y,
149 &ab.z,
150 &airspeed);
151}
152#endif
153
154/*
155 * ABI bindings
156 */
157
159#ifndef INS_MEKF_WIND_AIRSPEED_ID
160#define INS_MEKF_WIND_AIRSPEED_ID ABI_BROADCAST
161#endif
163
164
165#ifndef INS_MEKF_WIND_INCIDENCE_ID
166#define INS_MEKF_WIND_INCIDENCE_ID ABI_BROADCAST
167#endif
169
170
171#ifndef INS_MEKF_WIND_BARO_ID
172#if USE_BARO_BOARD
173#define INS_MEKF_WIND_BARO_ID BARO_BOARD_SENDER_ID
174#else
175#define INS_MEKF_WIND_BARO_ID ABI_BROADCAST
176#endif
177#endif
179
180
181#ifndef INS_MEKF_WIND_IMU_ID
182#define INS_MEKF_WIND_IMU_ID ABI_BROADCAST
183#endif
185
186
187#ifndef INS_MEKF_WIND_MAG_ID
188#define INS_MEKF_WIND_MAG_ID ABI_BROADCAST
189#endif
191
192
195#ifndef INS_MEKF_WIND_GPS_ID
196#define INS_MEKF_WIND_GPS_ID GPS_MULTI_ID
197#endif
199
211
213
214static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
215{
217 static float alpha = 10.0f;
218 static int32_t i = 1;
219 static float baro_moy = 0.0f;
220 static float baro_prev = 0.0f;
221
223 // try to find a stable qfe
224 // TODO generic function in pprz_isa ?
225 if (i == 1) {
226 baro_moy = pressure;
227 baro_prev = pressure;
228 }
229 baro_moy = (baro_moy * (i - 1) + pressure) / i;
230 alpha = (10.*alpha + (baro_moy - baro_prev)) / (11.0f);
232 // test stop condition
233 if (fabs(alpha) < 0.1f) {
236 }
237 if (i == 250) {
238 ins_qfe = pressure;
240 }
241 i++;
242 } else { /* normal update with baro measurement */
244 float baro_alt = -pprz_isa_height_of_pressure(pressure, ins_qfe); // Z down
246
247#if LOG_MEKF_WIND
248 if (LogFileIsOpen()) {
249 PrintLog(pprzLogFile, "%.3f baro %.3f \n", get_sys_time_float(), baro_alt);
250 }
251#endif
252 }
253 }
254}
255
257{
259 float airspeed = tas_from_dynamic_pressure(pdyn);
261
262#if LOG_MEKF_WIND
263 if (LogFileIsOpen()) {
264 PrintLog(pprzLogFile, "%.3f airspeed %.3f\n", get_sys_time_float(), airspeed);
265 }
266#endif
267 }
268}
269
270static void airspeed_cb(uint8_t __attribute__((unused)) sender_id, float airspeed)
271{
274
275#if LOG_MEKF_WIND
276 if (LogFileIsOpen()) {
277 PrintLog(pprzLogFile, "%.3f airspeed %.3f\n", get_sys_time_float(), airspeed);
278 }
279#endif
280 }
281}
282
283static void incidence_cb(uint8_t __attribute__((unused)) sender_id, uint8_t flag, float aoa, float sideslip)
284{
286 ins_mekf_wind_update_incidence(aoa, sideslip);
287
288#if LOG_MEKF_WIND
289 if (LogFileIsOpen()) {
290 PrintLog(pprzLogFile, "%.3f incidence %.3f %.3f\n", get_sys_time_float(), aoa, sideslip);
291 }
292#endif
293 }
294}
295
302 uint32_t stamp, struct Int32Rates *gyro)
303{
305 struct FloatRates gyro_body;
307
308#if USE_AUTO_INS_FREQ || !defined(INS_PROPAGATE_FREQUENCY)
309 PRINT_CONFIG_MSG("Calculating dt for INS MEKF_WIND propagation.")
310
311 if (last_imu_stamp > 0) {
312 float dt = (float)(stamp - last_imu_stamp) * 1e-6;
315 } else {
317 }
318 }
319#else
320 PRINT_CONFIG_MSG("Using fixed INS_PROPAGATE_FREQUENCY for INS MEKF_WIND propagation.")
322 const float dt = 1. / (INS_PROPAGATE_FREQUENCY);
325 } else {
327 }
328#endif
329
330 // update state interface
332
333#if LOG_MEKF_WIND
334 if (LogFileIsOpen()) {
335 float time = get_sys_time_float();
336
338 "%.3f gyro_accel %.3f %.3f %.3f %.3f %.3f %.3f \n",
339 time,
344
345 struct FloatEulers eulers;
346 struct FloatQuat quat = ins_mekf_wind_get_quat();
347 float_eulers_of_quat(&eulers, &quat);
350 struct NedCoor_f speed = ins_mekf_wind_get_speed_ned();
351 struct NedCoor_f accel = ins_mekf_wind_get_accel_ned();
356 float airspeed = ins_mekf_wind_get_airspeed_norm();
358 "%.3f output %.4f %.4f %.4f %.3f %.3f %.3f ",
359 time,
360 eulers.phi, eulers.theta, eulers.psi,
361 rates.p, rates.q, rates.r);
363 "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f ",
364 pos.x, pos.y, pos.z,
365 speed.x, speed.y, speed.z,
366 accel.x, accel.y, accel.z);
368 "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
369 ab.x, ab.y, ab.z, rb.p, rb.q, rb.r, bb,
370 wind.x, wind.y, wind.z, airspeed);
371 }
372#endif
373 }
374
375 /* timestamp in usec when last callback was received */
377}
378
380 uint32_t stamp __attribute__((unused)),
381 struct Int32Vect3 *accel)
382{
384}
385
386static void mag_cb(uint8_t sender_id __attribute__((unused)),
387 uint32_t stamp __attribute__((unused)),
388 struct Int32Vect3 *mag)
389{
391 struct FloatVect3 mag_body;
393
394 // only correct attitude if GPS is not initialized
396
397#if LOG_MEKF_WIND
398 if (LogFileIsOpen()) {
400 "%.3f magneto %.3f %.3f %.3f\n",
402 mag_body.x, mag_body.y, mag_body.z);
403 }
404#endif
405 }
406}
407
409 uint32_t stamp __attribute__((unused)),
410 struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
411 struct Int32Vect3 *lp_mag)
412{
414 struct FloatRates gyro_body;
415 RATES_FLOAT_OF_BFP(gyro_body, *lp_gyro);
416
417 struct FloatVect3 accel_body;
419
420 struct FloatVect3 mag_body;
421 MAGS_FLOAT_OF_BFP(mag_body, *lp_mag);
422
423 struct FloatQuat quat;
426 // udate state interface
428
429 // ins and ahrs are now running
431 }
432}
433
434static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
435{
437}
438
439static void gps_cb(uint8_t sender_id __attribute__((unused)),
440 uint32_t stamp __attribute__((unused)),
441 struct GpsState *gps_s)
442{
443 if (ins_mekf_wind.is_aligned && gps_s->fix >= GPS_FIX_3D) {
444
445#if FIXEDWING_FIRMWARE
448 struct NedCoor_f pos, speed;
449 // position (local ned)
450 pos.x = utm.north - stateGetUtmOrigin_f()->north;
451 pos.y = utm.east - stateGetUtmOrigin_f()->east;
452 pos.z = stateGetHmslOrigin_f() - utm.alt;
453 // speed
459 }
460 ins_mekf_wind_update_pos_speed((struct FloatVect3*)(&pos), (struct FloatVect3*)(&speed));
461
462#if LOG_MEKFW_FILTER
463 if (LogFileIsOpen()) {
465 "%.3f gps %.3f %.3f %.3f %.3f %.3f %.3f \n",
467 pos.x, pos.y, pos.z, speed.x, speed.y, speed.z);
468 }
469#endif
470 }
471
472#else
474 struct NedCoor_f pos, speed;
476 struct EcefCoor_i ecef_pos_i = ecef_int_from_gps(gps_s);
480 struct EcefCoor_f ecef_vel = ecef_vel_float_from_gps(gps_s);
481 ned_of_ecef_vect_f(&speed, stateGetNedOrigin_f(), &ecef_vel);
482 ins_mekf_wind_update_pos_speed((struct FloatVect3*)(&pos), (struct FloatVect3*)(&speed));
483
484#if LOG_MEKFW_FILTER
485 if (LogFileIsOpen()) {
487 "%.3f gps %.3f %.3f %.3f %.3f %.3f %.3f \n",
489 pos.x, pos.y, pos.z, speed.x, speed.y, speed.z);
490 }
491#endif
492 }
493#endif
494 }
495}
496
517
522{
523 // init position
524#if FIXEDWING_FIRMWARE
525 struct UtmCoor_f utm0;
527 utm0.east = (float)nav_utm_east0;
528 utm0.alt = GROUND_ALT;
529 utm0.zone = nav_utm_zone0;
532#else
533 struct LlaCoor_i llh_nav0;
535 llh_nav0.lon = NAV_LON0;
536 llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
537 struct EcefCoor_i ecef_nav0;
539 struct LtpDef_i ltp_def;
543#endif
544
545 // reset flags
547 ins_mekf_wind.reset = false;
550
551 // aligner
552#if USE_AHRS_ALIGNER
554#endif
555
556 // init filter
558 const struct FloatVect3 mag_h = { INS_H_X, INS_H_Y, INS_H_Z };
560
561 // Bind to ABI messages
573
574#if PERIODIC_TELEMETRY
579#endif
580
581 // init log
582#if LOG_MEKF_WIND && SITL
583 // open log file for writing
584 // path should be specified in airframe file
585 uint32_t counter = 0;
586 char filename[512];
587 snprintf(filename, 512, "%s/mekf_wind_%05d.csv", STRINGIFY(MEKF_WIND_LOG_PATH), counter);
588 // check availale name
589 while ((pprzLogFile = fopen(filename, "r"))) {
591 snprintf(filename, 512, "%s/mekf_wind_%05d.csv", STRINGIFY(MEKF_WIND_LOG_PATH), ++counter);
592 }
593 pprzLogFile = fopen(filename, "w");
594 if (pprzLogFile == NULL) {
595 printf("Failed to open WE log file '%s'\n",filename);
596 } else {
597 printf("Opening WE log file '%s'\n",filename);
598 }
599#endif
600
601}
602
603
604static void reset_ref(void)
605{
606#if FIXEDWING_FIRMWARE
607 struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
608 // reset state UTM ref
610#else
611 struct EcefCoor_i ecef_pos = ecef_int_from_gps(&gps);
612 struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
613 struct LtpDef_i ltp_def;
614 ltp_def_from_ecef_i(&ltp_def, &ecef_pos);
615 ltp_def.lla.alt = lla_pos.alt;
618#endif
620 //ins_mekf_wind.gps_initialized = false;
621}
622
623static void reset_vertical_ref(void)
624{
625#if FIXEDWING_FIRMWARE
627 utm.alt = gps.hmsl / 1000.0f;
629#else
630 struct LlaCoor_i lla = {
632 .lon = stateGetNedOrigin_i()->lla.lon,
633 .alt = gps.lla_pos.alt
634 };
635 struct LtpDef_i ltp_def;
639#endif
640}
641
643{
644 switch (flag) {
645 case INS_RESET_REF:
646 reset_ref();
647 break;
650 break;
651 default:
652 // unsupported cases
653 break;
654 }
655}
656
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Main include for ABI (AirBorneInterface).
#define ABI_BROADCAST
Broadcast address.
Definition abi_common.h:58
Event structure to store callbacks in a linked list.
Definition abi_common.h:67
void ahrs_aligner_init(void)
Interface to align the AHRS via low-passed measurements at startup.
Utility functions for floating point AHRS implementations.
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
float tas_from_dynamic_pressure(float q)
Calculate true airspeed from dynamic pressure.
Definition air_data.c:424
float tas_from_eas(float eas)
Calculate true airspeed from equivalent airspeed.
Definition air_data.c:395
Air Data interface.
float baro_alt
Definition baro_bmp280.c:59
#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
int32_t nav_utm_north0
Definition common_nav.c:49
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:577
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:544
struct EcefCoor_f ecef_vel_float_from_gps(struct GpsState *gps_s)
Get GPS ecef velocity (float) Converted on the fly if not available.
Definition gps.c:509
struct GpsState gps
global GPS state
Definition gps.c:74
struct LlaCoor_i lla_int_from_gps(struct GpsState *gps_s)
Get GPS lla (integer) Converted on the fly if not available.
Definition gps.c:464
struct EcefCoor_i ecef_int_from_gps(struct GpsState *gps_s)
Get GPS ecef pos (integer) Converted on the fly if not available.
Definition gps.c:493
int32_t hmsl
height above mean sea level (MSL) in mm
Definition gps.h:94
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition gps.h:92
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
data structure for GPS information
Definition gps.h:87
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
float psi
in radians
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
euler angles
Roation quaternion.
angular rates
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
#define INT32_POS_OF_CM_DEN
#define INT32_POS_OF_CM_NUM
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
angular rates
#define ENU_OF_TO_NED(_po, _pi)
int32_t lat
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
struct LlaCoor_i lla
Reference point in lla.
int32_t lon
in degrees*1e7
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
#define NED_FLOAT_OF_BFP(_o, _i)
void ned_of_ecef_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a point from ECEF to local NED.
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
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
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
Definition pprz_isa.h:50
static void stateSetAccelNed_f(uint16_t id, struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition state.h:1147
static void stateSetNedToBodyQuat_f(uint16_t id, struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition state.h:1253
struct State state
Definition state.c:36
static struct LtpDef_f * stateGetNedOrigin_f(void)
Get the coordinate NED frame origin (float)
Definition state.h:566
static void stateSetPositionNed_f(uint16_t id, struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition state.h:718
static void stateSetLocalOrigin_i(uint16_t id, struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition state.h:519
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition state.h:251
static void stateSetPositionUtm_f(uint16_t id, struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition state.h:698
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition state.h:264
static struct UtmCoor_f * stateGetUtmOrigin_f(void)
Get the coordinate UTM frame origin (int)
Definition state.h:576
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:541
float stateGetHmslOrigin_f(void)
Get the HMSL of the frame origin (float)
Definition state.c:204
static struct LtpDef_i * stateGetNedOrigin_i(void)
Get the coordinate NED frame origin (int)
Definition state.h:556
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition state.h:1346
static void stateSetSpeedNed_f(uint16_t id, struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition state.h:947
Integrated Navigation System interface.
#define INS_RESET_VERTICAL_REF
Definition ins.h:37
#define INS_RESET_REF
flags for INS reset
Definition ins.h:36
uint32_t counter
Definition ins_flow.c:187
float ins_mekf_wind_get_baro_bias(void)
struct NedCoor_f ins_mekf_wind_get_wind_ned(void)
void ins_mekf_wind_update_incidence(float aoa, float aos)
void ins_mekf_wind_set_pos_ned(struct NedCoor_f *p)
float ins_mekf_wind_get_airspeed_norm(void)
void ins_mekf_wind_update_baro(float baro_alt)
struct NedCoor_f ins_mekf_wind_get_accel_ned(void)
void ins_mekf_wind_init(void)
Init function.
void ins_mekf_wind_set_mag_h(const struct FloatVect3 *mag_h)
struct NedCoor_f ins_mekf_wind_get_pos_ned(void)
Getter/Setter functions.
struct FloatRates ins_mekf_wind_get_body_rates(void)
void ins_mekf_wind_set_speed_ned(struct NedCoor_f *s)
void ins_mekf_wind_update_mag(struct FloatVect3 *mag, bool attitude_only)
struct FloatVect3 ins_mekf_wind_get_accel_bias(void)
struct FloatQuat ins_mekf_wind_get_quat(void)
void ins_mekf_wind_propagate(struct FloatRates *gyro, struct FloatVect3 *acc, float dt)
Full INS propagation.
void ins_mekf_wind_align(struct FloatRates *gyro_bias, struct FloatQuat *quat)
void ins_mekf_wind_propagate_ahrs(struct FloatRates *gyro, struct FloatVect3 *acc, float dt)
AHRS-only propagation + accel correction.
struct FloatRates ins_mekf_wind_get_rates_bias(void)
struct NedCoor_f ins_mekf_wind_get_speed_ned(void)
void ins_mekf_wind_update_airspeed(float airspeed)
void ins_mekf_wind_update_pos_speed(struct FloatVect3 *pos, struct FloatVect3 *speed)
Multiplicative Extended Kalman Filter in rotation matrix formulation.
static void airspeed_cb(uint8_t sender_id, float airspeed)
static void reset_vertical_ref(void)
static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static abi_event airspeed_ev
#define INS_MEKF_WIND_IMU_ID
IMU (gyro, accel)
static void reset_cb(uint8_t sender_id, uint8_t flag)
static void send_wind(struct transport_tx *trans, struct link_device *dev)
static abi_event mag_ev
static void reset_ref(void)
static abi_event accel_ev
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Call ins_mekf_wind_propagate on new gyro measurements.
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
#define INS_MEKF_WIND_INCIDENCE_ID
incidence angles
static abi_event reset_ev
static abi_event pressure_diff_ev
static abi_event gyro_ev
#define INS_MEKF_WIND_FILTER_ID
#define INS_MEKF_WIND_AIRSPEED_ID
airspeed (Pitot tube)
static abi_event aligner_ev
static uint32_t last_imu_stamp
void ins_mekf_wind_wrapper_init(void)
Init function.
static abi_event baro_ev
static abi_event geo_mag_ev
static void set_state_from_ins(void)
update state interface
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
static void incidence_cb(uint8_t sender_id, uint8_t flag, float aoa, float sideslip)
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
static struct FloatVect3 ins_mekf_wind_accel
last accel measurement
static abi_event incidence_ev
#define INS_MEKF_WIND_BARO_ID
baro
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
struct InsMekfWind ins_mekf_wind
static void geo_mag_cb(uint8_t sender_id, struct FloatVect3 *h)
static void aligner_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
static void send_euler(struct transport_tx *trans, struct link_device *dev)
logging functions
#define INS_MEKF_WIND_GPS_ID
ABI binding for gps data.
static void pressure_diff_cb(uint8_t sender_id, float pdyn)
#define INS_MEKF_WIND_MAG_ID
magnetometer
static abi_event gps_ev
Paparazzi specific wrapper to run MEKF-Wind INS filter.
#define ins_mekf_wind_wrapper_Reset(_v)
filter structure
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
uint16_t foo
Definition main_demo5.c:58
Fixedwing Navigation library.
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
float z
in meters
float x
in meters
float east
in meters
float north
in meters
float y
in meters
vector in EarthCenteredEarthFixed coordinates
vector in East North Up coordinates Units: meters
vector in North East Down coordinates Units: meters
position in UTM coordinates Units: meters
Paparazzi atmospheric pressure conversion utilities.
FileDes pprzLogFile
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition usb_ser_hw.c:74
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition sys_time.h:138
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
uint16_t val[TCOUPLE_NB]
float alpha
Definition textons.c:133
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.