dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sensors/vehicle_angular_velocity: fix dynamic notch ESC in FIFO case 

- last timestamp sample must be set in FIFO case for ESC RPM dynamic filter update
 - slightly relax thresholds for dynamic notch FFT apply or reset
This commit is contained in:
Daniel Agar 2021-03-21 20:46:54 -04:00 committed by GitHub
parent 1e507f41ca
commit 7563438558
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -84,6 +84,7 @@ void VehicleAngularVelocity::Stop()
{
// clear all registered callbacks
_sensor_sub.unregisterCallback();
_sensor_fifo_sub.unregisterCallback();
_sensor_selection_sub.unregisterCallback();
Deinit();
@ -433,7 +434,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool 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)) {
&& (fabsf(sensor_gyro_fft.sensor_sample_rate_hz - _filter_sample_rate_hz) < 10.f)) {
float *peak_frequencies[] {sensor_gyro_fft.peak_frequencies_x, sensor_gyro_fft.peak_frequencies_y, sensor_gyro_fft.peak_frequencies_z};
@ -443,14 +444,15 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
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;
const float peak_diff_abs = fabsf(dnf.getNotchFreq() - peak_freq);
const float change_percent = peak_diff_abs / 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) && (_last_scale > 0.f)) {
if ((peak_diff_abs > sensor_gyro_fft.resolution_hz) && (_last_scale > 0.f)) {
dnf.reset(_angular_velocity(axis) / _last_scale);
}
@ -494,6 +496,8 @@ void VehicleAngularVelocity::Run()
static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]);
if ((sensor_fifo_data.samples > 0) && (sensor_fifo_data.samples <= FIFO_SIZE_MAX)) {
_timestamp_sample_last = sensor_fifo_data.timestamp_sample;
const int N = sensor_fifo_data.samples;
const float dt_s = sensor_fifo_data.dt * 1e-6f;
const enum Rotation fifo_rotation = static_cast<enum Rotation>(sensor_fifo_data.rotation);