From c86dcf91d6f2a1fc978a319023f427a4cb55408a Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Sat, 3 Oct 2020 20:18:30 -0700 Subject: [PATCH] AP_InertialSensor: Run vibration monitoring on all instances --- libraries/AP_InertialSensor/AP_InertialSensor.h | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 36c205e04a..3dcf6fe0c3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -7,6 +7,8 @@ #define AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ 2.0f // accel vibration filter hz #define AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS 500 // peak-hold detector timeout +#include + /** maximum number of INS instances available on this platform. If more than 1 then redundant sensors may be available @@ -14,7 +16,13 @@ #define INS_MAX_INSTANCES 3 #define INS_MAX_BACKENDS 6 #define INS_MAX_NOTCHES 4 -#define INS_VIBRATION_CHECK_INSTANCES 2 +#ifndef INS_VIBRATION_CHECK_INSTANCES + #if HAL_MEM_CLASS >= HAL_MEM_CLASS_300 + #define INS_VIBRATION_CHECK_INSTANCES INS_MAX_INSTANCES + #else + #define INS_VIBRATION_CHECK_INSTANCES 1 + #endif +#endif #define XYZ_AXIS_COUNT 3 // The maximum we need to store is gyro-rate / loop-rate, worst case ArduCopter with BMI088 is 2000/400 #define INS_MAX_GYRO_WINDOW_SAMPLES 8