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

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

#include "modules/ins/ins_float_invariant_wrapper.h"
#include "modules/core/abi.h"
#include "mcu_periph/sys_time.h"
#include "message_pragmas.h"
#include "modules/ahrs/ahrs_aligner.h"
#include "modules/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   AIR_DATA_SENDER_ID
 baro
#define INS_FINV_IMU_ID   ABI_BROADCAST
 IMU (gyro, accel).
#define INS_FINV_MAG_ID   ABI_BROADCAST
 magnetometer
#define INS_FINV_GPS_ID   GPS_MULTI_ID
 ABI binding for gps data.

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, uint32_t stamp, 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.
static void accel_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
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 mag_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *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)
static void reset_cb (uint8_t sender_id UNUSED, uint8_t flag)
void ins_float_invariant_wrapper_init (void)

Variables

static struct FloatVect3 ins_finv_accel
 last accel measurement
static uint32_t ins_finv_last_stamp = 0
 last gyro msg timestamp
static abi_event baro_ev
static abi_event gyro_ev
static abi_event accel_ev
static abi_event aligner_ev
static abi_event mag_ev
static abi_event geo_mag_ev
static abi_event gps_ev
static abi_event reset_ev

Detailed Description

Paparazzi specific wrapper to run INVARIANT filter.

Definition in file ins_float_invariant_wrapper.c.

Macro Definition Documentation

◆ INS_FINV_BARO_ID

#define INS_FINV_BARO_ID   AIR_DATA_SENDER_ID

baro

Definition at line 79 of file ins_float_invariant_wrapper.c.

Referenced by ins_float_invariant_wrapper_init().

◆ INS_FINV_FILTER_ID

#define INS_FINV_FILTER_ID   2

Definition at line 37 of file ins_float_invariant_wrapper.c.

Referenced by send_filter_status().

◆ INS_FINV_GPS_ID

#define INS_FINV_GPS_ID   GPS_MULTI_ID

ABI binding for gps data.

Used for GPS ABI messages.

Definition at line 103 of file ins_float_invariant_wrapper.c.

Referenced by ins_float_invariant_wrapper_init().

◆ INS_FINV_IMU_ID

#define INS_FINV_IMU_ID   ABI_BROADCAST

IMU (gyro, accel).

Definition at line 85 of file ins_float_invariant_wrapper.c.

Referenced by ins_float_invariant_wrapper_init().

◆ INS_FINV_MAG_ID

#define INS_FINV_MAG_ID   ABI_BROADCAST

magnetometer

Definition at line 92 of file ins_float_invariant_wrapper.c.

Referenced by ins_float_invariant_wrapper_init().

Function Documentation

◆ accel_cb()

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

Definition at line 160 of file ins_float_invariant_wrapper.c.

References ACCELS_FLOAT_OF_BFP, and ins_finv_accel.

◆ 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

Definition at line 167 of file ins_float_invariant_wrapper.c.

References ACCELS_FLOAT_OF_BFP, ins_float_inv, ins_float_invariant_align(), MAGS_FLOAT_OF_BFP, and RATES_FLOAT_OF_BFP.

Here is the call graph for this function:

◆ baro_cb()

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

Definition at line 118 of file ins_float_invariant_wrapper.c.

References ins_float_invariant_update_baro().

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 196 of file ins_float_invariant_wrapper.c.

References h(), and ins_float_inv.

Here is the call graph for this function:

◆ gps_cb()

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

Definition at line 202 of file ins_float_invariant_wrapper.c.

References ins_float_invariant_update_gps().

Here is the call graph for this function:

◆ gyro_cb()

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 128 of file ins_float_invariant_wrapper.c.

References ahrs_aligner_restart(), ins_finv_accel, ins_finv_last_stamp, ins_float_inv, ins_float_invariant_propagate(), PRINT_CONFIG_MSG(), PRINT_CONFIG_VAR(), and RATES_FLOAT_OF_BFP.

Here is the call graph for this function:

◆ ins_float_invariant_wrapper_init()

◆ mag_cb()

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

Definition at line 185 of file ins_float_invariant_wrapper.c.

References ins_float_inv, ins_float_invariant_update_mag(), and MAGS_FLOAT_OF_BFP.

Here is the call graph for this function:

◆ reset_cb()

void reset_cb ( uint8_t sender_id UNUSED,
uint8_t flag )
static

Definition at line 209 of file ins_float_invariant_wrapper.c.

References ins_float_invariant_reset_ref(), ins_float_invariant_reset_vertical_ref(), INS_RESET_REF, INS_RESET_VERTICAL_REF, and UNUSED.

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 60 of file ins_float_invariant_wrapper.c.

References dev, get_sys_time_usec(), INS_FINV_FILTER_ID, ins_finv_last_stamp, ins_float_inv, and val.

Here is the call graph for this function:

◆ send_ins_ref()

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

Variable Documentation

◆ accel_ev

abi_event accel_ev
static

Definition at line 109 of file ins_float_invariant_wrapper.c.

◆ aligner_ev

abi_event aligner_ev
static

Definition at line 110 of file ins_float_invariant_wrapper.c.

◆ baro_ev

abi_event baro_ev
static

Definition at line 107 of file ins_float_invariant_wrapper.c.

◆ geo_mag_ev

abi_event geo_mag_ev
static

Definition at line 113 of file ins_float_invariant_wrapper.c.

◆ gps_ev

abi_event gps_ev
static

Definition at line 115 of file ins_float_invariant_wrapper.c.

◆ gyro_ev

abi_event gyro_ev
static

Definition at line 108 of file ins_float_invariant_wrapper.c.

◆ ins_finv_accel

struct FloatVect3 ins_finv_accel
static

last accel measurement

Definition at line 41 of file ins_float_invariant_wrapper.c.

Referenced by accel_cb(), and gyro_cb().

◆ ins_finv_last_stamp

uint32_t ins_finv_last_stamp = 0
static

last gyro msg timestamp

Definition at line 44 of file ins_float_invariant_wrapper.c.

Referenced by gyro_cb(), and send_filter_status().

◆ mag_ev

abi_event mag_ev
static

Definition at line 112 of file ins_float_invariant_wrapper.c.

◆ reset_ev

abi_event reset_ev
static

Definition at line 116 of file ins_float_invariant_wrapper.c.