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

gyro_fft: support sensor_gyro (non-fifo)

release/1.12
Daniel Agar 4 years ago committed by Lorenz Meier
parent
69e57b22ac
commit
1429423876
5 changed files with 189 additions and 132 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      ROMFS/px4fmu_common/init.d-posix/rcS
  2. 4
      boards/cuav/x7pro/init/rc.board_defaults
  3. 9
      src/modules/gyro_fft/CMakeLists.txt
  4. 298
      src/modules/gyro_fft/GyroFFT.cpp
  5. 8
      src/modules/gyro_fft/GyroFFT.hpp

2
ROMFS/px4fmu_common/init.d-posix/rcS
Unescape View File

@ -154,6 +154,8 @@ param set-default SENS_IMU_MODE 0 @@ -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

4
boards/cuav/x7pro/init/rc.board_defaults
Unescape View File

@ -16,8 +16,8 @@ param set-default BAT2_A_PER_V 24 @@ -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

9
src/modules/gyro_fft/CMakeLists.txt
Unescape View File

@ -34,13 +34,6 @@ @@ -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( @@ -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

298
src/modules/gyro_fft/GyroFFT.cpp
Unescape View File

@ -93,6 +93,7 @@ GyroFFT::~GyroFFT() @@ -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() @@ -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) @@ -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;
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);
// 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 (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) @@ -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.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) {
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;
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)) {
} else {
_gyro_sample_rate_hz = vehicle_imu_status.gyro_rate_hz;
}
_gyro_sample_rate_hz = vehicle_imu_status.gyro_raw_rate_hz;
return;
}
}
}
}
@ -207,6 +233,7 @@ float GyroFFT::EstimatePeakFrequency(q15_t fft[], uint8_t peak_index) @@ -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,149 +254,176 @@ void GyroFFT::Run() @@ -227,149 +254,176 @@ 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;
perf_count(_gyro_fifo_generation_gap_perf);
}
// run on sensor gyro fifo updates
sensor_gyro_fifo_s sensor_gyro_fifo;
_gyro_last_generation = _sensor_gyro_fifo_sub.get_last_generation();
while (_sensor_gyro_fifo_sub.update(&sensor_gyro_fifo)) {
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;
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;
_fifo_last_scale = sensor_gyro_fifo.scale;
}
perf_count(_gyro_fifo_generation_gap_perf);
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 (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;
} else {
// run on sensor gyro fifo updates
sensor_gyro_s sensor_gyro;
_fifo_last_scale = sensor_gyro_fifo.scale;
}
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;
_gyro_last_generation = _sensor_gyro_fifo_sub.get_last_generation();
perf_count(_gyro_generation_gap_perf);
}
const int N = sensor_gyro_fifo.samples;
_gyro_last_generation = _sensor_gyro_sub.get_last_generation();
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};
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)};
int &buffer_index = _fft_buffer_index[axis];
int16_t *input[] {gyro_x, gyro_y, gyro_z};
Update(sensor_gyro.timestamp_sample, input, 1);
}
}
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++;
}
perf_end(_cycle_perf);
}
// 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);
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};
perf_begin(_fft_perf);
arm_rfft_q15(&_rfft_q15, _fft_input_buffer, _fft_outupt_buffer);
perf_end(_fft_perf);
fft_updated = true;
for (int axis = 0; axis < 3; axis++) {
int &buffer_index = _fft_buffer_index[axis];
static constexpr uint16_t MIN_SNR = 10; // TODO:
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++;
}
bool peaks_detected = false;
uint32_t peaks_magnitude[MAX_NUM_PEAKS] {};
uint8_t peak_index[MAX_NUM_PEAKS] {};
// 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);
// 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;
perf_begin(_fft_perf);
arm_rfft_q15(&_rfft_q15, _fft_input_buffer, _fft_outupt_buffer);
perf_end(_fft_perf);
fft_updated = true;
if (freq_hz > _param_imu_gyro_fft_max.get()) {
break;
}
static constexpr uint16_t MIN_SNR = 10; // TODO:
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];
bool peaks_detected = false;
uint32_t peaks_magnitude[MAX_NUM_PEAKS] {};
uint8_t peak_index[MAX_NUM_PEAKS] {};
const uint32_t fft_magnitude_squared = real * real + complex * complex;
// 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 (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 (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};
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;
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]);
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 (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;
}
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];
peak_frequencies[axis][num_peaks_found] = freq;
peak_magnitude[axis][num_peaks_found] = peaks_magnitude[i];
num_peaks_found++;
}
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;
// 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 = 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);
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;
}
publish = false;
}
perf_end(_cycle_perf);
}
int GyroFFT::task_spawn(int argc, char *argv[])

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

@ -45,6 +45,7 @@ @@ -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: @@ -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: @@ -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};

Write Preview
Loading…
Cancel
Save