ahrs_float_invariant_wrapper.c File Reference

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

#include "subsystems/ahrs/ahrs_float_invariant_wrapper.h"
#include "subsystems/ahrs.h"
#include "subsystems/abi.h"
#include "mcu_periph/sys_time.h"
#include "message_pragmas.h"
#include "state.h"

Include dependency graph for ahrs_float_invariant_wrapper.c:

Go to the source code of this file.

Macros
#define	AHRS_FINV_OUTPUT_ENABLED   TRUE
#define	AHRS_FINV_IMU_ID   ABI_BROADCAST
	IMU (gyro, accel) More...
#define	AHRS_FINV_MAG_ID   AHRS_FINV_IMU_ID
	magnetometer More...

Functions
static void	compute_body_orientation_and_rates (void)
	Compute body orientation and rates from imu orientation and rates. More...
static void	gyro_cb (uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
	Call ahrs_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 bool_t	ahrs_float_invariant_enable_output (bool_t enable)
void	ahrs_float_invariant_register (void)

Variables
static bool_t	ahrs_finv_output_enabled
	if TRUE with push the estimation results to the state interface More...
static uint32_t	ahrs_finv_last_stamp = 0
	last gyro msg timestamp More...
static uint8_t	ahrs_finv_id = AHRS_COMP_ID_FINV
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

Detailed Description

Paparazzi specific wrapper to run INVARIANT ahrs filter.

Definition in file ahrs_float_invariant_wrapper.c.

Macro Definition Documentation

#define AHRS_FINV_IMU_ID   ABI_BROADCAST

IMU (gyro, accel)

Definition at line 104 of file ahrs_float_invariant_wrapper.c.

Referenced by ahrs_float_invariant_register().

#define AHRS_FINV_MAG_ID   AHRS_FINV_IMU_ID

magnetometer

Definition at line 110 of file ahrs_float_invariant_wrapper.c.

Referenced by ahrs_float_invariant_register().

#define AHRS_FINV_OUTPUT_ENABLED   TRUE

Definition at line 36 of file ahrs_float_invariant_wrapper.c.

Referenced by ahrs_float_invariant_register().

Function Documentation

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

static

Definition at line 153 of file ahrs_float_invariant_wrapper.c.

References ahrs_float_inv, ahrs_float_invariant_update_accel(), and AhrsFloatInv::is_aligned.

Referenced by ahrs_float_invariant_register().

Here is the call graph for this function:

Here is the caller graph for this function:

static bool_t ahrs_float_invariant_enable_output ( bool_t  enable )

static

Definition at line 193 of file ahrs_float_invariant_wrapper.c.

References ahrs_finv_output_enabled.

Referenced by ahrs_float_invariant_register().

Here is the caller graph for this function:

void ahrs_float_invariant_register

(

void

)

Definition at line 223 of file ahrs_float_invariant_wrapper.c.

References ABI_BROADCAST, accel_cb(), accel_ev, AHRS_FINV_IMU_ID, AHRS_FINV_MAG_ID, AHRS_FINV_OUTPUT_ENABLED, ahrs_float_invariant_enable_output(), ahrs_float_invariant_init(), ahrs_register_impl(), aligner_cb(), aligner_ev, body_to_imu_cb(), body_to_imu_ev, DefaultPeriodic, geo_mag_cb(), geo_mag_ev, gyro_cb(), gyro_ev, mag_cb(), mag_ev, and register_periodic_telemetry().

Here is the call 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 171 of file ahrs_float_invariant_wrapper.c.

References ahrs_float_inv, ahrs_float_invariant_align(), compute_body_orientation_and_rates(), and AhrsFloatInv::is_aligned.

Referenced by ahrs_float_invariant_register().

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 182 of file ahrs_float_invariant_wrapper.c.

References ahrs_float_inv_set_body_to_imu_quat().

Referenced by ahrs_float_invariant_register().

Here is the call graph for this function:

Here is the caller graph for this function:

static void compute_body_orientation_and_rates ( void  )

static

Compute body orientation and rates from imu orientation and rates.

Definition at line 202 of file ahrs_float_invariant_wrapper.c.

References ahrs_float_inv, inv_state::bias, AhrsFloatInv::body_to_imu, AhrsFloatInv::cmd, float_quat_comp_inv(), float_rmat_transp_ratemult(), orientationGetQuat_f(), orientationGetRMat_f(), inv_state::quat, inv_command::rates, RATES_DIFF, AhrsFloatInv::state, stateSetBodyRates_f(), and stateSetNedToBodyQuat_f().

Referenced by aligner_cb(), and gyro_cb().

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 188 of file ahrs_float_invariant_wrapper.c.

References ahrs_float_inv, and AhrsFloatInv::mag_h.

Referenced by ahrs_float_invariant_register().

Here is the caller graph for this function:

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

static

Call ahrs_float_invariant_propagate on new gyro measurements.

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

Definition at line 126 of file ahrs_float_invariant_wrapper.c.

References ahrs_float_inv, ahrs_float_invariant_propagate(), AHRS_PROPAGATE_FREQUENCY, compute_body_orientation_and_rates(), AhrsFloatInv::is_aligned, and PRINT_CONFIG_MSG().

Referenced by ahrs_float_invariant_register().

Here is the call graph for this function:

Here is the caller graph for this function:

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

static

Definition at line 162 of file ahrs_float_invariant_wrapper.c.

References ahrs_float_inv, ahrs_float_invariant_update_mag(), and AhrsFloatInv::is_aligned.

Referenced by ahrs_float_invariant_register().

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 116 of file ahrs_float_invariant_wrapper.c.

Referenced by ahrs_float_invariant_register().

uint8_t ahrs_finv_id = AHRS_COMP_ID_FINV

static

Definition at line 44 of file ahrs_float_invariant_wrapper.c.

uint32_t ahrs_finv_last_stamp = 0

static

last gyro msg timestamp

Definition at line 43 of file ahrs_float_invariant_wrapper.c.

bool_t ahrs_finv_output_enabled

static

if TRUE with push the estimation results to the state interface

Definition at line 41 of file ahrs_float_invariant_wrapper.c.

Referenced by ahrs_float_invariant_enable_output().

abi_event aligner_ev

static

Definition at line 117 of file ahrs_float_invariant_wrapper.c.

Referenced by ahrs_float_invariant_register().

abi_event body_to_imu_ev

static

Definition at line 118 of file ahrs_float_invariant_wrapper.c.

Referenced by ahrs_float_invariant_register().

abi_event geo_mag_ev

static

Definition at line 119 of file ahrs_float_invariant_wrapper.c.

Referenced by ahrs_float_invariant_register().

abi_event gyro_ev

static

Definition at line 115 of file ahrs_float_invariant_wrapper.c.

Referenced by ahrs_float_invariant_register().

abi_event mag_ev

static

Definition at line 114 of file ahrs_float_invariant_wrapper.c.

Referenced by ahrs_float_invariant_register().