diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 0fd28a0f13ca..8902b3664f07 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -157,6 +157,7 @@ param set-default CBRK_SUPPLY_CHK 894281 # disable check, no CPU load reported on posix yet param set-default COM_CPU_MAX -1 +param set-default COM_RAM_MAX -1 # Don't require RC calibration and configuration param set-default COM_RC_IN_MODE 1 diff --git a/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp index c3651910c3c5..b433e47ba428 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp @@ -43,7 +43,10 @@ CpuResourceChecks::CpuResourceChecks() void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter) { - if (_param_com_cpu_max.get() < FLT_EPSILON) { + const bool cpu_load_check_enabled = _param_com_cpu_max.get() > FLT_EPSILON; + const bool ram_usage_check_enabled = _param_com_ram_max.get() > FLT_EPSILON; + + if (!cpu_load_check_enabled && !ram_usage_check_enabled) { return; } @@ -54,15 +57,15 @@ void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter) /* EVENT * @description * - * If the system does not provide any CPU load information, use the parameter COM_CPU_MAX - * to disable the check. + * If the system does not provide any CPU and RAM load information, use the parameters COM_CPU_MAX + * and COM_RAM_MAX to disable the checks. * */ reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_missing_cpuload"), - events::Log::Error, "No CPU load information"); + events::Log::Error, "No CPU and RAM load information"); if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No CPU load information"); + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No CPU and RAM load information"); } } else { @@ -71,7 +74,7 @@ void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter) _high_cpu_load_hysteresis.set_state_and_update(high_cpu_load, hrt_absolute_time()); // fail check if CPU load is above the threshold for 2 seconds - if (_high_cpu_load_hysteresis.get_state()) { + if (cpu_load_check_enabled && _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 @@ -88,5 +91,26 @@ void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter) mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: CPU load too high: %3.1f%%", (double)cpuload_percent); } } + + const float ram_usage_percent = cpuload.ram_usage * 100.f; + const bool high_ram_usage = ram_usage_percent > _param_com_ram_max.get(); + + if (ram_usage_check_enabled && high_ram_usage) { + /* EVENT + * @description + * The RAM usage can be reduced for example by disabling unused modules (e.g. mavlink instances). + * + * + * The threshold can be adjusted via COM_RAM_MAX parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_ram_usage_too_high"), + events::Log::Error, "RAM usage too high: {1:.1}%", ram_usage_percent); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: RAM usage too high: %3.1f%%", + (double)ram_usage_percent); + } + } } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.hpp index 10db83ad6bd7..d95c03d2908d 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.hpp @@ -54,6 +54,7 @@ class CpuResourceChecks : public HealthAndArmingCheckBase systemlib::Hysteresis _high_cpu_load_hysteresis{false}; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, - (ParamFloat) _param_com_cpu_max + (ParamFloat) _param_com_cpu_max, + (ParamFloat) _param_com_ram_max ) }; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 8f2c3d51ced0..e9d5f4de3acc 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -802,6 +802,21 @@ PARAM_DEFINE_FLOAT(COM_KILL_DISARM, 5.0f); */ PARAM_DEFINE_FLOAT(COM_CPU_MAX, 95.0f); +/** + * Maximum allowed RAM usage to pass checks + * + * The check fails if the RAM usage is above this threshold. + * + * A negative value disables the check. + * + * @group Commander + * @unit % + * @min -1 + * @max 100 + * @increment 1 + */ +PARAM_DEFINE_FLOAT(COM_RAM_MAX, 95.0f); + /** * Required number of redundant power modules *