Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
ins_mekf_wind_wrapper.c File Reference

Paparazzi specific wrapper to run MEKF-Wind INS filter. More...

#include "modules/ins/ins_mekf_wind_wrapper.h"
#include "modules/ins/ins_mekf_wind.h"
#include "modules/air_data/air_data.h"
#include "modules/ahrs/ahrs_float_utils.h"
#include "modules/ahrs/ahrs_aligner.h"
#include "modules/ins/ins.h"
#include "modules/core/abi.h"
#include "math/pprz_isa.h"
#include "state.h"
#include "generated/airframe.h"
#include "generated/flight_plan.h"
#include "modules/datalink/telemetry.h"
#include "mcu_periph/sys_time.h"
Include dependency graph for ins_mekf_wind_wrapper.c:

Go to the source code of this file.

Macros

#define INS_MEKF_WIND_FILTER_ID   3
#define INS_MEKF_WIND_AIRSPEED_ID   AIR_DATA_SENDER_ID
 airspeed (Pitot tube)
#define INS_MEKF_WIND_INCIDENCE_ID   ABI_BROADCAST
 incidence angles
#define INS_MEKF_WIND_BARO_ID   AIR_DATA_SENDER_ID
 baro
#define INS_MEKF_WIND_IMU_ID   ABI_BROADCAST
 IMU (gyro, accel).
#define INS_MEKF_WIND_MAG_ID   ABI_BROADCAST
 magnetometer
#define INS_MEKF_WIND_GPS_ID   GPS_MULTI_ID
 ABI binding for gps data.

Functions

static void set_state_from_ins (void)
 update state interface
static void send_euler (struct transport_tx *trans, struct link_device *dev)
 logging functions
static void send_wind (struct transport_tx *trans, struct link_device *dev)
static void send_filter_status (struct transport_tx *trans, struct link_device *dev)
static void send_inv_filter (struct transport_tx *trans, struct link_device *dev)
static void reset_cb (uint8_t sender_id, uint8_t flag)
static void baro_cb (uint8_t sender_id, uint32_t stamp, float pressure)
static void pressure_diff_cb (uint8_t sender_id, float pdyn)
static void airspeed_cb (uint8_t sender_id, float airspeed)
static void incidence_cb (uint8_t sender_id, uint8_t flag, float aoa, float sideslip)
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 accel_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
static void mag_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
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 geo_mag_cb (uint8_t sender_id, struct FloatVect3 *h)
static void gps_cb (uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
void ins_mekf_wind_wrapper_init (void)
 Init function.
static void reset_ref (void)
static void reset_vertical_ref (void)

Variables

struct InsMekfWind ins_mekf_wind
static struct FloatVect3 ins_mekf_wind_accel
 last accel measurement
static uint32_t last_imu_stamp = 0
static abi_event pressure_diff_ev
static abi_event airspeed_ev
static abi_event incidence_ev
static abi_event baro_ev
static abi_event mag_ev
static abi_event gyro_ev
static abi_event accel_ev
static abi_event aligner_ev
static abi_event geo_mag_ev
static abi_event gps_ev
static abi_event reset_ev

Detailed Description

Paparazzi specific wrapper to run MEKF-Wind INS filter.

Definition in file ins_mekf_wind_wrapper.c.

Macro Definition Documentation

◆ INS_MEKF_WIND_AIRSPEED_ID

#define INS_MEKF_WIND_AIRSPEED_ID   AIR_DATA_SENDER_ID

airspeed (Pitot tube)

Definition at line 160 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

◆ INS_MEKF_WIND_BARO_ID

#define INS_MEKF_WIND_BARO_ID   AIR_DATA_SENDER_ID

baro

Definition at line 172 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

◆ INS_MEKF_WIND_FILTER_ID

#define INS_MEKF_WIND_FILTER_ID   3

Definition at line 47 of file ins_mekf_wind_wrapper.c.

Referenced by send_euler(), and send_filter_status().

◆ INS_MEKF_WIND_GPS_ID

#define INS_MEKF_WIND_GPS_ID   GPS_MULTI_ID

ABI binding for gps data.

Used for GPS ABI messages.

Definition at line 192 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

◆ INS_MEKF_WIND_IMU_ID

#define INS_MEKF_WIND_IMU_ID   ABI_BROADCAST

IMU (gyro, accel).

Definition at line 178 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

◆ INS_MEKF_WIND_INCIDENCE_ID

#define INS_MEKF_WIND_INCIDENCE_ID   ABI_BROADCAST

incidence angles

Definition at line 166 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

◆ INS_MEKF_WIND_MAG_ID

#define INS_MEKF_WIND_MAG_ID   ABI_BROADCAST

magnetometer

Definition at line 184 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

Function Documentation

◆ accel_cb()

void accel_cb ( uint8_t sender_id,
uint32_t stamp,
struct Int32Vect3 * accel )
static

Definition at line 375 of file ins_mekf_wind_wrapper.c.

References ACCELS_FLOAT_OF_BFP, and ins_mekf_wind_accel.

◆ airspeed_cb()

void airspeed_cb ( uint8_t sender_id,
float airspeed )
static

Definition at line 266 of file ins_mekf_wind_wrapper.c.

References get_sys_time_float(), ins_mekf_wind, ins_mekf_wind_update_airspeed(), LogFileIsOpen, pprzLogFile, and tas_from_eas().

Here is the call graph for this function:

◆ aligner_cb()

void aligner_cb ( uint8_t sender_id,
uint32_t stamp,
struct Int32Rates * lp_gyro,
struct Int32Vect3 * lp_accel,
struct Int32Vect3 * lp_mag )
static

◆ baro_cb()

void baro_cb ( uint8_t sender_id,
uint32_t stamp,
float pressure )
static

Definition at line 210 of file ins_mekf_wind_wrapper.c.

References alpha, baro_cb(), if(), ins_mekf_wind, and PPRZ_ISA_SEA_LEVEL_PRESSURE.

Here is the call graph for this function:

◆ geo_mag_cb()

void geo_mag_cb ( uint8_t sender_id,
struct FloatVect3 * h )
static

Definition at line 430 of file ins_mekf_wind_wrapper.c.

References h(), and ins_mekf_wind_set_mag_h().

Here is the call graph for this function:

◆ gps_cb()

◆ gyro_cb()

◆ incidence_cb()

void incidence_cb ( uint8_t sender_id,
uint8_t flag,
float aoa,
float sideslip )
static

Definition at line 279 of file ins_mekf_wind_wrapper.c.

References get_sys_time_float(), ins_mekf_wind, ins_mekf_wind_update_incidence(), LogFileIsOpen, and pprzLogFile.

Here is the call graph for this function:

◆ ins_mekf_wind_wrapper_init()

◆ mag_cb()

void mag_cb ( uint8_t sender_id,
uint32_t stamp,
struct Int32Vect3 * mag )
static

◆ pressure_diff_cb()

void pressure_diff_cb ( uint8_t sender_id,
float pdyn )
static

Definition at line 252 of file ins_mekf_wind_wrapper.c.

References get_sys_time_float(), ins_mekf_wind, ins_mekf_wind_update_airspeed(), LogFileIsOpen, pprzLogFile, and tas_from_dynamic_pressure().

Referenced by ins_mekf_wind_wrapper_init().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ reset_cb()

void reset_cb ( uint8_t sender_id UNUSED,
uint8_t flag )
static

Definition at line 638 of file ins_mekf_wind_wrapper.c.

References INS_RESET_REF, INS_RESET_VERTICAL_REF, reset_ref(), reset_vertical_ref(), and UNUSED.

Here is the call graph for this function:

◆ reset_ref()

void reset_ref ( void )
static

Definition at line 600 of file ins_mekf_wind_wrapper.c.

References LlaCoor_i::alt, ecef_int_from_gps(), gps, ins_mekf_wind_wrapper_Reset, lla_int_from_gps(), ltp_def, ltp_def_from_ecef_i(), stateSetLocalOrigin_i(), stateSetLocalUtmOrigin_f(), and utm_float_from_gps().

Referenced by reset_cb().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ reset_vertical_ref()

void reset_vertical_ref ( void )
static

Definition at line 619 of file ins_mekf_wind_wrapper.c.

References UtmCoor_f::alt, gps, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, ltp_def, ltp_def_from_lla_i(), stateGetNedOrigin_i(), stateGetUtmOrigin_f(), stateSetLocalOrigin_i(), and stateSetLocalUtmOrigin_f().

Referenced by reset_cb().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ send_euler()

void send_euler ( struct transport_tx * trans,
struct link_device * dev )
static

logging functions

telemetry functions

Definition at line 78 of file ins_mekf_wind_wrapper.c.

References dev, float_eulers_of_quat(), INS_MEKF_WIND_FILTER_ID, ins_mekf_wind_get_quat(), FloatEulers::phi, FloatEulers::psi, and FloatEulers::theta.

Here is the call graph for this function:

◆ send_filter_status()

void send_filter_status ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 102 of file ins_mekf_wind_wrapper.c.

References dev, get_sys_time_usec(), ins_mekf_wind, INS_MEKF_WIND_FILTER_ID, last_imu_stamp, and val.

Here is the call graph for this function:

◆ send_inv_filter()

◆ send_wind()

void send_wind ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 91 of file ins_mekf_wind_wrapper.c.

References dev, ENU_OF_TO_NED, ins_mekf_wind_get_airspeed_norm(), ins_mekf_wind_get_wind_ned(), EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

Referenced by ins_mekf_wind_wrapper_init(), and wind_estimation_quadrotor_init().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ set_state_from_ins()

void set_state_from_ins ( void )
static

update state interface

Set current state (ltp to body orientation/rates and ned pos/speed/accel.

Definition at line 496 of file ins_mekf_wind_wrapper.c.

References ins_mekf_wind_get_accel_ned(), ins_mekf_wind_get_body_rates(), ins_mekf_wind_get_pos_ned(), ins_mekf_wind_get_quat(), ins_mekf_wind_get_speed_ned(), stateSetAccelNed_f(), stateSetBodyRates_f(), stateSetNedToBodyQuat_f(), stateSetPositionNed_f(), and stateSetSpeedNed_f().

Referenced by aligner_cb(), and gyro_cb().

Here is the call graph for this function:
Here is the caller graph for this function:

Variable Documentation

◆ accel_ev

abi_event accel_ev
static

Definition at line 202 of file ins_mekf_wind_wrapper.c.

◆ airspeed_ev

abi_event airspeed_ev
static

Definition at line 197 of file ins_mekf_wind_wrapper.c.

◆ aligner_ev

abi_event aligner_ev
static

Definition at line 203 of file ins_mekf_wind_wrapper.c.

◆ baro_ev

abi_event baro_ev
static

Definition at line 199 of file ins_mekf_wind_wrapper.c.

◆ geo_mag_ev

abi_event geo_mag_ev
static

Definition at line 204 of file ins_mekf_wind_wrapper.c.

◆ gps_ev

abi_event gps_ev
static

Definition at line 205 of file ins_mekf_wind_wrapper.c.

◆ gyro_ev

abi_event gyro_ev
static

Definition at line 201 of file ins_mekf_wind_wrapper.c.

◆ incidence_ev

abi_event incidence_ev
static

Definition at line 198 of file ins_mekf_wind_wrapper.c.

◆ ins_mekf_wind

◆ ins_mekf_wind_accel

struct FloatVect3 ins_mekf_wind_accel
static

last accel measurement

Definition at line 53 of file ins_mekf_wind_wrapper.c.

Referenced by accel_cb(), and gyro_cb().

◆ last_imu_stamp

uint32_t last_imu_stamp = 0
static

Definition at line 54 of file ins_mekf_wind_wrapper.c.

Referenced by gyro_cb(), and send_filter_status().

◆ mag_ev

abi_event mag_ev
static

Definition at line 200 of file ins_mekf_wind_wrapper.c.

◆ pressure_diff_ev

abi_event pressure_diff_ev
static

Definition at line 196 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

◆ reset_ev

abi_event reset_ev
static

Definition at line 206 of file ins_mekf_wind_wrapper.c.