Load monitor: report and log processes low on stack

msg/CMakeLists.txt

@@ -124,6 +124,7 @@ set(msg_file_names
 	vehicle_roi.msg
 	mount_orientation.msg
 	collision_report.msg
+	low_stack.msg
 	)
 
 # Get absolute paths

msg/low_stack.msg

@@ -0,0 +1,4 @@
+uint8 MAX_REPORT_TASK_NAME_LEN = 16
+
+uint16 stack_free
+uint8[16] task_name

src/modules/load_mon/load_mon.cpp

@@ -36,6 +36,7 @@
  *
  * @author Jonathan Challinger <jonathan@3drobotics.com>
  * @author Julian Oes <julian@oes.ch
+ * @author Andreas Antener <andreas@uaventure.com>
  */
 
 #include <stdio.h>
@@ -55,6 +56,7 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/cpuload.h>
+#include <uORB/topics/low_stack.h>
 
 extern struct system_load_s system_load;
 
@@ -96,6 +98,15 @@ private:
 	/* Calculate the memory usage */
 	float _ram_used();
 
+#ifdef __PX4_NUTTX
+	/* Calculate stack usage */
+	void _stack_usage();
+
+	struct low_stack_s _low_stack;
+	int _stack_task_index;
+	orb_advert_t _low_stack_pub;
+#endif
+
 	bool _taskShouldExit;
 	bool _taskIsRunning;
 	struct work_s _work;
@@ -105,8 +116,12 @@ private:
 	hrt_abstime _last_idle_time;
 };
 
-
 LoadMon::LoadMon() :
+#ifdef __PX4_NUTTX
+	_low_stack {},
+	_stack_task_index(0),
+	_low_stack_pub(nullptr),
+#endif
 	_taskShouldExit(false),
 	_taskIsRunning(false),
 	_work{},
@@ -117,14 +132,14 @@ LoadMon::LoadMon() :
 
 LoadMon::~LoadMon()
 {
-	work_cancel(HPWORK, &_work);
+	work_cancel(LPWORK, &_work);
 	_taskIsRunning = false;
 }
 
 int LoadMon::start()
 {
 	/* Schedule a cycle to start things. */
-	return work_queue(HPWORK, &_work, (worker_t)&LoadMon::cycle_trampoline, this, 0);
+	return work_queue(LPWORK, &_work, (worker_t)&LoadMon::cycle_trampoline, this, 0);
 }
 
 void LoadMon::stop()
@@ -147,7 +162,7 @@ void LoadMon::_cycle()
 	_compute();
 
 	if (!_taskShouldExit) {
-		work_queue(HPWORK, &_work, (worker_t)&LoadMon::cycle_trampoline, this,
+		work_queue(LPWORK, &_work, (worker_t)&LoadMon::cycle_trampoline, this,
 			   USEC2TICK(LOAD_MON_INTERVAL_US));
 	}
 }
@@ -168,6 +183,10 @@ void LoadMon::_compute()
 	_cpuload.load = 1.0f - (float)interval_idletime / (float)LOAD_MON_INTERVAL_US;
 	_cpuload.ram_usage = _ram_used();
 
+#ifdef __PX4_NUTTX
+	_stack_usage();
+#endif
+
 	if (_cpuload_pub == nullptr) {
 		_cpuload_pub = orb_advertise(ORB_ID(cpuload), &_cpuload);
 
@@ -205,6 +224,49 @@ float LoadMon::_ram_used()
 #endif
 }
 
+#ifdef __PX4_NUTTX
+void LoadMon::_stack_usage()
+{
+	for (int i = _stack_task_index; i < CONFIG_MAX_TASKS; i++) {
+		if (system_load.tasks[i].valid && system_load.tasks[i].tcb->pid > 0) {
+			unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
+					      (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
+			unsigned stack_free = 0;
+			uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
+
+			while (stack_free < stack_size) {
+				if (*stack_sweeper++ != 0xff) {
+					break;
+				}
+
+				stack_free++;
+			}
+
+			/*
+			 * 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[i].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);
+				}
+
+				// continue at next index
+				_stack_task_index = i + 1;
+				return;
+			}
+		}
+	}
+
+	_stack_task_index = 0;
+}
+#endif
+
 /**
  * Print the correct usage.
  */

src/modules/sdlog2/sdlog2.c

@@ -111,6 +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 <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -1222,6 +1223,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;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -1284,6 +1286,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_RPL6_s log_RPL6;
 			struct log_LOAD_s log_LOAD;
 			struct log_DPRS_s log_DPRS;
+			struct log_STCK_s log_STCK;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1334,6 +1337,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int commander_state_sub;
 		int cpuload_sub;
 		int diff_pres_sub;
+		int low_stack_sub;
 	} subs;
 
 	subs.cmd_sub = -1;
@@ -1377,6 +1381,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;
 
 	/* add new topics HERE */
 
@@ -2319,7 +2324,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.msg_type = LOG_LOAD_MSG;
 			log_msg.body.log_LOAD.cpu_load = buf.cpuload.load;
 			LOGBUFFER_WRITE_AND_COUNT(LOAD);
+		}
 
+		/* --- STACK --- */
+		if (copy_if_updated(ORB_ID(low_stack), &subs.low_stack_sub, &buf.low_stack)) {
+			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));
+			LOGBUFFER_WRITE_AND_COUNT(STCK);
 		}
 
 		pthread_mutex_lock(&logbuffer_mutex);

src/modules/sdlog2/sdlog2_messages.h

@@ -634,6 +634,13 @@ struct log_DPRS_s {
 	float temperature;
 };
 
+/* --- LOW STACK --- */
+#define LOG_STCK_MSG 63
+struct log_STCK_s {
+	char task_name[16];
+	uint16_t stack_free;
+};
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -724,6 +731,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(LAND, "B", "Landed"),
 	LOG_FORMAT(LOAD, "f", "CPU"),
 	LOG_FORMAT(DPRS, "Qffff", "errors,DPRESraw,DPRES,DPRESmax,Temp"),
+	LOG_FORMAT(STCK, "NH", "Task,Free"),
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */
 	LOG_FORMAT(TIME, "Q", "StartTime"),