diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml
index 36e979ea40..428fa2fe44 100644
--- a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml
+++ b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml
@@ -28,10 +28,11 @@
-
-
+
+
@@ -80,11 +81,6 @@
-
-
@@ -178,16 +174,16 @@
-
-
-
+
+
+
-
-
-
+
+
+
-
+
@@ -218,7 +214,7 @@
-
+
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml
index 40786c6218..ff2a2c3510 100644
--- a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml
+++ b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml
@@ -32,8 +32,9 @@
-
+
+ -->
+
@@ -83,11 +84,6 @@
-
-
@@ -181,16 +177,16 @@
-
-
-
+
+
+
-
-
-
+
+
+
-
+
@@ -221,7 +217,7 @@
-
+
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml
index 0b31cd058e..9166f503b2 100644
--- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml
+++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml
@@ -32,6 +32,7 @@
+
@@ -61,11 +62,6 @@
-
-
@@ -95,12 +91,12 @@
-
-
-
-
-
-
+
+
+
+
+
+
@@ -167,20 +163,20 @@
-
-
+
+
-
-
-
+
+
+
-
-
-
+
+
+
-
+
@@ -210,7 +206,7 @@
-
+
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml
index 9f3d371545..df4334d751 100644
--- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml
+++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml
@@ -29,9 +29,11 @@
-
-
+
+
@@ -56,11 +58,6 @@
-
-
@@ -162,21 +159,21 @@
-
-
+
+
-
-
-
+
+
+
-
-
-
+
+
+
-
-
-
+
+
+
@@ -205,7 +202,7 @@
-
+
diff --git a/sw/airborne/boards/krooz/imu_krooz.c b/sw/airborne/boards/krooz/imu_krooz.c
index 3aad6a7803..01fb83e3d2 100644
--- a/sw/airborne/boards/krooz/imu_krooz.c
+++ b/sw/airborne/boards/krooz/imu_krooz.c
@@ -33,6 +33,7 @@
#include "subsystems/imu/imu_krooz_sd_arch.h"
#include "mcu_periph/i2c.h"
#include "led.h"
+#include "filters/median_filter.h"
// Downlink
#include "mcu_periph/uart.h"
@@ -63,10 +64,13 @@ PRINT_CONFIG_VAR(KROOZ_ACCEL_RANGE)
struct ImuKrooz imu_krooz;
-#if KROOZ_USE_MEDIAN_FILTER
-#include "filters/median_filter.h"
-struct MedianFilter3Int median_gyro, median_accel, median_mag;
+#if IMU_KROOZ_USE_GYRO_MEDIAN_FILTER
+struct MedianFilter3Int median_gyro;
#endif
+#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
+struct MedianFilter3Int median_accel;
+#endif
+struct MedianFilter3Int median_mag;
void imu_impl_init( void )
{
@@ -82,12 +86,14 @@ void imu_impl_init( void )
hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR);
-#if KROOZ_USE_MEDIAN_FILTER
// Init median filters
+#if IMU_KROOZ_USE_GYRO_MEDIAN_FILTER
InitMedianFilterRatesInt(median_gyro);
+#endif
+#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
InitMedianFilterVect3Int(median_accel);
- InitMedianFilterVect3Int(median_mag);
#endif
+ InitMedianFilterVect3Int(median_mag);
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
@@ -110,14 +116,25 @@ void imu_periodic( void )
hmc58xx_start_configure(&imu_krooz.hmc);
if (imu_krooz.meas_nb) {
- RATES_ASSIGN(imu.gyro_unscaled, imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb);
-#if KROOZ_USE_MEDIAN_FILTER
+ RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb);
+#if IMU_KROOZ_USE_GYRO_MEDIAN_FILTER
UpdateMedianFilterRatesInt(median_gyro, imu.gyro_unscaled);
#endif
- VECT3_ASSIGN(imu.accel_unscaled, imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb);
-#if KROOZ_USE_MEDIAN_FILTER
+ VECT3_ASSIGN(imu.accel_unscaled, -imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb);
+#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif
+
+ RATES_SMUL(imu_krooz.gyro_filtered, imu_krooz.gyro_filtered, IMU_KROOZ_GYRO_AVG_FILTER);
+ RATES_ADD(imu_krooz.gyro_filtered, imu.gyro_unscaled);
+ RATES_SDIV(imu_krooz.gyro_filtered, imu_krooz.gyro_filtered, (IMU_KROOZ_GYRO_AVG_FILTER + 1));
+ RATES_COPY(imu.gyro_unscaled, imu_krooz.gyro_filtered);
+
+ VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER);
+ VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled);
+ VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1));
+ VECT3_COPY(imu.accel_unscaled, imu_krooz.accel_filtered);
+
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
@@ -150,10 +167,8 @@ void imu_krooz_event( void )
// If the HMC5883 I2C transaction has succeeded: convert the data
hmc58xx_event(&imu_krooz.hmc);
if (imu_krooz.hmc.data_available) {
- VECT3_COPY(imu.mag_unscaled, imu_krooz.hmc.data.vect);
-#if KROOZ_USE_MEDIAN_FILTER
+ VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.z, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.y);
UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
-#endif
imu_krooz.hmc.data_available = FALSE;
imu_krooz.mag_valid = TRUE;
}
diff --git a/sw/airborne/boards/krooz/imu_krooz.h b/sw/airborne/boards/krooz/imu_krooz.h
index 74afaf00f3..543306d225 100644
--- a/sw/airborne/boards/krooz/imu_krooz.h
+++ b/sw/airborne/boards/krooz/imu_krooz.h
@@ -39,12 +39,12 @@
// Default configuration
#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
-#define IMU_GYRO_P_SIGN -1
+#define IMU_GYRO_P_SIGN 1
#define IMU_GYRO_Q_SIGN 1
#define IMU_GYRO_R_SIGN 1
#endif
#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
-#define IMU_ACCEL_X_SIGN -1
+#define IMU_ACCEL_X_SIGN 1
#define IMU_ACCEL_Y_SIGN 1
#define IMU_ACCEL_Z_SIGN 1
#endif
@@ -100,6 +100,13 @@
#define IMU_ACCEL_Z_NEUTRAL 0
#endif
+#ifndef IMU_KROOZ_GYRO_AVG_FILTER
+#define IMU_KROOZ_GYRO_AVG_FILTER 5
+#endif
+#ifndef IMU_KROOZ_ACCEL_AVG_FILTER
+#define IMU_KROOZ_ACCEL_AVG_FILTER 10
+#endif
+
struct ImuKrooz {
volatile bool_t gyr_valid;
volatile bool_t acc_valid;
@@ -109,6 +116,8 @@ struct ImuKrooz {
struct Int32Rates rates_sum;
struct Int32Vect3 accel_sum;
volatile uint8_t meas_nb;
+ struct Int32Vect3 accel_filtered;
+ struct Int32Rates gyro_filtered;
};
extern struct ImuKrooz imu_krooz;