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

#include "modules/ahrs/ahrs_int_cmpl_quat_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_int_cmpl_quat_wrapper.c:

Go to the source code of this file.

Macros
#define	AHRS_ICQ_IMU_ID ABI_BROADCAST
	ABI binding for IMU data.

#define	AHRS_ICQ_MAG_ID AHRS_ICQ_IMU_ID
	ABI binding for magnetometer data.

#define	AHRS_ICQ_GPS_ID GPS_MULTI_ID
	ABI binding for gps data.

Functions
static void	set_body_state_from_quat (void)
	Rotate angles and rates from imu to body frame and set state.

static void	send_quat (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_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_icq_wrapper_init (void)

void	ahrs_int_cmpl_quat_wrapper_enable (uint8_t enable)

Detailed Description

Paparazzi specific wrapper to run floating point complementary filter.

Definition in file ahrs_int_cmpl_quat_wrapper.c.

Macro Definition Documentation

◆ AHRS_ICQ_GPS_ID

#define AHRS_ICQ_GPS_ID GPS_MULTI_ID

ABI binding for gps data.

Used for GPS ABI messages.

Definition at line 124 of file ahrs_int_cmpl_quat_wrapper.c.

◆ AHRS_ICQ_IMU_ID

#define AHRS_ICQ_IMU_ID ABI_BROADCAST

ABI binding for IMU data.

Used for gyro, accel ABI messages.

Definition at line 110 of file ahrs_int_cmpl_quat_wrapper.c.

◆ AHRS_ICQ_MAG_ID

#define AHRS_ICQ_MAG_ID AHRS_ICQ_IMU_ID

ABI binding for magnetometer data.

Used for IMU_MAG_INT32 ABI messages.

Definition at line 117 of file ahrs_int_cmpl_quat_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_int_cmpl_quat_wrapper.c.

References ahrs_icq, ahrs_icq_update_accel(), foo, AhrsIntCmplQuat::is_aligned, PRINT_CONFIG_MSG(), PRINT_CONFIG_VAR(), and set_body_state_from_quat().

Referenced by ahrs_icq_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ahrs_icq_wrapper_init()

void ahrs_icq_wrapper_init ( void )

Definition at line 242 of file ahrs_int_cmpl_quat_wrapper.c.

References ABI_BROADCAST, accel_cb(), accel_ev, AHRS_ICQ_GPS_ID, AHRS_ICQ_IMU_ID, ahrs_icq_init(), AHRS_ICQ_MAG_ID, ahrs_int_cmpl_quat_wrapper_enable(), AHRS_PRIMARY, aligner_cb(), aligner_ev, DefaultPeriodic, foo, 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_filter_status(), send_geo_mag(), and send_quat().

Here is the call graph for this function:

◆ ahrs_int_cmpl_quat_wrapper_enable()

void ahrs_int_cmpl_quat_wrapper_enable ( uint8_t enable )

Definition at line 270 of file ahrs_int_cmpl_quat_wrapper.c.

References ahrs_icq_enable, foo, STATE_INPUT_ATTITUDE, STATE_INPUT_RATES, and stateSetInputFilter().

Referenced by ahrs_icq_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 209 of file ahrs_int_cmpl_quat_wrapper.c.

References ahrs_icq, ahrs_icq_align(), AhrsIntCmplQuat::is_aligned, and set_body_state_from_quat().

Referenced by ahrs_icq_wrapper_init().

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 221 of file ahrs_int_cmpl_quat_wrapper.c.

References ahrs_icq, h(), MAG_BFP_OF_REAL, AhrsIntCmplQuat::mag_h, and VECT3_ASSIGN.

Referenced by ahrs_icq_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 227 of file ahrs_int_cmpl_quat_wrapper.c.

References ahrs_icq_update_gps(), and foo.

Referenced by ahrs_icq_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 135 of file ahrs_int_cmpl_quat_wrapper.c.

References ahrs_icq, ahrs_icq_last_stamp, ahrs_icq_propagate(), AHRS_ICQ_RUNNING, AHRS_PROPAGATE_FREQUENCY, foo, AhrsIntCmplQuat::is_aligned, PRINT_CONFIG_MSG(), PRINT_CONFIG_VAR(), set_body_state_from_quat(), and AhrsIntCmplQuat::status.

Referenced by ahrs_icq_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 185 of file ahrs_int_cmpl_quat_wrapper.c.

References ahrs_icq, ahrs_icq_update_mag(), foo, AhrsIntCmplQuat::is_aligned, PRINT_CONFIG_MSG(), PRINT_CONFIG_VAR(), and set_body_state_from_quat().

Referenced by ahrs_icq_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 76 of file ahrs_int_cmpl_quat_wrapper.c.

References ahrs_icq, ahrs_icq_id, dev, foo, AhrsIntCmplQuat::gyro_bias, Int32Rates::p, Int32Rates::q, and Int32Rates::r.

Referenced by ahrs_icq_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 61 of file ahrs_int_cmpl_quat_wrapper.c.

References ahrs_icq, ahrs_icq_id, dev, foo, int32_eulers_of_quat(), AhrsIntCmplQuat::ltp_to_body_quat, Int32Eulers::phi, Int32Eulers::psi, stateGetNedToBodyEulers_i(), and Int32Eulers::theta.

Referenced by ahrs_icq_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 93 of file ahrs_int_cmpl_quat_wrapper.c.

References ahrs_icq, ahrs_icq_id, ahrs_icq_last_stamp, dev, foo, get_sys_time_usec(), AhrsIntCmplQuat::is_aligned, and val.

Referenced by ahrs_icq_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 83 of file ahrs_int_cmpl_quat_wrapper.c.

References ahrs_icq, ahrs_icq_id, dev, foo, MAG_FLOAT_OF_BFP, AhrsIntCmplQuat::mag_h, FloatVect3::x, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Referenced by ahrs_icq_wrapper_init().

Here is the caller graph for this function:

◆ send_quat()

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

static

Definition at line 45 of file ahrs_int_cmpl_quat_wrapper.c.

References ahrs_icq, ahrs_icq_id, dev, foo, AhrsIntCmplQuat::ltp_to_body_quat, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, stateGetNedToBodyQuat_i(), and AhrsIntCmplQuat::weight.

Referenced by ahrs_icq_wrapper_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_body_state_from_quat()

static void set_body_state_from_quat ( void )

static

Rotate angles and rates from imu to body frame and set state.

Definition at line 235 of file ahrs_int_cmpl_quat_wrapper.c.

References ahrs_icq, AhrsIntCmplQuat::body_rate, foo, AhrsIntCmplQuat::ltp_to_body_quat, stateSetBodyRates_i(), and stateSetNedToBodyQuat_i().

Referenced by accel_cb(), aligner_cb(), gyro_cb(), and mag_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

◆ accel_ev

abi_event accel_ev

static

Definition at line 128 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_wrapper_init().

◆ ahrs_icq_enable

uint8_t ahrs_icq_enable

Definition at line 34 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_int_cmpl_quat_wrapper_enable().

◆ ahrs_icq_id

uint8_t ahrs_icq_id = AHRS_COMP_ID_ICQ

static

Definition at line 36 of file ahrs_int_cmpl_quat_wrapper.c.

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

◆ ahrs_icq_last_stamp

uint32_t ahrs_icq_last_stamp

static

Definition at line 35 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by gyro_cb(), and send_filter_status().

◆ aligner_ev

abi_event aligner_ev

static

Definition at line 130 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_wrapper_init().

◆ geo_mag_ev

abi_event geo_mag_ev

static

Definition at line 131 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_wrapper_init().

◆ gps_ev

abi_event gps_ev

static

Definition at line 132 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_wrapper_init().

◆ gyro_ev

abi_event gyro_ev

static

Definition at line 127 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_wrapper_init().

◆ mag_ev

abi_event mag_ev

static

Definition at line 129 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_wrapper_init().

Variables
uint8_t	ahrs_icq_enable

static uint32_t	ahrs_icq_last_stamp

static uint8_t	ahrs_icq_id = AHRS_COMP_ID_ICQ

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

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

◆ AHRS_ICQ_GPS_ID

◆ AHRS_ICQ_IMU_ID

◆ AHRS_ICQ_MAG_ID

Function Documentation

◆ accel_cb()

◆ ahrs_icq_wrapper_init()

◆ ahrs_int_cmpl_quat_wrapper_enable()

◆ aligner_cb()

◆ geo_mag_cb()

◆ gps_cb()

◆ gyro_cb()

◆ mag_cb()

◆ send_bias()

◆ send_euler()

◆ send_filter_status()

◆ send_geo_mag()

◆ send_quat()

◆ set_body_state_from_quat()

Variable Documentation

◆ accel_ev

◆ ahrs_icq_enable

◆ ahrs_icq_id

◆ ahrs_icq_last_stamp

◆ aligner_ev

◆ geo_mag_ev

◆ gps_ev

◆ gyro_ev

◆ mag_ev