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

sw/airborne/modules/imu/imu.c

@@ -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

@@ -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 {