mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-23 22:58:10 +08:00
sensors/vehicle_angular_velocity: dynamic notch filter support ESC RPM (untested)
This commit is contained in:
Daniel Agar
@@ -53,9 +53,13 @@ VehicleAngularVelocity::~VehicleAngularVelocity()
|
||||
Stop();
|
||||
|
||||
perf_free(_filter_reset_perf);
|
||||
perf_free(_dynamic_notch_filter_update_perf);
|
||||
perf_free(_selection_changed_perf);
|
||||
perf_free(_dynamic_notch_filter_perf);
|
||||
#if !defined(CONSTRAINED_FLASH)
|
||||
perf_free(_dynamic_notch_filter_esc_rpm_update_perf);
|
||||
perf_free(_dynamic_notch_filter_fft_update_perf);
|
||||
perf_free(_dynamic_notch_filter_esc_rpm_perf);
|
||||
perf_free(_dynamic_notch_filter_fft_perf);
|
||||
#endif // CONSTRAINED_FLASH
|
||||
}
|
||||
|
||||
bool VehicleAngularVelocity::Start()
|
||||
@@ -158,7 +162,10 @@ void VehicleAngularVelocity::ResetFilters(const Vector3f &angular_velocity, cons
|
||||
_lp_filter_acceleration[axis].reset(angular_acceleration(axis));
|
||||
|
||||
// dynamic notch filters, first disable, then force update (if available)
|
||||
DisableDynamicNotchEscRpm();
|
||||
DisableDynamicNotchFFT();
|
||||
|
||||
UpdateDynamicNotchEscRpm(true);
|
||||
UpdateDynamicNotchFFT(true);
|
||||
}
|
||||
|
||||
@@ -214,6 +221,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
|
||||
_reset_filters = true;
|
||||
_bias.zero();
|
||||
_fifo_available = true;
|
||||
_last_scale = 0.f;
|
||||
|
||||
perf_count(_selection_changed_perf);
|
||||
|
||||
@@ -239,6 +247,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
|
||||
_reset_filters = true;
|
||||
_bias.zero();
|
||||
_fifo_available = false;
|
||||
_last_scale = 1.f;
|
||||
|
||||
perf_count(_selection_changed_perf);
|
||||
|
||||
@@ -295,26 +304,97 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
|
||||
}
|
||||
}
|
||||
|
||||
void VehicleAngularVelocity::DisableDynamicNotchFFT()
|
||||
void VehicleAngularVelocity::DisableDynamicNotchEscRpm()
|
||||
{
|
||||
#if !defined(CONSTRAINED_FLASH)
|
||||
|
||||
// device id mismatch, disable all
|
||||
for (auto &dnf : _dynamic_notch_filter) {
|
||||
for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
dnf[axis].setParameters(0, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
_dynamic_notch_available = false;
|
||||
_dynamic_notch_esc_rpm_available = false;
|
||||
#endif // !CONSTRAINED_FLASH
|
||||
}
|
||||
|
||||
void VehicleAngularVelocity::DisableDynamicNotchFFT()
|
||||
{
|
||||
#if !defined(CONSTRAINED_FLASH)
|
||||
|
||||
// device id mismatch, disable all
|
||||
for (auto &dnf : _dynamic_notch_filter_fft) {
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
dnf[axis].setParameters(0, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
_dynamic_notch_fft_available = false;
|
||||
#endif // !CONSTRAINED_FLASH
|
||||
}
|
||||
|
||||
void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
|
||||
{
|
||||
#if !defined(CONSTRAINED_FLASH)
|
||||
const bool enabled = _param_imu_gyro_dyn_nf.get() & 0b01;
|
||||
|
||||
if (enabled && (_esc_status_sub.updated() || force)) {
|
||||
_dynamic_notch_esc_rpm_available = false;
|
||||
|
||||
esc_status_s esc_status;
|
||||
|
||||
if (_esc_status_sub.copy(&esc_status)) {
|
||||
for (size_t i = 0; i < MAX_NUM_ESC_RPM; i++) {
|
||||
static constexpr int32_t MIN_ESC_RPM = 20 * 60; // 20 Hz safe minimum limit TODO: configurable
|
||||
|
||||
if ((esc_status.esc[i].timestamp != 0) && ((_timestamp_sample_last - esc_status.esc[i].timestamp) < 1_s)
|
||||
&& (esc_status.esc[i].esc_rpm > MIN_ESC_RPM)) {
|
||||
|
||||
// TODO: rotor frequency, blade pass frequency, harmonics
|
||||
const float frequency_hz = static_cast<float>(esc_status.esc[i].esc_rpm) / 60.f;
|
||||
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
auto &dnf = _dynamic_notch_filter_esc_rpm[i][axis];
|
||||
const float change_percent = fabsf(dnf.getNotchFreq() - frequency_hz) / frequency_hz;
|
||||
|
||||
if (change_percent > 0.001f) {
|
||||
// peak frequency changed by at least 0.1%
|
||||
dnf.setParameters(_filter_sample_rate_hz, frequency_hz, 1.f); // TODO: configurable bandwidth
|
||||
|
||||
// only reset if there's sufficient change (> 1%)
|
||||
if ((change_percent > 0.01f) && (_last_scale > 0.f)) {
|
||||
dnf.reset(_angular_velocity(axis) / _last_scale);
|
||||
}
|
||||
|
||||
perf_count(_dynamic_notch_filter_esc_rpm_update_perf);
|
||||
}
|
||||
}
|
||||
|
||||
_dynamic_notch_esc_rpm_available = true;
|
||||
|
||||
} else {
|
||||
// disable this notch filter
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
auto &dnf = _dynamic_notch_filter_esc_rpm[i][axis];
|
||||
dnf.setParameters(0, 0, 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif // !CONSTRAINED_FLASH
|
||||
}
|
||||
|
||||
void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
|
||||
{
|
||||
#if !defined(CONSTRAINED_FLASH)
|
||||
const bool enabled = _param_imu_gyro_dyn_nf.get() & 0b10;
|
||||
|
||||
if (enabled && (_sensor_gyro_fft_sub.updated() || force)) {
|
||||
_dynamic_notch_fft_available = false;
|
||||
|
||||
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)
|
||||
@@ -324,7 +404,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
|
||||
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
for (int i = 0; i < MAX_NUM_FFT_PEAKS; i++) {
|
||||
auto &dnf = _dynamic_notch_filter[i][axis];
|
||||
auto &dnf = _dynamic_notch_filter_fft[i][axis];
|
||||
const float &peak_freq = peak_frequencies[axis][i];
|
||||
|
||||
if (PX4_ISFINITE(peak_freq) && (peak_freq > 1.f)) {
|
||||
@@ -335,14 +415,14 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
|
||||
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);
|
||||
if ((change_percent > 0.01f) && (_last_scale > 0.f)) {
|
||||
dnf.reset(_angular_velocity(axis) / _last_scale);
|
||||
}
|
||||
|
||||
perf_count(_dynamic_notch_filter_update_perf);
|
||||
perf_count(_dynamic_notch_filter_fft_update_perf);
|
||||
}
|
||||
|
||||
_dynamic_notch_available = true;
|
||||
_dynamic_notch_fft_available = true;
|
||||
|
||||
} else {
|
||||
// disable this notch filter
|
||||
@@ -383,13 +463,13 @@ void VehicleAngularVelocity::Run()
|
||||
const float dt_s = sensor_fifo_data.dt * 1e-6f;
|
||||
const enum Rotation fifo_rotation = static_cast<enum Rotation>(sensor_fifo_data.rotation);
|
||||
|
||||
if (_reset_filters || (fabsf(sensor_fifo_data.scale - _fifo_last_scale) > FLT_EPSILON)) {
|
||||
if (_reset_filters || (fabsf(sensor_fifo_data.scale - _last_scale) > FLT_EPSILON)) {
|
||||
if (UpdateSampleRate()) {
|
||||
// in FIFO mode the unscaled raw data is filtered
|
||||
_angular_velocity_prev = _angular_velocity / sensor_fifo_data.scale;
|
||||
ResetFilters(_angular_velocity_prev, _angular_acceleration / sensor_fifo_data.scale);
|
||||
|
||||
_fifo_last_scale = sensor_fifo_data.scale;
|
||||
_last_scale = sensor_fifo_data.scale;
|
||||
}
|
||||
|
||||
if (_reset_filters) {
|
||||
@@ -397,6 +477,7 @@ void VehicleAngularVelocity::Run()
|
||||
}
|
||||
}
|
||||
|
||||
UpdateDynamicNotchEscRpm();
|
||||
UpdateDynamicNotchFFT();
|
||||
|
||||
Vector3f angular_velocity_unscaled;
|
||||
@@ -414,17 +495,30 @@ void VehicleAngularVelocity::Run()
|
||||
|
||||
#if !defined(CONSTRAINED_FLASH)
|
||||
|
||||
// Apply dynamic notch filter from FFT
|
||||
if (_dynamic_notch_available) {
|
||||
perf_begin(_dynamic_notch_filter_perf);
|
||||
// Apply dynamic notch filter from ESC RPM
|
||||
if (_dynamic_notch_esc_rpm_available) {
|
||||
perf_begin(_dynamic_notch_filter_esc_rpm_perf);
|
||||
|
||||
for (auto &dnf : _dynamic_notch_filter) {
|
||||
for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
|
||||
if (dnf[axis].getNotchFreq() > 0.f) {
|
||||
dnf[axis].applyDF1(data, N);
|
||||
}
|
||||
}
|
||||
|
||||
perf_end(_dynamic_notch_filter_perf);
|
||||
perf_end(_dynamic_notch_filter_esc_rpm_perf);
|
||||
}
|
||||
|
||||
// Apply dynamic notch filter from FFT
|
||||
if (_dynamic_notch_fft_available) {
|
||||
perf_begin(_dynamic_notch_filter_fft_perf);
|
||||
|
||||
for (auto &dnf : _dynamic_notch_filter_fft) {
|
||||
if (dnf[axis].getNotchFreq() > 0.f) {
|
||||
dnf[axis].applyDF1(data, N);
|
||||
}
|
||||
}
|
||||
|
||||
perf_end(_dynamic_notch_filter_fft_perf);
|
||||
}
|
||||
|
||||
#endif // !CONSTRAINED_FLASH
|
||||
@@ -488,10 +582,43 @@ void VehicleAngularVelocity::Run()
|
||||
}
|
||||
}
|
||||
|
||||
UpdateDynamicNotchEscRpm();
|
||||
UpdateDynamicNotchFFT();
|
||||
|
||||
// Apply calibration, rotation, and correct for in-run bias errors
|
||||
Vector3f angular_velocity{_calibration.Correct(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z}) - _bias};
|
||||
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
#if !defined(CONSTRAINED_FLASH)
|
||||
|
||||
// Apply dynamic notch filter from ESC RPM
|
||||
if (_dynamic_notch_esc_rpm_available) {
|
||||
perf_begin(_dynamic_notch_filter_esc_rpm_perf);
|
||||
|
||||
for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
|
||||
if (dnf[axis].getNotchFreq() > 0.f) {
|
||||
dnf[axis].applyDF1(&angular_velocity(axis), 1);
|
||||
}
|
||||
}
|
||||
|
||||
perf_end(_dynamic_notch_filter_esc_rpm_perf);
|
||||
}
|
||||
|
||||
// Apply dynamic notch filter from FFT
|
||||
if (_dynamic_notch_fft_available) {
|
||||
perf_begin(_dynamic_notch_filter_fft_perf);
|
||||
|
||||
for (auto &dnf : _dynamic_notch_filter_fft) {
|
||||
if (dnf[axis].getNotchFreq() > 0.f) {
|
||||
dnf[axis].applyDF1(&angular_velocity(axis), 1);
|
||||
}
|
||||
}
|
||||
|
||||
perf_end(_dynamic_notch_filter_fft_perf);
|
||||
}
|
||||
|
||||
#endif // !CONSTRAINED_FLASH
|
||||
|
||||
// Apply general notch filter (IMU_GYRO_NF_FREQ)
|
||||
_notch_filter_velocity[axis].apply(&angular_velocity(axis), 1);
|
||||
|
||||
|
||||
@@ -46,6 +46,7 @@
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <uORB/topics/estimator_selector_status.h>
|
||||
#include <uORB/topics/estimator_sensor_bias.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
@@ -74,12 +75,14 @@ public:
|
||||
private:
|
||||
void Run() override;
|
||||
|
||||
void DisableDynamicNotchEscRpm();
|
||||
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 UpdateDynamicNotchEscRpm(bool force = false);
|
||||
void UpdateDynamicNotchFFT(bool force = false);
|
||||
bool UpdateSampleRate();
|
||||
|
||||
@@ -91,6 +94,7 @@ 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 _esc_status_sub {ORB_ID(esc_status)};
|
||||
uORB::Subscription _sensor_gyro_fft_sub {ORB_ID(sensor_gyro_fft)};
|
||||
#endif // !CONSTRAINED_FLASH
|
||||
|
||||
@@ -122,11 +126,20 @@ private:
|
||||
math::NotchFilterArray<float> _notch_filter_velocity[3] {};
|
||||
|
||||
#if !defined(CONSTRAINED_FLASH)
|
||||
static constexpr int MAX_NUM_ESC_RPM = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]);
|
||||
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};
|
||||
math::NotchFilterArray<float> _dynamic_notch_filter_esc_rpm[MAX_NUM_ESC_RPM][3] {};
|
||||
math::NotchFilterArray<float> _dynamic_notch_filter_fft[MAX_NUM_FFT_PEAKS][3] {};
|
||||
|
||||
perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter ESC RPM update")};
|
||||
perf_counter_t _dynamic_notch_filter_fft_update_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update")};
|
||||
perf_counter_t _dynamic_notch_filter_esc_rpm_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch ESC RPM filter")};
|
||||
perf_counter_t _dynamic_notch_filter_fft_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch FFT filter")};
|
||||
|
||||
bool _dynamic_notch_esc_rpm_available{false};
|
||||
bool _dynamic_notch_fft_available{false};
|
||||
#endif // !CONSTRAINED_FLASH
|
||||
|
||||
// angular acceleration filter
|
||||
@@ -134,23 +147,23 @@ private:
|
||||
|
||||
uint32_t _selected_sensor_device_id{0};
|
||||
|
||||
float _fifo_last_scale{0};
|
||||
float _last_scale{0.f};
|
||||
|
||||
bool _reset_filters{true};
|
||||
bool _fifo_available{false};
|
||||
|
||||
perf_counter_t _filter_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro filter reset")};
|
||||
perf_counter_t _dynamic_notch_filter_update_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter update")};
|
||||
perf_counter_t _selection_changed_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro selection changed")};
|
||||
perf_counter_t _dynamic_notch_filter_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch filter")};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
#if !defined(CONSTRAINED_FLASH)
|
||||
(ParamInt<px4::params::IMU_GYRO_DYN_NF>) _param_imu_gyro_dyn_nf,
|
||||
#endif // !CONSTRAINED_FLASH
|
||||
(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
|
||||
(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,
|
||||
(ParamBool<px4::params::IMU_GYRO_DYN_NF>) _param_imu_gyro_dyn_nf
|
||||
(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff
|
||||
)
|
||||
};
|
||||
|
||||
|
||||
@@ -126,10 +126,11 @@ 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
|
||||
* Requires ESC RPM feedback or onboard FFT (IMU_GYRO_FFT_EN).
|
||||
* @group Sensors
|
||||
* @min 0
|
||||
* @max 2
|
||||
* @bit 0 ESC RPM
|
||||
* @bit 1 FFT
|
||||
*/
|
||||
PARAM_DEFINE_INT32(IMU_GYRO_DYN_NF, 0);
|
||||
|
||||
Reference in New Issue
Block a user