NuttX cpuload monitoring optimization

- Nuttx only process all suspend & resume scheduling notes when top is running, otherwise only track IDLE
 - convert cpuload and print load to c++
 - delete unused fields from print_load struct
 - update hrt_store_absolute_time (previous unused)

.ci/Jenkinsfile-hardware

@@ -895,7 +895,7 @@ void statusFTDI() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "px4io status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sd_bench -r 2"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener cpuload; top once; listener cpuload"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1 -a"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
@@ -967,7 +967,7 @@ void statusSEGGER() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "px4io status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "sd_bench -r 2"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "sensors status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "top once"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener cpuload; top once; listener cpuload"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "uorb status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "uorb top -1 -a"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ver all"'

platforms/common/include/px4_platform_common/printload.h

@@ -50,22 +50,22 @@
 #endif
 
 struct print_load_s {
-	uint64_t total_user_time;
+	uint64_t total_user_time{0};
 
-	int running_count;
+	int running_count{0};
-	int blocked_count;
+	int blocked_count{0};
 
-	uint64_t new_time;
+	uint64_t new_time{0};
-	uint64_t interval_start_time;
+	uint64_t interval_start_time{0};
-	uint32_t last_times[CONFIG_MAX_TASKS]; // in [ms]. This wraps if a process needs more than 49 days of CPU
+	uint64_t last_times[CONFIG_MAX_TASKS] {};
-	float interval_time_ms_inv;
+	float interval_time_us{0.f};
 };
 
 __BEGIN_DECLS
 
-__EXPORT void init_print_load_s(uint64_t t, struct print_load_s *s);
+__EXPORT void init_print_load(struct print_load_s *s);
 
-__EXPORT void print_load(uint64_t t, int fd, struct print_load_s *print_state);
+__EXPORT void print_load(int fd, struct print_load_s *print_state);
 
 
 typedef void (*print_load_callback_f)(void *user);
@@ -73,7 +73,7 @@ typedef void (*print_load_callback_f)(void *user);
 /**
  * Print load to a buffer, and call cb after each written line (buffer will not include '\n')
  */
-void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_callback_f cb, void *user,
+void print_load_buffer(char *buffer, int buffer_length, print_load_callback_f cb, void *user,
 		       struct print_load_s *print_state);
 
 __END_DECLS

platforms/nuttx/src/px4/common/CMakeLists.txt

@@ -39,7 +39,9 @@ if(NOT PX4_BOARD MATCHES "px4_io")
 		board_dma_alloc.c
 		board_fat_dma_alloc.c
 		console_buffer.cpp
+		cpuload.cpp
 		gpio.c
+		print_load.cpp
 		tasks.cpp
 		px4_nuttx_impl.cpp
 		px4_init.cpp

platforms/nuttx/src/px4/common/cpuload.cpp

@@ -40,25 +40,14 @@
  * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
  */
 #include <px4_platform_common/px4_config.h>
-
+#include <px4_platform_common/atomic.h>
-#include <sys/types.h>
+#include <px4_platform/cpuload.h>
-#include <stdint.h>
-#include <stdbool.h>
-
-//#include <arch/arch.h>
-
-//#include <debug.h>
-
-#include <sys/time.h>
-
 
 #include <drivers/drv_hrt.h>
 
-#include "cpuload.h"
+#include <sys/time.h>
 
-#ifdef CONFIG_SCHED_INSTRUMENTATION
+#if defined(__PX4_NUTTX) && defined(CONFIG_SCHED_INSTRUMENTATION)
-
-#ifdef __PX4_NUTTX
 
 # include <nuttx/sched_note.h>
 
@@ -71,13 +60,37 @@ __EXPORT struct system_load_s system_load;
 
 extern FAR struct tcb_s *sched_gettcb(pid_t pid);
 
+static px4::atomic_int cpuload_monitor_all_count{0};
+
+void cpuload_monitor_start()
+{
+	if (cpuload_monitor_all_count.fetch_add(1) == 0) {
+		// if the count was previously 0 (idle thread only) then clear any existing runtime data
+		sched_lock();
+
+		system_load.start_time = hrt_absolute_time();
+
+		for (int i = 1; i < CONFIG_MAX_TASKS; i++) {
+			system_load.tasks[i].total_runtime = 0;
+			system_load.tasks[i].curr_start_time = 0;
+		}
+
+		sched_unlock();
+	}
+}
+
+void cpuload_monitor_stop()
+{
+	if (cpuload_monitor_all_count.fetch_sub(1) <= 1) {
+		// don't allow the count to go negative
+		cpuload_monitor_all_count.store(0);
+	}
+}
+
 void cpuload_initialize_once()
 {
-	system_load.start_time = hrt_absolute_time();
+	for (auto &task : system_load.tasks) {
-	int i;
+		task.valid = false;
-
-	for (i = 0; i < CONFIG_MAX_TASKS; i++) {
-		system_load.tasks[i].valid = false;
 	}
 
 	int static_tasks_count = 2;	// there are at least 2 threads that should be initialized statically - "idle" and "init"
@@ -106,17 +119,15 @@ void cpuload_initialize_once()
 
 void sched_note_start(FAR struct tcb_s *tcb)
 {
-	/* search first free slot */
+	// find first free slot
-	int i;
-
 	if (system_load.initialized) {
-		for (i = 1; i < CONFIG_MAX_TASKS; i++) {
+		for (auto &task : system_load.tasks) {
-			if (!system_load.tasks[i].valid) {
+			if (!task.valid) {
-				/* slot is available */
+				// slot is available
-				system_load.tasks[i].total_runtime = 0;
+				task.total_runtime = 0;
-				system_load.tasks[i].curr_start_time = 0;
+				task.curr_start_time = 0;
-				system_load.tasks[i].tcb = tcb;
+				task.tcb = tcb;
-				system_load.tasks[i].valid = true;
+				task.valid = true;
 				system_load.total_count++;
 				break;
 			}
@@ -126,16 +137,14 @@ void sched_note_start(FAR struct tcb_s *tcb)
 
 void sched_note_stop(FAR struct tcb_s *tcb)
 {
-	int i;
-
 	if (system_load.initialized) {
-		for (i = 1; i < CONFIG_MAX_TASKS; i++) {
+		for (auto &task : system_load.tasks) {
-			if (system_load.tasks[i].tcb != 0 && system_load.tasks[i].tcb->pid == tcb->pid) {
+			if (task.tcb && task.tcb->pid == tcb->pid) {
-				/* mark slot as fee */
+				// mark slot as free
-				system_load.tasks[i].valid = false;
+				task.valid = false;
-				system_load.tasks[i].total_runtime = 0;
+				task.total_runtime = 0;
-				system_load.tasks[i].curr_start_time = 0;
+				task.curr_start_time = 0;
-				system_load.tasks[i].tcb = NULL;
+				task.tcb = nullptr;
 				system_load.total_count--;
 				break;
 			}
@@ -145,14 +154,23 @@ void sched_note_stop(FAR struct tcb_s *tcb)
 
 void sched_note_suspend(FAR struct tcb_s *tcb)
 {
-
 	if (system_load.initialized) {
-		uint64_t new_time = hrt_absolute_time();
+		if (tcb->pid == 0) {
+			system_load.tasks[0].total_runtime += hrt_elapsed_time(&system_load.tasks[0].curr_start_time);
+			return;
 
-		for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
+		} else {
-			/* Task ending its current scheduling run */
+			if (cpuload_monitor_all_count.load() == 0) {
-			if (system_load.tasks[i].valid && system_load.tasks[i].tcb != 0 && system_load.tasks[i].tcb->pid == tcb->pid) {
+				return;
-				system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time;
+			}
+		}
+
+		for (auto &task : system_load.tasks) {
+			// Task ending its current scheduling run
+			if (task.valid && (task.curr_start_time > 0)
+			    && task.tcb && task.tcb->pid == tcb->pid) {
+
+				task.total_runtime += hrt_elapsed_time(&task.curr_start_time);
 				break;
 			}
 		}
@@ -161,25 +179,26 @@ void sched_note_suspend(FAR struct tcb_s *tcb)
 
 void sched_note_resume(FAR struct tcb_s *tcb)
 {
-
 	if (system_load.initialized) {
-		uint64_t new_time = hrt_absolute_time();
+		if (tcb->pid == 0) {
+			hrt_store_absolute_time(&system_load.tasks[0].curr_start_time);
+			return;
 
-		for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
+		} else {
-			if (system_load.tasks[i].valid && system_load.tasks[i].tcb->pid == tcb->pid) {
+			if (cpuload_monitor_all_count.load() == 0) {
+				return;
+			}
+		}
+
+		for (auto &task : system_load.tasks) {
+			if (task.valid && task.tcb && task.tcb->pid == tcb->pid) {
 				// curr_start_time is accessed from an IRQ handler (in logger), so we need
 				// to make the update atomic
-				irqstate_t irq_state = px4_enter_critical_section();
+				hrt_store_absolute_time(&task.curr_start_time);
-				system_load.tasks[i].curr_start_time = new_time;
-				px4_leave_critical_section(irq_state);
 				break;
 			}
 		}
 	}
-
 }
 
-#else
+#endif // PX4_NUTTX && CONFIG_SCHED_INSTRUMENTATION
-__EXPORT struct system_load_s system_load;
-#endif
-#endif /* CONFIG_SCHED_INSTRUMENTATION */

platforms/nuttx/src/px4/common/include/px4_platform/cpuload.h

@@ -40,21 +40,18 @@
 #include <stdbool.h>
 
 struct system_load_taskinfo_s {
-	uint64_t total_runtime;			///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
+	uint64_t total_runtime{0};		///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
-	uint64_t curr_start_time;		///< Start time of the current scheduling slot
+	uint64_t curr_start_time{0};		///< Start time of the current scheduling slot
-#ifdef __PX4_NUTTX
+	struct tcb_s *tcb {nullptr};
-	FAR struct tcb_s *tcb;			///<
+	bool valid{false};			///< Task is currently active / valid
-#endif
-	bool valid;						///< Task is currently active / valid
 };
 
 struct system_load_s {
-	uint64_t start_time;			///< Global start time of measurements
+	uint64_t start_time{0};
-	struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS];
+	system_load_taskinfo_s tasks[CONFIG_MAX_TASKS] {};
-	uint8_t initialized;
+	int total_count{0};
-	int total_count;
+	int running_count{0};
-	int running_count;
+	bool initialized{false};
-	int sleeping_count;
 };
 
 __BEGIN_DECLS
@@ -63,6 +60,9 @@ __EXPORT extern struct system_load_s system_load;
 
 __EXPORT void cpuload_initialize_once(void);
 
+__EXPORT void cpuload_monitor_start(void);
+__EXPORT void cpuload_monitor_stop(void);
+
 __END_DECLS
 
 #endif

platforms/nuttx/src/px4/common/print_load.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file printload.c
+ * @file print_load.cpp
  *
  * Print the current system load.
  *
@@ -42,8 +42,8 @@
 #include <string.h>
 #include <stdio.h>
 
-#include <systemlib/cpuload.h>
+#include <px4_platform/cpuload.h>
-#include <systemlib/printload.h>
+#include <px4_platform_common/printload.h>
 #include <drivers/drv_hrt.h>
 
 #if defined(BOARD_DMA_ALLOC_POOL_SIZE)
@@ -64,26 +64,31 @@ extern struct system_load_s system_load;
 
 #define CL "\033[K" // clear line
 
-void init_print_load_s(uint64_t t, struct print_load_s *s)
+void init_print_load(struct print_load_s *s)
 {
+	cpuload_monitor_start();
 
 	s->total_user_time = 0;
 
 	s->running_count = 0;
 	s->blocked_count = 0;
 
-	s->new_time = t;
+	s->new_time = system_load.start_time;
-	s->interval_start_time = t;
+	s->interval_start_time = system_load.start_time;
 
-	for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
+	sched_lock();
+	// special case for IDLE thread
+	s->last_times[0] = system_load.tasks[0].total_runtime;
+	sched_unlock();
+
+	for (int i = 1; i < CONFIG_MAX_TASKS; i++) {
 		s->last_times[i] = 0;
 	}
 
-	s->interval_time_ms_inv = 0.f;
+	s->interval_time_us = 0.f;
 }
 
-static const char *
+static constexpr const char *tstate_name(const tstate_t s)
-tstate_name(const tstate_t s)
 {
 	switch (s) {
 	case TSTATE_TASK_INVALID:
@@ -127,24 +132,31 @@ tstate_name(const tstate_t s)
 	}
 }
 
-void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_callback_f cb, void *user,
+void print_load_buffer(char *buffer, int buffer_length, print_load_callback_f cb, void *user,
 		       struct print_load_s *print_state)
 {
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wformat" // NuttX uses a different printf format
 #pragma GCC diagnostic ignored "-Wformat-extra-args"
-	print_state->new_time = t;
 
-	int   i;
-	uint64_t curr_time_us;
-	uint64_t idle_time_us;
 	float idle_load = 0.f;
 
-	curr_time_us = t;
+	// create a copy of the runtimes because this could be updated during the print output
-	idle_time_us = system_load.tasks[0].total_runtime;
+	uint64_t total_runtime[CONFIG_MAX_TASKS] {};
+	sched_lock();
+
+	print_state->new_time = hrt_absolute_time();
+
+	for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
+		if (system_load.tasks[i].valid) {
+			total_runtime[i] = system_load.tasks[i].total_runtime;
+		}
+	}
+
+	sched_unlock();
 
 	if (print_state->new_time > print_state->interval_start_time) {
-		print_state->interval_time_ms_inv = 1.f / ((float)((print_state->new_time - print_state->interval_start_time) / 1000));
+		print_state->interval_time_us = print_state->new_time - print_state->interval_start_time;
 
 		/* header for task list */
 		snprintf(buffer, buffer_length, "%4s %-*s %8s %6s %11s %10s %-5s %2s",
@@ -161,22 +173,16 @@ void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_c
 #endif
 			 "FD"
 			);
-		cb(user);
 
+		cb(user);
 	}
 
 	print_state->running_count = 0;
 	print_state->blocked_count = 0;
 	print_state->total_user_time = 0;
 
-	// create a copy of the runtimes because this could be updated during the print output
-	uint32_t total_runtime[CONFIG_MAX_TASKS];
 
-	for (i = 0; i < CONFIG_MAX_TASKS; i++) {
+	for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
-		total_runtime[i] = (uint32_t)(system_load.tasks[i].total_runtime / 1000);
-	}
-
-	for (i = 0; i < CONFIG_MAX_TASKS; i++) {
 
 		sched_lock(); // need to lock the tcb access (but make it as short as possible)
 
@@ -185,7 +191,6 @@ void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_c
 			continue;
 		}
 
-		uint64_t interval_runtime;
 		unsigned tcb_pid = system_load.tasks[i].tcb->pid;
 		size_t stack_size = system_load.tasks[i].tcb->adj_stack_size;
 		ssize_t stack_free = 0;
@@ -213,8 +218,8 @@ void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_c
 #if CONFIG_RR_INTERVAL > 0
 		unsigned tcb_timeslice = system_load.tasks[i].tcb->timeslice;
 #endif
-		unsigned tcb_task_state = system_load.tasks[i].tcb->task_state;
+		tstate_t tcb_task_state = (tstate_t)system_load.tasks[i].tcb->task_state;
-		unsigned tcb_sched_priority = system_load.tasks[i].tcb->sched_priority;
+		uint8_t tcb_sched_priority = system_load.tasks[i].tcb->sched_priority;
 
 		unsigned int tcb_num_used_fds = 0; // number of used file descriptors
 #if CONFIG_NFILE_DESCRIPTORS > 0
@@ -255,39 +260,43 @@ void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_c
 		case TSTATE_WAIT_SEM:
 			print_state->blocked_count++;
 			break;
+
+		case TSTATE_TASK_STOPPED:
+			// DO NOTHING
+			break;
+
+		case NUM_TASK_STATES:
+			// DO NOTHING
+			break;
 		}
 
-		interval_runtime = (print_state->last_times[i] > 0 && total_runtime[i] > print_state->last_times[i])
-				   ? (total_runtime[i] - print_state->last_times[i]) : 0;
-
-		print_state->last_times[i] = total_runtime[i];
-
 		float current_load = 0.f;
 
-		if (print_state->new_time > print_state->interval_start_time) {
+		if (total_runtime[i] > print_state->last_times[i]) {
-			current_load = interval_runtime * print_state->interval_time_ms_inv;
+			const uint64_t interval_runtime = total_runtime[i] - print_state->last_times[i];
 
-			if (tcb_pid != 0) {
+			current_load = interval_runtime / print_state->interval_time_us;
-				print_state->total_user_time += interval_runtime;
+
+			if (tcb_pid == 0) {
+				idle_load = current_load;
 
 			} else {
-				idle_load = current_load;
+				print_state->total_user_time += interval_runtime;
-			}
+			}
-
 		}
 
+		print_state->last_times[i] = total_runtime[i];
 
 		if (print_state->new_time <= print_state->interval_start_time) {
 			continue; // not enough data yet
 		}
 
 		// print output
-		int print_len = snprintf(buffer, buffer_length, "%4d %-*s %8d %2d.%03d %5u/%5u %3u (%3u) ",
+		int print_len = snprintf(buffer, buffer_length, "%4d %-*s %8d %6.3f %5u/%5u %3u (%3u) ",
 					 tcb_pid,
 					 CONFIG_TASK_NAME_SIZE, tcb_name,
-					 total_runtime[i],
+					 total_runtime[i] / 1000, // us -> ms
-					 (int)(current_load * 100.0f),
+					 (double)(current_load * 100.f),
-					 (int)((current_load * 100.0f - (int)(current_load * 100.0f)) * 1000),
 					 stack_size - stack_free,
 					 stack_size,
 					 tcb_sched_priority,
@@ -315,10 +324,8 @@ void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_c
 	// Print footer
 	buffer[0] = 0;
 	cb(user);
-	float task_load;
-	float sched_load;
 
-	task_load = (float)(print_state->total_user_time) * print_state->interval_time_ms_inv;
+	float task_load = (float)(print_state->total_user_time) / print_state->interval_time_us;
 
 	/* this can happen if one tasks total runtime was not computed
 	   correctly by the scheduler instrumentation TODO */
@@ -326,7 +333,7 @@ void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_c
 		task_load = (1.f - idle_load);
 	}
 
-	sched_load = 1.f - idle_load - task_load;
+	const float sched_load = 1.f - idle_load - task_load;
 
 	snprintf(buffer, buffer_length, "Processes: %d total, %d running, %d sleeping, max FDs: %d",
 		 system_load.total_count,
@@ -354,8 +361,8 @@ void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_c
 
 #endif
 	snprintf(buffer, buffer_length, "Uptime: %.3fs total, %.3fs idle",
-		 (double)curr_time_us / 1000000.d,
+		 (double)print_state->new_time / 1e6, (double)total_runtime[0] / 1e6);
-		 (double)idle_time_us / 1000000.d);
+
 	cb(user);
 
 	print_state->interval_start_time = print_state->new_time;
@@ -371,28 +378,28 @@ struct print_load_callback_data_s {
 
 static void print_load_callback(void *user)
 {
-	char *clear_line = "";
+	char clear_line[] {CL};
 	struct print_load_callback_data_s *data = (struct print_load_callback_data_s *)user;
 
-	if (data->fd == 1) {
+	if (data->fd != STDOUT_FILENO) {
-		clear_line = CL;
+		clear_line[0] = '\0';
 	}
 
 	dprintf(data->fd, "%s%s\n", clear_line, data->buffer);
 }
 
-void print_load(uint64_t t, int fd, struct print_load_s *print_state)
+void print_load(int fd, struct print_load_s *print_state)
 {
-	/* print system information */
+	// print system information
-	if (fd == 1) {
+	if (fd == STDOUT_FILENO) {
-		dprintf(fd, "\033[H"); /* move cursor home and clear screen */
+		// move cursor home and clear screen
+		dprintf(fd, "\033[H");
 	}
 
-	struct print_load_callback_data_s data;
+	print_load_callback_data_s data{};
-
 	data.fd = fd;
 
-	print_load_buffer(t, data.buffer, sizeof(data.buffer), print_load_callback, &data, print_state);
+	print_load_buffer(data.buffer, sizeof(data.buffer), print_load_callback, &data, print_state);
 }
 
 #endif // if CONFIG_SCHED_INSTRUMENTATION

platforms/nuttx/src/px4/common/px4_init.cpp

@@ -38,7 +38,7 @@
 #include <drivers/drv_hrt.h>
 #include <lib/parameters/param.h>
 #include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
-#include <systemlib/cpuload.h>
+#include <px4_platform/cpuload.h>
 
 #include <fcntl.h>
 

platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c

@@ -604,16 +604,12 @@ hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
 /**
  * Store the absolute time in an interrupt-safe fashion
  */
-hrt_abstime
+void
-hrt_store_absolute_time(volatile hrt_abstime *now)
+hrt_store_absolute_time(volatile hrt_abstime *t)
 {
 	irqstate_t flags = px4_enter_critical_section();
-
+	*t = hrt_absolute_time();
-	hrt_abstime ts = hrt_absolute_time();
-
 	px4_leave_critical_section(flags);
-
-	return ts;
 }
 
 /**

platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c

@@ -616,16 +616,12 @@ hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
 /**
  * Store the absolute time in an interrupt-safe fashion
  */
-hrt_abstime
+void
-hrt_store_absolute_time(volatile hrt_abstime *now)
+hrt_store_absolute_time(volatile hrt_abstime *t)
 {
 	irqstate_t flags = px4_enter_critical_section();
-
+	*t = hrt_absolute_time();
-	hrt_abstime ts = hrt_absolute_time();
-
 	px4_leave_critical_section(flags);
-
-	return ts;
 }
 
 /**

platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c

@@ -604,16 +604,12 @@ hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
 /**
  * Store the absolute time in an interrupt-safe fashion
  */
-hrt_abstime
+void
-hrt_store_absolute_time(volatile hrt_abstime *now)
+hrt_store_absolute_time(volatile hrt_abstime *t)
 {
 	irqstate_t flags = px4_enter_critical_section();
-
+	*t = hrt_absolute_time();
-	hrt_abstime ts = hrt_absolute_time();
-
 	px4_leave_critical_section(flags);
-
-	return ts;
 }
 
 /**

platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c

@@ -737,16 +737,12 @@ hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
 /**
  * Store the absolute time in an interrupt-safe fashion
  */
-hrt_abstime
+void
-hrt_store_absolute_time(volatile hrt_abstime *now)
+hrt_store_absolute_time(volatile hrt_abstime *t)
 {
 	irqstate_t flags = px4_enter_critical_section();
-
+	*t = hrt_absolute_time();
-	hrt_abstime ts = hrt_absolute_time();
-
 	px4_leave_critical_section(flags);
-
-	return ts;
 }
 
 /**

platforms/posix/src/px4/common/CMakeLists.txt

@@ -55,6 +55,8 @@ add_library(px4_layer
 	px4_init.cpp
 	lib_crc32.c
 	drv_hrt.cpp
+	cpuload.cpp
+	print_load.cpp
 	${SHMEM_SRCS}
 )
 target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4")

platforms/posix/src/px4/common/cpuload.cpp

@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file cpuload.cpp
+ *
+ * Measurement of CPU load of each individual task.
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ */
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/atomic.h>
+#include <px4_platform/cpuload.h>
+
+#include <drivers/drv_hrt.h>
+
+#include <sys/time.h>
+
+static px4::atomic_int cpuload_monitor_all_count{0};
+
+void cpuload_monitor_start()
+{
+	cpuload_monitor_all_count.fetch_add(1);
+}
+
+void cpuload_monitor_stop()
+{
+	if (cpuload_monitor_all_count.fetch_sub(1) <= 1) {
+		// don't all the count to go negative
+		cpuload_monitor_all_count.store(0);
+	}
+}
+
+// TODO

platforms/posix/src/px4/common/drv_hrt.cpp

@@ -176,13 +176,11 @@ hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
  *
  * This function ensures that the timestamp cannot be seen half-written by an interrupt handler.
  */
-hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now)
+void hrt_store_absolute_time(volatile hrt_abstime *t)
 {
-	hrt_abstime ts = hrt_absolute_time();
+	*t = hrt_absolute_time();
-	return ts;
 }
 
-
 /*
  * If this returns true, the entry has been invoked and removed from the callout list,
  * or it has never been entered.

platforms/posix/src/px4/common/include/px4_platform/cpuload.h

@@ -0,0 +1,41 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+__BEGIN_DECLS
+
+__EXPORT void cpuload_monitor_start(void);
+__EXPORT void cpuload_monitor_stop(void);
+
+__END_DECLS

platforms/posix/src/px4/common/print_load.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file print_load_posix.c
+ * @file print_load.cpp
  *
  * Print the current system load.
  *
@@ -46,8 +46,7 @@
 #include <stdio.h>
 #include <stdbool.h>
 
-#include <systemlib/cpuload.h>
+#include <px4_platform_common/printload.h>
-#include <systemlib/printload.h>
 #include <drivers/drv_hrt.h>
 
 #ifdef __PX4_DARWIN
@@ -63,32 +62,33 @@ extern struct system_load_s system_load;
 
 #define CL "\033[K" // clear line
 
-void init_print_load_s(uint64_t t, struct print_load_s *s)
+void init_print_load(struct print_load_s *s)
 {
-
 	s->total_user_time = 0;
 
 	s->running_count = 0;
 	s->blocked_count = 0;
 
-	s->new_time = t;
+	s->new_time = hrt_absolute_time();
-	s->interval_start_time = t;
+	s->interval_start_time = s->new_time;
 
 	for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
 		s->last_times[i] = 0;
 	}
 
-	s->interval_time_ms_inv = 0.f;
+	s->interval_time_us = 0.f;
 }
 
-void print_load(uint64_t t, int fd, struct print_load_s *print_state)
+void print_load(int fd, struct print_load_s *print_state)
 {
-	char *clear_line = "";
+	char clear_line[] = CL;
 
 	/* print system information */
 	if (fd == 1) {
 		dprintf(fd, "\033[H"); /* move cursor home and clear screen */
-		clear_line = CL;
+
+	} else {
+		memset(clear_line, 0, sizeof(clear_line));
 	}
 
 #if defined(__PX4_LINUX) || defined(__PX4_CYGWIN) || defined(__PX4_QURT)
@@ -175,9 +175,8 @@ void print_load(uint64_t t, int fd, struct print_load_s *print_state)
 #endif
 }
 
-void print_load_buffer(uint64_t t, char *buffer, int buffer_length, print_load_callback_f cb, void *user,
+void print_load_buffer(char *buffer, int buffer_length, print_load_callback_f cb, void *user,
 		       struct print_load_s *print_state)
 {
 
 }
-

platforms/qurt/src/px4/common/drv_hrt.cpp

@@ -156,13 +156,11 @@ hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
  *
  * This function ensures that the timestamp cannot be seen half-written by an interrupt handler.
  */
-hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now)
+void hrt_store_absolute_time(volatile hrt_abstime *t)
 {
-	hrt_abstime ts = hrt_absolute_time();
+	*t = hrt_absolute_time();
-	return ts;
 }
 
-
 /*
  * If this returns true, the entry has been invoked and removed from the callout list,
  * or it has never been entered.

src/drivers/drv_hrt.h

@@ -124,7 +124,7 @@ __EXPORT extern hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *
  *
  * This function ensures that the timestamp cannot be seen half-written by an interrupt handler.
  */
-__EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now);
+__EXPORT extern void hrt_store_absolute_time(volatile hrt_abstime *time);
 
 #ifdef __PX4_QURT
 /**

src/lib/systemlib/CMakeLists.txt

@@ -42,15 +42,8 @@ set(SRCS
 
 if(${PX4_PLATFORM} STREQUAL "nuttx")
 	list(APPEND SRCS
-		cpuload.c
-		cpuload.h
-		print_load_nuttx.c
 		otp.c
 	)
-else()
-	list(APPEND SRCS
-		print_load_posix.c
-	)
 endif()
 
 px4_add_library(systemlib ${SRCS})

src/modules/load_mon/LoadMon.hpp

@@ -40,7 +40,7 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <systemlib/cpuload.h>
+#include <px4_platform/cpuload.h>
 #include <uORB/Publication.hpp>
 #include <uORB/topics/cpuload.h>
 #include <uORB/topics/task_stack_info.h>

src/modules/logger/logger.cpp

@@ -53,6 +53,7 @@
 
 #include <drivers/drv_hrt.h>
 #include <mathlib/math/Limits.hpp>
+#include <px4_platform/cpuload.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/posix.h>
@@ -1225,6 +1226,11 @@ void Logger::start_log_file(LogType type)
 		return;
 	}
 
+	if (type == LogType::Full) {
+		// initialize cpu load as early as possible to get more data
+		initialize_load_output(PrintLoadReason::Preflight);
+	}
+
 	PX4_INFO("Start file log (type: %s)", log_type_str(type));
 
 	char file_name[LOG_DIR_LEN] = "";
@@ -1260,8 +1266,6 @@ void Logger::start_log_file(LogType type)
 	if (type == LogType::Full) {
 		/* reset performance counters to get in-flight min and max values in post flight log */
 		perf_reset_all();
-
-		initialize_load_output(PrintLoadReason::Preflight);
 	}
 
 	_statistics[(int)type].start_time_file = hrt_absolute_time();
@@ -1289,6 +1293,9 @@ void Logger::start_log_mavlink()
 		return;
 	}
 
+	// initialize cpu load as early as possible to get more data
+	initialize_load_output(PrintLoadReason::Preflight);
+
 	PX4_INFO("Start mavlink log");
 
 	_writer.start_log_mavlink();
@@ -1304,8 +1311,6 @@ void Logger::start_log_mavlink()
 	_writer.set_need_reliable_transfer(false);
 	_writer.unselect_write_backend();
 	_writer.notify();
-
-	initialize_load_output(PrintLoadReason::Preflight);
 }
 
 void Logger::stop_log_mavlink()
@@ -1388,16 +1393,8 @@ void Logger::print_load_callback(void *user)
 
 void Logger::initialize_load_output(PrintLoadReason reason)
 {
-	perf_callback_data_t callback_data;
+	init_print_load(&_load);
-	callback_data.logger = this;
+	_next_load_print = hrt_absolute_time() + 1_s;
-	callback_data.counter = 0;
-	callback_data.buffer = nullptr;
-	char buffer[140];
-	hrt_abstime curr_time = hrt_absolute_time();
-	init_print_load_s(curr_time, &_load);
-	// this will not yet print anything
-	print_load_buffer(curr_time, buffer, sizeof(buffer), print_load_callback, &callback_data, &_load);
-	_next_load_print = curr_time + 1000000;
 	_print_load_reason = reason;
 }
 
@@ -1412,11 +1409,11 @@ void Logger::write_load_output()
 	callback_data.logger = this;
 	callback_data.counter = 0;
 	callback_data.buffer = buffer;
-	hrt_abstime curr_time = hrt_absolute_time();
 	_writer.set_need_reliable_transfer(true);
 	// TODO: maybe we should restrict the output to a selected backend (eg. when file logging is running
 	// and mavlink log is started, this will be added to the file as well)
-	print_load_buffer(curr_time, buffer, sizeof(buffer), print_load_callback, &callback_data, &_load);
+	print_load_buffer(buffer, sizeof(buffer), print_load_callback, &callback_data, &_load);
+	cpuload_monitor_stop();
 	_writer.set_need_reliable_transfer(false);
 }
 

src/modules/logger/logger.h

@@ -41,7 +41,7 @@
 #include <drivers/drv_hrt.h>
 #include <version/version.h>
 #include <parameters/param.h>
-#include <systemlib/printload.h>
+#include <px4_platform_common/printload.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 

src/modules/logger/watchdog.h

@@ -37,7 +37,7 @@
 
 #ifdef __PX4_NUTTX
 #include <sched.h>
-#include <systemlib/cpuload.h>
+#include <px4_platform/cpuload.h>
 #endif /* __PX4_NUTTX */
 
 namespace px4

src/systemcmds/top/CMakeLists.txt

@@ -33,10 +33,11 @@
 px4_add_module(
 	MODULE systemcmds__top
 	MAIN top
+	STACK_MAIN 4096
 	PRIORITY
 		"SCHED_PRIORITY_MAX - 16" # max priority below high priority WQ threads
 	SRCS
-		top.c
+		top.cpp
 	DEPENDS
 		systemlib
 )

src/systemcmds/top/top.cpp

@@ -47,17 +47,11 @@
 #include <string.h>
 #include <poll.h>
 
-#include <systemlib/cpuload.h>
+#include <px4_platform/cpuload.h>
-#include <systemlib/printload.h>
+#include <px4_platform_common/printload.h>
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/module.h>
 
-/**
- * Start the top application.
- */
-__EXPORT int top_main(int argc, char *argv[]);
-
-
 static void print_usage(void)
 {
 	PRINT_MODULE_DESCRIPTION("Monitor running processes and their CPU, stack usage, priority and state");
@@ -66,32 +60,30 @@ static void print_usage(void)
 	PRINT_MODULE_USAGE_COMMAND_DESCR("once", "print load only once");
 }
 
-int
+extern "C" __EXPORT int top_main(int argc, char *argv[])
-top_main(int argc, char *argv[])
 {
-	hrt_abstime curr_time = hrt_absolute_time();
+	print_load_s load{};
-
+	init_print_load(&load);
-	struct print_load_s load;
+	px4_usleep(200000);
-	init_print_load_s(curr_time, &load);
 
 	/* clear screen */
 	dprintf(1, "\033[2J\n");
 
 	if (argc > 1) {
 		if (!strcmp(argv[1], "once")) {
-			print_load(curr_time, 1, &load);
 			px4_sleep(1);
-			print_load(hrt_absolute_time(), 1, &load);
+			print_load(STDOUT_FILENO, &load);
 
 		} else {
 			print_usage();
 		}
 
+		cpuload_monitor_stop();
 		return 0;
 	}
 
 	for (;;) {
-		print_load(curr_time, 1, &load);
+		print_load(STDOUT_FILENO, &load);
 
 		/* Sleep 200 ms waiting for user input five times ~ 1s */
 		for (int k = 0; k < 5; k++) {
@@ -108,6 +100,7 @@ top_main(int argc, char *argv[])
 				ret = read(0, &c, 1);
 
 				if (ret) {
+					cpuload_monitor_stop();
 					return 1;
 				}
 
@@ -116,6 +109,7 @@ top_main(int argc, char *argv[])
 				case 0x1b: // esc
 				case 'c':
 				case 'q':
+					cpuload_monitor_stop();
 					return 0;
 					/* not reached */
 				}
@@ -123,9 +117,8 @@ top_main(int argc, char *argv[])
 
 			px4_usleep(200000);
 		}
-
-		curr_time = hrt_absolute_time();
 	}
 
+	cpuload_monitor_stop();
 	return 0;
 }