Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_float_mlkf_wrapper.c File Reference
+ Include dependency graph for ahrs_float_mlkf_wrapper.c:

Go to the source code of this file.

Macros

#define AHRS_MLKF_OUTPUT_ENABLED   TRUE
 
#define AHRS_MLKF_IMU_ID   ABI_BROADCAST
 ABI binding for IMU data. More...
 
#define AHRS_MLKF_MAG_ID   AHRS_MLKF_IMU_ID
 ABI binding for magnetometer data. More...
 

Functions

static void set_body_state_from_quat (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_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 bool ahrs_mlkf_enable_output (bool enable)
 
void ahrs_mlkf_register (void)
 

Variables

static bool ahrs_mlkf_output_enabled
 if TRUE with push the estimation results to the state interface More...
 
static uint32_t ahrs_mlkf_last_stamp
 
static uint8_t ahrs_mlkf_id = AHRS_COMP_ID_MLKF
 
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
 

Detailed Description

Paparazzi specific wrapper to run MLKF filter.

Definition in file ahrs_float_mlkf_wrapper.c.

Macro Definition Documentation

◆ AHRS_MLKF_IMU_ID

#define AHRS_MLKF_IMU_ID   ABI_BROADCAST

ABI binding for IMU data.

Used for gyro, accel ABI messages.

Definition at line 91 of file ahrs_float_mlkf_wrapper.c.

◆ AHRS_MLKF_MAG_ID

#define AHRS_MLKF_MAG_ID   AHRS_MLKF_IMU_ID

ABI binding for magnetometer data.

Used for IMU_MAG_INT32 ABI messages.

Definition at line 98 of file ahrs_float_mlkf_wrapper.c.

◆ AHRS_MLKF_OUTPUT_ENABLED

#define AHRS_MLKF_OUTPUT_ENABLED   TRUE

Definition at line 34 of file ahrs_float_mlkf_wrapper.c.

Function Documentation

◆ accel_cb()

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

Definition at line 138 of file ahrs_float_mlkf_wrapper.c.

References ACCELS_FLOAT_OF_BFP, ahrs_mlkf, ahrs_mlkf_update_accel(), AhrsMlkf::is_aligned, and set_body_state_from_quat().

Referenced by ahrs_mlkf_register().

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

◆ ahrs_mlkf_enable_output()

static bool ahrs_mlkf_enable_output ( bool  enable)
static

Definition at line 193 of file ahrs_float_mlkf_wrapper.c.

References ahrs_mlkf_output_enabled.

Referenced by ahrs_mlkf_register().

+ Here is the caller graph for this function:

◆ ahrs_mlkf_register()

◆ 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 162 of file ahrs_float_mlkf_wrapper.c.

References ACCELS_FLOAT_OF_BFP, ahrs_mlkf, ahrs_mlkf_align(), AhrsMlkf::is_aligned, MAGS_FLOAT_OF_BFP, RATES_FLOAT_OF_BFP, and set_body_state_from_quat().

Referenced by ahrs_mlkf_register().

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

◆ body_to_imu_cb()

static void body_to_imu_cb ( uint8_t  sender_id,
struct FloatQuat q_b2i_f 
)
static

Definition at line 182 of file ahrs_float_mlkf_wrapper.c.

References ahrs_mlkf_set_body_to_imu_quat().

Referenced by ahrs_mlkf_register().

+ 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 188 of file ahrs_float_mlkf_wrapper.c.

References ahrs_mlkf, h(), and AhrsMlkf::mag_h.

Referenced by ahrs_mlkf_register().

+ 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 109 of file ahrs_float_mlkf_wrapper.c.

References ahrs_mlkf, ahrs_mlkf_last_stamp, ahrs_mlkf_propagate(), AHRS_MLKF_RUNNING, AHRS_PROPAGATE_FREQUENCY, AhrsMlkf::is_aligned, PRINT_CONFIG_MSG(), RATES_FLOAT_OF_BFP, set_body_state_from_quat(), and AhrsMlkf::status.

Referenced by ahrs_mlkf_register().

+ 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 150 of file ahrs_float_mlkf_wrapper.c.

References ahrs_mlkf, ahrs_mlkf_update_mag(), AhrsMlkf::is_aligned, MAGS_FLOAT_OF_BFP, and set_body_state_from_quat().

Referenced by ahrs_mlkf_register().

+ 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 60 of file ahrs_float_mlkf_wrapper.c.

References ahrs_mlkf, ahrs_mlkf_id, dev, AhrsMlkf::gyro_bias, Int32Rates::p, Int32Rates::q, Int32Rates::r, and RATES_BFP_OF_REAL.

Referenced by ahrs_mlkf_register().

+ 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 49 of file ahrs_float_mlkf_wrapper.c.

References ahrs_mlkf, ahrs_mlkf_id, dev, float_eulers_of_quat(), AhrsMlkf::ltp_to_imu_quat, FloatEulers::phi, FloatEulers::psi, and FloatEulers::theta.

Referenced by ahrs_mlkf_register().

+ 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 74 of file ahrs_float_mlkf_wrapper.c.

References ahrs_mlkf, ahrs_mlkf_id, ahrs_mlkf_last_stamp, dev, get_sys_time_usec(), AhrsMlkf::is_aligned, and val.

Referenced by ahrs_mlkf_register().

+ 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 68 of file ahrs_float_mlkf_wrapper.c.

References ahrs_mlkf, ahrs_mlkf_id, dev, AhrsMlkf::mag_h, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ahrs_mlkf_register().

+ Here is the caller graph for this function:

◆ set_body_state_from_quat()

static void set_body_state_from_quat ( void  )
static

Compute body orientation and rates from imu orientation and rates.

Definition at line 202 of file ahrs_float_mlkf_wrapper.c.

References ahrs_mlkf, ahrs_mlkf_output_enabled, AhrsMlkf::body_to_imu, float_quat_comp_inv(), float_rmat_transp_ratemult(), AhrsMlkf::imu_rate, AhrsMlkf::ltp_to_imu_quat, orientationGetQuat_f(), orientationGetRMat_f(), stateSetBodyRates_f(), and stateSetNedToBodyQuat_f().

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 102 of file ahrs_float_mlkf_wrapper.c.

Referenced by ahrs_mlkf_register().

◆ ahrs_mlkf_id

uint8_t ahrs_mlkf_id = AHRS_COMP_ID_MLKF
static

Definition at line 41 of file ahrs_float_mlkf_wrapper.c.

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

◆ ahrs_mlkf_last_stamp

uint32_t ahrs_mlkf_last_stamp
static

Definition at line 40 of file ahrs_float_mlkf_wrapper.c.

Referenced by gyro_cb(), and send_filter_status().

◆ ahrs_mlkf_output_enabled

bool ahrs_mlkf_output_enabled
static

if TRUE with push the estimation results to the state interface

Definition at line 39 of file ahrs_float_mlkf_wrapper.c.

Referenced by ahrs_mlkf_enable_output(), ahrs_mlkf_register(), and set_body_state_from_quat().

◆ aligner_ev

abi_event aligner_ev
static

Definition at line 104 of file ahrs_float_mlkf_wrapper.c.

Referenced by ahrs_mlkf_register().

◆ body_to_imu_ev

abi_event body_to_imu_ev
static

Definition at line 105 of file ahrs_float_mlkf_wrapper.c.

Referenced by ahrs_mlkf_register().

◆ geo_mag_ev

abi_event geo_mag_ev
static

Definition at line 106 of file ahrs_float_mlkf_wrapper.c.

Referenced by ahrs_mlkf_register().

◆ gyro_ev

abi_event gyro_ev
static

Definition at line 101 of file ahrs_float_mlkf_wrapper.c.

Referenced by ahrs_mlkf_register().

◆ mag_ev

abi_event mag_ev
static

Definition at line 103 of file ahrs_float_mlkf_wrapper.c.

Referenced by ahrs_mlkf_register().