forked from Archive/PX4-Autopilot
sensors/vehicle_angular_velocity: gyro dynamic notch filters updated from onboard FFT
This commit is contained in:
Daniel Agar
parent
ed6269b9a5
commit
00b3b3678b
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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()
|
|||
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)
|
|||
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)
|
|||
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()
|
|||
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)
|
|||
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
|
|||
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()
|
|||
}
|
||||
|
||||
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()
|
|||
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()
|
|||
|
||||
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()
|
|||
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()
|
|||
}
|
||||
}
|
||||
|
||||
if (max_peak_index > 0) {
|
||||
_sensor_gyro_fft.peak_frequency[axis] = _median_filter[axis].apply(EstimatePeakFrequency(_fft_outupt_buffer,
|
||||
max_peak_index));
|
||||
}
|
||||
if (peaks_detected) {
|
||||
float *peak_frequencies[] { _sensor_gyro_fft.peak_frequencies_x, _sensor_gyro_fft.peak_frequencies_y, _sensor_gyro_fft.peak_frequencies_z};
|
||||
|
||||
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;
|
||||
|
||||
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[])
|
|||
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -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:
|
|||
|
||||
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
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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)
|
|||
}
|
||||
}
|
||||
|
||||
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()
|
|||
}
|
||||
}
|
||||
|
||||
UpdateDynamicNotchFFT();
|
||||
|
||||
Vector3f angular_velocity_unscaled;
|
||||
Vector3f angular_acceleration_unscaled;
|
||||
|
||||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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 @@
|
|||
#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:
|
|||
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 ×tamp_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 ×tamp_sample);
|
||||
void UpdateDynamicNotchFFT(bool force = false);
|
||||
bool UpdateSampleRate();
|
||||
|
||||
static constexpr int MAX_SENSOR_COUNT = 4;
|
||||
|
||||
|
|
@ -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:
|
|||
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:
|
|||
(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
|
||||
)
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue