Paparazzi UAS  v5.15_devel-81-gd13dafb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "subsystems/ahrs/ahrs_float_utils.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "subsystems/ins.h"
#include "subsystems/abi.h"
#include "math/pprz_isa.h"
#include "state.h"
#include "generated/airframe.h"
#include "generated/flight_plan.h"
#include "subsystems/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 MEKF_WIND_USE_UTM   TRUE
 
#define INS_MEKF_WIND_FILTER_ID   3
 
#define INS_MEKF_WIND_AIRSPEED_ID   ABI_BROADCAST
 airspeed (Pitot tube) More...
 
#define INS_MEKF_WIND_INCIDENCE_ID   ABI_BROADCAST
 incidence angles More...
 
#define INS_MEKF_WIND_BARO_ID   ABI_BROADCAST
 baro More...
 
#define INS_MEKF_WIND_IMU_ID   ABI_BROADCAST
 IMU (gyro, accel) More...
 
#define INS_MEKF_WIND_MAG_ID   ABI_BROADCAST
 magnetometer More...
 
#define INS_MEKF_WIND_GPS_ID   GPS_MULTI_ID
 ABI binding for gps data. More...
 

Functions

static void set_state_from_ins (void)
 update state interface More...
 
static void send_euler (struct transport_tx *trans, struct link_device *dev)
 logging functions More...
 
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 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. More...
 
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 body_to_imu_cb (uint8_t sender_id, struct FloatQuat *q_b2i)
 
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. More...
 
void ins_reset_local_origin (void)
 local implemetation of the ins_reset functions More...
 
void ins_reset_altitude_ref (void)
 INS altitude reference reset. More...
 

Variables

struct InsMekfWind ins_mekf_wind
 
static struct FloatVect3 ins_mekf_wind_accel
 last accel measurement More...
 
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 body_to_imu_ev
 
static abi_event geo_mag_ev
 
static abi_event gps_ev
 

Detailed Description

Paparazzi specific wrapper to run MEKF-Wind INS filter.

Definition in file ins_mekf_wind_wrapper.c.

Macro Definition Documentation

#define INS_MEKF_WIND_AIRSPEED_ID   ABI_BROADCAST

airspeed (Pitot tube)

Definition at line 160 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

#define INS_MEKF_WIND_BARO_ID   ABI_BROADCAST

baro

Definition at line 175 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

#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().

#define INS_MEKF_WIND_GPS_ID   GPS_MULTI_ID

ABI binding for gps data.

Used for GPS ABI messages.

Definition at line 196 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

#define INS_MEKF_WIND_IMU_ID   ABI_BROADCAST

IMU (gyro, accel)

Definition at line 182 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

#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().

#define INS_MEKF_WIND_MAG_ID   ABI_BROADCAST

magnetometer

Definition at line 188 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

#define MEKF_WIND_USE_UTM   TRUE

Definition at line 41 of file ins_mekf_wind_wrapper.c.

Function Documentation

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

Definition at line 381 of file ins_mekf_wind_wrapper.c.

References ACCELS_FLOAT_OF_BFP, InsMekfWind::body_to_imu, float_rmat_transp_vmult(), ins_mekf_wind, ins_mekf_wind_accel, and orientationGetRMat_f().

Referenced by ins_mekf_wind_wrapper_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void airspeed_cb ( uint8_t  sender_id,
float  airspeed 
)
static

Definition at line 269 of file ins_mekf_wind_wrapper.c.

References get_sys_time_float(), ins_mekf_wind, ins_mekf_wind_update_airspeed(), InsMekfWind::is_aligned, pprzLogFile, and tas_from_eas().

Referenced by ins_mekf_wind_wrapper_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void aligner_cb ( uint8_t  sender_id,
uint32_t  stamp,
struct Int32Rates lp_gyro,
struct Int32Vect3 lp_accel,
struct Int32Vect3 lp_mag 
)
static
static void baro_cb ( uint8_t  sender_id,
uint32_t  stamp,
float  pressure 
)
static
static void body_to_imu_cb ( uint8_t  sender_id,
struct FloatQuat q_b2i 
)
static

Definition at line 447 of file ins_mekf_wind_wrapper.c.

References InsMekfWind::body_to_imu, ins_mekf_wind, ins_mekf_wind_set_quat(), InsMekfWind::is_aligned, and orientationSetQuat_f().

Referenced by ins_mekf_wind_wrapper_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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

Definition at line 457 of file ins_mekf_wind_wrapper.c.

References ins_mekf_wind_set_mag_h().

Referenced by ins_mekf_wind_wrapper_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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

Definition at line 282 of file ins_mekf_wind_wrapper.c.

References get_sys_time_float(), ins_mekf_wind, ins_mekf_wind_update_incidence(), InsMekfWind::is_aligned, and pprzLogFile.

Referenced by ins_mekf_wind_wrapper_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void ins_reset_altitude_ref ( void  )

INS altitude reference reset.

Reset only vertical reference to the current altitude. Does nothing if not implemented by specific INS algorithm.

Definition at line 648 of file ins_mekf_wind_wrapper.c.

Referenced by nav_reset_alt().

+ Here is the caller graph for this function:

void ins_reset_local_origin ( void  )

local implemetation of the ins_reset functions

INS local origin reset.

Definition at line 632 of file ins_mekf_wind_wrapper.c.

Referenced by nav_reset_reference().

+ Here is the caller graph for this function:

static void mag_cb ( uint8_t  sender_id,
uint32_t  stamp,
struct Int32Vect3 mag 
)
static
static void pressure_diff_cb ( uint8_t  sender_id,
float  pdyn 
)
static

Definition at line 255 of file ins_mekf_wind_wrapper.c.

References get_sys_time_float(), ins_mekf_wind, ins_mekf_wind_update_airspeed(), InsMekfWind::is_aligned, 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:

static 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 float_eulers_of_quat(), INS_MEKF_WIND_FILTER_ID, ins_mekf_wind_get_quat(), FloatEulers::phi, FloatEulers::psi, and FloatEulers::theta.

Referenced by ins_mekf_wind_wrapper_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static 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 InsMekfWind::baro_initialized, get_sys_time_usec(), InsMekfWind::gps_initialized, ins_mekf_wind, INS_MEKF_WIND_FILTER_ID, InsMekfWind::is_aligned, last_imu_stamp, and val.

Referenced by ins_mekf_wind_wrapper_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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

Definition at line 91 of file ins_mekf_wind_wrapper.c.

References 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().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static 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 525 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

abi_event accel_ev
static

Definition at line 206 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

abi_event airspeed_ev
static

Definition at line 201 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

abi_event aligner_ev
static

Definition at line 207 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

abi_event baro_ev
static

Definition at line 203 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

abi_event body_to_imu_ev
static

Definition at line 208 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

abi_event geo_mag_ev
static

Definition at line 209 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

abi_event gps_ev
static

Definition at line 210 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

abi_event gyro_ev
static

Definition at line 205 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

abi_event incidence_ev
static

Definition at line 202 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

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().

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().

abi_event mag_ev
static

Definition at line 204 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().

abi_event pressure_diff_ev
static

Definition at line 200 of file ins_mekf_wind_wrapper.c.

Referenced by ins_mekf_wind_wrapper_init().