load_mon: rename low_stack -> task_stack_info & always publish it

- use uorb queue to not drop any info, only do 2 tasks per cycle
- also print a warning on low stack (which will be added to ulog)

this allows to gather statistics of each task's stack usage over time.

msg/CMakeLists.txt

@@ -42,6 +42,7 @@ set(msg_file_names
 	battery_status.msg
 	camera_trigger.msg
 	commander_state.msg
+	collision_report.msg
 	control_state.msg
 	cpuload.msg
 	debug_key_value.msg
@@ -73,6 +74,7 @@ set(msg_file_names
 	mc_virtual_rates_setpoint.msg
 	mission.msg
 	mission_result.msg
+	mount_orientation.msg
 	multirotor_motor_limits.msg
 	offboard_control_mode.msg
 	optical_flow.msg
@@ -96,6 +98,7 @@ set(msg_file_names
 	servorail_status.msg
 	subsystem_info.msg
 	system_power.msg
+	task_stack_info.msg
 	tecs_status.msg
 	telemetry_status.msg
 	test_motor.msg
@@ -118,14 +121,11 @@ set(msg_file_names
 	vehicle_local_position.msg
 	vehicle_local_position_setpoint.msg
 	vehicle_rates_setpoint.msg
+	vehicle_roi.msg
 	vehicle_status.msg
 	vision_position_estimate.msg
 	vtol_vehicle_status.msg
 	wind_estimate.msg
-	vehicle_roi.msg
-	mount_orientation.msg
-	collision_report.msg
-	low_stack.msg
 	)
 
 # Get absolute paths

msg/task_stack_info.msg

@@ -1,3 +1,5 @@
+# stack information for a single running process
+
 uint8 MAX_REPORT_TASK_NAME_LEN = 16
 
 uint16 stack_free

src/modules/load_mon/load_mon.cpp

@@ -57,10 +57,11 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/cpuload.h>
-#include <uORB/topics/low_stack.h>
+#include <uORB/topics/task_stack_info.h>
 
 extern struct system_load_s system_load;
 
+#define STACK_LOW_WARNING_THRESHOLD 300 ///< if free stack space falls below this, print a warning
 
 namespace load_mon
 {
@@ -105,9 +106,9 @@ private:
 	/* Calculate stack usage */
 	void _stack_usage();
 
-	struct low_stack_s _low_stack;
+	struct task_stack_info_s _task_stack_info;
 	int _stack_task_index;
-	orb_advert_t _low_stack_pub;
+	orb_advert_t _task_stack_info_pub;
 #endif
 
 	bool _taskShouldExit;
@@ -123,9 +124,9 @@ private:
 
 LoadMon::LoadMon() :
 #ifdef __PX4_NUTTX
-	_low_stack {},
+	_task_stack_info {},
 	_stack_task_index(0),
-	_low_stack_pub(nullptr),
+	_task_stack_info_pub(nullptr),
 #endif
 	_taskShouldExit(false),
 	_taskIsRunning(false),
@@ -255,8 +256,10 @@ void LoadMon::_stack_usage()
 {
 	int task_index = 0;
 
-	/* Scan maximum 3 tasks per cycle to reduce load. */
-	for (int i = _stack_task_index; i < _stack_task_index + 3; i++) {
+	/* Scan maximum num_tasks_per_cycle tasks to reduce load. */
+	const int num_tasks_per_cycle = 2;
+
+	for (int i = _stack_task_index; i < _stack_task_index + num_tasks_per_cycle; i++) {
 		task_index = i % CONFIG_MAX_TASKS;
 		unsigned stack_free = 0;
 		bool checked_task = false;
@@ -268,6 +271,8 @@ void LoadMon::_stack_usage()
 
 			stack_free = up_check_tcbstack_remain(system_load.tasks[task_index].tcb);
 
+			strncpy((char *)_task_stack_info.task_name, system_load.tasks[task_index].tcb->name,
+				task_stack_info_s::MAX_REPORT_TASK_NAME_LEN);
 
 			checked_task = true;
 		}
@@ -276,20 +281,22 @@ void LoadMon::_stack_usage()
 		perf_end(_stack_perf);
 
 		if (checked_task) {
+
+			_task_stack_info.stack_free = stack_free;
+			_task_stack_info.timestamp = hrt_absolute_time();
+
+			if (_task_stack_info_pub == nullptr) {
+				_task_stack_info_pub = orb_advertise_queue(ORB_ID(task_stack_info), &_task_stack_info, num_tasks_per_cycle);
+
+			} else {
+				orb_publish(ORB_ID(task_stack_info), _task_stack_info_pub, &_task_stack_info);
+			}
+
 			/*
 			 * Found task low on stack, report and exit. Continue here in next cycle.
 			 */
-			if (stack_free < 300) {
-				strncpy((char *)_low_stack.task_name, system_load.tasks[task_index].tcb->name, low_stack_s::MAX_REPORT_TASK_NAME_LEN);
-				_low_stack.stack_free = stack_free;
-
-				if (_low_stack_pub == nullptr) {
-					_low_stack_pub = orb_advertise(ORB_ID(low_stack), &_low_stack);
-
-				} else {
-					orb_publish(ORB_ID(low_stack), _low_stack_pub, &_low_stack);
-				}
-
+			if (stack_free < STACK_LOW_WARNING_THRESHOLD) {
+				PX4_WARN("%s low on stack! (%i bytes left)", _task_stack_info.task_name, stack_free);
 				break;
 			}
 

src/modules/logger/logger.cpp

@@ -558,7 +558,7 @@ void Logger::add_default_topics()
 	add_topic("cpuload");
 	add_topic("gps_dump"); //this will only be published if GPS_DUMP_COMM is set
 	add_topic("sensor_preflight");
-	add_topic("low_stack");
+	add_topic("task_stack_info");
 
 	/* for estimator replay (need to be at full rate) */
 	add_topic("sensor_combined");

src/modules/sdlog2/sdlog2.c

@@ -111,7 +111,7 @@
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/commander_state.h>
 #include <uORB/topics/cpuload.h>
-#include <uORB/topics/low_stack.h>
+#include <uORB/topics/task_stack_info.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -1214,7 +1214,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_land_detected_s land_detected;
 		struct cpuload_s cpuload;
 		struct vehicle_gps_position_s dual_gps_pos;
-		struct low_stack_s low_stack;
+		struct task_stack_info_s task_stack_info;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -1328,7 +1328,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int commander_state_sub;
 		int cpuload_sub;
 		int diff_pres_sub;
-		int low_stack_sub;
+		int task_stack_info_sub;
 	} subs;
 
 	subs.cmd_sub = -1;
@@ -1372,7 +1372,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.commander_state_sub = -1;
 	subs.cpuload_sub = -1;
 	subs.diff_pres_sub = -1;
-	subs.low_stack_sub = -1;
+	subs.task_stack_info_sub = -1;
 
 	/* add new topics HERE */
 
@@ -2318,10 +2318,11 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- STACK --- */
-		if (copy_if_updated(ORB_ID(low_stack), &subs.low_stack_sub, &buf.low_stack)) {
+		if (copy_if_updated(ORB_ID(task_stack_info), &subs.task_stack_info_sub, &buf.task_stack_info)) {
 			log_msg.msg_type = LOG_STCK_MSG;
-			log_msg.body.log_STCK.stack_free = buf.low_stack.stack_free;
-			strncpy(log_msg.body.log_STCK.task_name, (char*)buf.low_stack.task_name, sizeof(log_msg.body.log_STCK.task_name));
+			log_msg.body.log_STCK.stack_free = buf.task_stack_info.stack_free;
+			strncpy(log_msg.body.log_STCK.task_name, (char*)buf.task_stack_info.task_name,
+					sizeof(log_msg.body.log_STCK.task_name));
 			LOGBUFFER_WRITE_AND_COUNT(STCK);
 		}