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; }