diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index d195f6fa72..a15bd8c6e4 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -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; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 8809604d98..877c9fdf65 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -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 _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[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[3][MAX_NUM_ICE_ENGINES]; - NotchFilterIceHarmonic *_dynamic_notch_filter_ice_rpm{nullptr}; - int _esc_rpm_harmonics{0}; - - int _ice_rpm_harmonics{0}; px4::Bitset _esc_available{}; hrt_abstime _last_esc_rpm_notch_update[MAX_NUM_ESCS] {}; - // ICE availability/timeouts (separate from ESC) - px4::Bitset _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[3][MAX_NUM_ICE_ENGINES]; + NotchFilterIceHarmonic *_dynamic_notch_filter_ice_rpm{nullptr}; + + int _ice_rpm_harmonics{0}; + px4::Bitset _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}; diff --git a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c index 82df797905..2b9ef80386 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -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);