diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 0d76fbee5e..6275300251 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -154,6 +154,8 @@ param set-default SENS_IMU_MODE 0 param set-default EKF2_MULTI_MAG 2 param set-default SENS_MAG_MODE 0 +param set-default IMU_GYRO_FFT_EN 1 + # By default log from boot until first disarm. param set-default SDLOG_MODE 1 # enable default, estimator replay and vision/avoidance logging profiles diff --git a/boards/cuav/x7pro/init/rc.board_defaults b/boards/cuav/x7pro/init/rc.board_defaults index 53027b9821..bc7bbe1ec2 100644 --- a/boards/cuav/x7pro/init/rc.board_defaults +++ b/boards/cuav/x7pro/init/rc.board_defaults @@ -16,8 +16,8 @@ param set-default BAT2_A_PER_V 24 # Enable IMU thermal control param set-default SENS_EN_THERMAL 1 -# Disable FFT because of lack of FIFO for ADIS16470 -param set-default IMU_GYRO_FFT_EN 0 + +param set-default IMU_GYRO_FFT_EN 1 rgbled_pwm start safety_button start diff --git a/src/modules/gyro_fft/CMakeLists.txt b/src/modules/gyro_fft/CMakeLists.txt index 8af2756d77..032109e883 100644 --- a/src/modules/gyro_fft/CMakeLists.txt +++ b/src/modules/gyro_fft/CMakeLists.txt @@ -34,13 +34,6 @@ set(CMSIS_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/CMSIS_5) set(CMSIS_DSP ${CMSIS_ROOT}/CMSIS/DSP) -add_compile_options( - -Wno-error - - -DARM_ALL_FFT_TABLES - -DARM_MATH_LOOPUNROLL -) - if(${PX4_PLATFORM} MATCHES "NuttX") add_compile_options(-DARM_MATH_DSP) endif() @@ -54,6 +47,8 @@ px4_add_module( 4096 COMPILE_FLAGS ${MAX_CUSTOM_OPT_LEVEL} + -DARM_ALL_FFT_TABLES + -DARM_MATH_LOOPUNROLL INCLUDES ${CMSIS_ROOT}/CMSIS/Core/Include ${CMSIS_DSP}/Include diff --git a/src/modules/gyro_fft/GyroFFT.cpp b/src/modules/gyro_fft/GyroFFT.cpp index 862fe34c7c..aefde79b9d 100644 --- a/src/modules/gyro_fft/GyroFFT.cpp +++ b/src/modules/gyro_fft/GyroFFT.cpp @@ -93,6 +93,7 @@ GyroFFT::~GyroFFT() perf_free(_cycle_perf); perf_free(_cycle_interval_perf); perf_free(_fft_perf); + perf_free(_gyro_generation_gap_perf); perf_free(_gyro_fifo_generation_gap_perf); delete _gyro_data_buffer_x; @@ -106,7 +107,7 @@ GyroFFT::~GyroFFT() bool GyroFFT::init() { if (!SensorSelectionUpdate(true)) { - PX4_WARN("sensor_gyro_fifo callback registration failed!"); + PX4_WARN("sensor_gyro callback registration failed!"); ScheduleDelayed(500_ms); } @@ -120,28 +121,31 @@ bool GyroFFT::SensorSelectionUpdate(bool force) _sensor_selection_sub.copy(&sensor_selection); if ((sensor_selection.gyro_device_id != 0) && (_selected_sensor_device_id != sensor_selection.gyro_device_id)) { + // prefer sensor_gyro_fifo if available for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { uORB::SubscriptionData sensor_gyro_fifo_sub{ORB_ID(sensor_gyro_fifo), i}; 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); + _selected_sensor_device_id = sensor_selection.gyro_device_id; + _gyro_fifo = true; + return true; + } + } + } - // find corresponding vehicle_imu_status instance - for (uint8_t imu_status = 0; imu_status < MAX_SENSOR_COUNT; imu_status++) { - uORB::Subscription imu_status_sub{ORB_ID(vehicle_imu_status), imu_status}; - vehicle_imu_status_s vehicle_imu_status; + // otherwise use sensor_gyro + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; - 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; - } - } - } - - PX4_WARN("unable to find IMU status for gyro %d", sensor_selection.gyro_device_id); + 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); + _selected_sensor_device_id = sensor_selection.gyro_device_id; + _gyro_fifo = false; return true; } } @@ -154,16 +158,38 @@ bool GyroFFT::SensorSelectionUpdate(bool force) return false; } -void GyroFFT::VehicleIMUStatusUpdate() +void GyroFFT::VehicleIMUStatusUpdate(bool force) { - vehicle_imu_status_s vehicle_imu_status; + if (_vehicle_imu_status_sub.updated() || force) { + vehicle_imu_status_s vehicle_imu_status; - if (_vehicle_imu_status_sub.update(&vehicle_imu_status)) { - 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)) { + if (_vehicle_imu_status_sub.copy(&vehicle_imu_status)) { + // find corresponding vehicle_imu_status instance if the device_id doesn't match + if (vehicle_imu_status.gyro_device_id != _selected_sensor_device_id) { - _gyro_sample_rate_hz = vehicle_imu_status.gyro_raw_rate_hz; + for (uint8_t imu_status = 0; imu_status < MAX_SENSOR_COUNT; imu_status++) { + uORB::Subscription imu_status_sub{ORB_ID(vehicle_imu_status), imu_status}; + + if (imu_status_sub.copy(&vehicle_imu_status)) { + if (vehicle_imu_status.gyro_device_id == _selected_sensor_device_id) { + _vehicle_imu_status_sub.ChangeInstance(imu_status); + break; + } + } + } + } + + // update gyro sample rate + if ((vehicle_imu_status.gyro_device_id == _selected_sensor_device_id) && (vehicle_imu_status.gyro_rate_hz > 0)) { + if (_gyro_fifo) { + _gyro_sample_rate_hz = vehicle_imu_status.gyro_raw_rate_hz; + + } else { + _gyro_sample_rate_hz = vehicle_imu_status.gyro_rate_hz; + } + + return; + } } } } @@ -207,6 +233,7 @@ float GyroFFT::EstimatePeakFrequency(q15_t fft[], uint8_t peak_index) void GyroFFT::Run() { if (should_exit()) { + _sensor_gyro_sub.unregisterCallback(); _sensor_gyro_fifo_sub.unregisterCallback(); exit_and_cleanup(); return; @@ -227,151 +254,178 @@ void GyroFFT::Run() updateParams(); } - SensorSelectionUpdate(); - VehicleIMUStatusUpdate(); + const bool selection_updated = SensorSelectionUpdate(); + VehicleIMUStatusUpdate(selection_updated); - const float resolution_hz = _gyro_sample_rate_hz / _imu_gyro_fft_len; + if (_gyro_fifo) { + // run on sensor gyro fifo updates + sensor_gyro_fifo_s sensor_gyro_fifo; - bool publish = false; - bool fft_updated = false; + while (_sensor_gyro_fifo_sub.update(&sensor_gyro_fifo)) { + if (_sensor_gyro_fifo_sub.get_last_generation() != _gyro_last_generation + 1) { + // force reset if we've missed a sample + _fft_buffer_index[0] = 0; + _fft_buffer_index[1] = 0; + _fft_buffer_index[2] = 0; - // run on sensor gyro fifo updates - sensor_gyro_fifo_s sensor_gyro_fifo; - - while (_sensor_gyro_fifo_sub.update(&sensor_gyro_fifo)) { - - if (_sensor_gyro_fifo_sub.get_last_generation() != _gyro_last_generation + 1) { - // force reset if we've missed a sample - _fft_buffer_index[0] = 0; - _fft_buffer_index[1] = 0; - _fft_buffer_index[2] = 0; - - 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[] {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 < _imu_gyro_fft_len) { - // convert int16_t -> q15_t (scaling isn't relevant) - 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 >= _imu_gyro_fft_len) && !fft_updated) { - arm_mult_q15(gyro_data_buffer[axis], _hanning_window, _fft_input_buffer, _imu_gyro_fft_len); - - perf_begin(_fft_perf); - arm_rfft_q15(&_rfft_q15, _fft_input_buffer, _fft_outupt_buffer); - perf_end(_fft_perf); - fft_updated = true; - - static constexpr uint16_t MIN_SNR = 10; // TODO: - - 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 (uint16_t bucket_index = 2; bucket_index < (_imu_gyro_fft_len / 2); bucket_index = bucket_index + 2) { - const float freq_hz = (bucket_index / 2) * resolution_hz; - - 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_magnitude_squared = real * real + complex * complex; - - if (fft_magnitude_squared > MIN_SNR) { - 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; - peaks_detected = true; - break; - } - } - } - } - } - - if (peaks_detected) { - float *peak_frequencies[] {_sensor_gyro_fft.peak_frequencies_x, _sensor_gyro_fft.peak_frequencies_y, _sensor_gyro_fft.peak_frequencies_z}; - uint32_t *peak_magnitude[] {_sensor_gyro_fft.peak_magnitude_x, _sensor_gyro_fft.peak_magnitude_y, _sensor_gyro_fft.peak_magnitude_z}; - - int num_peaks_found = 0; - - for (int i = 0; i < MAX_NUM_PEAKS; i++) { - if ((peak_index[i] > 0) && (peak_index[i] < _imu_gyro_fft_len) && (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()) { - - if (fabsf(peak_frequencies[axis][num_peaks_found] - freq) > 0.1f) { - publish = true; - } - - peak_frequencies[axis][num_peaks_found] = freq; - peak_magnitude[axis][num_peaks_found] = peaks_magnitude[i]; - - num_peaks_found++; - } - } - } - - // mark remaining slots empty - for (int i = num_peaks_found; i < MAX_NUM_PEAKS; i++) { - peak_frequencies[axis][i] = NAN; - } - } - - // reset - // shift buffer (3/4 overlap) - 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_count(_gyro_fifo_generation_gap_perf); } + + _gyro_last_generation = _sensor_gyro_fifo_sub.get_last_generation(); + + 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; + } + + int16_t *input[] {sensor_gyro_fifo.x, sensor_gyro_fifo.y, sensor_gyro_fifo.z}; + Update(sensor_gyro_fifo.timestamp_sample, input, sensor_gyro_fifo.samples); } - if (publish) { - _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(); - _sensor_gyro_fft_pub.publish(_sensor_gyro_fft); + } else { + // run on sensor gyro fifo updates + sensor_gyro_s sensor_gyro; - publish = false; + while (_sensor_gyro_sub.update(&sensor_gyro)) { + if (_sensor_gyro_sub.get_last_generation() != _gyro_last_generation + 1) { + // force reset if we've missed a sample + _fft_buffer_index[0] = 0; + _fft_buffer_index[1] = 0; + _fft_buffer_index[2] = 0; + + perf_count(_gyro_generation_gap_perf); + } + + _gyro_last_generation = _sensor_gyro_sub.get_last_generation(); + + const float gyro_scale = math::radians(1000.f); // arbitrary scaling float32 rad/s -> raw int16 + int16_t gyro_x[1] {(int16_t)roundf(sensor_gyro.x * gyro_scale)}; + int16_t gyro_y[1] {(int16_t)roundf(sensor_gyro.y * gyro_scale)}; + int16_t gyro_z[1] {(int16_t)roundf(sensor_gyro.z * gyro_scale)}; + + int16_t *input[] {gyro_x, gyro_y, gyro_z}; + Update(sensor_gyro.timestamp_sample, input, 1); } } perf_end(_cycle_perf); } +void GyroFFT::Update(const hrt_abstime ×tamp_sample, int16_t *input[], uint8_t N) +{ + bool publish = false; + bool fft_updated = false; + const float resolution_hz = _gyro_sample_rate_hz / _imu_gyro_fft_len; + q15_t *gyro_data_buffer[] {_gyro_data_buffer_x, _gyro_data_buffer_y, _gyro_data_buffer_z}; + + for (int axis = 0; axis < 3; axis++) { + int &buffer_index = _fft_buffer_index[axis]; + + for (int n = 0; n < N; n++) { + if (buffer_index < _imu_gyro_fft_len) { + // convert int16_t -> q15_t (scaling isn't relevant) + 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 >= _imu_gyro_fft_len) && !fft_updated) { + arm_mult_q15(gyro_data_buffer[axis], _hanning_window, _fft_input_buffer, _imu_gyro_fft_len); + + perf_begin(_fft_perf); + arm_rfft_q15(&_rfft_q15, _fft_input_buffer, _fft_outupt_buffer); + perf_end(_fft_perf); + fft_updated = true; + + static constexpr uint16_t MIN_SNR = 10; // TODO: + + 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 (uint16_t bucket_index = 2; bucket_index < (_imu_gyro_fft_len / 2); bucket_index = bucket_index + 2) { + const float freq_hz = (bucket_index / 2) * resolution_hz; + + 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_magnitude_squared = real * real + complex * complex; + + if (fft_magnitude_squared > MIN_SNR) { + 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; + peaks_detected = true; + break; + } + } + } + } + } + + if (peaks_detected) { + float *peak_frequencies[] {_sensor_gyro_fft.peak_frequencies_x, _sensor_gyro_fft.peak_frequencies_y, _sensor_gyro_fft.peak_frequencies_z}; + uint32_t *peak_magnitude[] {_sensor_gyro_fft.peak_magnitude_x, _sensor_gyro_fft.peak_magnitude_y, _sensor_gyro_fft.peak_magnitude_z}; + + int num_peaks_found = 0; + + for (int i = 0; i < MAX_NUM_PEAKS; i++) { + if ((peak_index[i] > 0) && (peak_index[i] < _imu_gyro_fft_len) && (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()) { + + if (fabsf(peak_frequencies[axis][num_peaks_found] - freq) > 0.1f) { + publish = true; + _sensor_gyro_fft.timestamp_sample = timestamp_sample; + } + + peak_frequencies[axis][num_peaks_found] = freq; + peak_magnitude[axis][num_peaks_found] = peaks_magnitude[i]; + + num_peaks_found++; + } + } + } + + // mark remaining slots empty + for (int i = num_peaks_found; i < MAX_NUM_PEAKS; i++) { + peak_frequencies[axis][i] = NAN; + } + } + + // reset + // shift buffer (3/4 overlap) + 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; + } + } + } + + if (publish) { + _sensor_gyro_fft.device_id = _selected_sensor_device_id; + _sensor_gyro_fft.sensor_sample_rate_hz = _gyro_sample_rate_hz; + _sensor_gyro_fft.resolution_hz = resolution_hz; + _sensor_gyro_fft.timestamp = hrt_absolute_time(); + _sensor_gyro_fft_pub.publish(_sensor_gyro_fft); + + publish = false; + } +} + int GyroFFT::task_spawn(int argc, char *argv[]) { GyroFFT *instance = new GyroFFT(); diff --git a/src/modules/gyro_fft/GyroFFT.hpp b/src/modules/gyro_fft/GyroFFT.hpp index 7a4977721d..e9cd9c26fd 100644 --- a/src/modules/gyro_fft/GyroFFT.hpp +++ b/src/modules/gyro_fft/GyroFFT.hpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -79,7 +80,8 @@ private: float EstimatePeakFrequency(q15_t fft[], uint8_t peak_index); void Run() override; bool SensorSelectionUpdate(bool force = false); - void VehicleIMUStatusUpdate(); + void Update(const hrt_abstime ×tamp_sample, int16_t *input[], uint8_t N); + void VehicleIMUStatusUpdate(bool force = false); template void AllocateBuffers() @@ -104,15 +106,19 @@ private: uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)}; + uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub{this, ORB_ID(sensor_gyro)}; uORB::SubscriptionCallbackWorkItem _sensor_gyro_fifo_sub{this, ORB_ID(sensor_gyro_fifo)}; 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")}; uint32_t _selected_sensor_device_id{0}; + bool _gyro_fifo{false}; + arm_rfft_instance_q15 _rfft_q15; q15_t *_gyro_data_buffer_x{nullptr};