commander: move to WQ with callback scheduling (vehicle_command, action_request, etc)

- backup scheduling still at 100 ms (10 Hz)
 - this considerably increases the priorty of commander (140->242)

src/modules/commander/Commander.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2023 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
@@ -471,20 +471,16 @@ int Commander::custom_command(int argc, char *argv[])
 	return print_usage("unknown command");
 }
 
-int Commander::print_status()
+bool Commander::init()
 {
-	PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName());
-	PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
-	PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
-	PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
-	perf_print_counter(_loop_perf);
-	perf_print_counter(_preflight_check_perf);
-	return 0;
+	if (!_action_request_sub.registerCallback() || !_vehicle_command_sub.registerCallback()) {
+		PX4_ERR("callback registration failed!");
+		return false;
 	}
 
-extern "C" __EXPORT int commander_main(int argc, char *argv[])
-{
-	return Commander::main(argc, argv);
+	ScheduleNow();
+
+	return true;
 }
 
 bool Commander::shutdownIfAllowed()
@@ -639,7 +635,8 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
 }
 
 Commander::Commander() :
-	ModuleParams(nullptr)
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
 {
 	_vehicle_land_detected.landed = true;
 
@@ -1610,8 +1607,25 @@ void Commander::updateParameters()
 
 }
 
-void Commander::run()
+void Commander::Run()
 {
+	if (should_exit()) {
+
+		_action_request_sub.unregisterCallback();
+		_vehicle_command_sub.unregisterCallback();
+
+		rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_OFF);
+
+		/* close fds */
+		led_deinit();
+		buzzer_deinit();
+
+		exit_and_cleanup();
+		return;
+	}
+
+
+	if (!_initialized) {
 		/* initialize */
 		led_init();
 		buzzer_init();
@@ -1642,7 +1656,10 @@ void Commander::run()
 
 		arm_auth_init(&_mavlink_log_pub, &_vehicle_status.system_id);
 
-	while (!should_exit()) {
+
+		_initialized = true;
+	}
+
 
 	perf_begin(_loop_perf);
 
@@ -1828,19 +1845,15 @@ void Commander::run()
 
 	px4_indicate_external_reset_lockout(LockoutComponent::Commander, _arm_state_machine.isArmed());
 
+
+	if (_vehicle_command_sub.updated() || _action_request_sub.updated()) {
+		ScheduleNow();
+
+	} else {
+		ScheduleDelayed(100_ms);
+	}
+
 	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);
-		}
-	}
-
-	rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_OFF);
-
-	/* close fds */
-	led_deinit();
-	buzzer_deinit();
 }
 
 void Commander::checkForMissionUpdate()
@@ -2468,42 +2481,6 @@ void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result)
 	_vehicle_command_ack_pub.publish(command_ack);
 }
 
-int Commander::task_spawn(int argc, char *argv[])
-{
-	_task_id = px4_task_spawn_cmd("commander",
-				      SCHED_DEFAULT,
-				      SCHED_PRIORITY_DEFAULT + 40,
-				      3250,
-				      (px4_main_t)&run_trampoline,
-				      (char *const *)argv);
-
-	if (_task_id < 0) {
-		_task_id = -1;
-		return -errno;
-	}
-
-	// wait until task is up & running
-	if (wait_until_running() < 0) {
-		_task_id = -1;
-		return -1;
-	}
-
-	return 0;
-}
-
-Commander *Commander::instantiate(int argc, char *argv[])
-{
-	Commander *instance = new Commander();
-
-	if (instance) {
-		if (argc >= 2 && !strcmp(argv[1], "-h")) {
-			instance->enable_hil();
-		}
-	}
-
-	return instance;
-}
-
 void Commander::enable_hil()
 {
 	_vehicle_status.hil_state = vehicle_status_s::HIL_STATE_ON;
@@ -2807,6 +2784,41 @@ void Commander::send_parachute_command()
 	set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
 }
 
+int Commander::task_spawn(int argc, char *argv[])
+{
+	Commander *instance = new Commander();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int Commander::print_status()
+{
+	PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName());
+	PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
+	PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
+	PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
+	perf_print_counter(_loop_perf);
+	perf_print_counter(_preflight_check_perf);
+
+	return 0;
+}
+
 int Commander::print_usage(const char *reason)
 {
 	if (reason) {
@@ -2849,3 +2861,9 @@ The commander module contains the state machine for mode switching and failsafe
 
 	return 1;
 }
+
+extern "C" __EXPORT int commander_main(int argc, char *argv[])
+{
+	return Commander::main(argc, argv);
+}
+

src/modules/commander/Commander.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2017-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2017-2023 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
@@ -49,6 +49,7 @@
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 
 // publications
 #include <uORB/Publication.hpp>
@@ -61,6 +62,7 @@
 
 // subscriptions
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/action_request.h>
@@ -89,7 +91,7 @@ using systemlib::Hysteresis;
 
 using namespace time_literals;
 
-class Commander : public ModuleBase<Commander>, public ModuleParams
+class Commander : public ModuleBase<Commander>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
 	Commander();
@@ -98,24 +100,22 @@ public:
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
 
-	/** @see ModuleBase */
-	static Commander *instantiate(int argc, char *argv[]);
-
 	/** @see ModuleBase */
 	static int custom_command(int argc, char *argv[]);
 
 	/** @see ModuleBase */
 	static int print_usage(const char *reason = nullptr);
 
-	/** @see ModuleBase::run() */
-	void run() override;
-
 	/** @see ModuleBase::print_status() */
 	int print_status() override;
 
 	void enable_hil();
 
+	bool init();
+
 private:
+	void Run() override;
+
 	void answer_command(const vehicle_command_s &cmd, uint8_t result);
 
 	transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
@@ -194,10 +194,10 @@ private:
 		OFFBOARD_MODE_BIT = (1 << 1),
 	};
 
-	/* Decouple update interval and hysteresis counters, all depends on intervals */
-	static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
 	static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
 
+	bool _initialized{false};
+
 	vehicle_status_s        _vehicle_status{};
 
 	ArmStateMachine		_arm_state_machine{};
@@ -267,12 +267,13 @@ private:
 	vtol_vehicle_status_s	_vtol_vehicle_status{};
 
 	// Subscriptions
-	uORB::Subscription					_action_request_sub{ORB_ID(action_request)};
+	uORB::SubscriptionCallbackWorkItem _action_request_sub{this, ORB_ID(action_request)};
+	uORB::SubscriptionCallbackWorkItem _vehicle_command_sub{this, ORB_ID(vehicle_command)};
+
 	uORB::Subscription					_cpuload_sub{ORB_ID(cpuload)};
 	uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
 	uORB::Subscription					_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
-	uORB::Subscription					_vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription					_vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};