AP_InertialSensor: expose statically filtered gyro values for FFT analysis and allow sampling to gyro window for FFT analysis.

FFT windows can be dynamically allocated
add harmonic notch dynamic tracking mode
unwind gyro window allocation in the case of failure
allow access to harmonic notch harmonics

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -655,6 +655,31 @@ AP_InertialSensor_Backend *AP_InertialSensor::_find_backend(int16_t backend_id,
     return nullptr;
 }
 
+bool AP_InertialSensor::set_gyro_window_size(uint16_t size) {
+    _gyro_window_size = size;
+
+    // allocate FFT gyro window
+    for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
+        for (uint8_t j = 0; j < XYZ_AXIS_COUNT; j++) {
+            _gyro_window[i][j] = (float*)hal.util->malloc_type(sizeof(float) * (size), DSP_MEM_REGION);
+            if (_gyro_window[i][j] == nullptr) {
+                gcs().send_text(MAV_SEVERITY_WARNING, "Failed to allocate window for INS");
+                // clean up whatever we have currently allocated
+                for (uint8_t ii = 0; ii <= i; ii++) {
+                    for (uint8_t jj = 0; jj < j; jj++) {
+                        hal.util->free_type(_gyro_window[ii][jj], sizeof(float) * (size), DSP_MEM_REGION);
+                        _gyro_window[ii][jj] = nullptr;
+                        _gyro_window_size = 0;
+                    }
+                }
+                return false;
+            }
+        }
+    }
+
+    return true;
+}
+
 void
 AP_InertialSensor::init(uint16_t sample_rate)
 {

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -14,6 +14,10 @@
 #define INS_MAX_INSTANCES 3
 #define INS_MAX_BACKENDS  6
 #define INS_VIBRATION_CHECK_INSTANCES 2
+#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
+typedef float* GyroWindow;
 
 #define DEFAULT_IMU_LOG_BAT_MASK 0
 
@@ -148,6 +152,17 @@ public:
     uint16_t get_gyro_rate_hz(uint8_t instance) const { return uint16_t(_gyro_raw_sample_rates[instance] * _gyro_over_sampling[instance]); }
     uint16_t get_accel_rate_hz(uint8_t instance) const { return uint16_t(_accel_raw_sample_rates[instance] * _accel_over_sampling[instance]); }
 
+    // FFT support access
+    const Vector3f     &get_raw_gyro(void) const { return _gyro_raw[_primary_gyro]; }
+    GyroWindow  get_raw_gyro_window(uint8_t instance, uint8_t axis) const { return _gyro_window[instance][axis]; }
+    GyroWindow  get_raw_gyro_window(uint8_t axis) const { return get_raw_gyro_window(_primary_gyro, axis); }
+    // returns the index one above the last valid gyro sample
+    uint16_t  get_raw_gyro_window_index(void) const { return get_raw_gyro_window_index(_primary_gyro); }
+    uint16_t  get_raw_gyro_window_index(uint8_t instance) const { return _circular_buffer_idx[instance]; }
+    uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_primary_gyro); }
+    uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_primary_gyro]; }
+    bool set_gyro_window_size(uint16_t size);
+
     // get accel offsets in m/s/s
     const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; }
     const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(_primary_accel); }
@@ -227,6 +242,9 @@ public:
     // harmonic notch tracking mode
     HarmonicNotchDynamicMode get_gyro_harmonic_notch_tracking_mode(void) const { return _harmonic_notch_filter.tracking_mode(); }
 
+    // harmonic notch harmonics
+    uint8_t get_gyro_harmonic_notch_harmonics(void) const { return _harmonic_notch_filter.harmonics(); }
+
     // indicate which bit in LOG_BITMASK indicates raw logging enabled
     void set_log_raw_bit(uint32_t log_raw_bit) { _log_raw_bit = log_raw_bit; }
 
@@ -423,6 +441,13 @@ private:
     LowPassFilter2pVector3f _gyro_filter[INS_MAX_INSTANCES];
     Vector3f _accel_filtered[INS_MAX_INSTANCES];
     Vector3f _gyro_filtered[INS_MAX_INSTANCES];
+    // Thread-safe public version of _last_raw_gyro
+    Vector3f _gyro_raw[INS_MAX_INSTANCES];
+    // circular buffer of gyro data for frequency analysis
+    uint16_t _circular_buffer_idx[INS_MAX_INSTANCES];
+    GyroWindow _gyro_window[INS_MAX_INSTANCES][XYZ_AXIS_COUNT];
+    uint16_t _gyro_window_size;
+
     bool _new_accel_data[INS_MAX_INSTANCES];
     bool _new_gyro_data[INS_MAX_INSTANCES];
 

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -226,6 +226,10 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
         _imu._last_delta_angle[instance] = delta_angle;
         _imu._last_raw_gyro[instance] = gyro;
 
+        // capture gyro window for FFT analysis
+        _last_gyro_window[_num_gyro_samples++] = gyro * _imu._gyro_raw_sampling_multiplier[instance];
+        _num_gyro_samples = _num_gyro_samples % INS_MAX_GYRO_WINDOW_SAMPLES; // protect against overrun
+
         // apply the low pass filter
         Vector3f gyro_filtered = _imu._gyro_filter[instance].apply(gyro);
 
@@ -509,6 +513,19 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
     }
     if (_imu._new_gyro_data[instance]) {
         _publish_gyro(instance, _imu._gyro_filtered[instance]);
+        // copy the gyro samples from the backend to the frontend window
+        if (_imu._gyro_window_size > 0) {
+            uint8_t idx = _imu._circular_buffer_idx[instance];
+            for (uint8_t i = 0; i < _num_gyro_samples; i++) {
+                _imu._gyro_window[instance][0][idx] = _last_gyro_window[i].x;
+                _imu._gyro_window[instance][1][idx] = _last_gyro_window[i].y;
+                _imu._gyro_window[instance][2][idx] = _last_gyro_window[i].z;
+                idx = (idx + 1) % _imu._gyro_window_size;
+            }
+            _num_gyro_samples = 0;
+            _imu._circular_buffer_idx[instance] = idx;
+        }
+        _imu._gyro_raw[instance] = _imu._last_raw_gyro[instance] * _imu._gyro_raw_sampling_multiplier[instance];
         _imu._new_gyro_data[instance] = false;
     }
 

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -173,7 +173,7 @@ protected:
     void _set_raw_sample_accel_multiplier(uint8_t instance, uint16_t mul) {
         _imu._accel_raw_sampling_multiplier[instance] = mul;
     }
-    void _set_raw_sampl_gyro_multiplier(uint8_t instance, uint16_t mul) {
+    void _set_raw_sample_gyro_multiplier(uint8_t instance, uint16_t mul) {
         _imu._gyro_raw_sampling_multiplier[instance] = mul;
     }
 
@@ -274,7 +274,12 @@ protected:
     float _last_harmonic_notch_center_freq_hz;
     float _last_harmonic_notch_bandwidth_hz;
     float _last_harmonic_notch_attenuation_dB;
-    
+
+    // local window of gyro values to be copied to the frontend for FFT analysis
+    uint16_t _last_circular_buffer_idx;
+    uint16_t _num_gyro_samples;
+    Vector3f _last_gyro_window[INS_MAX_GYRO_WINDOW_SAMPLES]; // The maximum we need to store is gyro-rate / loop-rate
+
     void set_gyro_orientation(uint8_t instance, enum Rotation rotation) {
         _imu._gyro_orientation[instance] = rotation;
     }