From a83675c622d7a9f6738692d5c7eb5fafab97331c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 13 Aug 2021 13:13:01 -0400 Subject: [PATCH] gyro_fft: improve scheduling - move to high priority work queue (from low priority) - schedule slightly more often to avoid missing messages - perf counter include all FFT processing work - lazily allocate gyro gap perf counters on initial sensor selection --- src/modules/gyro_fft/GyroFFT.cpp | 20 ++++++++++++++++---- src/modules/gyro_fft/GyroFFT.hpp | 4 ++-- 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/src/modules/gyro_fft/GyroFFT.cpp b/src/modules/gyro_fft/GyroFFT.cpp index 44f2e0a208..21e00e64ab 100644 --- a/src/modules/gyro_fft/GyroFFT.cpp +++ b/src/modules/gyro_fft/GyroFFT.cpp @@ -41,7 +41,7 @@ using namespace matrix; GyroFFT::GyroFFT() : ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { for (int i = 0; i < MAX_NUM_PEAKS; i++) { _sensor_gyro_fft.peak_frequencies_x[i] = NAN; @@ -176,9 +176,14 @@ bool GyroFFT::SensorSelectionUpdate(bool force) 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()) { _sensor_gyro_sub.unregisterCallback(); - _sensor_gyro_fifo_sub.set_required_updates(sensor_gyro_fifo_s::ORB_QUEUE_LENGTH - 1); + _sensor_gyro_fifo_sub.set_required_updates(sensor_gyro_fifo_s::ORB_QUEUE_LENGTH / 2); _selected_sensor_device_id = sensor_selection.gyro_device_id; _gyro_fifo = true; + + if (_gyro_fifo_generation_gap_perf == nullptr) { + _gyro_fifo_generation_gap_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro FIFO data gap"); + } + return true; } } @@ -191,9 +196,14 @@ bool GyroFFT::SensorSelectionUpdate(bool force) if (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id) { if (_sensor_gyro_sub.ChangeInstance(i) && _sensor_gyro_sub.registerCallback()) { _sensor_gyro_fifo_sub.unregisterCallback(); - _sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH - 1); + _sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH / 2); _selected_sensor_device_id = sensor_selection.gyro_device_id; _gyro_fifo = false; + + if (_gyro_generation_gap_perf == nullptr) { + _gyro_generation_gap_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap"); + } + return true; } } @@ -396,9 +406,9 @@ void GyroFFT::Update(const hrt_abstime ×tamp_sample, int16_t *input[], uint // if we have enough samples begin processing, but only one FFT per cycle if ((buffer_index >= _imu_gyro_fft_len) && !fft_updated) { perf_begin(_fft_perf); + arm_mult_q15(gyro_data_buffer[axis], _hanning_window, _fft_input_buffer, _imu_gyro_fft_len); arm_rfft_q15(&_rfft_q15, _fft_input_buffer, _fft_outupt_buffer); - perf_end(_fft_perf); fft_updated = true; @@ -580,6 +590,8 @@ void GyroFFT::Update(const hrt_abstime ×tamp_sample, int16_t *input[], uint const int overlap_start = _imu_gyro_fft_len / 4; memmove(&gyro_data_buffer[axis][0], &gyro_data_buffer[axis][overlap_start], sizeof(q15_t) * overlap_start * 3); buffer_index = overlap_start * 3; + + perf_end(_fft_perf); } } } diff --git a/src/modules/gyro_fft/GyroFFT.hpp b/src/modules/gyro_fft/GyroFFT.hpp index fd1b0fe289..e19e5d4dc7 100644 --- a/src/modules/gyro_fft/GyroFFT.hpp +++ b/src/modules/gyro_fft/GyroFFT.hpp @@ -118,8 +118,8 @@ private: perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _cycle_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; perf_counter_t _fft_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": FFT")}; - perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")}; - perf_counter_t _gyro_fifo_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro FIFO data gap")}; + perf_counter_t _gyro_generation_gap_perf{nullptr}; + perf_counter_t _gyro_fifo_generation_gap_perf{nullptr}; uint32_t _selected_sensor_device_id{0};