Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: FFT gyro window would overflow on windows >= 256

Browse Source 
convert FFT buffers to ObjectBuffer<float> for lock-free access
push gyro samples directly into the FFT ring buffer from the gyro thread
This commit is contained in:
Andy Piper 2020-03-27 22:08:30 +00:00 committed by Andrew Tridgell
parent 94235c5739
commit 8ac79da643
4 changed files with 12 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

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

13
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -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 <AP_AccelCal/AP_AccelCal.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/RingBuffer.h>
#include <AP_Math/AP_Math.h>
#include <Filter/LowPassFilter2p.h>
#include <Filter/LowPassFilter.h>
@ -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];

19
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

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

5
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

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