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

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

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)

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

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.

Referenced by ahrs_icq_wrapper_init().

◆ 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.

Referenced by ahrs_icq_wrapper_init().

◆ 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.

Referenced by ahrs_icq_wrapper_init().

Function Documentation

◆ accel_cb()

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(), PRINT_CONFIG_MSG(), PRINT_CONFIG_VAR(), and set_body_state_from_quat().

Here is the call graph for this function:

◆ ahrs_icq_wrapper_init()

◆ 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, 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()

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(), and set_body_state_from_quat().

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

References ahrs_icq, h(), MAG_BFP_OF_REAL, and VECT3_ASSIGN.

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

References ahrs_icq_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

◆ mag_cb()

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(), PRINT_CONFIG_MSG(), PRINT_CONFIG_VAR(), and set_body_state_from_quat().

Here is the call graph for this function:

◆ send_bias()

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, and dev.

◆ send_euler()

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, int32_eulers_of_quat(), Int32Eulers::phi, Int32Eulers::psi, stateGetNedToBodyEulers_i(), and Int32Eulers::theta.

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

References ahrs_icq, ahrs_icq_id, ahrs_icq_last_stamp, dev, get_sys_time_usec(), and val.

Here is the call graph for this function:

◆ send_geo_mag()

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

◆ send_quat()

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, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, and stateGetNedToBodyQuat_i().

Referenced by ahrs_icq_wrapper_init(), and ins_flow_init().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ set_body_state_from_quat()

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, 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.

◆ 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

◆ ahrs_icq_last_stamp

uint32_t ahrs_icq_last_stamp
static

◆ aligner_ev

abi_event aligner_ev
static

Definition at line 130 of file ahrs_int_cmpl_quat_wrapper.c.

◆ geo_mag_ev

abi_event geo_mag_ev
static

Definition at line 131 of file ahrs_int_cmpl_quat_wrapper.c.

◆ gps_ev

abi_event gps_ev
static

Definition at line 132 of file ahrs_int_cmpl_quat_wrapper.c.

◆ gyro_ev

abi_event gyro_ev
static

Definition at line 127 of file ahrs_int_cmpl_quat_wrapper.c.

◆ mag_ev

abi_event mag_ev
static

Definition at line 129 of file ahrs_int_cmpl_quat_wrapper.c.