Paparazzi UAS  v5.15_devel-88-gb3ad7fe
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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_OUTPUT_ENABLED   TRUE
 
#define AHRS_ICQ_IMU_ID   ABI_BROADCAST
 ABI binding for IMU data. More...
 
#define AHRS_ICQ_MAG_ID   AHRS_ICQ_IMU_ID
 ABI binding for magnetometer data. More...
 
#define AHRS_ICQ_GPS_ID   GPS_MULTI_ID
 ABI binding for gps data. More...
 

Functions

static void set_body_state_from_quat (void)
 Rotate angles and rates from imu to body frame and set state. More...
 
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 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 void gps_cb (uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
 
static bool ahrs_icq_enable_output (bool enable)
 
void ahrs_icq_register (void)
 

Variables

static bool ahrs_icq_output_enabled
 if TRUE with push the estimation results to the state interface More...
 
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 body_to_imu_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

#define AHRS_ICQ_GPS_ID   GPS_MULTI_ID

ABI binding for gps data.

Used for GPS ABI messages.

Definition at line 128 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_register().

#define AHRS_ICQ_IMU_ID   ABI_BROADCAST

ABI binding for IMU data.

Used for gyro, accel ABI messages.

Definition at line 114 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_register().

#define AHRS_ICQ_MAG_ID   AHRS_ICQ_IMU_ID

ABI binding for magnetometer data.

Used for IMU_MAG_INT32 ABI messages.

Definition at line 121 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_register().

#define AHRS_ICQ_OUTPUT_ENABLED   TRUE

Definition at line 33 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_register().

Function Documentation

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

Definition at line 166 of file ahrs_int_cmpl_quat_wrapper.c.

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

Referenced by ahrs_icq_register().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static bool ahrs_icq_enable_output ( bool  enable)
static

Definition at line 245 of file ahrs_int_cmpl_quat_wrapper.c.

References ahrs_icq_output_enabled.

Referenced by ahrs_icq_register().

+ Here is the caller 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 214 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_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 226 of file ahrs_int_cmpl_quat_wrapper.c.

References ahrs_icq_set_body_to_imu_quat().

Referenced by ahrs_icq_register().

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

References ahrs_icq, MAG_BFP_OF_REAL, AhrsIntCmplQuat::mag_h, VECT3_ASSIGN, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ahrs_icq_register().

+ Here is the caller graph for this function:

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

Definition at line 238 of file ahrs_int_cmpl_quat_wrapper.c.

References ahrs_icq_update_gps().

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

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

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

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

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

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

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

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

Referenced by ahrs_icq_register().

+ Here is the call graph for this function:

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

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

Referenced by ahrs_icq_register().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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

Definition at line 87 of file ahrs_int_cmpl_quat_wrapper.c.

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

Referenced by ahrs_icq_register().

+ Here is the caller graph for this function:

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

Definition at line 49 of file ahrs_int_cmpl_quat_wrapper.c.

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

Referenced by ahrs_icq_register().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void set_body_state_from_quat ( void  )
static

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

Definition at line 252 of file ahrs_int_cmpl_quat_wrapper.c.

References ahrs_icq, AhrsIntCmplQuat::body_to_imu, AhrsIntCmplQuat::imu_rate, int32_quat_comp_inv(), int32_rmat_transp_ratemult(), AhrsIntCmplQuat::ltp_to_imu_quat, orientationGetQuat_i(), orientationGetRMat_i(), 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

abi_event accel_ev
static

Definition at line 132 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_register().

uint8_t ahrs_icq_id = AHRS_COMP_ID_ICQ
static
uint32_t ahrs_icq_last_stamp
static

Definition at line 39 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by send_filter_status().

bool ahrs_icq_output_enabled
static

if TRUE with push the estimation results to the state interface

Definition at line 38 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_enable_output().

abi_event aligner_ev
static

Definition at line 134 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_register().

abi_event body_to_imu_ev
static

Definition at line 135 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_register().

abi_event geo_mag_ev
static

Definition at line 136 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_register().

abi_event gps_ev
static

Definition at line 137 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_register().

abi_event gyro_ev
static

Definition at line 131 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_register().

abi_event mag_ev
static

Definition at line 133 of file ahrs_int_cmpl_quat_wrapper.c.

Referenced by ahrs_icq_register().