load_mon: add linux support

boards/aerotenna/ocpoc/default.cmake

@@ -48,7 +48,7 @@ px4_add_board(
 		fw_pos_control_l1
 		land_detector
 		landing_target_estimator
-		#load_mon
+		load_mon
 		local_position_estimator
 		logger
 		mavlink

boards/beaglebone/blue/default.cmake

@@ -43,7 +43,7 @@ px4_add_board(
 		fw_pos_control_l1
 		land_detector
 		landing_target_estimator
-		#load_mon
+		load_mon
 		local_position_estimator
 		logger
 		mavlink

boards/emlid/navio2/default.cmake

@@ -43,7 +43,7 @@ px4_add_board(
 		fw_pos_control_l1
 		land_detector
 		landing_target_estimator
-		#load_mon
+		load_mon
 		local_position_estimator
 		logger
 		mavlink

boards/px4/raspberrypi/default.cmake

@@ -42,7 +42,7 @@ px4_add_board(
 		fw_pos_control_l1
 		land_detector
 		landing_target_estimator
-		#load_mon
+		load_mon
 		local_position_estimator
 		logger
 		mavlink

posix-configs/bbblue/px4.config

@@ -47,6 +47,8 @@ param set BAT_V_DIV 		11.0
 
 dataman start
 
+load_mon start
+
 bmp280 -I start
 
 mpu9250 -I start

posix-configs/ocpoc/px4.config

@@ -16,6 +16,8 @@ param set MAV_BROADCAST 1
 param set MAV_TYPE 2
 param set MAV_SYS_ID 1
 
+load_mon start
+
 mpu9250 -s -R 4 start
 hmc5883 -I start
 ms5611 -s start

posix-configs/rpi/px4.config

@@ -19,6 +19,8 @@ param set IMU_GYRO_RATEMAX 400
 
 dataman start
 
+load_mon start
+
 mpu9250 -s -R 2 start
 lsm9ds1 -s -R 4 start
 lsm9ds1_mag -s -R 4 start

posix-configs/rpi/px4_fw.config

@@ -19,6 +19,8 @@ param set IMU_GYRO_RATEMAX 400
 
 dataman start
 
+load_mon start
+
 mpu9250 -s -R 2 start
 lsm9ds1 -s -R 4 start
 lsm9ds1_mag -s -R 4 start

posix-configs/rpi/px4_hil.config

@@ -20,6 +20,8 @@ param set MAV_TYPE 2
 
 dataman start
 
+load_mon start
+
 rc_update start
 sensors start -hil
 commander start -hil

posix-configs/rpi/px4_test.config

@@ -19,6 +19,8 @@ param set IMU_GYRO_RATEMAX 400
 
 dataman start
 
+load_mon start
+
 mpu9250 -s -R 2 start
 lsm9ds1 -s -R 4 start
 lsm9ds1_mag -s -R 4 start

src/modules/load_mon/LoadMon.cpp

@@ -33,6 +33,7 @@
 
 #include "LoadMon.hpp"
 
+#if defined(__PX4_NUTTX)
 // if free stack space falls below this, print a warning
 #if defined(CONFIG_ARMV7M_STACKCHECK)
 static constexpr unsigned STACK_LOW_WARNING_THRESHOLD = 100;
@@ -41,6 +42,7 @@ static constexpr unsigned STACK_LOW_WARNING_THRESHOLD = 300;
 #endif
 
 static constexpr unsigned FDS_LOW_WARNING_THRESHOLD = 2; ///< if free file descriptors fall below this, print a warning
+#endif
 
 using namespace time_literals;
 
@@ -88,10 +90,14 @@ void LoadMon::Run()
 
 	cpuload();
 
+#if defined(__PX4_NUTTX)
+
 	if (_param_sys_stck_en.get()) {
 		stack_usage();
 	}
 
+#endif
+
 	if (should_exit()) {
 		ScheduleClear();
 		exit_and_cleanup();
@@ -102,6 +108,23 @@ void LoadMon::Run()
 
 void LoadMon::cpuload()
 {
+#if defined(__PX4_LINUX)
+	tms spent_time_stamp_struct;
+	clock_t total_time_stamp = times(&spent_time_stamp_struct);
+	clock_t spent_time_stamp = spent_time_stamp_struct.tms_utime + spent_time_stamp_struct.tms_stime;
+
+	if (_last_total_time_stamp == 0 || _last_spent_time_stamp == 0) {
+		// Just get the time in the first iteration */
+		_last_total_time_stamp = total_time_stamp;
+		_last_spent_time_stamp = spent_time_stamp;
+		return;
+	}
+
+	// compute system load
+	const float interval = total_time_stamp - _last_total_time_stamp;
+	const float interval_spent_time = spent_time_stamp - _last_spent_time_stamp;
+#elif defined(__PX4_NUTTX)
+
 	if (_last_idle_time == 0) {
 		// Just get the time in the first iteration */
 		_last_idle_time = system_load.tasks[0].total_runtime;
@@ -117,23 +140,34 @@ void LoadMon::cpuload()
 	// compute system load
 	const float interval = now - _last_idle_time_sample;
 	const float interval_idletime = total_runtime - _last_idle_time;
+#endif
 
 	// get ram usage
 	struct mallinfo mem = mallinfo();
 	float ram_usage = (float)mem.uordblks / mem.arena;
 
 	cpuload_s cpuload{};
+#if defined(__PX4_LINUX)
+	cpuload.load = interval_spent_time / interval;
+#elif defined(__PX4_NUTTX)
 	cpuload.load = 1.f - interval_idletime / interval;
+#endif
 	cpuload.ram_usage = ram_usage;
 	cpuload.timestamp = hrt_absolute_time();
 
 	_cpuload_pub.publish(cpuload);
 
 	// store for next iteration
+#if defined(__PX4_LINUX)
+	_last_total_time_stamp = total_time_stamp;
+	_last_spent_time_stamp = spent_time_stamp;
+#elif defined(__PX4_NUTTX)
 	_last_idle_time = total_runtime;
 	_last_idle_time_sample = now;
+#endif
 }
 
+#if defined(__PX4_NUTTX)
 void LoadMon::stack_usage()
 {
 	unsigned stack_free = 0;
@@ -196,6 +230,7 @@ void LoadMon::stack_usage()
 	// Continue after last checked task next cycle
 	_stack_task_index = (_stack_task_index + 1) % CONFIG_MAX_TASKS;
 }
+#endif
 
 int LoadMon::print_usage(const char *reason)
 {

src/modules/load_mon/LoadMon.hpp

@@ -45,6 +45,11 @@
 #include <uORB/topics/cpuload.h>
 #include <uORB/topics/task_stack_info.h>
 
+#if defined(__PX4_LINUX)
+#include <sys/times.h>
+#include <malloc.h>
+#endif
+
 namespace load_mon
 {
 
@@ -74,16 +79,25 @@ private:
 	/** Do a calculation of the CPU load and publish it. */
 	void cpuload();
 
+	/* Stack check only available on Nuttx */
+#if defined(__PX4_NUTTX)
 	/* Calculate stack usage */
 	void stack_usage();
 
 	int _stack_task_index{0};
 
 	uORB::PublicationQueued<task_stack_info_s> _task_stack_info_pub{ORB_ID(task_stack_info)};
-	uORB::Publication<cpuload_s> _cpuload_pub{ORB_ID(cpuload)};
-
-	hrt_abstime _last_idle_time{0};
+#endif
+	uORB::Publication<cpuload_s> _cpuload_pub {ORB_ID(cpuload)};
+
+#if defined(__PX4_LINUX)
+	/* calculate usage directly from clock ticks on Linux */
+	clock_t _last_total_time_stamp{};
+	clock_t _last_spent_time_stamp{};
+#elif defined(__PX4_NUTTX)
+	hrt_abstime _last_idle_time {0};
 	hrt_abstime _last_idle_time_sample{0};
+#endif
 
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};