31 #include "generated/airframe.h"
34 #ifndef FILTER_1EURO_ENABLED
35 #define FILTER_1EURO_ENABLED TRUE
39 #ifndef FILTER_1EURO_GYRO_MINCUTOFF
40 #define FILTER_1EURO_GYRO_MINCUTOFF 10.f
44 #ifndef FILTER_1EURO_GYRO_BETA
45 #define FILTER_1EURO_GYRO_BETA 0.1f
49 #ifndef FILTER_1EURO_GYRO_DCUTOFF
50 #define FILTER_1EURO_GYRO_DCUTOFF 1.f
54 #ifndef FILTER_1EURO_ACCEL_MINCUTOFF
55 #define FILTER_1EURO_ACCEL_MINCUTOFF 0.1f
59 #ifndef FILTER_1EURO_ACCEL_BETA
60 #define FILTER_1EURO_ACCEL_BETA 0.01f
64 #ifndef FILTER_1EURO_ACCEL_DCUTOFF
65 #define FILTER_1EURO_ACCEL_DCUTOFF 1.f
69 #ifndef FILTER_1EURO_FREQ
70 #if defined AHRS_PROPAGATE_FREQUENCY
71 #define FILTER_1EURO_FREQ AHRS_PROPAGATE_FREQUENCY
73 #elif defined INS_PROPAGATE_FREQUENCY
74 #define FILTER_1EURO_FREQ INS_PROPAGATE_FREQUENCY
79 #define FILTER_1EURO_FREQ PERIODIC_FREQUENCY
105 #ifndef IMU_F1E_BIND_ID
106 #define IMU_F1E_BIND_ID ABI_BROADCAST
124 #ifdef FILTER_1EURO_FREQ
137 AbiSendMsgIMU_GYRO(
IMU_F1E_ID, stamp, &gyro_i);
153 #ifdef FILTER_1EURO_FREQ
166 AbiSendMsgIMU_ACCEL(
IMU_F1E_ID, stamp, &accel_i);
168 AbiSendMsgIMU_ACCEL(
IMU_F1E_ID, stamp, accel);
195 for (
int i = 0; i < 3; i++) {
220 for (
int i = 0; i < 3; i++) {
229 for (
int i = 0; i < 3; i++) {
237 for (
int i = 0; i < 3; i++) {
245 for (
int i = 0; i < 3; i++) {
253 for (
int i = 0; i < 3; i++) {
261 for (
int i = 0; i < 3; i++) {
269 for (
int i = 0; i < 3; i++) {
Implementation of the 1 euro filter.
float mincutoff
min cutoff freq (in Hz)
static float update_1e_filter_at_time(struct OneEuroFilter *filter, float x, uint32_t timestamp)
Filter a float using the given One Euro Filter and the given timestamp.
float dcutoff
derivative cutoff freq (in Hz)
static void reset_1e_filter(struct OneEuroFilter *filter)
Reset filter (gains and parameters unchanged)
static void init_1e_filter(struct OneEuroFilter *filter, float rate, float mincutoff, float beta, float dcutoff)
Initialize a 1 Euro Filter instance.
static float update_1e_filter(struct OneEuroFilter *filter, float x)
Filter a float using the given One Euro Filter.
configuration parameters.
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
void filter_1euro_imu_update_gyro_mincutoff(float mincutoff)
struct Filter1eImu filter_1e_imu
configuration structure
#define FILTER_1EURO_ACCEL_MINCUTOFF
Default accel min cutoff freq.
void filter_1euro_imu_update_gyro_beta(float beta)
#define FILTER_1EURO_FREQ
Auto freq if not defined.
void filter_1euro_imu_update_accel_dcutoff(float dcutoff)
#define FILTER_1EURO_ENABLED
Enable by default.
void filter_1euro_imu_update_gyro_dcutoff(float dcutoff)
#define FILTER_1EURO_GYRO_DCUTOFF
Default gyro dcutoff (not recommanded to change)
static abi_event accel_ev
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
#define FILTER_1EURO_GYRO_MINCUTOFF
Default gyro min cutoff freq.
static struct OneEuroFilter gyro_1e[3]
array of 1 euro filters for gyrometer
#define FILTER_1EURO_ACCEL_BETA
Default accel beta coef.
void filter_1euro_imu_reset(float enabled)
settings handlers
void filter_1euro_imu_update_accel_beta(float beta)
void filter_1euro_imu_update_accel_mincutoff(float mincutoff)
#define FILTER_1EURO_GYRO_BETA
Default gyro beta coef.
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
#define IMU_F1E_BIND_ID
ABI bindings.
#define FILTER_1EURO_ACCEL_DCUTOFF
Default accel dcutoff (not recommanded to change)
void filter_1euro_imu_init(void)
Init and bindings.
static struct OneEuroFilter accel_1e[3]
array of 1 euro filters for accelerometer
#define RATES_BFP_OF_REAL(_ri, _rf)
#define ACCELS_BFP_OF_REAL(_ef, _ei)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
Paparazzi floating point algebra.
Paparazzi fixed point algebra.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.