diff --git a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp index 69afd1c12c..a27d17f1be 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp @@ -36,6 +36,14 @@ using namespace time_literals; +PowerChecks::PowerChecks() +{ + _voltage_low_hysteresis.set_hysteresis_time_from(false, 0_s); + _voltage_low_hysteresis.set_hysteresis_time_from(true, 15_s); + _voltage_high_hysteresis.set_hysteresis_time_from(false, 0_s); + _voltage_high_hysteresis.set_hysteresis_time_from(true, 15_s); +} + void PowerChecks::checkAndReport(const Context &context, Report &reporter) { if (circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(), CBRK_SUPPLY_CHK_KEY)) { @@ -77,7 +85,11 @@ void PowerChecks::checkAndReport(const Context &context, Report &reporter) const float low_error_threshold = 4.7f; const float high_error_threshold = 5.4f; - if (avionics_power_rail_voltage < low_error_threshold) { + const auto now = hrt_absolute_time(); + _voltage_low_hysteresis.set_state_and_update(avionics_power_rail_voltage < low_error_threshold, now); + _voltage_high_hysteresis.set_state_and_update(avionics_power_rail_voltage > high_error_threshold, now); + + if (_voltage_low_hysteresis.get_state()) { /* EVENT * @description @@ -96,7 +108,7 @@ void PowerChecks::checkAndReport(const Context &context, Report &reporter) (double)avionics_power_rail_voltage); } - } else if (avionics_power_rail_voltage > high_error_threshold) { + } else if (_voltage_high_hysteresis.get_state()) { /* EVENT * @description * Check the voltage supply to the FMU, it must be below {2:.2} Volt. diff --git a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp index b81fadaee0..76d8b9aae0 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp @@ -35,13 +35,14 @@ #include "../Common.hpp" +#include #include #include class PowerChecks : public HealthAndArmingCheckBase { public: - PowerChecks() = default; + PowerChecks(); ~PowerChecks() = default; void checkAndReport(const Context &context, Report &reporter) override; @@ -49,6 +50,8 @@ public: private: uORB::Subscription _system_power_sub{ORB_ID(system_power)}; bool _overcurrent_warning_sent{false}; + systemlib::Hysteresis _voltage_low_hysteresis{false}; + systemlib::Hysteresis _voltage_high_hysteresis{false}; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, (ParamInt) _param_cbrk_supply_chk,