Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
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"
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 |
Paparazzi specific wrapper to run Madgwick ahrs filter.
Definition in file ahrs_madgwick_wrapper.c.
#define AHRS_MADGWICK_IMU_ID ABI_BROADCAST |
IMU (gyro, accel)
Definition at line 88 of file ahrs_madgwick_wrapper.c.
#define AHRS_MADGWICK_MAG_ID AHRS_MADGWICK_IMU_ID |
magnetometer
Definition at line 94 of file ahrs_madgwick_wrapper.c.
|
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().
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().
void ahrs_madgwick_wrapper_init | ( | void | ) |
Definition at line 177 of file ahrs_madgwick_wrapper.c.
References ABI_BROADCAST, accel_cb(), accel_ev, AHRS_MADGWICK_IMU_ID, ahrs_madgwick_init(), ahrs_madgwick_wrapper_enable(), AHRS_PRIMARY, aligner_cb(), aligner_ev, DefaultPeriodic, gyro_cb(), gyro_ev, register_periodic_telemetry(), send_att(), and send_filter_status().
|
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().
|
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().
|
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().
|
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().
|
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().
|
static |
Definition at line 99 of file ahrs_madgwick_wrapper.c.
Referenced by ahrs_madgwick_wrapper_init().
uint8_t ahrs_madgwick_enable |
Definition at line 36 of file ahrs_madgwick_wrapper.c.
Referenced by ahrs_madgwick_wrapper_enable().
|
static |
Definition at line 39 of file ahrs_madgwick_wrapper.c.
Referenced by send_att(), and send_filter_status().
|
static |
last gyro msg timestamp
Definition at line 38 of file ahrs_madgwick_wrapper.c.
Referenced by gyro_cb(), and send_filter_status().
|
static |
Definition at line 100 of file ahrs_madgwick_wrapper.c.
Referenced by ahrs_madgwick_wrapper_init().
|
static |
Definition at line 98 of file ahrs_madgwick_wrapper.c.
Referenced by ahrs_madgwick_wrapper_init().