fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

dynamic-notch: general cleanup

Browse Source
This commit is contained in:
mahima-yoga 2025-10-15 16:24:47 +02:00
parent cdf8cd274c
commit 91bc8046f2
3 changed files with 20 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -652,13 +652,14 @@ void VehicleAngularVelocity::UpdateDynamicNotchIceRpm(const hrt_abstime &time_no
const float bandwidth_hz = _param_imu_gyro_dnf_bw.get();
const float freq_min = math::max(_param_imu_gyro_dnf_min.get(), bandwidth_hz);
for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
for (unsigned i = 0; i < MAX_NUM_ICE_ENGINES; i++) {
internal_combustion_engine_status_s ice_status{};
if (_ice_status_sub[i].copy(&ice_status) &&
(time_now_us < ice_status.timestamp + DYNAMIC_NOTCH_FITLER_TIMEOUT)) {
const bool engine_running = (ice_status.state == internal_combustion_engine_status_s::STATE_RUNNING);
const bool engine_running = (ice_status.state == internal_combustion_engine_status_s::STATE_RUNNING
|| ice_status.state == internal_combustion_engine_status_s::STATE_STARTING);
if (engine_running && ice_status.engine_speed_rpm > 0.f) {
const float engine_hz = fabsf(ice_status.engine_speed_rpm) / 60.f;
@ -695,7 +696,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchIceRpm(const hrt_abstime &time_no
}
// timeout handling
for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
for (unsigned i = 0; i < MAX_NUM_ICE_ENGINES; i++) {
if (_ice_available[i] && (time_now_us > _last_ice_rpm_notch_update[i] + DYNAMIC_NOTCH_FITLER_TIMEOUT)) {
bool all_disabled = true;

26
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
View File

@ -109,6 +109,7 @@ private:
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)};
uORB::SubscriptionMultiArray<internal_combustion_engine_status_s> _ice_status_sub{ORB_ID::internal_combustion_engine_status};
uORB::Subscription _sensor_gyro_fft_sub {ORB_ID(sensor_gyro_fft)};
@ -144,8 +145,8 @@ private:
enum DynamicNotch {
EscRpm = 1,
FFT = 2,
IceRpm = 4,
IceRpm = 2,
FFT = 4,
};
static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 3_s;
@ -156,25 +157,24 @@ private:
using NotchFilterHarmonic = math::NotchFilter<float>[3][MAX_NUM_ESCS];
NotchFilterHarmonic *_dynamic_notch_filter_esc_rpm{nullptr};
// Internal Combustion Engine (ICE) dynamic notch filters (separate storage)
static constexpr int MAX_NUM_ICE_ENGINES = 5;
using NotchFilterIceHarmonic = math::NotchFilter<float>[3][MAX_NUM_ICE_ENGINES];
NotchFilterIceHarmonic *_dynamic_notch_filter_ice_rpm{nullptr};
int _esc_rpm_harmonics{0};
int _ice_rpm_harmonics{0};
px4::Bitset<MAX_NUM_ESCS> _esc_available{};
hrt_abstime _last_esc_rpm_notch_update[MAX_NUM_ESCS] {};
// ICE availability/timeouts (separate from ESC)
px4::Bitset<MAX_NUM_ICE_ENGINES> _ice_available{};
hrt_abstime _last_ice_rpm_notch_update[MAX_NUM_ICE_ENGINES] {};
perf_counter_t _dynamic_notch_filter_esc_rpm_disable_perf{nullptr};
perf_counter_t _dynamic_notch_filter_esc_rpm_init_perf{nullptr};
perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr};
// ICE RPM
static constexpr int MAX_NUM_ICE_ENGINES = ORB_MULTI_MAX_INSTANCES;
using NotchFilterIceHarmonic = math::NotchFilter<float>[3][MAX_NUM_ICE_ENGINES];
NotchFilterIceHarmonic *_dynamic_notch_filter_ice_rpm{nullptr};
int _ice_rpm_harmonics{0};
px4::Bitset<MAX_NUM_ICE_ENGINES> _ice_available{};
hrt_abstime _last_ice_rpm_notch_update[MAX_NUM_ICE_ENGINES] {};
perf_counter_t _dynamic_notch_filter_ice_rpm_disable_perf{nullptr};
perf_counter_t _dynamic_notch_filter_ice_rpm_init_perf{nullptr};
perf_counter_t _dynamic_notch_filter_ice_rpm_update_perf{nullptr};

6
src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c
View File

@ -175,13 +175,13 @@ PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 20.0f);
* IMU gyro dynamic notch filtering
*
* Enable bank of dynamically updating notch filters.
* Requires ESC RPM feedback or onboard FFT (IMU_GYRO_FFT_EN).
* Requires RPM feedback or onboard FFT (IMU_GYRO_FFT_EN).
* @group Sensors
* @min 0
* @max 7
* @bit 0 ESC RPM
* @bit 1 FFT
* @bit 2 ICE RPM
* @bit 1 ICE RPM
* @bit 2 FFT
*/
PARAM_DEFINE_INT32(IMU_GYRO_DNF_EN, 0);