From ae947513d7b3f53a04a3ac990825d4b47493b865 Mon Sep 17 00:00:00 2001
From: Eric Katzfey <53063038+katzfey@users.noreply.github.com>
Date: Wed, 13 Mar 2024 18:33:58 -0700
Subject: [PATCH] add load_mon support for Qurt platform (#22883)

 - Added check in commander to see if CPU load monitoring has been disabled before signalling overload
---
 boards/modalai/voxl2/default.px4board           | 1 -
 boards/modalai/voxl2/target/voxl-px4-hitl-start | 1 -
 boards/modalai/voxl2/target/voxl-px4-start      | 1 -
 src/modules/commander/Commander.cpp             | 9 ++++++++-
 src/modules/load_mon/LoadMon.cpp                | 3 +++
 5 files changed, 11 insertions(+), 4 deletions(-)

diff --git a/boards/modalai/voxl2/default.px4board b/boards/modalai/voxl2/default.px4board
index da04429b98..8ac3169572 100644
--- a/boards/modalai/voxl2/default.px4board
+++ b/boards/modalai/voxl2/default.px4board
@@ -11,7 +11,6 @@ CONFIG_DRIVERS_VOXL2_IO=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
-CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MUORB_APPS=y
diff --git a/boards/modalai/voxl2/target/voxl-px4-hitl-start b/boards/modalai/voxl2/target/voxl-px4-hitl-start
index eff2059ce7..45ef0b2ddf 100755
--- a/boards/modalai/voxl2/target/voxl-px4-hitl-start
+++ b/boards/modalai/voxl2/target/voxl-px4-hitl-start
@@ -76,7 +76,6 @@ qshell flight_mode_manager start
 # Start all of the processing modules on the applications processor
 dataman start
 navigator start
-load_mon start
 
 # Start microdds_client for ros2 offboard messages from agent over localhost
 microdds_client start -t udp -h 127.0.0.1 -p 8888
diff --git a/boards/modalai/voxl2/target/voxl-px4-start b/boards/modalai/voxl2/target/voxl-px4-start
index e6bfc8b7b4..5a9149f225 100755
--- a/boards/modalai/voxl2/target/voxl-px4-start
+++ b/boards/modalai/voxl2/target/voxl-px4-start
@@ -207,7 +207,6 @@ qshell flight_mode_manager start
 # Start all of the processing modules on the applications processor
 dataman start
 navigator start
-load_mon start
 
 # This bridge allows raw data packets to be sent over UART to the ESC
 voxl2_io_bridge start
diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index f9fa07c257..bca8ef3f68 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -2419,7 +2419,14 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 		if (_cpuload_sub.copy(&cpuload)) {
 			const float cpuload_percent = cpuload.load * 100.f;
 
-			bool overload = (cpuload_percent > _param_com_cpu_max.get()) || (cpuload.ram_usage > 0.99f);
+			bool overload = false;
+
+			// Only check CPU load if it hasn't been disabled
+			if (!(_param_com_cpu_max.get() < FLT_EPSILON)) {
+				overload = (cpuload_percent > _param_com_cpu_max.get());
+			}
+
+			overload = overload || (cpuload.ram_usage > 0.99f);
 
 			if (_overload_start == 0 && overload) {
 				_overload_start = time_now_us;
diff --git a/src/modules/load_mon/LoadMon.cpp b/src/modules/load_mon/LoadMon.cpp
index 80c54f4826..aa400391dc 100644
--- a/src/modules/load_mon/LoadMon.cpp
+++ b/src/modules/load_mon/LoadMon.cpp
@@ -229,6 +229,9 @@ void LoadMon::cpuload()
 	struct mallinfo mem = mallinfo();
 	cpuload.ram_usage = (float)mem.uordblks / mem.arena;
 	cpuload.load = 1.f - interval_idletime / interval;
+#elif defined(__PX4_QURT)
+	cpuload.ram_usage = 0.0f;
+	cpuload.load = px4muorb_get_cpu_load() / 100.0f;
 #endif
 	cpuload.timestamp = hrt_absolute_time();