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

gyro_fft: improve scheduling

Browse Source 
- 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
This commit is contained in:
Daniel Agar
2021-08-13 15:00:08 -04:00
parent 8952fa2935
commit a83675c622
2 changed files with 18 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/gyro_fft/GyroFFT.cpp
+16 -4
View File
@@ -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 &timestamp_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 &timestamp_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);
}
}
}
src/modules/gyro_fft/GyroFFT.hpp
+2 -2
View File
@@ -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};