fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 06:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors/vehicle_angular_velocity: gyro dynamic notch filters updated from onboard FFT

Browse Source
This commit is contained in:
Daniel Agar
2021-03-05 10:56:54 -05:00
parent ed6269b9a5
commit 00b3b3678b
7 changed files with 250 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/examples/gyro_fft/GyroFFT.cpp
+92 -89
View File
@@ -44,11 +44,45 @@ GyroFFT::GyroFFT() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
{
arm_rfft_init_q15(&_rfft_q15, FFT_LENGTH, 0, 1);
switch (_param_imu_gyro_fft_len.get()) {
case 128:
AllocateBuffers<128>();
break;
case 256:
AllocateBuffers<256>();
break;
case 512:
AllocateBuffers<512>();
break;
case 1024:
AllocateBuffers<1024>();
break;
case 2048:
AllocateBuffers<2048>();
break;
case 4096:
AllocateBuffers<4096>();
break;
default:
// otherwise default to 256
PX4_ERR("Invalid IMU_GYRO_FFT_LEN=%.3f, resetting", (double)_param_imu_gyro_fft_len.get());
AllocateBuffers<256>();
_param_imu_gyro_fft_len.set(256);
_param_imu_gyro_fft_len.commit();
break;
}
arm_rfft_init_q15(&_rfft_q15, _param_imu_gyro_fft_len.get(), 0, 1);
// init Hanning window
for (int n = 0; n < FFT_LENGTH; n++) {
const float hanning_value = 0.5f * (1.f - cosf(2.f * M_PI_F * n / (FFT_LENGTH - 1)));
for (int n = 0; n < _param_imu_gyro_fft_len.get(); n++) {
const float hanning_value = 0.5f * (1.f - cosf(2.f * M_PI_F * n / (_param_imu_gyro_fft_len.get() - 1)));
arm_float_to_q15(&hanning_value, &_hanning_window[n], 1);
}
}
@@ -59,6 +93,13 @@ GyroFFT::~GyroFFT()
perf_free(_cycle_interval_perf);
perf_free(_fft_perf);
perf_free(_gyro_fifo_generation_gap_perf);
delete _gyro_data_buffer_x;
delete _gyro_data_buffer_y;
delete _gyro_data_buffer_z;
delete _hanning_window;
delete _fft_input_buffer;
delete _fft_outupt_buffer;
}
bool GyroFFT::init()
@@ -77,13 +118,11 @@ bool GyroFFT::SensorSelectionUpdate(bool force)
sensor_selection_s sensor_selection{};
_sensor_selection_sub.copy(&sensor_selection);
if (_selected_sensor_device_id != sensor_selection.gyro_device_id) {
if ((sensor_selection.gyro_device_id != 0) && (_selected_sensor_device_id != sensor_selection.gyro_device_id)) {
for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
uORB::SubscriptionData<sensor_gyro_fifo_s> sensor_gyro_fifo_sub{ORB_ID(sensor_gyro_fifo), i};
if ((sensor_gyro_fifo_sub.get().device_id != 0)
&& (sensor_gyro_fifo_sub.get().device_id == sensor_selection.gyro_device_id)) {
if (sensor_gyro_fifo_sub.get().device_id == sensor_selection.gyro_device_id) {
if (_sensor_gyro_fifo_sub.ChangeInstance(i) && _sensor_gyro_fifo_sub.registerCallback()) {
// find corresponding vehicle_imu_status instance
for (uint8_t imu_status = 0; imu_status < MAX_SENSOR_COUNT; imu_status++) {
@@ -94,6 +133,7 @@ bool GyroFFT::SensorSelectionUpdate(bool force)
if (imu_status_sub.copy(&vehicle_imu_status)) {
if (vehicle_imu_status.gyro_device_id == sensor_selection.gyro_device_id) {
_vehicle_imu_status_sub.ChangeInstance(imu_status);
_selected_sensor_device_id = sensor_selection.gyro_device_id;
return true;
}
}
@@ -117,8 +157,11 @@ void GyroFFT::VehicleIMUStatusUpdate()
vehicle_imu_status_s vehicle_imu_status;
if (_vehicle_imu_status_sub.update(&vehicle_imu_status)) {
if ((vehicle_imu_status.gyro_rate_hz > 0) && (fabsf(vehicle_imu_status.gyro_rate_hz - _gyro_sample_rate_hz) > 1.f)) {
_gyro_sample_rate_hz = vehicle_imu_status.gyro_rate_hz;
if ((vehicle_imu_status.gyro_device_id == _selected_sensor_device_id)
&& (vehicle_imu_status.gyro_rate_hz > 0)
&& (fabsf(vehicle_imu_status.gyro_rate_hz - _gyro_sample_rate_hz) > 1.f)) {
_gyro_sample_rate_hz = vehicle_imu_status.gyro_raw_rate_hz;
}
}
}
@@ -133,7 +176,7 @@ static constexpr float tau(float x)
return (1.f / 4.f * p1 - sqrtf(6) / 24 * p2);
}
float GyroFFT::EstimatePeakFrequency(q15_t fft[FFT_LENGTH * 2], uint8_t peak_index)
float GyroFFT::EstimatePeakFrequency(q15_t fft[], uint8_t peak_index)
{
// find peak location using Quinn's Second Estimator (2020-06-14: http://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/)
int16_t real[3] { fft[peak_index - 2], fft[peak_index], fft[peak_index + 2] };
@@ -154,20 +197,11 @@ float GyroFFT::EstimatePeakFrequency(q15_t fft[FFT_LENGTH * 2], uint8_t peak_ind
float d = (dp + dm) / 2 + tau(dp * dp) - tau(dm * dm);
float adjusted_bin = peak_index + d;
float peak_freq_adjusted = (_gyro_sample_rate_hz * adjusted_bin / (FFT_LENGTH * 2.f));
float peak_freq_adjusted = (_gyro_sample_rate_hz * adjusted_bin / (_param_imu_gyro_fft_len.get() * 2.f));
return peak_freq_adjusted;
}
static int float_cmp(const void *elem1, const void *elem2)
{
if (*(const float *)elem1 < * (const float *)elem2) {
return -1;
}
return *(const float *)elem1 > *(const float *)elem2;
}
void GyroFFT::Run()
{
if (should_exit()) {
@@ -192,8 +226,9 @@ void GyroFFT::Run()
}
SensorSelectionUpdate();
VehicleIMUStatusUpdate();
const float resolution_hz = _gyro_sample_rate_hz / FFT_LENGTH;
const float resolution_hz = _gyro_sample_rate_hz / _param_imu_gyro_fft_len.get();
bool publish = false;
bool fft_updated = false;
@@ -212,40 +247,35 @@ void GyroFFT::Run()
perf_count(_gyro_fifo_generation_gap_perf);
}
if (fabsf(sensor_gyro_fifo.scale - _fifo_last_scale) > FLT_EPSILON) {
// force reset if scale has changed
_fft_buffer_index[0] = 0;
_fft_buffer_index[1] = 0;
_fft_buffer_index[2] = 0;
_fifo_last_scale = sensor_gyro_fifo.scale;
}
_gyro_last_generation = _sensor_gyro_fifo_sub.get_last_generation();
const int N = sensor_gyro_fifo.samples;
for (int axis = 0; axis < 3; axis++) {
int16_t *input = nullptr;
switch (axis) {
case 0:
input = sensor_gyro_fifo.x;
break;
case 1:
input = sensor_gyro_fifo.y;
break;
case 2:
input = sensor_gyro_fifo.z;
break;
}
int16_t *input[] {sensor_gyro_fifo.x, sensor_gyro_fifo.y, sensor_gyro_fifo.z};
q15_t *gyro_data_buffer[] {_gyro_data_buffer_x, _gyro_data_buffer_y, _gyro_data_buffer_z};
int &buffer_index = _fft_buffer_index[axis];
for (int n = 0; n < N; n++) {
if (buffer_index < FFT_LENGTH) {
if (buffer_index < _param_imu_gyro_fft_len.get()) {
// convert int16_t -> q15_t (scaling isn't relevant)
_gyro_data_buffer[axis][buffer_index] = input[n] / 2;
gyro_data_buffer[axis][buffer_index] = input[axis][n] / 2;
buffer_index++;
}
// if we have enough samples begin processing, but only one FFT per cycle
if ((buffer_index >= FFT_LENGTH) && !fft_updated) {
arm_mult_q15(_gyro_data_buffer[axis], _hanning_window, _fft_input_buffer, FFT_LENGTH);
if ((buffer_index >= _param_imu_gyro_fft_len.get()) && !fft_updated) {
arm_mult_q15(gyro_data_buffer[axis], _hanning_window, _fft_input_buffer, _param_imu_gyro_fft_len.get());
perf_begin(_fft_perf);
arm_rfft_q15(&_rfft_q15, _fft_input_buffer, _fft_outupt_buffer);
@@ -254,16 +284,13 @@ void GyroFFT::Run()
static constexpr uint16_t MIN_SNR = 10; // TODO:
uint32_t max_peak_magnitude = 0;
uint8_t max_peak_index = 0;
static constexpr int MAX_NUM_PEAKS = 4;
bool peaks_detected = false;
uint32_t peaks_magnitude[MAX_NUM_PEAKS] {};
uint8_t peak_index[MAX_NUM_PEAKS] {};
// start at 2 to skip DC
// output is ordered [real[0], imag[0], real[1], imag[1], real[2], imag[2] ... real[(N/2)-1], imag[(N/2)-1]
for (uint8_t bucket_index = 2; bucket_index < (FFT_LENGTH / 2); bucket_index = bucket_index + 2) {
for (uint8_t bucket_index = 2; bucket_index < (_param_imu_gyro_fft_len.get() / 2); bucket_index = bucket_index + 2) {
const float freq_hz = (bucket_index / 2) * resolution_hz;
if (freq_hz > _param_imu_gyro_fft_max.get()) {
@@ -277,17 +304,11 @@ void GyroFFT::Run()
const uint32_t fft_magnitude_squared = real * real + complex * complex;
if (fft_magnitude_squared > MIN_SNR) {
if (fft_magnitude_squared > max_peak_magnitude) {
max_peak_magnitude = fft_magnitude_squared;
max_peak_index = bucket_index;
}
for (int i = 0; i < MAX_NUM_PEAKS; i++) {
if (fft_magnitude_squared > peaks_magnitude[i]) {
peaks_magnitude[i] = fft_magnitude_squared;
peak_index[i] = bucket_index;
publish = true;
peaks_detected = true;
break;
}
}
@@ -295,64 +316,45 @@ void GyroFFT::Run()
}
}
if (max_peak_index > 0) {
_sensor_gyro_fft.peak_frequency[axis] = _median_filter[axis].apply(EstimatePeakFrequency(_fft_outupt_buffer,
max_peak_index));
}
if (peaks_detected) {
float *peak_frequencies[] { _sensor_gyro_fft.peak_frequencies_x, _sensor_gyro_fft.peak_frequencies_y, _sensor_gyro_fft.peak_frequencies_z};
if (publish) {
float *peak_frequencies;
switch (axis) {
case 0:
peak_frequencies = _sensor_gyro_fft.peak_frequencies_x;
break;
case 1:
peak_frequencies = _sensor_gyro_fft.peak_frequencies_y;
break;
case 2:
peak_frequencies = _sensor_gyro_fft.peak_frequencies_z;
break;
}
int peaks_found = 0;
int num_peaks_found = 0;
for (int i = 0; i < MAX_NUM_PEAKS; i++) {
if ((peak_index[i] > 0) && (peak_index[i] < FFT_LENGTH) && (peaks_magnitude[i] > 0)) {
if ((peak_index[i] > 0) && (peak_index[i] < _param_imu_gyro_fft_len.get()) && (peaks_magnitude[i] > 0)) {
const float freq = EstimatePeakFrequency(_fft_outupt_buffer, peak_index[i]);
if (freq >= _param_imu_gyro_fft_min.get() && freq <= _param_imu_gyro_fft_max.get()) {
peak_frequencies[peaks_found] = freq;
peaks_found++;
if (fabsf(peak_frequencies[axis][num_peaks_found] - freq) > 0.1f) {
publish = true;
}
peak_frequencies[axis][num_peaks_found] = freq;
num_peaks_found++;
}
}
}
// mark remaining slots empty
for (int i = peaks_found; i < MAX_NUM_PEAKS; i++) {
peak_frequencies[i] = NAN;
}
// publish in sorted order for convenience
if (peaks_found > 0) {
qsort(peak_frequencies, peaks_found, sizeof(float), float_cmp);
for (int i = num_peaks_found; i < MAX_NUM_PEAKS; i++) {
peak_frequencies[axis][i] = NAN;
}
}
// reset
// shift buffer (75% overlap)
int overlap_start = FFT_LENGTH / 4;
memmove(&_gyro_data_buffer[axis][0], &_gyro_data_buffer[axis][overlap_start], sizeof(q15_t) * overlap_start * 3);
// shift buffer (3/4 overlap)
const int overlap_start = _param_imu_gyro_fft_len.get() / 4;
memmove(&gyro_data_buffer[axis][0], &gyro_data_buffer[axis][overlap_start], sizeof(q15_t) * overlap_start * 3);
buffer_index = overlap_start * 3;
}
}
}
if (publish) {
_sensor_gyro_fft.dt = 1e6f / _gyro_sample_rate_hz;
_sensor_gyro_fft.device_id = sensor_gyro_fifo.device_id;
_sensor_gyro_fft.sensor_sample_rate_hz = _gyro_sample_rate_hz;
_sensor_gyro_fft.resolution_hz = resolution_hz;
_sensor_gyro_fft.timestamp_sample = sensor_gyro_fifo.timestamp_sample;
_sensor_gyro_fft.timestamp = hrt_absolute_time();
@@ -390,6 +392,7 @@ int GyroFFT::task_spawn(int argc, char *argv[])
int GyroFFT::print_status()
{
PX4_INFO("gyro sample rate: %.3f Hz", (double)_gyro_sample_rate_hz);
perf_print_counter(_cycle_perf);
perf_print_counter(_cycle_interval_perf);
perf_print_counter(_fft_perf);
src/examples/gyro_fft/GyroFFT.hpp
+25 -10
View File
@@ -77,14 +77,26 @@ public:
bool init();
private:
static constexpr uint16_t FFT_LENGTH = 2048;
float EstimatePeakFrequency(q15_t fft[FFT_LENGTH * 2], uint8_t peak_index);
float EstimatePeakFrequency(q15_t fft[], uint8_t peak_index);
void Run() override;
bool SensorSelectionUpdate(bool force = false);
void VehicleIMUStatusUpdate();
static constexpr int MAX_SENSOR_COUNT = 3;
template<size_t N>
void AllocateBuffers()
{
_gyro_data_buffer_x = new q15_t[N];
_gyro_data_buffer_y = new q15_t[N];
_gyro_data_buffer_z = new q15_t[N];
_hanning_window = new q15_t[N];
_fft_input_buffer = new q15_t[N];
_fft_outupt_buffer = new q15_t[N * 2];
}
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr int MAX_NUM_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof(
sensor_gyro_fft_s::peak_frequencies_x[0]);
uORB::Publication<sensor_gyro_fft_s> _sensor_gyro_fft_pub{ORB_ID(sensor_gyro_fft)};
@@ -104,22 +116,25 @@ private:
arm_rfft_instance_q15 _rfft_q15;
q15_t _gyro_data_buffer[3][FFT_LENGTH] {};
q15_t _hanning_window[FFT_LENGTH] {};
q15_t _fft_input_buffer[FFT_LENGTH] {};
q15_t _fft_outupt_buffer[FFT_LENGTH * 2] {};
q15_t *_gyro_data_buffer_x{nullptr};
q15_t *_gyro_data_buffer_y{nullptr};
q15_t *_gyro_data_buffer_z{nullptr};
q15_t *_hanning_window{nullptr};
q15_t *_fft_input_buffer{nullptr};
q15_t *_fft_outupt_buffer{nullptr};
float _gyro_sample_rate_hz{8000}; // 8 kHz default
float _fifo_last_scale{0};
int _fft_buffer_index[3] {};
unsigned _gyro_last_generation{0};
math::MedianFilter<float, 3> _median_filter[3] {};
sensor_gyro_fft_s _sensor_gyro_fft{};
DEFINE_PARAMETERS(
(ParamInt<px4::params::IMU_GYRO_FFT_LEN>) _param_imu_gyro_fft_len,
(ParamFloat<px4::params::IMU_GYRO_FFT_MIN>) _param_imu_gyro_fft_min,
(ParamFloat<px4::params::IMU_GYRO_FFT_MAX>) _param_imu_gyro_fft_max
)
src/examples/gyro_fft/parameters.c
+15
View File
@@ -61,3 +61,18 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MIN, 50.0f);
* @group Sensors
*/
PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MAX, 200.0f);
/**
* IMU gyro FFT length.
*
* @value 128 128
* @value 256 256
* @value 512 512
* @value 1024 1024
* @value 2048 2048
* @value 4096 4096
* @unit Hz
* @reboot_required true
* @group Sensors
*/
PARAM_DEFINE_INT32(IMU_GYRO_FFT_LEN, 256);