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

Paparazzi specific wrapper to run INVARIANT ahrs filter. More...

#include "modules/ahrs/ahrs_float_invariant_wrapper.h"
#include "modules/ahrs/ahrs.h"
#include "modules/core/abi.h"
#include "mcu_periph/sys_time.h"
#include "message_pragmas.h"
#include "state.h"
#include "modules/datalink/telemetry.h"
Include dependency graph for ahrs_float_invariant_wrapper.c:

Go to the source code of this file.

Macros

#define AHRS_FINV_IMU_ID   ABI_BROADCAST
 IMU (gyro, accel).
#define AHRS_FINV_MAG_ID   AHRS_FINV_IMU_ID
 magnetometer

Functions

static void compute_body_orientation_and_rates (void)
 Compute body orientation and rates from imu orientation and rates.
static void send_att (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)
 Call ahrs_float_invariant_propagate on new gyro measurements.
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)
void ahrs_finv_wrapper_init (void)
void ahrs_float_invariant_wrapper_enable (uint8_t enable)

Variables

uint8_t ahrs_finv_enable
static uint32_t ahrs_finv_last_stamp = 0
 last gyro msg timestamp
static uint8_t ahrs_finv_id = AHRS_COMP_ID_FINV
static abi_event mag_ev
static abi_event gyro_ev
static abi_event accel_ev
static abi_event aligner_ev
static abi_event geo_mag_ev

Detailed Description

Paparazzi specific wrapper to run INVARIANT ahrs filter.

Definition in file ahrs_float_invariant_wrapper.c.

Macro Definition Documentation

◆ AHRS_FINV_IMU_ID

#define AHRS_FINV_IMU_ID   ABI_BROADCAST

IMU (gyro, accel).

Definition at line 91 of file ahrs_float_invariant_wrapper.c.

Referenced by ahrs_finv_wrapper_init().

◆ AHRS_FINV_MAG_ID

#define AHRS_FINV_MAG_ID   AHRS_FINV_IMU_ID

magnetometer

Definition at line 97 of file ahrs_float_invariant_wrapper.c.

Referenced by ahrs_finv_wrapper_init().

Function Documentation

◆ accel_cb()

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

Definition at line 142 of file ahrs_float_invariant_wrapper.c.

References ACCELS_FLOAT_OF_BFP, ahrs_float_inv, and ahrs_float_invariant_update_accel().

Here is the call graph for this function:

◆ ahrs_finv_wrapper_init()

◆ ahrs_float_invariant_wrapper_enable()

void ahrs_float_invariant_wrapper_enable ( uint8_t enable)

Definition at line 227 of file ahrs_float_invariant_wrapper.c.

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

Referenced by ahrs_finv_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

◆ compute_body_orientation_and_rates()

void compute_body_orientation_and_rates ( void )
static

Compute body orientation and rates from imu orientation and rates.

Definition at line 190 of file ahrs_float_invariant_wrapper.c.

References ahrs_float_inv, RATES_DIFF, stateSetBodyRates_f(), and stateSetNedToBodyQuat_f().

Referenced by aligner_cb(), and gyro_cb().

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

◆ geo_mag_cb()

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

Definition at line 182 of file ahrs_float_invariant_wrapper.c.

References ahrs_float_inv, and h().

Here is the call graph for this function:

◆ gyro_cb()

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

Call ahrs_float_invariant_propagate on new gyro measurements.

Since acceleration measurement is also needed for propagation, use the last stored accel from #ahrs_finv_accel.

Definition at line 112 of file ahrs_float_invariant_wrapper.c.

References ahrs_finv_last_stamp, ahrs_float_inv, ahrs_float_invariant_propagate(), AHRS_PROPAGATE_FREQUENCY, compute_body_orientation_and_rates(), gyro_cb(), PRINT_CONFIG_MSG(), PRINT_CONFIG_VAR(), and RATES_FLOAT_OF_BFP.

Here is the call graph for this function:

◆ mag_cb()

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

Definition at line 153 of file ahrs_float_invariant_wrapper.c.

References ahrs_float_inv, ahrs_float_invariant_update_mag(), and MAGS_FLOAT_OF_BFP.

Here is the call graph for this function:

◆ send_att()

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

Definition at line 47 of file ahrs_float_invariant_wrapper.c.

References ahrs_finv_id, ahrs_float_inv, dev, EULERS_BFP_OF_REAL, float_eulers_of_quat(), Int32Eulers::phi, Int32Eulers::psi, 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 73 of file ahrs_float_invariant_wrapper.c.

References ahrs_finv_id, ahrs_finv_last_stamp, ahrs_float_inv, 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

Definition at line 65 of file ahrs_float_invariant_wrapper.c.

References ahrs_finv_id, ahrs_float_inv, and dev.

Variable Documentation

◆ accel_ev

abi_event accel_ev
static

Definition at line 103 of file ahrs_float_invariant_wrapper.c.

◆ ahrs_finv_enable

uint8_t ahrs_finv_enable

Definition at line 37 of file ahrs_float_invariant_wrapper.c.

Referenced by ahrs_float_invariant_wrapper_enable().

◆ ahrs_finv_id

uint8_t ahrs_finv_id = AHRS_COMP_ID_FINV
static

Definition at line 40 of file ahrs_float_invariant_wrapper.c.

Referenced by send_att(), send_filter_status(), and send_geo_mag().

◆ ahrs_finv_last_stamp

uint32_t ahrs_finv_last_stamp = 0
static

last gyro msg timestamp

Definition at line 39 of file ahrs_float_invariant_wrapper.c.

Referenced by gyro_cb(), and send_filter_status().

◆ aligner_ev

abi_event aligner_ev
static

Definition at line 104 of file ahrs_float_invariant_wrapper.c.

◆ geo_mag_ev

abi_event geo_mag_ev
static

Definition at line 105 of file ahrs_float_invariant_wrapper.c.

◆ gyro_ev

abi_event gyro_ev
static

Definition at line 102 of file ahrs_float_invariant_wrapper.c.

◆ mag_ev

abi_event mag_ev
static

Definition at line 101 of file ahrs_float_invariant_wrapper.c.