commander: remove loop counters and update UI LED control to use monotonic time

src/modules/commander/Commander.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2022 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
@@ -463,6 +463,8 @@ int Commander::print_status()
 {
 	PX4_INFO("arming: %s", arming_state_names[_status.arming_state]);
 	PX4_INFO("navigation: %s", nav_state_names[_status.nav_state]);
+	perf_print_counter(_loop_perf);
+	perf_print_counter(_preflight_check_perf);
 	return 0;
 }
 
@@ -817,6 +819,12 @@ Commander::Commander() :
 	_vehicle_gps_position_valid.set_hysteresis_time_from(true, GPS_VALID_TIME);
 }
 
+Commander::~Commander()
+{
+	perf_free(_loop_perf);
+	perf_free(_preflight_check_perf);
+}
+
 bool
 Commander::handle_command(const vehicle_command_s &cmd)
 {
@@ -1975,8 +1983,6 @@ Commander::run()
 
 	bool param_init_forced = true;
 
-	control_status_leds(true, _battery_warning);
-
 	/* update vehicle status to find out vehicle type (required for preflight checks) */
 	_status.system_type = _param_mav_type.get();
 
@@ -2012,6 +2018,8 @@ Commander::run()
 
 	while (!should_exit()) {
 
+		perf_begin(_loop_perf);
+
 		/* update parameters */
 		const bool params_updated = _parameter_update_sub.updated();
 
@@ -2348,8 +2356,6 @@ Commander::run()
 			_geofence_warning_action_on = false;
 		}
 
-		_cpuload_sub.update(&_cpuload);
-
 		if (_battery_status_subs.updated()) {
 			battery_status_check();
 		}
@@ -2548,7 +2554,8 @@ Commander::run()
 				send_parachute_command();
 			}
 
-			if (_counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+			if (hrt_elapsed_time(&_last_termination_message_sent) > 4_s) {
+				_last_termination_message_sent = hrt_absolute_time();
 				mavlink_log_critical(&_mavlink_log_pub, "Flight termination active\t");
 				events::send(events::ID("commander_mission_termination_active"), {events::Log::Alert, events::LogInternal::Warning},
 					     "Flight termination active");
@@ -2959,8 +2966,10 @@ Commander::run()
 
 			// Evaluate current prearm status
 			if (!_armed.armed && !_status_flags.calibration_enabled) {
+				perf_begin(_preflight_check_perf);
 				bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, _vehicle_control_mode,
 							   false, true, hrt_elapsed_time(&_boot_timestamp));
+				perf_end(_preflight_check_perf);
 
 				// skip arm authorization check until actual arming attempt
 				PreFlightCheck::arm_requirements_t arm_req = _arm_requirements;
@@ -3039,22 +3048,6 @@ Commander::run()
 			_status_changed = true;
 		}
 
-		_counter++;
-
-		int blink_state = blink_msg_state();
-
-		if (blink_state > 0) {
-			/* blinking LED message, don't touch LEDs */
-			if (blink_state == 2) {
-				/* blinking LED message completed, restore normal state */
-				control_status_leds(true, _battery_warning);
-			}
-
-		} else {
-			/* normal state */
-			control_status_leds(_status_changed, _battery_warning);
-		}
-
 		// check if the worker has finished
 		if (_worker_thread.hasResult()) {
 			int ret = _worker_thread.getResultAndReset();
@@ -3072,6 +3065,8 @@ Commander::run()
 			}
 		}
 
+		control_status_leds(_status_changed, _battery_warning);
+
 		_status_changed = false;
 
 		/* store last position lock state */
@@ -3085,6 +3080,8 @@ Commander::run()
 
 		px4_indicate_external_reset_lockout(LockoutComponent::Commander, _armed.armed);
 
+		perf_end(_loop_perf);
+
 		// sleep if there are no vehicle_commands or action_requests to process
 		if (!_vehicle_command_sub.updated() && !_action_request_sub.updated()) {
 			px4_usleep(COMMANDER_MONITORING_INTERVAL);
@@ -3117,18 +3114,42 @@ Commander::get_circuit_breaker_params()
 			CBRK_VTOLARMING_KEY);
 }
 
-void
+void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
-Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 {
-	bool overload = (_cpuload.load > 0.95f) || (_cpuload.ram_usage > 0.98f);
+	switch (blink_msg_state()) {
+	case 1:
+		// blinking LED message, don't touch LEDs
+		return;
 
-	if (_overload_start == 0 && overload) {
+	case 2:
-		_overload_start = hrt_absolute_time();
+		// blinking LED message completed, restore normal state
+		changed = true;
+		break;
 
-	} else if (!overload) {
+	default:
-		_overload_start = 0;
+		break;
 	}
 
+	const hrt_abstime time_now_us = hrt_absolute_time();
+
+	if (_cpuload_sub.updated()) {
+		cpuload_s cpuload;
+
+		if (_cpuload_sub.copy(&cpuload)) {
+
+			bool overload = (cpuload.load > 0.95f) || (cpuload.ram_usage > 0.98f);
+
+			if (_overload_start == 0 && overload) {
+				_overload_start = time_now_us;
+
+			} else if (!overload) {
+				_overload_start = 0;
+			}
+		}
+	}
+
+	const bool overload = (_overload_start != 0);
+
 	// driving the RGB led
 	if (changed || _last_overload != overload) {
 		uint8_t led_mode = led_control_s::MODE_OFF;
@@ -3137,8 +3158,8 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 
 		uint64_t overload_warn_delay = (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 1_ms : 250_ms;
 
-		/* set mode */
+		// set mode
-		if (overload && (hrt_elapsed_time(&_overload_start) > overload_warn_delay)) {
+		if (overload && (time_now_us >= _overload_start + overload_warn_delay)) {
 			led_mode = led_control_s::MODE_BLINK_FAST;
 			led_color = led_control_s::COLOR_PURPLE;
 
@@ -3162,13 +3183,14 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 			// if in init status it should not be in the error state
 			led_mode = led_control_s::MODE_OFF;
 
-		} else {	// STANDBY_ERROR and other states
+		} else {
+			// STANDBY_ERROR and other states
 			led_mode = led_control_s::MODE_BLINK_NORMAL;
 			led_color = led_control_s::COLOR_RED;
 		}
 
 		if (set_normal_color) {
-			/* set color */
+			// set color
 			if (_status.failsafe) {
 				led_color = led_control_s::COLOR_PURPLE;
 
@@ -3197,52 +3219,53 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 
 #if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS)
 
-	/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
 	if (_armed.armed) {
 		if (_status.failsafe) {
 			BOARD_ARMED_LED_OFF();
 
-			if (_leds_counter % 5 == 0) {
+			if (time_now_us >= _led_armed_state_toggle + 250_ms) {
+				_led_armed_state_toggle = time_now_us;
 				BOARD_ARMED_STATE_LED_TOGGLE();
 			}
 
 		} else {
 			BOARD_ARMED_STATE_LED_OFF();
 
-			/* armed, solid */
+			// armed, solid
 			BOARD_ARMED_LED_ON();
 		}
 
 	} else if (_armed.ready_to_arm) {
 		BOARD_ARMED_LED_OFF();
 
-		/* ready to arm, blink at 1Hz */
+		// ready to arm, blink at 1Hz
-		if (_leds_counter % 20 == 0) {
+		if (time_now_us >= _led_armed_state_toggle + 1_s) {
+			_led_armed_state_toggle = time_now_us;
 			BOARD_ARMED_STATE_LED_TOGGLE();
 		}
 
 	} else {
 		BOARD_ARMED_LED_OFF();
 
-		/* not ready to arm, blink at 10Hz */
+		// not ready to arm, blink at 10Hz
-		if (_leds_counter % 2 == 0) {
+		if (time_now_us >= _led_armed_state_toggle + 100_ms) {
+			_led_armed_state_toggle = time_now_us;
 			BOARD_ARMED_STATE_LED_TOGGLE();
 		}
 	}
 
 #endif
 
-	/* give system warnings on error LED */
+	// give system warnings on error LED
 	if (overload) {
-		if (_leds_counter % 2 == 0) {
+		if (time_now_us >= _led_overload_toggle + 50_ms) {
+			_led_overload_toggle = time_now_us;
 			BOARD_OVERLOAD_LED_TOGGLE();
 		}
 
 	} else {
 		BOARD_OVERLOAD_LED_OFF();
 	}
-
-	_leds_counter++;
 }
 
 bool Commander::check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,

src/modules/commander/Commander.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2017, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2017-2022 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
@@ -42,6 +42,7 @@
 #include <lib/controllib/blocks.hpp>
 #include <lib/hysteresis/hysteresis.h>
 #include <lib/mathlib/mathlib.h>
+#include <lib/perf/perf_counter.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 
@@ -100,6 +101,7 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 {
 public:
 	Commander();
+	~Commander();
 
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -358,8 +360,6 @@ private:
 
 	bool		_last_overload{false};
 
-	unsigned int	_leds_counter{0};
-
 	hrt_abstime	_last_valid_manual_control_setpoint{0};
 
 	bool		_is_throttle_above_center{false};
@@ -370,7 +370,11 @@ private:
 	hrt_abstime	_timestamp_engine_healthy{0}; ///< absolute time when engine was healty
 	hrt_abstime	_overload_start{0};		///< time when CPU overload started
 
-	uint32_t	_counter{0};
+	hrt_abstime _led_armed_state_toggle{0};
+	hrt_abstime _led_overload_toggle{0};
+
+	hrt_abstime _last_termination_message_sent{0};
+
 	uint8_t		_heading_reset_counter{0};
 
 	bool		_status_changed{true};
@@ -381,7 +385,6 @@ private:
 	bool		_should_set_home_on_takeoff{true};
 	bool		_system_power_usb_connected{false};
 
-	cpuload_s		_cpuload{};
 	geofence_result_s	_geofence_result{};
 	vehicle_land_detected_s	_vehicle_land_detected{};
 	safety_s		_safety{};
@@ -450,4 +453,7 @@ private:
 	uORB::Publication<vehicle_command_ack_s>		_command_ack_pub{ORB_ID(vehicle_command_ack)};
 
 	orb_advert_t _mavlink_log_pub{nullptr};
+
+	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+	perf_counter_t _preflight_check_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": preflight check")};
 };