diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 8bc98df0dc..fad238f4c9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -663,14 +663,12 @@ bool AP_InertialSensor::set_gyro_window_size(uint16_t 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) { + if (!_gyro_window[i][j].set_size(size)) { 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[ii][jj].set_size(0); _gyro_window_size = 0; } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index db25f3d988..6e31ae3fd8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -17,7 +17,6 @@ #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 @@ -25,6 +24,7 @@ typedef float* GyroWindow; #include #include +#include #include #include #include @@ -155,11 +155,8 @@ public: // FFT support access #if HAL_WITH_DSP 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]; } + FloatBuffer& get_raw_gyro_window(uint8_t instance, uint8_t axis) { return _gyro_window[instance][axis]; } + FloatBuffer& get_raw_gyro_window(uint8_t axis) { return get_raw_gyro_window(_primary_gyro, axis); } 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]; } #endif @@ -445,9 +442,7 @@ private: #if HAL_WITH_DSP // 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]; + FloatBuffer _gyro_window[INS_MAX_INSTANCES][XYZ_AXIS_COUNT]; uint16_t _gyro_window_size; #endif bool _new_accel_data[INS_MAX_INSTANCES]; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 77f6cdd0ed..2d78cb93d1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -227,8 +227,12 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance, _imu._last_raw_gyro[instance] = gyro; #if HAL_WITH_DSP // 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 + if (_imu._gyro_window_size > 0) { + const Vector3f& scaled_gyro = gyro * _imu._gyro_raw_sampling_multiplier[instance]; + _imu._gyro_window[instance][0].push(scaled_gyro.x); + _imu._gyro_window[instance][1].push(scaled_gyro.y); + _imu._gyro_window[instance][2].push(scaled_gyro.z); + } #endif // apply the low pass filter Vector3f gyro_filtered = _imu._gyro_filter[instance].apply(gyro); @@ -515,17 +519,6 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) _publish_gyro(instance, _imu._gyro_filtered[instance]); // copy the gyro samples from the backend to the frontend window #if HAL_WITH_DSP - 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]; #endif _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 8317d56ae1..baca174ca4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -276,11 +276,6 @@ protected: 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; }