Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-09-27 23:08:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

[imu] Add accel and gyro filtering options (#3164)

Browse Source
This commit is contained in:
Freek van Tienen
2023-11-07 10:17:27 +01:00
committed by GitHub
parent dc042ac992
commit dfb08fa733
2 changed files with 48 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/imu/imu.c
+40
View File
@@ -385,6 +385,12 @@ void imu_init(void)
int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.gyros[i].body_to_sensor);
RMAT_COPY(imu.gyros[i].body_to_sensor, body_to_sensor);
if(imu.gyros[i].calibrated.filter) {
float tau = 1.0 / (2.0 * M_PI * imu.gyros[i].filter_freq);
float sample_time = 1 / imu.gyros[i].filter_sample_freq;
for(uint8_t j = 0; j < 3; j++)
init_butterworth_2_low_pass_int(&imu.gyros[i].filter[j], tau, sample_time, 0.0);
}
/* Copy accel calibration if needed */
if(i >= accel_calib_len) {
@@ -411,6 +417,12 @@ void imu_init(void)
int32_rmat_comp(&body_to_sensor, body_to_imu_rmat, &imu.accels[i].body_to_sensor);
RMAT_COPY(imu.accels[i].body_to_sensor, body_to_sensor);
if(imu.accels[i].calibrated.filter) {
float tau = 1.0 / (2.0 * M_PI * imu.accels[i].filter_freq);
float sample_time = 1 / imu.accels[i].filter_sample_freq;
for(uint8_t j = 0; j < 3; j++)
init_butterworth_2_low_pass_int(&imu.accels[i].filter[j], tau, sample_time, 0.0);
}
/* Copy mag calibrated if needed */
if(i >= mag_calib_len) {
@@ -563,6 +575,20 @@ static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates
if(gyro == NULL || samples < 1)
return;
// Filter the gyro
if(gyro->calibrated.filter) {
struct Int32Rates data_filtered[samples];
for(uint8_t i = 0; i < samples; i++) {
update_butterworth_2_low_pass_int(&gyro->filter[0], data[i].p);
data_filtered[i].p = get_butterworth_2_low_pass_int(&gyro->filter[0]);
update_butterworth_2_low_pass_int(&gyro->filter[1], data[i].q);
data_filtered[i].q = get_butterworth_2_low_pass_int(&gyro->filter[1]);
update_butterworth_2_low_pass_int(&gyro->filter[2], data[i].r);
data_filtered[i].r = get_butterworth_2_low_pass_int(&gyro->filter[2]);
}
data = data_filtered;
}
// Copy last sample as unscaled
RATES_COPY(gyro->unscaled, data[samples-1]);
@@ -646,6 +672,20 @@ static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect
if(accel == NULL || samples < 1)
return;
// Filter the accel
if(accel->calibrated.filter) {
struct Int32Vect3 data_filtered[samples];
for(uint8_t i = 0; i < samples; i++) {
update_butterworth_2_low_pass_int(&accel->filter[0], data[i].x);
data_filtered[i].x = get_butterworth_2_low_pass_int(&accel->filter[0]);
update_butterworth_2_low_pass_int(&accel->filter[1], data[i].y);
data_filtered[i].y = get_butterworth_2_low_pass_int(&accel->filter[1]);
update_butterworth_2_low_pass_int(&accel->filter[2], data[i].z);
data_filtered[i].z = get_butterworth_2_low_pass_int(&accel->filter[2]);
}
data = data_filtered;
}
// Copy last sample as unscaled
VECT3_COPY(accel->unscaled, data[samples-1]);
sw/airborne/modules/imu/imu.h
+8
View File
@@ -31,6 +31,7 @@
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_orientation_conversion.h"
#include "filters/low_pass_filter.h"
#include "generated/airframe.h"
#ifndef IMU_MAX_SENSORS
@@ -42,6 +43,7 @@ struct imu_calib_t {
bool scale: 1; ///< Scale calibrated
bool rotation: 1; ///< Rotation calibrated
bool current: 1; ///< Current calibrated
bool filter: 1; ///< Enable the lowpass filter
};
struct imu_gyro_t {
@@ -54,6 +56,9 @@ struct imu_gyro_t {
struct Int32Rates neutral; ///< Neutral values, compensation on unscaled->scaled
struct Int32Rates scale[2]; ///< Scaling, first is numerator and second denominator
struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame (body to imu combined with imu to sensor)
float filter_freq; ///< Filter frequency
float filter_sample_freq; ///< Lowpass filter sample frequency (Hz)
Butterworth2LowPass_int filter[3]; ///< Lowpass filter optional
};
struct imu_accel_t {
@@ -66,6 +71,9 @@ struct imu_accel_t {
struct Int32Vect3 neutral; ///< Neutral values, compensation on unscaled->scaled
struct Int32Vect3 scale[2]; ///< Scaling, first is numerator and second denominator
struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame (body to imu combined with imu to sensor)
float filter_freq; ///< Lowpass filter frequency (Hz)
float filter_sample_freq; ///< Lowpass filter sample frequency (Hz)
Butterworth2LowPass_int filter[3]; ///< Lowpass filter optional
};
struct imu_mag_t {