Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

gyro_fft: support sensor_gyro (non-fifo)

Browse Source
This commit is contained in:
Daniel Agar
2021-03-17 20:44:38 -04:00
committed by Lorenz Meier
parent 69e57b22ac
commit 1429423876
5 changed files with 221 additions and 164 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d-posix/rcS
+2
View File
@@ -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
boards/cuav/x7pro/init/rc.board_defaults
+2 -2
View File
@@ -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
src/modules/gyro_fft/CMakeLists.txt
+2 -7
View File
@@ -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
src/modules/gyro_fft/GyroFFT.cpp
+208 -154
View File
@@ -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_s> 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_s> 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 &timestamp_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();
src/modules/gyro_fft/GyroFFT.hpp
+7 -1
View File
@@ -45,6 +45,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_gyro_fft.h>
#include <uORB/topics/sensor_gyro_fifo.h>
#include <uORB/topics/sensor_selection.h>
@@ -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 &timestamp_sample, int16_t *input[], uint8_t N);
void VehicleIMUStatusUpdate(bool force = false);
template<size_t N>
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};