From 3d9776dd6d85ebab722eaeb6c568f35c5f465273 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 23 Jul 2019 09:43:18 +0100 Subject: [PATCH] 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 --- .../AP_InertialSensor/AP_InertialSensor.cpp | 25 +++++++++++++++++++ .../AP_InertialSensor/AP_InertialSensor.h | 25 +++++++++++++++++++ .../AP_InertialSensor_Backend.cpp | 17 +++++++++++++ .../AP_InertialSensor_Backend.h | 9 +++++-- 4 files changed, 74 insertions(+), 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index c7b3976558..3bf2ce3a30 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/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) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 0ff1aaa84f..4446000b8e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/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]; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index cae816c48f..25288015c5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/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; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 20481c62c7..5f93526a92 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/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; }