ahrs_int_cmpl_euler_wrapper.c File Reference

Paparazzi specific wrapper to run floating point complementary filter. More...

#include "subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h"
#include "subsystems/ahrs.h"
#include "subsystems/abi.h"
#include "state.h"
#include "subsystems/datalink/telemetry.h"
#include "mcu_periph/sys_time.h"

Include dependency graph for ahrs_int_cmpl_euler_wrapper.c:

Go to the source code of this file.

Macros
#define	AHRS_ICE_OUTPUT_ENABLED   TRUE
#define	AHRS_ICE_IMU_ID   ABI_BROADCAST
	ABI binding for IMU data. More...
#define	AHRS_ICE_MAG_ID   AHRS_ICE_IMU_ID
	ABI binding for magnetometer data. More...

Functions
static void	set_body_state_from_euler (void)
static void	send_filter (struct transport_tx *trans, struct link_device *dev)
static void	send_euler (struct transport_tx *trans, struct link_device *dev)
static void	send_bias (struct transport_tx *trans, struct link_device *dev)
static void	send_filter_status (struct transport_tx *trans, struct link_device *dev)
static void	gyro_cb (uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
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 bool	ahrs_ice_enable_output (bool enable)
void	ahrs_ice_register (void)

Variables
static bool	ahrs_ice_output_enabled
	if TRUE with push the estimation results to the state interface More...
static uint32_t	ahrs_ice_last_stamp
static uint8_t	ahrs_ice_id = AHRS_COMP_ID_ICE
static abi_event	gyro_ev
static abi_event	accel_ev
static abi_event	mag_ev
static abi_event	aligner_ev
static abi_event	body_to_imu_ev

Detailed Description

Paparazzi specific wrapper to run floating point complementary filter.

Definition in file ahrs_int_cmpl_euler_wrapper.c.

Macro Definition Documentation

#define AHRS_ICE_IMU_ID   ABI_BROADCAST

ABI binding for IMU data.

Used for gyro, accel ABI messages.

Definition at line 106 of file ahrs_int_cmpl_euler_wrapper.c.

Referenced by ahrs_ice_register().

#define AHRS_ICE_MAG_ID   AHRS_ICE_IMU_ID

ABI binding for magnetometer data.

Used for IMU_MAG_INT32 ABI messages.

Definition at line 113 of file ahrs_int_cmpl_euler_wrapper.c.

Referenced by ahrs_ice_register().

#define AHRS_ICE_OUTPUT_ENABLED   TRUE

Definition at line 33 of file ahrs_int_cmpl_euler_wrapper.c.

Referenced by ahrs_ice_register().

Function Documentation

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

static

Definition at line 133 of file ahrs_int_cmpl_euler_wrapper.c.

References ahrs_ice, ahrs_ice_update_accel(), and AhrsIntCmplEuler::is_aligned.

Referenced by ahrs_ice_register().

Here is the call graph for this function:

Here is the caller graph for this function:

static bool ahrs_ice_enable_output ( bool  enable )

static

Definition at line 169 of file ahrs_int_cmpl_euler_wrapper.c.

References ahrs_ice_output_enabled.

Referenced by ahrs_ice_register().

Here is the caller graph for this function:

void ahrs_ice_register

(

void

)

Definition at line 196 of file ahrs_int_cmpl_euler_wrapper.c.

References ABI_BROADCAST, accel_cb(), accel_ev, ahrs_ice_enable_output(), AHRS_ICE_IMU_ID, ahrs_ice_init(), AHRS_ICE_MAG_ID, AHRS_ICE_OUTPUT_ENABLED, ahrs_register_impl(), aligner_cb(), aligner_ev, body_to_imu_cb(), body_to_imu_ev, DefaultPeriodic, gyro_cb(), gyro_ev, mag_cb(), mag_ev, register_periodic_telemetry(), send_bias(), send_euler(), send_filter(), and send_filter_status().

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 151 of file ahrs_int_cmpl_euler_wrapper.c.

References ahrs_ice, ahrs_ice_align(), AhrsIntCmplEuler::is_aligned, and set_body_state_from_euler().

Referenced by ahrs_ice_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 163 of file ahrs_int_cmpl_euler_wrapper.c.

References ahrs_ice_set_body_to_imu_quat().

Referenced by ahrs_ice_register().

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

Definition at line 123 of file ahrs_int_cmpl_euler_wrapper.c.

References ahrs_ice, ahrs_ice_propagate(), AhrsIntCmplEuler::is_aligned, and set_body_state_from_euler().

Referenced by ahrs_ice_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 142 of file ahrs_int_cmpl_euler_wrapper.c.

References ahrs_ice, ahrs_ice_update_mag(), and AhrsIntCmplEuler::is_aligned.

Referenced by ahrs_ice_register().

Here is the call graph for this function:

Here is the caller graph for this function:

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

static

Definition at line 83 of file ahrs_int_cmpl_euler_wrapper.c.

References ahrs_ice, ahrs_ice_id, AhrsIntCmplEuler::gyro_bias, Int32Rates::p, Int32Rates::q, and Int32Rates::r.

Referenced by ahrs_ice_register().

Here is the caller graph for this function:

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

static

Definition at line 70 of file ahrs_int_cmpl_euler_wrapper.c.

References ahrs_ice, ahrs_ice_id, AhrsIntCmplEuler::ltp_to_imu_euler, Int32Eulers::phi, Int32Eulers::psi, stateGetNedToBodyEulers_i(), and Int32Eulers::theta.

Referenced by ahrs_ice_register().

Here is the call graph for this function:

Here is the caller graph for this function:

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

static

Definition at line 49 of file ahrs_int_cmpl_euler_wrapper.c.

References ahrs_ice, ahrs_ice_id, AhrsIntCmplEuler::gyro_bias, AhrsIntCmplEuler::hi_res_euler, AhrsIntCmplEuler::ltp_to_imu_euler, AhrsIntCmplEuler::measure, Int32Rates::p, Int32Eulers::phi, Int32Eulers::psi, Int32Rates::q, Int32Rates::r, AhrsIntCmplEuler::residual, and Int32Eulers::theta.

Referenced by ahrs_ice_register().

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 90 of file ahrs_int_cmpl_euler_wrapper.c.

References ahrs_ice, ahrs_ice_id, ahrs_ice_last_stamp, get_sys_time_usec(), AhrsIntCmplEuler::is_aligned, and val.

Referenced by ahrs_ice_register().

Here is the call graph for this function:

Here is the caller graph for this function:

static void set_body_state_from_euler ( void  )

static

Definition at line 176 of file ahrs_int_cmpl_euler_wrapper.c.

References ahrs_ice, AhrsIntCmplEuler::body_to_imu, AhrsIntCmplEuler::imu_rate, int32_rmat_comp_inv(), int32_rmat_of_eulers, int32_rmat_transp_ratemult(), AhrsIntCmplEuler::ltp_to_imu_euler, orientationGetRMat_i(), stateSetBodyRates_i(), and stateSetNedToBodyRMat_i().

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 117 of file ahrs_int_cmpl_euler_wrapper.c.

Referenced by ahrs_ice_register().

uint8_t ahrs_ice_id = AHRS_COMP_ID_ICE

static

Definition at line 40 of file ahrs_int_cmpl_euler_wrapper.c.

Referenced by send_bias(), send_euler(), send_filter(), and send_filter_status().

uint32_t ahrs_ice_last_stamp

static

Definition at line 39 of file ahrs_int_cmpl_euler_wrapper.c.

Referenced by send_filter_status().

bool ahrs_ice_output_enabled

static

if TRUE with push the estimation results to the state interface

Definition at line 38 of file ahrs_int_cmpl_euler_wrapper.c.

Referenced by ahrs_ice_enable_output().

abi_event aligner_ev

static

Definition at line 119 of file ahrs_int_cmpl_euler_wrapper.c.

Referenced by ahrs_ice_register().

abi_event body_to_imu_ev

static

Definition at line 120 of file ahrs_int_cmpl_euler_wrapper.c.

Referenced by ahrs_ice_register().

abi_event gyro_ev

static

Definition at line 116 of file ahrs_int_cmpl_euler_wrapper.c.

Referenced by ahrs_ice_register().

abi_event mag_ev

static

Definition at line 118 of file ahrs_int_cmpl_euler_wrapper.c.

Referenced by ahrs_ice_register().