ahrs_float_cmpl_wrapper.c File Reference

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

#include "modules/ahrs/ahrs_float_cmpl_wrapper.h"
#include "modules/ahrs/ahrs.h"
#include "modules/core/abi.h"
#include "state.h"
#include "modules/datalink/telemetry.h"
#include "mcu_periph/sys_time.h"

Include dependency graph for ahrs_float_cmpl_wrapper.c:

Go to the source code of this file.

Macros
#define	AHRS_FC_IMU_ID   ABI_BROADCAST
	ABI binding for IMU data. More...
#define	AHRS_FC_MAG_ID   AHRS_FC_IMU_ID
	ABI binding for magnetometer data. More...
#define	AHRS_FC_GPS_ID   GPS_MULTI_ID
	ABI binding for gps data. More...

Functions
static void	compute_body_orientation_and_rates (void)
	Compute body orientation and rates from imu orientation and rates. More...
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_euler_int (struct transport_tx *trans, struct link_device *dev)
static void	send_geo_mag (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	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	ahrs_fc_wrapper_init (void)
void	ahrs_float_cmp_quat_wrapper_enable (uint8_t enable)

Variables
uint8_t	ahrs_fc_enable
	if TRUE with push the estimation results to the state interface More...
static uint32_t	ahrs_fc_last_stamp
static uint8_t	ahrs_fc_id = AHRS_COMP_ID_FC
static abi_event	gyro_ev
static abi_event	accel_ev
static abi_event	mag_ev
static abi_event	aligner_ev
static abi_event	geo_mag_ev
static abi_event	gps_ev

Detailed Description

Paparazzi specific wrapper to run floating point complementary filter.

Definition in file ahrs_float_cmpl_wrapper.c.

Macro Definition Documentation

AHRS_FC_GPS_ID

#define AHRS_FC_GPS_ID   GPS_MULTI_ID

ABI binding for gps data.

Used for GPS ABI messages.

Definition at line 121 of file ahrs_float_cmpl_wrapper.c.

AHRS_FC_IMU_ID

#define AHRS_FC_IMU_ID   ABI_BROADCAST

ABI binding for IMU data.

Used for gyro and accel ABI messages.

Definition at line 107 of file ahrs_float_cmpl_wrapper.c.

AHRS_FC_MAG_ID

#define AHRS_FC_MAG_ID   AHRS_FC_IMU_ID

ABI binding for magnetometer data.

Used for IMU_MAG_INT32 ABI messages.

Definition at line 114 of file ahrs_float_cmpl_wrapper.c.

Function Documentation

accel_cb()

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

static

Definition at line 161 of file ahrs_float_cmpl_wrapper.c.

References ACCELS_FLOAT_OF_BFP, ahrs_fc, ahrs_fc_update_accel(), AhrsFloatCmpl::is_aligned, PRINT_CONFIG_MSG(), and PRINT_CONFIG_VAR().

Referenced by ahrs_fc_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_fc_wrapper_init()

void ahrs_fc_wrapper_init

(

void

)

Definition at line 260 of file ahrs_float_cmpl_wrapper.c.

References ABI_BROADCAST, accel_cb(), accel_ev, AHRS_FC_GPS_ID, AHRS_FC_IMU_ID, ahrs_fc_init(), AHRS_FC_MAG_ID, ahrs_float_cmp_quat_wrapper_enable(), AHRS_PRIMARY, aligner_cb(), aligner_ev, DefaultPeriodic, geo_mag_cb(), geo_mag_ev, gps_cb(), gps_ev, gyro_cb(), gyro_ev, mag_cb(), mag_ev, register_periodic_telemetry(), send_bias(), send_euler(), send_euler_int(), send_filter_status(), and send_geo_mag().

Here is the call graph for this function:

ahrs_float_cmp_quat_wrapper_enable()

void ahrs_float_cmp_quat_wrapper_enable

(

uint8_t

enable

)

Definition at line 288 of file ahrs_float_cmpl_wrapper.c.

References ahrs_fc_enable, STATE_INPUT_ATTITUDE, STATE_INPUT_RATES, and stateSetInputFilter().

Referenced by ahrs_fc_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

aligner_cb()

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 211 of file ahrs_float_cmpl_wrapper.c.

References ACCELS_FLOAT_OF_BFP, ahrs_fc, ahrs_fc_align(), compute_body_orientation_and_rates(), AhrsFloatCmpl::is_aligned, MAGS_FLOAT_OF_BFP, and RATES_FLOAT_OF_BFP.

Referenced by ahrs_fc_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

compute_body_orientation_and_rates()

static void compute_body_orientation_and_rates ( void  )

static

Compute body orientation and rates from imu orientation and rates.

Definition at line 254 of file ahrs_float_cmpl_wrapper.c.

References ahrs_fc, AhrsFloatCmpl::body_rate, AhrsFloatCmpl::ltp_to_body_quat, stateSetBodyRates_f(), and stateSetNedToBodyQuat_f().

Referenced by aligner_cb(), gps_cb(), and gyro_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

geo_mag_cb()

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

static

Definition at line 231 of file ahrs_float_cmpl_wrapper.c.

References ahrs_fc, h(), and AhrsFloatCmpl::mag_h.

Referenced by ahrs_fc_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

gps_cb()

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

static

Definition at line 236 of file ahrs_float_cmpl_wrapper.c.

References ahrs_fc_update_gps(), and compute_body_orientation_and_rates().

Referenced by ahrs_fc_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

gyro_cb()

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

static

Definition at line 132 of file ahrs_float_cmpl_wrapper.c.

References ahrs_fc, ahrs_fc_last_stamp, ahrs_fc_propagate(), AHRS_FC_RUNNING, AHRS_PROPAGATE_FREQUENCY, compute_body_orientation_and_rates(), AhrsFloatCmpl::is_aligned, PRINT_CONFIG_MSG(), PRINT_CONFIG_VAR(), RATES_FLOAT_OF_BFP, and AhrsFloatCmpl::status.

Referenced by ahrs_fc_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

mag_cb()

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

static

Definition at line 186 of file ahrs_float_cmpl_wrapper.c.

References ahrs_fc, ahrs_fc_update_mag(), AhrsFloatCmpl::is_aligned, MAGS_FLOAT_OF_BFP, PRINT_CONFIG_MSG(), and PRINT_CONFIG_VAR().

Referenced by ahrs_fc_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

send_bias()

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

static

Definition at line 58 of file ahrs_float_cmpl_wrapper.c.

References ahrs_fc, ahrs_fc_id, dev, AhrsFloatCmpl::gyro_bias, Int32Rates::p, Int32Rates::q, Int32Rates::r, and RATES_BFP_OF_REAL.

Referenced by ahrs_fc_wrapper_init().

Here is the caller graph for this function:

send_euler()

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

static

Definition at line 47 of file ahrs_float_cmpl_wrapper.c.

References ahrs_fc, ahrs_fc_id, dev, float_eulers_of_quat(), AhrsFloatCmpl::ltp_to_body_quat, FloatEulers::phi, FloatEulers::psi, and FloatEulers::theta.

Referenced by ahrs_fc_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

send_euler_int()

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

static

Definition at line 66 of file ahrs_float_cmpl_wrapper.c.

References ahrs_fc, ahrs_fc_id, dev, EULERS_BFP_OF_REAL, float_eulers_of_quat(), AhrsFloatCmpl::ltp_to_body_quat, Int32Eulers::phi, Int32Eulers::psi, and Int32Eulers::theta.

Referenced by ahrs_fc_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

send_filter_status()

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

static

Definition at line 90 of file ahrs_float_cmpl_wrapper.c.

References ahrs_fc, ahrs_fc_id, ahrs_fc_last_stamp, dev, get_sys_time_usec(), AhrsFloatCmpl::is_aligned, and val.

Referenced by ahrs_fc_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

send_geo_mag()

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

static

Definition at line 84 of file ahrs_float_cmpl_wrapper.c.

References ahrs_fc, ahrs_fc_id, dev, AhrsFloatCmpl::mag_h, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ahrs_fc_wrapper_init().

Here is the caller graph for this function:

Variable Documentation

accel_ev

abi_event accel_ev

static

Definition at line 125 of file ahrs_float_cmpl_wrapper.c.

Referenced by ahrs_fc_wrapper_init().

ahrs_fc_enable

uint8_t ahrs_fc_enable

if TRUE with push the estimation results to the state interface

Definition at line 35 of file ahrs_float_cmpl_wrapper.c.

Referenced by ahrs_float_cmp_quat_wrapper_enable().

ahrs_fc_id

uint8_t ahrs_fc_id = AHRS_COMP_ID_FC

static

Definition at line 38 of file ahrs_float_cmpl_wrapper.c.

Referenced by send_bias(), send_euler(), send_euler_int(), send_filter_status(), and send_geo_mag().

ahrs_fc_last_stamp

uint32_t ahrs_fc_last_stamp

static

Definition at line 37 of file ahrs_float_cmpl_wrapper.c.

Referenced by gyro_cb(), and send_filter_status().

aligner_ev

abi_event aligner_ev

static

Definition at line 127 of file ahrs_float_cmpl_wrapper.c.

Referenced by ahrs_fc_wrapper_init().

geo_mag_ev

abi_event geo_mag_ev

static

Definition at line 128 of file ahrs_float_cmpl_wrapper.c.

Referenced by ahrs_fc_wrapper_init().

gps_ev

abi_event gps_ev

static

Definition at line 129 of file ahrs_float_cmpl_wrapper.c.

Referenced by ahrs_fc_wrapper_init().

gyro_ev

abi_event gyro_ev

static

Definition at line 124 of file ahrs_float_cmpl_wrapper.c.

Referenced by ahrs_fc_wrapper_init().

mag_ev

abi_event mag_ev

static

Definition at line 126 of file ahrs_float_cmpl_wrapper.c.

Referenced by ahrs_fc_wrapper_init().