zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

experimental/gyro_fft: improve peak detection, add start parameter 

- add new parameter `IMU_GYRO_FFT_EN` to start
 - add 75% overlap in buffer to increase FFT update rate
 - space out FFT calls (no more than 1 per cycle)
 - increase `IMU_GYRO_FFT_MIN` default
 - decrease main stack usage
release/1.12
Daniel Agar 4 years ago committed by GitHub
parent
945c17bc3f
commit
614a0ac2a2
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 127 additions and 104 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      ROMFS/px4fmu_common/init.d/rcS
  2. 11
      msg/sensor_gyro_fft.msg
  3. 2
      src/examples/gyro_fft/CMakeLists.txt
  4. 194
      src/examples/gyro_fft/GyroFFT.cpp
  5. 8
      src/examples/gyro_fft/GyroFFT.hpp
  6. 11
      src/examples/gyro_fft/parameters.c

5
ROMFS/px4fmu_common/init.d/rcS
Unescape View File

@ -526,6 +526,11 @@ else
bst start -X bst start -X
fi fi
if param compare IMU_GYRO_FFT_EN 1
then
gyro_fft start
fi
# #
# Optional board supplied extras: rc.board_extras # Optional board supplied extras: rc.board_extras
# #

11
msg/sensor_gyro_fft.msg
Unescape View File

@ -7,11 +7,6 @@ float32 dt # delta time between samples (microseconds)
float32 resolution_hz float32 resolution_hz
uint8[3] peak_index_0 float32[2] peak_frequency_x # x axis peak frequencies
uint8[3] peak_index_1 float32[2] peak_frequency_y # y axis peak frequencies
float32[2] peak_frequency_z # z axis peak frequencies
float32[3] peak_frequency_0 # largest frequency peak found per sensor axis (0 if none)
float32[3] peak_frequency_1 # second largest frequency peak found per sensor axis (0 if none)
uint8[3] peak_index_quinns
float32[3] peak_frequency_quinns

2
src/examples/gyro_fft/CMakeLists.txt
Unescape View File

@ -71,7 +71,7 @@ px4_add_module(
MODULE modules__gyro_fft MODULE modules__gyro_fft
MAIN gyro_fft MAIN gyro_fft
STACK_MAIN STACK_MAIN
8192 4096
COMPILE_FLAGS COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL} ${MAX_CUSTOM_OPT_LEVEL}
INCLUDES INCLUDES

194
src/examples/gyro_fft/GyroFFT.cpp
Unescape View File

@ -45,18 +45,13 @@ GyroFFT::GyroFFT() :
ModuleParams(nullptr), ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
{ {
for (int axis = 0; axis < 3; axis++) { arm_rfft_init_q15(&_rfft_q15, FFT_LENGTH, 0, 1);
arm_rfft_init_q15(&_rfft_q15[axis], FFT_LENGTH, 0, 1);
}
// init Hanning window // init Hanning window
float hanning_window[FFT_LENGTH];
for (int n = 0; n < FFT_LENGTH; n++) { for (int n = 0; n < FFT_LENGTH; n++) {
hanning_window[n] = 0.5f * (1.f - cosf(2.f * M_PI_F * n / (FFT_LENGTH - 1))); const float hanning_value = 0.5f * (1.f - cosf(2.f * M_PI_F * n / (FFT_LENGTH - 1)));
arm_float_to_q15(&hanning_value, &_hanning_window[n], 1);
} }
arm_float_to_q15(hanning_window, _hanning_window, FFT_LENGTH);
} }
GyroFFT::~GyroFFT() GyroFFT::~GyroFFT()
@ -70,8 +65,8 @@ GyroFFT::~GyroFFT()
bool GyroFFT::init() bool GyroFFT::init()
{ {
if (!SensorSelectionUpdate(true)) { if (!SensorSelectionUpdate(true)) {
PX4_ERR("sensor_gyro_fifo callback registration failed!"); PX4_WARN("sensor_gyro_fifo callback registration failed!");
return false; ScheduleDelayed(500_ms);
} }
return true; return true;
@ -79,7 +74,7 @@ bool GyroFFT::init()
bool GyroFFT::SensorSelectionUpdate(bool force) bool GyroFFT::SensorSelectionUpdate(bool force)
{ {
if (_sensor_selection_sub.updated() || force) { if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
sensor_selection_s sensor_selection{}; sensor_selection_s sensor_selection{};
_sensor_selection_sub.copy(&sensor_selection); _sensor_selection_sub.copy(&sensor_selection);
@ -139,6 +134,41 @@ static constexpr float tau(float x)
return (1.f / 4.f * p1 - sqrtf(6) / 24 * p2); return (1.f / 4.f * p1 - sqrtf(6) / 24 * p2);
} }
float GyroFFT::EstimatePeakFrequency(q15_t fft[FFT_LENGTH * 2], 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]};
int16_t imag[3] {fft[peak_index - 2 + 1], fft[peak_index + 1], fft[peak_index + 2 + 1]};
const int k = 1;
float divider = (real[k] * real[k] + imag[k] * imag[k]);
// ap = (X[k + 1].r * X[k].r + X[k+1].i * X[k].i) / (X[k].r * X[k].r + X[k].i * X[k].i)
float ap = (real[k + 1] * real[k] + imag[k + 1] * imag[k]) / divider;
// am = (X[k – 1].r * X[k].r + X[k – 1].i * X[k].i) / (X[k].r * X[k].r + X[k].i * X[k].i)
float am = (real[k - 1] * real[k] + imag[k - 1] * imag[k]) / divider;
float dp = -ap / (1.f - ap);
float dm = am / (1.f - am);
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));
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() void GyroFFT::Run()
{ {
if (should_exit()) { if (should_exit()) {
@ -164,9 +194,10 @@ void GyroFFT::Run()
SensorSelectionUpdate(); SensorSelectionUpdate();
const float resolution_hz = _gyro_sample_rate_hz / (FFT_LENGTH * 2); const float resolution_hz = _gyro_sample_rate_hz / FFT_LENGTH;
bool publish = false; bool publish = false;
bool fft_updated = false;
// run on sensor gyro fifo updates // run on sensor gyro fifo updates
sensor_gyro_fifo_s sensor_gyro_fifo; sensor_gyro_fifo_s sensor_gyro_fifo;
@ -203,31 +234,35 @@ void GyroFFT::Run()
break; break;
} }
for (int n = 0; n < N; n++) { int &buffer_index = _fft_buffer_index[axis];
int &buffer_index = _fft_buffer_index[axis];
_data_buffer[axis][buffer_index] = input[n] / 2;
buffer_index++; for (int n = 0; n < N; n++) {
if (buffer_index < FFT_LENGTH) {
// convert int16_t -> q15_t (scaling isn't relevant)
_gyro_data_buffer[axis][buffer_index] = input[n] / 2;
buffer_index++;
}
// if we have enough samples, begin processing // if we have enough samples begin processing, but only one FFT per cycle
if (buffer_index >= FFT_LENGTH) { if ((buffer_index >= FFT_LENGTH) && !fft_updated) {
arm_mult_q15(_data_buffer[axis], _hanning_window, _fft_input_buffer, FFT_LENGTH); arm_mult_q15(_gyro_data_buffer[axis], _hanning_window, _fft_input_buffer, FFT_LENGTH);
perf_begin(_fft_perf); perf_begin(_fft_perf);
arm_rfft_q15(&_rfft_q15[axis], _fft_input_buffer, _fft_outupt_buffer); arm_rfft_q15(&_rfft_q15, _fft_input_buffer, _fft_outupt_buffer);
perf_end(_fft_perf); perf_end(_fft_perf);
fft_updated = true;
static constexpr uint16_t MIN_SNR = 10; // TODO:
static constexpr uint16_t MIN_SNR = 100; // TODO: static constexpr int MAX_NUM_PEAKS = 2;
uint32_t max_peak_0 = 0; uint32_t peaks_magnitude[MAX_NUM_PEAKS] {};
uint8_t max_peak_index_0 = 0; uint8_t peak_index[MAX_NUM_PEAKS] {};
bool peak_0_found = false;
// start at 2 to skip DC // 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] // output is ordered [real[0], imag[0], real[1], imag[1], real[2], imag[2] ... real[(N/2)-1], imag[(N/2)-1]
for (uint16_t bucket_index = 2; bucket_index < FFT_LENGTH; bucket_index = bucket_index + 2) { for (uint8_t bucket_index = 2; bucket_index < (FFT_LENGTH / 2); bucket_index = bucket_index + 2) {
const float freq_hz = bucket_index * resolution_hz; const float freq_hz = (bucket_index / 2) * resolution_hz;
if (freq_hz > _param_imu_gyro_fft_max.get()) { if (freq_hz > _param_imu_gyro_fft_max.get()) {
break; break;
@ -236,91 +271,68 @@ void GyroFFT::Run()
if (freq_hz >= _param_imu_gyro_fft_min.get()) { if (freq_hz >= _param_imu_gyro_fft_min.get()) {
const int16_t real = _fft_outupt_buffer[bucket_index]; const int16_t real = _fft_outupt_buffer[bucket_index];
const int16_t complex = _fft_outupt_buffer[bucket_index + 1]; const int16_t complex = _fft_outupt_buffer[bucket_index + 1];
const uint32_t fft_value_squared = real * real + complex * complex;
if ((fft_value_squared > MIN_SNR) && (fft_value_squared >= max_peak_0)) { const uint32_t fft_magnitude_squared = real * real + complex * complex;
max_peak_index_0 = bucket_index;
max_peak_0 = fft_value_squared; if (fft_magnitude_squared > MIN_SNR) {
peak_0_found = true; 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;
break;
}
}
} }
} }
} }
if (peak_0_found) { if (publish) {
{ float *peak_frequencies;
// 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_outupt_buffer[max_peak_index_0 - 2], _fft_outupt_buffer[max_peak_index_0], _fft_outupt_buffer[max_peak_index_0 + 2]};
int16_t imag[3] {_fft_outupt_buffer[max_peak_index_0 - 2 + 1], _fft_outupt_buffer[max_peak_index_0 + 1], _fft_outupt_buffer[max_peak_index_0 + 2 + 1]};
const int k = 1;
float divider = (real[k] * real[k] + imag[k] * imag[k]);
// ap = (X[k + 1].r * X[k].r + X[k+1].i * X[k].i) / (X[k].r * X[k].r + X[k].i * X[k].i)
float ap = (real[k + 1] * real[k] + imag[k + 1] * imag[k]) / divider;
// am = (X[k – 1].r * X[k].r + X[k – 1].i * X[k].i) / (X[k].r * X[k].r + X[k].i * X[k].i)
float am = (real[k - 1] * real[k] + imag[k - 1] * imag[k]) / divider;
float dp = -ap / (1.f - ap); switch (axis) {
float dm = am / (1.f - am); case 0:
float d = (dp + dm) / 2 + tau(dp * dp) - tau(dm * dm); peak_frequencies = _sensor_gyro_fft.peak_frequency_x;
break;
uint8_t adjustedBinLocation = roundf(max_peak_index_0 + d); case 1:
float peakFreqAdjusted = (_gyro_sample_rate_hz * adjustedBinLocation / (FFT_LENGTH * 2)); peak_frequencies = _sensor_gyro_fft.peak_frequency_y;
break;
_sensor_gyro_fft.peak_index_quinns[axis] = adjustedBinLocation; case 2:
_sensor_gyro_fft.peak_frequency_quinns[axis] = peakFreqAdjusted; peak_frequencies = _sensor_gyro_fft.peak_frequency_z;
break;
} }
int peaks_found = 0;
// find next peak for (int i = 0; i < MAX_NUM_PEAKS; i++) {
uint32_t max_peak_1 = 0; if ((peak_index[i] > 0) && (peak_index[i] < FFT_LENGTH) && (peaks_magnitude[i] > 0)) {
uint8_t max_peak_index_1 = 0; const float freq = EstimatePeakFrequency(_fft_outupt_buffer, peak_index[i]);
bool peak_1_found = false;
for (uint16_t bucket_index = 2; bucket_index < FFT_LENGTH; bucket_index = bucket_index + 2) { if (freq >= _param_imu_gyro_fft_min.get() && freq <= _param_imu_gyro_fft_max.get()) {
if (bucket_index != max_peak_index_0) { peak_frequencies[peaks_found] = freq;
const float freq_hz = bucket_index * resolution_hz; peaks_found++;
if (freq_hz > _param_imu_gyro_fft_max.get()) {
break;
}
if (freq_hz >= _param_imu_gyro_fft_min.get()) {
const int16_t real = _fft_outupt_buffer[bucket_index];
const int16_t complex = _fft_outupt_buffer[bucket_index + 1];
const uint32_t fft_value_squared = real * real + complex * complex;
if ((fft_value_squared > MIN_SNR) && (fft_value_squared >= max_peak_1)) {
max_peak_index_1 = bucket_index;
max_peak_1 = fft_value_squared;
peak_1_found = true;
}
} }
} }
} }
if (peak_1_found) { // mark remaining slots empty
// if 2 peaks found then log them in order for (int i = peaks_found; i < MAX_NUM_PEAKS; i++) {
_sensor_gyro_fft.peak_index_0[axis] = math::min(max_peak_index_0, max_peak_index_1); peak_frequencies[i] = NAN;
_sensor_gyro_fft.peak_index_1[axis] = math::max(max_peak_index_0, max_peak_index_1);
_sensor_gyro_fft.peak_frequency_0[axis] = _sensor_gyro_fft.peak_index_0[axis] * resolution_hz;
_sensor_gyro_fft.peak_frequency_1[axis] = _sensor_gyro_fft.peak_index_1[axis] * resolution_hz;
} else {
// only 1 peak found
_sensor_gyro_fft.peak_index_0[axis] = max_peak_index_0;
_sensor_gyro_fft.peak_index_1[axis] = 0;
_sensor_gyro_fft.peak_frequency_0[axis] = max_peak_index_0 * resolution_hz;
_sensor_gyro_fft.peak_frequency_1[axis] = 0;
} }
publish = true; // publish in sorted order for convenience
if (peaks_found > 0) {
qsort(peak_frequencies, peaks_found, sizeof(float), float_cmp);
}
} }
// reset // reset
buffer_index = 0; // 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);
buffer_index = overlap_start * 3;
} }
} }
} }

8
src/examples/gyro_fft/GyroFFT.hpp
Unescape View File

@ -74,6 +74,9 @@ public:
bool init(); bool init();
private: private:
static constexpr uint16_t FFT_LENGTH = 2048;
float EstimatePeakFrequency(q15_t fft[FFT_LENGTH * 2], uint8_t peak_index);
void Run() override; void Run() override;
bool SensorSelectionUpdate(bool force = false); bool SensorSelectionUpdate(bool force = false);
void VehicleIMUStatusUpdate(); void VehicleIMUStatusUpdate();
@ -95,10 +98,9 @@ private:
uint32_t _selected_sensor_device_id{0}; uint32_t _selected_sensor_device_id{0};
static constexpr uint16_t FFT_LENGTH = 1024; arm_rfft_instance_q15 _rfft_q15;
arm_rfft_instance_q15 _rfft_q15[3];
q15_t _data_buffer[3][FFT_LENGTH] {}; q15_t _gyro_data_buffer[3][FFT_LENGTH] {};
q15_t _hanning_window[FFT_LENGTH] {}; q15_t _hanning_window[FFT_LENGTH] {};
q15_t _fft_input_buffer[FFT_LENGTH] {}; q15_t _fft_input_buffer[FFT_LENGTH] {};
q15_t _fft_outupt_buffer[FFT_LENGTH * 2] {}; q15_t _fft_outupt_buffer[FFT_LENGTH * 2] {};

11
src/examples/gyro_fft/parameters.c
Unescape View File

@ -31,6 +31,15 @@
* *
****************************************************************************/ ****************************************************************************/
/**
* IMU gyro FFT enable.
*
* @boolean
* @reboot_required true
* @group Sensors
*/
PARAM_DEFINE_INT32(IMU_GYRO_FFT_EN, 0);
/** /**
* IMU gyro FFT minimum frequency. * IMU gyro FFT minimum frequency.
* *
@ -40,7 +49,7 @@
* @reboot_required true * @reboot_required true
* @group Sensors * @group Sensors
*/ */
PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MIN, 30.0f); PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MIN, 50.0f);
/** /**
* IMU gyro FFT maximum frequency. * IMU gyro FFT maximum frequency.

Write Preview
Loading…
Cancel
Save