diff --git a/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp b/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp index 4926d1ca9f6b..ce3793373745 100644 --- a/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp +++ b/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp @@ -36,14 +36,19 @@ * @file rpm_simulator.cpp * Simple app for publishing RPM messages with custom value. * - * Usage: rpm_simulator + * Usage: rpm_simulator [duration_s] * rpm_simulator 344.2 + * rpm_simulator 344.2 10 # keep republishing for 10s (uORB::Publication + * # unadvertises on destruction, so a single-shot + * # publish disappears again as soon as this + * # command returns) * * @author ThunderFly s.r.o., Roman Dvorak */ #include #include +#include #include #include @@ -52,8 +57,8 @@ extern "C" __EXPORT int rpm_simulator_main(int argc, char *argv[]); int rpm_simulator_main(int argc, char *argv[]) { // check input - if (argc != 2) { - PX4_INFO("Usage: rpm_simulator "); + if (argc < 2) { + PX4_INFO("Usage: rpm_simulator [duration_s]"); PX4_INFO("Exit. Without publishing any message."); return 0; } @@ -61,16 +66,26 @@ int rpm_simulator_main(int argc, char *argv[]) rpm_s rpm{}; uORB::Publication rpm_pub{ORB_ID(rpm)}; - uint64_t timestamp_us = hrt_absolute_time(); float frequency = atof(argv[1]); + float duration_s = (argc >= 3) ? (float)atof(argv[2]) : 0.f; // prpepare RPM data message - rpm.timestamp = timestamp_us; + rpm.timestamp = hrt_absolute_time(); rpm.rpm_estimate = frequency; // Publish data and let the user know what was published rpm_pub.publish(rpm); print_message(ORB_ID(rpm), rpm); + if (duration_s > 0.f) { + const hrt_abstime end = hrt_absolute_time() + (hrt_abstime)(duration_s * 1e6f); + + while (hrt_absolute_time() < end) { + px4_usleep(100000); + rpm.timestamp = hrt_absolute_time(); + rpm_pub.publish(rpm); + } + } + return 0; } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 23111d2f3fd6..a10e85f5c65e 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -63,6 +63,12 @@ VehicleAngularVelocity::~VehicleAngularVelocity() perf_free(_dynamic_notch_filter_esc_rpm_init_perf); perf_free(_dynamic_notch_filter_esc_rpm_update_perf); + delete[] _dynamic_notch_filter_rotor_rpm; + perf_free(_dynamic_notch_filter_rotor_rpm_disable_perf); + perf_free(_dynamic_notch_filter_rotor_rpm_init_perf); + perf_free(_dynamic_notch_filter_rotor_rpm_update_perf); + + perf_free(_dynamic_notch_filter_fft_disable_perf); perf_free(_dynamic_notch_filter_fft_update_perf); #endif // CONSTRAINED_FLASH @@ -195,6 +201,7 @@ void VehicleAngularVelocity::ResetFilters(const hrt_abstime &time_now_us) // force reset notch filters on any scale change UpdateDynamicNotchEscRpm(time_now_us, true); + UpdateDynamicNotchRotorRpm(time_now_us, true); UpdateDynamicNotchFFT(time_now_us, true); _angular_velocity_raw_prev = angular_velocity_uncalibrated; @@ -502,6 +509,56 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) DisableDynamicNotchFFT(); } + // Rotor RPM dynamic notches: use same harmonics parameter for now + if (_param_imu_gyro_dnf_en.get() & DynamicNotch::RotorRpm) { + + const int32_t rotor_rpm_harmonics = math::constrain(_param_imu_gyro_dnf_hmc.get(), (int32_t)1, (int32_t)10); + + if (_dynamic_notch_filter_rotor_rpm && (rotor_rpm_harmonics != _rotor_rpm_harmonics)) { + delete[] _dynamic_notch_filter_rotor_rpm; + _dynamic_notch_filter_rotor_rpm = nullptr; + _rotor_rpm_harmonics = 0; + } + + if (_dynamic_notch_filter_rotor_rpm == nullptr) { + + _dynamic_notch_filter_rotor_rpm = new NotchFilterRotorRpmHarmonic[rotor_rpm_harmonics]; + + if (_dynamic_notch_filter_rotor_rpm) { + _rotor_rpm_harmonics = rotor_rpm_harmonics; + + if (_dynamic_notch_filter_rotor_rpm_disable_perf == nullptr) { + _dynamic_notch_filter_rotor_rpm_disable_perf = perf_alloc(PC_COUNT, + MODULE_NAME": gyro dynamic notch filter Rotor RPM disable"); + } + + if (_dynamic_notch_filter_rotor_rpm_init_perf == nullptr) { + _dynamic_notch_filter_rotor_rpm_init_perf = perf_alloc(PC_COUNT, + MODULE_NAME": gyro dynamic notch filter Rotor RPM init"); + } + + if (_dynamic_notch_filter_rotor_rpm_update_perf == nullptr) { + _dynamic_notch_filter_rotor_rpm_update_perf = perf_alloc(PC_COUNT, + MODULE_NAME": gyro dynamic notch filter Rotor RPM update"); + } + + } else { + _rotor_rpm_harmonics = 0; + + perf_free(_dynamic_notch_filter_rotor_rpm_disable_perf); + perf_free(_dynamic_notch_filter_rotor_rpm_init_perf); + perf_free(_dynamic_notch_filter_rotor_rpm_update_perf); + + _dynamic_notch_filter_rotor_rpm_disable_perf = nullptr; + _dynamic_notch_filter_rotor_rpm_init_perf = nullptr; + _dynamic_notch_filter_rotor_rpm_update_perf = nullptr; + } + } + + } else { + DisableDynamicNotchRotorRpm(); + } + #endif // !CONSTRAINED_FLASH } } @@ -573,6 +630,25 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT() #endif // !CONSTRAINED_FLASH } +void VehicleAngularVelocity::DisableDynamicNotchRotorRpm() +{ +#if !defined(CONSTRAINED_FLASH) + + if (_dynamic_notch_filter_rotor_rpm) { + for (int harmonic = 0; harmonic < _rotor_rpm_harmonics; harmonic++) { + for (int axis = 0; axis < 3; axis++) { + for (int sensor = 0; sensor < MAX_NUM_ROTOR_RPM_SENSORS; sensor++) { + _dynamic_notch_filter_rotor_rpm[harmonic][axis][sensor].disable(); + _rotor_rpm_available.set(sensor, false); + perf_count(_dynamic_notch_filter_rotor_rpm_disable_perf); + } + } + } + } + +#endif // !CONSTRAINED_FLASH +} + void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(const hrt_abstime &time_now_us, bool force) { #if !defined(CONSTRAINED_FLASH) @@ -729,6 +805,91 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(const hrt_abstime &time_now_u #endif // !CONSTRAINED_FLASH } +void VehicleAngularVelocity::UpdateDynamicNotchRotorRpm(const hrt_abstime &time_now_us, bool force) +{ +#if !defined(CONSTRAINED_FLASH) + const bool enabled = _dynamic_notch_filter_rotor_rpm && (_param_imu_gyro_dnf_en.get() & DynamicNotch::RotorRpm); + + if (enabled) { + bool axis_init[3] {false, false, false}; + + 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 < MAX_NUM_ROTOR_RPM_SENSORS; i++) { + rpm_s rpm_status{}; + + if (_rpm_sub[i].copy(&rpm_status) && + (time_now_us < rpm_status.timestamp + DYNAMIC_NOTCH_FITLER_TIMEOUT)) { + + // rpm_estimate of 0 means no movement measured within the driver's own timeout + if (rpm_status.rpm_estimate > FLT_EPSILON) { + const float rotor_hz = rpm_status.rpm_estimate / 60.f; + const bool force_update = force || !_rotor_rpm_available[i]; + + for (int harmonic = 0; harmonic < _rotor_rpm_harmonics; harmonic++) { + const float frequency_hz = math::max(rotor_hz * (harmonic + 1), + freq_min + (harmonic * 0.5f * bandwidth_hz)); + + for (int axis = 0; axis < 3; axis++) { + auto &nf = _dynamic_notch_filter_rotor_rpm[harmonic][axis][i]; + + const float notch_freq_delta = fabsf(nf.getNotchFreq() - frequency_hz); + const bool notch_freq_changed = (notch_freq_delta > 0.1f); + const bool allow_update = !axis_init[axis] || (nf.initialized() && notch_freq_delta < nf.getBandwidth()); + + if ((force_update || notch_freq_changed) && allow_update) { + if (nf.setParameters(_filter_sample_rate_hz, frequency_hz, bandwidth_hz)) { + perf_count(_dynamic_notch_filter_rotor_rpm_update_perf); + + if (!nf.initialized()) { + perf_count(_dynamic_notch_filter_rotor_rpm_init_perf); + axis_init[axis] = true; + } + } + } + } + } + + _rotor_rpm_available.set(i, true); + _last_rotor_rpm_notch_update[i] = rpm_status.timestamp; + } + } + } + + // timeout handling + for (unsigned i = 0; i < MAX_NUM_ROTOR_RPM_SENSORS; i++) { + if (_rotor_rpm_available[i] && (time_now_us > _last_rotor_rpm_notch_update[i] + DYNAMIC_NOTCH_FITLER_TIMEOUT)) { + bool all_disabled = true; + + for (int harmonic = _rotor_rpm_harmonics - 1; harmonic >= 0; harmonic--) { + for (int axis = 0; axis < 3; axis++) { + auto &nf = _dynamic_notch_filter_rotor_rpm[harmonic][axis][i]; + + if (nf.getNotchFreq() > 0.f) { + if (nf.initialized() && !axis_init[axis]) { + nf.disable(); + perf_count(_dynamic_notch_filter_rotor_rpm_disable_perf); + axis_init[axis] = true; + } + } + + if (nf.getNotchFreq() > 0.f) { + all_disabled = false; + } + } + } + + if (all_disabled) { + _rotor_rpm_available.set(i, false); + } + } + } + } + +#endif // !CONSTRAINED_FLASH +} + float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int N) { #if !defined(CONSTRAINED_FLASH) @@ -746,6 +907,19 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int } } + // Apply dynamic notch filter from Rotor RPM + if (_dynamic_notch_filter_rotor_rpm) { + for (int inst = 0; inst < MAX_NUM_ROTOR_RPM_SENSORS; inst++) { + if (_rotor_rpm_available[inst]) { + for (int harmonic = 0; harmonic < _rotor_rpm_harmonics; harmonic++) { + if (_dynamic_notch_filter_rotor_rpm[harmonic][axis][inst].getNotchFreq() > 0.f) { + _dynamic_notch_filter_rotor_rpm[harmonic][axis][inst].applyArray(data, N); + } + } + } + } + } + // Apply dynamic notch filter from FFT if (_dynamic_notch_fft_available) { for (int peak = MAX_NUM_FFT_PEAKS - 1; peak >= 0; peak--) { @@ -826,6 +1000,7 @@ void VehicleAngularVelocity::Run() UpdateDynamicNotchEscRpm(time_now_us); UpdateDynamicNotchFFT(time_now_us); + UpdateDynamicNotchRotorRpm(time_now_us); if (_fifo_available) { // process all outstanding fifo messages @@ -975,6 +1150,10 @@ void VehicleAngularVelocity::PrintStatus() perf_print_counter(_dynamic_notch_filter_fft_disable_perf); perf_print_counter(_dynamic_notch_filter_fft_update_perf); + + perf_print_counter(_dynamic_notch_filter_rotor_rpm_disable_perf); + perf_print_counter(_dynamic_notch_filter_rotor_rpm_init_perf); + perf_print_counter(_dynamic_notch_filter_rotor_rpm_update_perf); #endif // CONSTRAINED_FLASH } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 8aae8a3c03e9..a33b3c16d35c 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -56,6 +57,7 @@ #include #include #include +#include using namespace time_literals; @@ -83,6 +85,7 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte void DisableDynamicNotchEscRpm(); void DisableDynamicNotchFFT(); + void DisableDynamicNotchRotorRpm(); void ParametersUpdate(bool force = false); void ResetFilters(const hrt_abstime &time_now_us); @@ -90,6 +93,7 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte bool SensorSelectionUpdate(const hrt_abstime &time_now_us, bool force = false); void UpdateDynamicNotchEscRpm(const hrt_abstime &time_now_us, bool force = false); void UpdateDynamicNotchFFT(const hrt_abstime &time_now_us, bool force = false); + void UpdateDynamicNotchRotorRpm(const hrt_abstime &time_now_us, bool force = false); bool UpdateSampleRate(); // scaled appropriately for current sensor @@ -105,6 +109,7 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte #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 _rpm_sub{ORB_ID::rpm}; #endif // !CONSTRAINED_FLASH uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -136,8 +141,10 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte #if !defined(CONSTRAINED_FLASH) enum DynamicNotch { + // Power of two for bitwise comparison EscRpm = 1, FFT = 2, + RotorRpm = 4, }; static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 3_s; @@ -166,6 +173,21 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr}; bool _dynamic_notch_fft_available{false}; + + // Rotor RPM (dedicated tachometer, eg autorotating gyrocopter main rotor) + static constexpr int MAX_NUM_ROTOR_RPM_SENSORS = ORB_MULTI_MAX_INSTANCES; + + using NotchFilterRotorRpmHarmonic = math::NotchFilter[3][MAX_NUM_ROTOR_RPM_SENSORS]; + NotchFilterRotorRpmHarmonic *_dynamic_notch_filter_rotor_rpm{nullptr}; + + int _rotor_rpm_harmonics{0}; + px4::Bitset _rotor_rpm_available{}; + hrt_abstime _last_rotor_rpm_notch_update[MAX_NUM_ROTOR_RPM_SENSORS] {}; + + perf_counter_t _dynamic_notch_filter_rotor_rpm_disable_perf{nullptr}; + perf_counter_t _dynamic_notch_filter_rotor_rpm_init_perf{nullptr}; + perf_counter_t _dynamic_notch_filter_rotor_rpm_update_perf{nullptr}; + #endif // !CONSTRAINED_FLASH // angular acceleration filter diff --git a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.yaml b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.yaml index 9b07202eb72f..68b1a086d250 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.yaml +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.yaml @@ -138,9 +138,10 @@ parameters: bit: 0: ESC RPM 1: FFT + 2: Rotor RPM default: 0 min: 0 - max: 3 + max: 7 IMU_GYRO_DNF_BW: description: short: IMU gyro ESC notch filter bandwidth