ahrs_madgwick_wrapper.c File Reference

#include "modules/ahrs/ahrs_madgwick_wrapper.h"
#include "subsystems/ahrs.h"
#include "subsystems/abi.h"
#include "mcu_periph/sys_time.h"
#include "message_pragmas.h"
#include "state.h"
#include "subsystems/datalink/telemetry.h"

Include dependency graph for ahrs_madgwick_wrapper.c:

Go to the source code of this file.

Macros
#define	AHRS_MADGWICK_OUTPUT_ENABLED   TRUE
#define	AHRS_MADGWICK_IMU_ID   ABI_BROADCAST
	IMU (gyro, accel) More...
#define	AHRS_MADGWICK_MAG_ID   AHRS_MADGWICK_IMU_ID
	magnetometer More...

Functions
static void	compute_body_orientation_and_rates (void)
	Compute body orientation and rates from imu orientation and rates. More...
static void	send_att (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_madgwick_propagate on new gyro measurements. More...
static void	accel_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
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 bool	ahrs_madgwick_enable_output (bool enable)
void	ahrs_madgwick_register (void)

Variables
static bool	ahrs_madgwick_output_enabled
	if TRUE with push the estimation results to the state interface More...
static uint32_t	ahrs_madgwick_last_stamp = 0
	last gyro msg timestamp More...
static uint8_t	ahrs_madgwick_id = AHRS_COMP_ID_MADGWICK
static abi_event	gyro_ev
static abi_event	accel_ev
static abi_event	aligner_ev
static abi_event	body_to_imu_ev

Detailed Description

Paparazzi specific wrapper to run Madgwick ahrs filter.

Definition in file ahrs_madgwick_wrapper.c.

Macro Definition Documentation

AHRS_MADGWICK_IMU_ID

#define AHRS_MADGWICK_IMU_ID   ABI_BROADCAST

IMU (gyro, accel)

Definition at line 95 of file ahrs_madgwick_wrapper.c.

AHRS_MADGWICK_MAG_ID

#define AHRS_MADGWICK_MAG_ID   AHRS_MADGWICK_IMU_ID

magnetometer

Definition at line 101 of file ahrs_madgwick_wrapper.c.

AHRS_MADGWICK_OUTPUT_ENABLED

#define AHRS_MADGWICK_OUTPUT_ENABLED   TRUE

Definition at line 35 of file ahrs_madgwick_wrapper.c.

Function Documentation

accel_cb()

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

static

Definition at line 145 of file ahrs_madgwick_wrapper.c.

References ACCELS_FLOAT_OF_BFP, ahrs_madgwick, ahrs_madgwick_update_accel(), and AhrsMadgwick::is_aligned.

Referenced by ahrs_madgwick_register().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_madgwick_enable_output()

static bool ahrs_madgwick_enable_output ( bool  enable )

static

Definition at line 178 of file ahrs_madgwick_wrapper.c.

References ahrs_madgwick_output_enabled.

Referenced by ahrs_madgwick_register().

Here is the caller graph for this function:

ahrs_madgwick_register()

void ahrs_madgwick_register

(

void

)

Definition at line 203 of file ahrs_madgwick_wrapper.c.

References accel_cb(), accel_ev, ahrs_madgwick_enable_output(), AHRS_MADGWICK_IMU_ID, ahrs_madgwick_init(), AHRS_MADGWICK_OUTPUT_ENABLED, ahrs_madgwick_output_enabled, ahrs_register_impl(), aligner_cb(), aligner_ev, body_to_imu_cb(), body_to_imu_ev, DefaultPeriodic, gyro_cb(), gyro_ev, register_periodic_telemetry(), send_att(), and send_filter_status().

Here is the call 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 156 of file ahrs_madgwick_wrapper.c.

References ACCELS_FLOAT_OF_BFP, ahrs_madgwick, ahrs_madgwick_align(), compute_body_orientation_and_rates(), AhrsMadgwick::is_aligned, and RATES_FLOAT_OF_BFP.

Referenced by ahrs_madgwick_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 172 of file ahrs_madgwick_wrapper.c.

References ahrs_madgwick_set_body_to_imu_quat().

Referenced by ahrs_madgwick_register().

Here is the call graph for this function:

Here is the caller graph for this function:

compute_body_orientation_and_rates()

static void compute_body_orientation_and_rates ( void  )

static

Compute body orientation and rates from imu orientation and rates.

Definition at line 187 of file ahrs_madgwick_wrapper.c.

References ahrs_madgwick, ahrs_madgwick_output_enabled, AhrsMadgwick::body_to_imu, float_quat_comp_inv(), orientationGetQuat_f(), AhrsMadgwick::quat, AhrsMadgwick::rates, 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:

gyro_cb()

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

static

Call ahrs_madgwick_propagate on new gyro measurements.

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

Definition at line 115 of file ahrs_madgwick_wrapper.c.

References ahrs_madgwick, ahrs_madgwick_last_stamp, ahrs_madgwick_propagate(), AHRS_PROPAGATE_FREQUENCY, compute_body_orientation_and_rates(), AhrsMadgwick::is_aligned, PRINT_CONFIG_MSG(), and RATES_FLOAT_OF_BFP.

Referenced by ahrs_madgwick_register().

Here is the call graph for this function:

Here is the caller graph for this function:

send_att()

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

static

Definition at line 50 of file ahrs_madgwick_wrapper.c.

References ahrs_madgwick, ahrs_madgwick_id, AhrsMadgwick::body_to_imu, dev, EULERS_BFP_OF_REAL, float_eulers_of_quat(), float_quat_comp_inv(), orientationGetQuat_f(), Int32Eulers::phi, Int32Eulers::psi, AhrsMadgwick::quat, and Int32Eulers::theta.

Referenced by ahrs_madgwick_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 77 of file ahrs_madgwick_wrapper.c.

References ahrs_madgwick, ahrs_madgwick_id, ahrs_madgwick_last_stamp, dev, get_sys_time_usec(), AhrsMadgwick::is_aligned, and val.

Referenced by ahrs_madgwick_register().

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 106 of file ahrs_madgwick_wrapper.c.

Referenced by ahrs_madgwick_register().

ahrs_madgwick_id

uint8_t ahrs_madgwick_id = AHRS_COMP_ID_MADGWICK

static

Definition at line 43 of file ahrs_madgwick_wrapper.c.

Referenced by send_att(), and send_filter_status().

ahrs_madgwick_last_stamp

uint32_t ahrs_madgwick_last_stamp = 0

static

last gyro msg timestamp

Definition at line 42 of file ahrs_madgwick_wrapper.c.

Referenced by gyro_cb(), and send_filter_status().

ahrs_madgwick_output_enabled

bool ahrs_madgwick_output_enabled

static

if TRUE with push the estimation results to the state interface

Definition at line 40 of file ahrs_madgwick_wrapper.c.

Referenced by ahrs_madgwick_enable_output(), ahrs_madgwick_register(), and compute_body_orientation_and_rates().

aligner_ev

abi_event aligner_ev

static

Definition at line 107 of file ahrs_madgwick_wrapper.c.

Referenced by ahrs_madgwick_register().

body_to_imu_ev

abi_event body_to_imu_ev

static

Definition at line 108 of file ahrs_madgwick_wrapper.c.

Referenced by ahrs_madgwick_register().

gyro_ev

abi_event gyro_ev

static

Definition at line 105 of file ahrs_madgwick_wrapper.c.

Referenced by ahrs_madgwick_register().