Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_madgwick_wrapper.c File Reference

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

#include "modules/ahrs/ahrs_madgwick_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_madgwick_wrapper.c:

Go to the source code of this file.

Macros

#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)
 
void ahrs_madgwick_wrapper_init (void)
 
void ahrs_madgwick_wrapper_enable (uint8_t enable)
 

Variables

uint8_t ahrs_madgwick_enable
 
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
 

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

◆ AHRS_MADGWICK_MAG_ID

#define AHRS_MADGWICK_MAG_ID   AHRS_MADGWICK_IMU_ID

magnetometer

Definition at line 94 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 137 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_wrapper_init().

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

◆ ahrs_madgwick_wrapper_enable()

void ahrs_madgwick_wrapper_enable ( uint8_t  enable)

Definition at line 199 of file ahrs_madgwick_wrapper.c.

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

Referenced by ahrs_madgwick_wrapper_init().

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

◆ ahrs_madgwick_wrapper_init()

◆ 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 148 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_wrapper_init().

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

References ahrs_madgwick, 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 107 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(), PRINT_CONFIG_VAR(), and RATES_FLOAT_OF_BFP.

Referenced by ahrs_madgwick_wrapper_init().

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

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

Referenced by ahrs_madgwick_wrapper_init().

+ 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 70 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_wrapper_init().

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

Referenced by ahrs_madgwick_wrapper_init().

◆ ahrs_madgwick_enable

uint8_t ahrs_madgwick_enable

Definition at line 36 of file ahrs_madgwick_wrapper.c.

Referenced by ahrs_madgwick_wrapper_enable().

◆ ahrs_madgwick_id

uint8_t ahrs_madgwick_id = AHRS_COMP_ID_MADGWICK
static

Definition at line 39 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 38 of file ahrs_madgwick_wrapper.c.

Referenced by gyro_cb(), and send_filter_status().

◆ aligner_ev

abi_event aligner_ev
static

Definition at line 100 of file ahrs_madgwick_wrapper.c.

Referenced by ahrs_madgwick_wrapper_init().

◆ gyro_ev

abi_event gyro_ev
static

Definition at line 98 of file ahrs_madgwick_wrapper.c.

Referenced by ahrs_madgwick_wrapper_init().