ins_float_invariant_wrapper.c File Reference

Paparazzi specific wrapper to run INVARIANT filter. More...

#include "subsystems/ins/ins_float_invariant_wrapper.h"
#include "subsystems/abi.h"
#include "mcu_periph/sys_time.h"
#include "message_pragmas.h"
#include "subsystems/datalink/telemetry.h"
#include "state.h"

Include dependency graph for ins_float_invariant_wrapper.c:

Go to the source code of this file.

Macros
#define	INS_FINV_FILTER_ID   2
#define	INS_FINV_BARO_ID   ABI_BROADCAST
	baro More...
#define	INS_FINV_IMU_ID   ABI_BROADCAST
	IMU (gyro, accel) More...
#define	INS_FINV_MAG_ID   ABI_BROADCAST
	magnetometer More...
#define	INS_FINV_GPS_ID   GPS_MULTI_ID
	ABI binding for gps data. More...

Functions
static void	send_ins_ref (struct transport_tx *trans, struct link_device *dev)
static void	send_filter_status (struct transport_tx *trans, struct link_device *dev)
static void	baro_cb (uint8_t sender_id, float pressure)
static void	gyro_cb (uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
	Call ins_float_invariant_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_f)
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_float_invariant_wrapper_init (void)

Variables
static struct FloatVect3	ins_finv_accel
	last accel measurement More...
static uint32_t	ins_finv_last_stamp = 0
	last gyro msg timestamp More...
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 INVARIANT filter.

Definition in file ins_float_invariant_wrapper.c.

Macro Definition Documentation

#define INS_FINV_BARO_ID   ABI_BROADCAST

baro

Definition at line 80 of file ins_float_invariant_wrapper.c.

Referenced by ins_float_invariant_wrapper_init().

#define INS_FINV_FILTER_ID   2

Definition at line 34 of file ins_float_invariant_wrapper.c.

Referenced by send_filter_status().

#define INS_FINV_GPS_ID   GPS_MULTI_ID

ABI binding for gps data.

Used for GPS ABI messages.

Definition at line 101 of file ins_float_invariant_wrapper.c.

Referenced by ins_float_invariant_wrapper_init().

#define INS_FINV_IMU_ID   ABI_BROADCAST

IMU (gyro, accel)

Definition at line 87 of file ins_float_invariant_wrapper.c.

Referenced by ins_float_invariant_wrapper_init().

#define INS_FINV_MAG_ID   ABI_BROADCAST

magnetometer

Definition at line 93 of file ins_float_invariant_wrapper.c.

Referenced by ins_float_invariant_wrapper_init().

Function Documentation

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

static

Definition at line 150 of file ins_float_invariant_wrapper.c.

References ACCELS_FLOAT_OF_BFP, and ins_finv_accel.

Referenced by ins_float_invariant_wrapper_init().

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

Definition at line 168 of file ins_float_invariant_wrapper.c.

References ACCELS_FLOAT_OF_BFP, ins_float_inv, ins_float_invariant_align(), InsFloatInv::is_aligned, MAGS_FLOAT_OF_BFP, and RATES_FLOAT_OF_BFP.

Referenced by ins_float_invariant_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

static void baro_cb ( uint8_t  sender_id, float  pressure  )

static

Definition at line 114 of file ins_float_invariant_wrapper.c.

References ins_float_invariant_update_baro().

Referenced by ins_float_invariant_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

static void body_to_imu_cb ( uint8_t  sender_id, struct FloatQuat *  q_b2i_f  )

static

Definition at line 185 of file ins_float_invariant_wrapper.c.

References ins_float_inv_set_body_to_imu_quat().

Referenced by ins_float_invariant_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 191 of file ins_float_invariant_wrapper.c.

References ins_float_inv, and InsFloatInv::mag_h.

Referenced by ins_float_invariant_wrapper_init().

Here is the caller graph for this function:

static void gps_cb ( uint8_t  sender_id, uint32_t  stamp, struct GpsState *  gps_s  )

static

Definition at line 196 of file ins_float_invariant_wrapper.c.

References ins_float_invariant_update_gps().

Referenced by ins_float_invariant_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

static void gyro_cb ( uint8_t  sender_id, uint32_t  stamp, struct Int32Rates *  gyro  )

static

Call ins_float_invariant_propagate on new gyro measurements.

Since acceleration measurement is also needed for propagation, use the last stored accel from ins_finv_accel.

Definition at line 124 of file ins_float_invariant_wrapper.c.

References dt, ins_finv_accel, ins_finv_last_stamp, ins_float_invariant_propagate(), PRINT_CONFIG_MSG(), and RATES_FLOAT_OF_BFP.

Referenced by ins_float_invariant_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

void ins_float_invariant_wrapper_init

(

void

)

Definition at line 204 of file ins_float_invariant_wrapper.c.

References ABI_BROADCAST, accel_cb(), accel_ev, aligner_cb(), aligner_ev, baro_cb(), baro_ev, body_to_imu_cb(), body_to_imu_ev, DefaultPeriodic, geo_mag_cb(), geo_mag_ev, gps_cb(), gps_ev, gyro_cb(), gyro_ev, INS_FINV_BARO_ID, INS_FINV_GPS_ID, INS_FINV_IMU_ID, INS_FINV_MAG_ID, ins_float_invariant_init(), mag_cb(), mag_ev, register_periodic_telemetry(), send_filter_status(), and send_ins_ref().

Here is the call graph for this function:

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

static

Definition at line 157 of file ins_float_invariant_wrapper.c.

References ins_float_inv, ins_float_invariant_update_mag(), InsFloatInv::is_aligned, and MAGS_FLOAT_OF_BFP.

Referenced by ins_float_invariant_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 58 of file ins_float_invariant_wrapper.c.

References get_sys_time_usec(), INS_FINV_FILTER_ID, ins_finv_last_stamp, ins_float_inv, InsFloatInv::is_aligned, and val.

Referenced by ins_float_invariant_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

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

static

Definition at line 46 of file ins_float_invariant_wrapper.c.

References LlaCoor_i::alt, LtpDef_i::ecef, foo, LtpDef_i::hmsl, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, State::ned_initialized_i, State::ned_origin_i, state, EcefCoor_i::x, EcefCoor_i::y, and EcefCoor_i::z.

Referenced by ins_float_invariant_wrapper_init().

Here is the caller graph for this function:

Variable Documentation

abi_event accel_ev

static

Definition at line 108 of file ins_float_invariant_wrapper.c.

Referenced by ins_float_invariant_wrapper_init().

abi_event aligner_ev

static

Definition at line 109 of file ins_float_invariant_wrapper.c.

Referenced by ins_float_invariant_wrapper_init().

abi_event baro_ev

static

Definition at line 105 of file ins_float_invariant_wrapper.c.

Referenced by ins_float_invariant_wrapper_init().

abi_event body_to_imu_ev

static

Definition at line 110 of file ins_float_invariant_wrapper.c.

Referenced by ins_float_invariant_wrapper_init().

abi_event geo_mag_ev

static

Definition at line 111 of file ins_float_invariant_wrapper.c.

Referenced by ins_float_invariant_wrapper_init().

abi_event gps_ev

static

Definition at line 112 of file ins_float_invariant_wrapper.c.

Referenced by ins_float_invariant_wrapper_init().

abi_event gyro_ev

static

Definition at line 107 of file ins_float_invariant_wrapper.c.

Referenced by ins_float_invariant_wrapper_init().

struct FloatVect3 ins_finv_accel

static

last accel measurement

Definition at line 38 of file ins_float_invariant_wrapper.c.

Referenced by accel_cb(), and gyro_cb().

uint32_t ins_finv_last_stamp = 0

static

last gyro msg timestamp

Definition at line 41 of file ins_float_invariant_wrapper.c.

Referenced by gyro_cb(), and send_filter_status().

abi_event mag_ev

static

Definition at line 106 of file ins_float_invariant_wrapper.c.

Referenced by ins_float_invariant_wrapper_init().