gimbal: add control setter and status

This adds the ability to manually set who has primary control over a
gimbal and also adds printing it as part of the status.

This is helpful, especially while working on the QGC gimbal v2
implementation.

Signed-off-by: Julian Oes <julian@oes.ch>

src/modules/gimbal/gimbal.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
@@ -44,6 +44,7 @@
  * - MAVLink gimbal protocol v2
  */
 
+#include <cstdint>
 #include <stdlib.h>
 #include <string.h>
 #include <px4_platform_common/defines.h>
@@ -77,6 +78,7 @@ struct ThreadData {
 	int last_input_active = -1;
 	OutputBase *output_obj = nullptr;
 	InputTest *test_input = nullptr;
+	ControlData control_data {};
 };
 static ThreadData *g_thread_data = nullptr;
 
@@ -102,7 +104,6 @@ static int gimbal_thread_main(int argc, char *argv[])
 
 	uORB::SubscriptionInterval parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	thread_running.store(true);
-	ControlData control_data {};
 	g_thread_data = &thread_data;
 
 	thread_data.test_input = new InputTest(params);
@@ -208,8 +209,8 @@ static int gimbal_thread_main(int argc, char *argv[])
 
 		if (thread_data.last_input_active == -1) {
 			// Reset control as no one is active anymore, or yet.
-			control_data.sysid_primary_control = 0;
-			control_data.compid_primary_control = 0;
+			thread_data.control_data.sysid_primary_control = 0;
+			thread_data.control_data.compid_primary_control = 0;
 		}
 
 		InputBase::UpdateResult update_result = InputBase::UpdateResult::NoUpdate;
@@ -226,7 +227,7 @@ static int gimbal_thread_main(int argc, char *argv[])
 				const unsigned int poll_timeout =
 					(already_active || thread_data.last_input_active == -1) ? 20 : 0;
 
-				update_result = thread_data.input_objs[i]->update(poll_timeout, control_data, already_active);
+				update_result = thread_data.input_objs[i]->update(poll_timeout, thread_data.control_data, already_active);
 
 				bool break_loop = false;
 
@@ -271,7 +272,7 @@ static int gimbal_thread_main(int argc, char *argv[])
 
 			// Update output
 			thread_data.output_obj->update(
-				control_data,
+				thread_data.control_data,
 				update_result != InputBase::UpdateResult::NoUpdate);
 
 			// Only publish the mount orientation if the mode is not mavlink v1 or v2
@@ -399,6 +400,33 @@ int gimbal_main(int argc, char *argv[])
 		}
 	}
 
+	else if (!strcmp(argv[1], "primary-control")) {
+
+		if (thread_running.load() && g_thread_data && g_thread_data->test_input) {
+
+			if (argc == 4) {
+				g_thread_data->control_data.sysid_primary_control = (uint8_t)strtol(argv[2], nullptr, 0);
+				g_thread_data->control_data.compid_primary_control = (uint8_t)strtol(argv[3], nullptr, 0);
+
+				PX4_INFO("Control set to: %d/%d",
+					 g_thread_data->control_data.sysid_primary_control,
+					 g_thread_data->control_data.compid_primary_control);
+
+				return 0;
+
+			} else {
+				PX4_ERR("not enough arguments");
+				usage();
+				return 1;
+			}
+
+		} else {
+			PX4_WARN("not running");
+			usage();
+			return 1;
+		}
+	}
+
 	else if (!strcmp(argv[1], "status")) {
 		if (thread_running.load() && g_thread_data && g_thread_data->test_input) {
 
@@ -422,6 +450,9 @@ int gimbal_main(int argc, char *argv[])
 					}
 				}
 
+				PX4_INFO("Primary control:   %d/%d",
+					 g_thread_data->control_data.sysid_primary_control, g_thread_data->control_data.compid_primary_control);
+
 			}
 
 			if (g_thread_data->output_obj) {
@@ -538,6 +569,9 @@ $ gimbal test pitch -45 yaw 30
 
 	PRINT_MODULE_USAGE_NAME("gimbal", "driver");
 	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_COMMAND("status");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("primary-control", "Set who is in control of gimbal");
+	PRINT_MODULE_USAGE_ARG("<sysid> <compid>", "MAVLink system ID and MAVLink component ID", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test the output: set a fixed angle for one or multiple axes (gimbal must be running)");
 	PRINT_MODULE_USAGE_ARG("roll|pitch|yaw <angle>", "Specify an axis and an angle in degrees", false);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();