Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Andy Piper 2019-07-23 09:43:18 +01:00 committed by Andrew Tridgell
parent 88f0c26636
commit 3d9776dd6d
4 changed files with 74 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -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)
{

25
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

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

17
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

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

9
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

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