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

sensors/vehicle_angular_velocity: gyro dynamic notch filters updated from onboard FFT

release/1.12
Daniel Agar 4 years ago
parent
ed6269b9a5
commit
00b3b3678b
7 changed files with 250 additions and 115 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. 11
      msg/sensor_gyro_fft.msg
  2. 181
      src/examples/gyro_fft/GyroFFT.cpp
  3. 35
      src/examples/gyro_fft/GyroFFT.hpp
  4. 15
      src/examples/gyro_fft/parameters.c
  5. 81
      src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
  6. 30
      src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
  7. 12
      src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c

11
msg/sensor_gyro_fft.msg
Unescape View File

@ -3,12 +3,9 @@ uint64 timestamp_sample @@ -3,12 +3,9 @@ uint64 timestamp_sample
uint32 device_id # unique device ID for the sensor that does not change between power cycles
float32 dt # delta time between samples (microseconds)
float32 sensor_sample_rate_hz
float32 resolution_hz
float32[3] peak_frequency # peak frequency per axis
float32[4] peak_frequencies_x # x axis peak frequencies
float32[4] peak_frequencies_y # y axis peak frequencies
float32[4] peak_frequencies_z # z axis peak frequencies
float32[6] peak_frequencies_x # x axis peak frequencies
float32[6] peak_frequencies_y # y axis peak frequencies
float32[6] peak_frequencies_z # z axis peak frequencies

181
src/examples/gyro_fft/GyroFFT.cpp
Unescape View File

@ -44,11 +44,45 @@ GyroFFT::GyroFFT() : @@ -44,11 +44,45 @@ GyroFFT::GyroFFT() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
{
arm_rfft_init_q15(&_rfft_q15, FFT_LENGTH, 0, 1);
switch (_param_imu_gyro_fft_len.get()) {
case 128:
AllocateBuffers<128>();
break;
case 256:
AllocateBuffers<256>();
break;
case 512:
AllocateBuffers<512>();
break;
case 1024:
AllocateBuffers<1024>();
break;
case 2048:
AllocateBuffers<2048>();
break;
case 4096:
AllocateBuffers<4096>();
break;
default:
// otherwise default to 256
PX4_ERR("Invalid IMU_GYRO_FFT_LEN=%.3f, resetting", (double)_param_imu_gyro_fft_len.get());
AllocateBuffers<256>();
_param_imu_gyro_fft_len.set(256);
_param_imu_gyro_fft_len.commit();
break;
}
arm_rfft_init_q15(&_rfft_q15, _param_imu_gyro_fft_len.get(), 0, 1);
// init Hanning window
for (int n = 0; n < FFT_LENGTH; n++) {
const float hanning_value = 0.5f * (1.f - cosf(2.f * M_PI_F * n / (FFT_LENGTH - 1)));
for (int n = 0; n < _param_imu_gyro_fft_len.get(); n++) {
const float hanning_value = 0.5f * (1.f - cosf(2.f * M_PI_F * n / (_param_imu_gyro_fft_len.get() - 1)));
arm_float_to_q15(&hanning_value, &_hanning_window[n], 1);
}
}
@ -59,6 +93,13 @@ GyroFFT::~GyroFFT() @@ -59,6 +93,13 @@ GyroFFT::~GyroFFT()
perf_free(_cycle_interval_perf);
perf_free(_fft_perf);
perf_free(_gyro_fifo_generation_gap_perf);
delete _gyro_data_buffer_x;
delete _gyro_data_buffer_y;
delete _gyro_data_buffer_z;
delete _hanning_window;
delete _fft_input_buffer;
delete _fft_outupt_buffer;
}
bool GyroFFT::init()
@ -77,13 +118,11 @@ bool GyroFFT::SensorSelectionUpdate(bool force) @@ -77,13 +118,11 @@ bool GyroFFT::SensorSelectionUpdate(bool force)
sensor_selection_s sensor_selection{};
_sensor_selection_sub.copy(&sensor_selection);
if (_selected_sensor_device_id != sensor_selection.gyro_device_id) {
if ((sensor_selection.gyro_device_id != 0) && (_selected_sensor_device_id != sensor_selection.gyro_device_id)) {
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 != 0)
&& (sensor_gyro_fifo_sub.get().device_id == sensor_selection.gyro_device_id)) {
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()) {
// find corresponding vehicle_imu_status instance
for (uint8_t imu_status = 0; imu_status < MAX_SENSOR_COUNT; imu_status++) {
@ -94,6 +133,7 @@ bool GyroFFT::SensorSelectionUpdate(bool force) @@ -94,6 +133,7 @@ bool GyroFFT::SensorSelectionUpdate(bool force)
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;
}
}
@ -117,8 +157,11 @@ void GyroFFT::VehicleIMUStatusUpdate() @@ -117,8 +157,11 @@ void GyroFFT::VehicleIMUStatusUpdate()
vehicle_imu_status_s vehicle_imu_status;
if (_vehicle_imu_status_sub.update(&vehicle_imu_status)) {
if ((vehicle_imu_status.gyro_rate_hz > 0) && (fabsf(vehicle_imu_status.gyro_rate_hz - _gyro_sample_rate_hz) > 1.f)) {
_gyro_sample_rate_hz = vehicle_imu_status.gyro_rate_hz;
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)) {
_gyro_sample_rate_hz = vehicle_imu_status.gyro_raw_rate_hz;
}
}
}
@ -133,7 +176,7 @@ static constexpr float tau(float x) @@ -133,7 +176,7 @@ static constexpr float tau(float x)
return (1.f / 4.f * p1 - sqrtf(6) / 24 * p2);
}
float GyroFFT::EstimatePeakFrequency(q15_t fft[FFT_LENGTH * 2], uint8_t peak_index)
float GyroFFT::EstimatePeakFrequency(q15_t fft[], uint8_t peak_index)
{
// find peak location using Quinn's Second Estimator (2020-06-14: http://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/)
int16_t real[3] { fft[peak_index - 2], fft[peak_index], fft[peak_index + 2] };
@ -154,20 +197,11 @@ float GyroFFT::EstimatePeakFrequency(q15_t fft[FFT_LENGTH * 2], uint8_t peak_ind @@ -154,20 +197,11 @@ float GyroFFT::EstimatePeakFrequency(q15_t fft[FFT_LENGTH * 2], uint8_t peak_ind
float d = (dp + dm) / 2 + tau(dp * dp) - tau(dm * dm);
float adjusted_bin = peak_index + d;
float peak_freq_adjusted = (_gyro_sample_rate_hz * adjusted_bin / (FFT_LENGTH * 2.f));
float peak_freq_adjusted = (_gyro_sample_rate_hz * adjusted_bin / (_param_imu_gyro_fft_len.get() * 2.f));
return peak_freq_adjusted;
}
static int float_cmp(const void *elem1, const void *elem2)
{
if (*(const float *)elem1 < * (const float *)elem2) {
return -1;
}
return *(const float *)elem1 > *(const float *)elem2;
}
void GyroFFT::Run()
{
if (should_exit()) {
@ -192,8 +226,9 @@ void GyroFFT::Run() @@ -192,8 +226,9 @@ void GyroFFT::Run()
}
SensorSelectionUpdate();
VehicleIMUStatusUpdate();
const float resolution_hz = _gyro_sample_rate_hz / FFT_LENGTH;
const float resolution_hz = _gyro_sample_rate_hz / _param_imu_gyro_fft_len.get();
bool publish = false;
bool fft_updated = false;
@ -212,40 +247,35 @@ void GyroFFT::Run() @@ -212,40 +247,35 @@ void GyroFFT::Run()
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 = nullptr;
switch (axis) {
case 0:
input = sensor_gyro_fifo.x;
break;
case 1:
input = sensor_gyro_fifo.y;
break;
case 2:
input = sensor_gyro_fifo.z;
break;
}
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 < FFT_LENGTH) {
if (buffer_index < _param_imu_gyro_fft_len.get()) {
// convert int16_t -> q15_t (scaling isn't relevant)
_gyro_data_buffer[axis][buffer_index] = input[n] / 2;
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 >= FFT_LENGTH) && !fft_updated) {
arm_mult_q15(_gyro_data_buffer[axis], _hanning_window, _fft_input_buffer, FFT_LENGTH);
if ((buffer_index >= _param_imu_gyro_fft_len.get()) && !fft_updated) {
arm_mult_q15(gyro_data_buffer[axis], _hanning_window, _fft_input_buffer, _param_imu_gyro_fft_len.get());
perf_begin(_fft_perf);
arm_rfft_q15(&_rfft_q15, _fft_input_buffer, _fft_outupt_buffer);
@ -254,16 +284,13 @@ void GyroFFT::Run() @@ -254,16 +284,13 @@ void GyroFFT::Run()
static constexpr uint16_t MIN_SNR = 10; // TODO:
uint32_t max_peak_magnitude = 0;
uint8_t max_peak_index = 0;
static constexpr int MAX_NUM_PEAKS = 4;
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 (uint8_t bucket_index = 2; bucket_index < (FFT_LENGTH / 2); bucket_index = bucket_index + 2) {
for (uint8_t bucket_index = 2; bucket_index < (_param_imu_gyro_fft_len.get() / 2); bucket_index = bucket_index + 2) {
const float freq_hz = (bucket_index / 2) * resolution_hz;
if (freq_hz > _param_imu_gyro_fft_max.get()) {
@ -277,17 +304,11 @@ void GyroFFT::Run() @@ -277,17 +304,11 @@ void GyroFFT::Run()
const uint32_t fft_magnitude_squared = real * real + complex * complex;
if (fft_magnitude_squared > MIN_SNR) {
if (fft_magnitude_squared > max_peak_magnitude) {
max_peak_magnitude = fft_magnitude_squared;
max_peak_index = bucket_index;
}
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;
publish = true;
peaks_detected = true;
break;
}
}
@ -295,64 +316,45 @@ void GyroFFT::Run() @@ -295,64 +316,45 @@ void GyroFFT::Run()
}
}
if (max_peak_index > 0) {
_sensor_gyro_fft.peak_frequency[axis] = _median_filter[axis].apply(EstimatePeakFrequency(_fft_outupt_buffer,
max_peak_index));
}
if (publish) {
float *peak_frequencies;
switch (axis) {
case 0:
peak_frequencies = _sensor_gyro_fft.peak_frequencies_x;
break;
case 1:
peak_frequencies = _sensor_gyro_fft.peak_frequencies_y;
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};
case 2:
peak_frequencies = _sensor_gyro_fft.peak_frequencies_z;
break;
}
int 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] < FFT_LENGTH) && (peaks_magnitude[i] > 0)) {
if ((peak_index[i] > 0) && (peak_index[i] < _param_imu_gyro_fft_len.get()) && (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()) {
peak_frequencies[peaks_found] = freq;
peaks_found++;
if (fabsf(peak_frequencies[axis][num_peaks_found] - freq) > 0.1f) {
publish = true;
}
peak_frequencies[axis][num_peaks_found] = freq;
num_peaks_found++;
}
}
}
// mark remaining slots empty
for (int i = peaks_found; i < MAX_NUM_PEAKS; i++) {
peak_frequencies[i] = NAN;
}
// publish in sorted order for convenience
if (peaks_found > 0) {
qsort(peak_frequencies, peaks_found, sizeof(float), float_cmp);
for (int i = num_peaks_found; i < MAX_NUM_PEAKS; i++) {
peak_frequencies[axis][i] = NAN;
}
}
// reset
// shift buffer (75% overlap)
int overlap_start = FFT_LENGTH / 4;
memmove(&_gyro_data_buffer[axis][0], &_gyro_data_buffer[axis][overlap_start], sizeof(q15_t) * overlap_start * 3);
// shift buffer (3/4 overlap)
const int overlap_start = _param_imu_gyro_fft_len.get() / 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.dt = 1e6f / _gyro_sample_rate_hz;
_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();
@ -390,6 +392,7 @@ int GyroFFT::task_spawn(int argc, char *argv[]) @@ -390,6 +392,7 @@ int GyroFFT::task_spawn(int argc, char *argv[])
int GyroFFT::print_status()
{
PX4_INFO("gyro sample rate: %.3f Hz", (double)_gyro_sample_rate_hz);
perf_print_counter(_cycle_perf);
perf_print_counter(_cycle_interval_perf);
perf_print_counter(_fft_perf);

35
src/examples/gyro_fft/GyroFFT.hpp
Unescape View File

@ -77,14 +77,26 @@ public: @@ -77,14 +77,26 @@ public:
bool init();
private:
static constexpr uint16_t FFT_LENGTH = 2048;
float EstimatePeakFrequency(q15_t fft[FFT_LENGTH * 2], uint8_t peak_index);
float EstimatePeakFrequency(q15_t fft[], uint8_t peak_index);
void Run() override;
bool SensorSelectionUpdate(bool force = false);
void VehicleIMUStatusUpdate();
static constexpr int MAX_SENSOR_COUNT = 3;
template<size_t N>
void AllocateBuffers()
{
_gyro_data_buffer_x = new q15_t[N];
_gyro_data_buffer_y = new q15_t[N];
_gyro_data_buffer_z = new q15_t[N];
_hanning_window = new q15_t[N];
_fft_input_buffer = new q15_t[N];
_fft_outupt_buffer = new q15_t[N * 2];
}
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr int MAX_NUM_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof(
sensor_gyro_fft_s::peak_frequencies_x[0]);
uORB::Publication<sensor_gyro_fft_s> _sensor_gyro_fft_pub{ORB_ID(sensor_gyro_fft)};
@ -104,22 +116,25 @@ private: @@ -104,22 +116,25 @@ private:
arm_rfft_instance_q15 _rfft_q15;
q15_t _gyro_data_buffer[3][FFT_LENGTH] {};
q15_t _hanning_window[FFT_LENGTH] {};
q15_t _fft_input_buffer[FFT_LENGTH] {};
q15_t _fft_outupt_buffer[FFT_LENGTH * 2] {};
q15_t *_gyro_data_buffer_x{nullptr};
q15_t *_gyro_data_buffer_y{nullptr};
q15_t *_gyro_data_buffer_z{nullptr};
q15_t *_hanning_window{nullptr};
q15_t *_fft_input_buffer{nullptr};
q15_t *_fft_outupt_buffer{nullptr};
float _gyro_sample_rate_hz{8000}; // 8 kHz default
float _fifo_last_scale{0};
int _fft_buffer_index[3] {};
unsigned _gyro_last_generation{0};
math::MedianFilter<float, 3> _median_filter[3] {};
sensor_gyro_fft_s _sensor_gyro_fft{};
DEFINE_PARAMETERS(
(ParamInt<px4::params::IMU_GYRO_FFT_LEN>) _param_imu_gyro_fft_len,
(ParamFloat<px4::params::IMU_GYRO_FFT_MIN>) _param_imu_gyro_fft_min,
(ParamFloat<px4::params::IMU_GYRO_FFT_MAX>) _param_imu_gyro_fft_max
)

15
src/examples/gyro_fft/parameters.c
Unescape View File

@ -61,3 +61,18 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MIN, 50.0f); @@ -61,3 +61,18 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MIN, 50.0f);
* @group Sensors
*/
PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MAX, 200.0f);
/**
* IMU gyro FFT length.
*
* @value 128 128
* @value 256 256
* @value 512 512
* @value 1024 1024
* @value 2048 2048
* @value 4096 4096
* @unit Hz
* @reboot_required true
* @group Sensors
*/
PARAM_DEFINE_INT32(IMU_GYRO_FFT_LEN, 256);

81
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
Unescape View File

@ -151,6 +151,10 @@ void VehicleAngularVelocity::ResetFilters(const Vector3f &angular_velocity, cons @@ -151,6 +151,10 @@ void VehicleAngularVelocity::ResetFilters(const Vector3f &angular_velocity, cons
// angular acceleration low pass
_lp_filter_acceleration[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get());
_lp_filter_acceleration[axis].reset(angular_acceleration(axis));
// dynamic notch filters, first disable, then force update (if available)
DisableDynamicNotchFFT();
UpdateDynamicNotchFFT(true);
}
_reset_filters = false;
@ -281,6 +285,68 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) @@ -281,6 +285,68 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
}
}
void VehicleAngularVelocity::DisableDynamicNotchFFT()
{
#if !defined(CONSTRAINED_FLASH)
// device id mismatch, disable all
for (auto &dnf : _dynamic_notch_filter) {
for (int axis = 0; axis < 3; axis++) {
dnf[axis].setParameters(0, 0, 0);
}
}
_dynamic_notch_available = false;
#endif // !CONSTRAINED_FLASH
}
void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
{
#if !defined(CONSTRAINED_FLASH)
if (_param_imu_gyro_dyn_nf.get() && (_sensor_gyro_fft_sub.updated() || force)) {
sensor_gyro_fft_s sensor_gyro_fft;
if (_sensor_gyro_fft_sub.copy(&sensor_gyro_fft) && (sensor_gyro_fft.device_id == _selected_sensor_device_id)
&& (fabsf(sensor_gyro_fft.sensor_sample_rate_hz - _filter_sample_rate_hz) < 0.05f)) {
float *peak_frequencies[] { sensor_gyro_fft.peak_frequencies_x, sensor_gyro_fft.peak_frequencies_y, sensor_gyro_fft.peak_frequencies_z};
for (int axis = 0; axis < 3; axis++) {
for (int i = 0; i < MAX_NUM_FFT_PEAKS; i++) {
auto &dnf = _dynamic_notch_filter[i][axis];
const float &peak_freq = peak_frequencies[axis][i];
if (PX4_ISFINITE(peak_freq) && (peak_freq > 1.f)) {
const float change_percent = fabsf(dnf.getNotchFreq() - peak_freq) / peak_freq;
if (change_percent > 0.001f) {
// peak frequency changed by at least 0.1%
dnf.setParameters(_filter_sample_rate_hz, peak_freq, sensor_gyro_fft.resolution_hz);
// only reset if there's sufficient change (> 1%)
if ((change_percent > 0.01f) && (_fifo_last_scale > 0.f)) {
dnf.reset(_angular_velocity(axis) / _fifo_last_scale);
}
}
_dynamic_notch_available = true;
} else {
// disable this notch filter
dnf.setParameters(0, 0, 0);
}
}
}
} else {
DisableDynamicNotchFFT();
}
}
#endif // !CONSTRAINED_FLASH
}
void VehicleAngularVelocity::Run()
{
// backup schedule
@ -319,6 +385,8 @@ void VehicleAngularVelocity::Run() @@ -319,6 +385,8 @@ void VehicleAngularVelocity::Run()
}
}
UpdateDynamicNotchFFT();
Vector3f angular_velocity_unscaled;
Vector3f angular_acceleration_unscaled;
@ -332,6 +400,19 @@ void VehicleAngularVelocity::Run() @@ -332,6 +400,19 @@ void VehicleAngularVelocity::Run()
data[n] = raw_data_array[axis][n];
}
#if !defined(CONSTRAINED_FLASH)
// Apply dynamic notch filter from FFT
if (_dynamic_notch_available) {
for (auto &dnf : _dynamic_notch_filter) {
if (dnf[axis].getNotchFreq() > 0.f) {
dnf[axis].applyDF1(data, N);
}
}
}
#endif // !CONSTRAINED_FLASH
// Apply general notch filter (IMU_GYRO_NF_FREQ)
if (_notch_filter_velocity[axis].getNotchFreq() > 0.f) {
_notch_filter_velocity[axis].apply(data, N);

30
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
Unescape View File

@ -1,6 +1,6 @@ @@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -50,6 +50,7 @@ @@ -50,6 +50,7 @@
#include <uORB/topics/estimator_sensor_bias.h>
#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>
#include <uORB/topics/vehicle_angular_acceleration.h>
@ -66,22 +67,21 @@ public: @@ -66,22 +67,21 @@ public:
VehicleAngularVelocity();
~VehicleAngularVelocity() override;
void PrintStatus();
bool Start();
void Stop();
void PrintStatus();
private:
void Run() override;
void ResetFilters(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_acceleration);
bool UpdateSampleRate();
void DisableDynamicNotchFFT();
void ParametersUpdate(bool force = false);
void Publish(const hrt_abstime &timestamp_sample);
void ResetFilters(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_acceleration);
void SensorBiasUpdate(bool force = false);
bool SensorSelectionUpdate(bool force = false);
void Publish(const hrt_abstime &timestamp_sample);
void UpdateDynamicNotchFFT(bool force = false);
bool UpdateSampleRate();
static constexpr int MAX_SENSOR_COUNT = 4;
@ -90,6 +90,9 @@ private: @@ -90,6 +90,9 @@ private:
uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
#if !defined(CONSTRAINED_FLASH)
uORB::Subscription _sensor_gyro_fft_sub {ORB_ID(sensor_gyro_fft)};
#endif // !CONSTRAINED_FLASH
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@ -118,6 +121,14 @@ private: @@ -118,6 +121,14 @@ private:
math::LowPassFilter2pArray _lp_filter_velocity[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
math::NotchFilterArray<float> _notch_filter_velocity[3] {};
#if !defined(CONSTRAINED_FLASH)
static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof(
sensor_gyro_fft_s::peak_frequencies_x[0]);
math::NotchFilterArray<float> _dynamic_notch_filter[MAX_NUM_FFT_PEAKS][3] {};
bool _dynamic_notch_available{false};
#endif // !CONSTRAINED_FLASH
// angular acceleration filter
math::LowPassFilter2p _lp_filter_acceleration[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
@ -133,7 +144,8 @@ private: @@ -133,7 +144,8 @@ private:
(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,
(ParamFloat<px4::params::IMU_GYRO_NF_BW>) _param_imu_gyro_nf_bw,
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax,
(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff
(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff,
(ParamBool<px4::params::IMU_GYRO_DYN_NF>) _param_imu_gyro_dyn_nf
)
};

12
src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c
Unescape View File

@ -121,3 +121,15 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 400); @@ -121,3 +121,15 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 400);
* @group Sensors
*/
PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 10.0f);
/**
* IMU gyro dynamic notch filtering
*
* Enable bank of dynamically updating notch filters.
* Requires onboard FFT (IMU_GYRO_FFT_EN).
*
* @boolean
* @reboot_required true
* @group Sensors
*/
PARAM_DEFINE_INT32(IMU_GYRO_DYN_NF, 0);

Write Preview
Loading…
Cancel
Save