diff --git a/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp
index 51df7df324..c3651910c3 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp
@@ -35,6 +35,12 @@
using namespace time_literals;
+CpuResourceChecks::CpuResourceChecks()
+{
+ _high_cpu_load_hysteresis.set_hysteresis_time_from(false, 2_s);
+ _high_cpu_load_hysteresis.set_hysteresis_time_from(true, 2_s);
+}
+
void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter)
{
if (_param_com_cpu_max.get() < FLT_EPSILON) {
@@ -59,37 +65,28 @@ void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter)
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No CPU load information");
}
- high_cpu_load_start_time_us_ = hrt_absolute_time(); // reset
-
} else {
const float cpuload_percent = cpuload.load * 100.f;
+ const bool high_cpu_load = cpuload_percent > _param_com_cpu_max.get();
+ _high_cpu_load_hysteresis.set_state_and_update(high_cpu_load, hrt_absolute_time());
- if (cpuload_percent > _param_com_cpu_max.get()) {
+ // fail check if CPU load is above the threshold for 2 seconds
+ if (_high_cpu_load_hysteresis.get_state()) {
+ /* EVENT
+ * @description
+ * The CPU load can be reduced for example by disabling unused modules (e.g. mavlink instances) or reducing the gyro update
+ * rate via IMU_GYRO_RATEMAX.
+ *
+ *
+ * The threshold can be adjusted via COM_CPU_MAX parameter.
+ *
+ */
+ reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_cpuload_too_high"),
+ events::Log::Error, "CPU load too high: {1:.1}%", cpuload_percent);
- // trigger the failure if the CPU load is above the threshold for 2 seconds
- if (high_cpu_load_start_time_us_ == 0) {
- high_cpu_load_start_time_us_ = hrt_absolute_time();
-
- } else if (hrt_elapsed_time(&high_cpu_load_start_time_us_) > 2_s) {
- /* EVENT
- * @description
- * The CPU load can be reduced for example by disabling unused modules (e.g. mavlink instances) or reducing the gyro update
- * rate via IMU_GYRO_RATEMAX.
- *
- *
- * The threshold can be adjusted via COM_CPU_MAX parameter.
- *
- */
- reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_cpuload_too_high"),
- events::Log::Error, "CPU load too high: {1:.1}%", cpuload_percent);
-
- if (reporter.mavlink_log_pub()) {
- mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: CPU load too high: %3.1f%%", (double)cpuload_percent);
- }
+ if (reporter.mavlink_log_pub()) {
+ mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: CPU load too high: %3.1f%%", (double)cpuload_percent);
}
-
- } else {
- high_cpu_load_start_time_us_ = 0; // reset
}
}
}
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.hpp
index 22f0007015..10db83ad6b 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.hpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.hpp
@@ -35,13 +35,15 @@
#include "../Common.hpp"
+#include
+
#include
#include
class CpuResourceChecks : public HealthAndArmingCheckBase
{
public:
- CpuResourceChecks() = default;
+ CpuResourceChecks();
~CpuResourceChecks() = default;
void checkAndReport(const Context &context, Report &reporter) override;
@@ -49,7 +51,7 @@ public:
private:
uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
- hrt_abstime high_cpu_load_start_time_us_{0};
+ systemlib::Hysteresis _high_cpu_load_hysteresis{false};
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamFloat) _param_com_cpu_max