Gimbal: add angular rate test and fix deg/rad input

ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink

@@ -29,7 +29,7 @@ mavlink start -x -u $udp_offboard_port_local -r 4000000 -f -m onboard -o $udp_of
 mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote
 
 # Onboard link to gimbal
-mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -m gimbal -o $udp_onboard_gimbal_port_remote
+mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -f -m gimbal -o $udp_onboard_gimbal_port_remote
 
 # To display for SIH sitl
 if [ "$PX4_SIMULATOR" = "sihsim" ]; then

Tools/px4moduledoc/srcparser.py

@@ -442,7 +442,7 @@ def _do_consistency_check(self, contents, scope, module_doc):
                    re.findall(r"\bstrcmp\b.*\bverb\b.*\"(.+)\"", contents)
 
         doc_commands = module_doc.all_commands() + \
-                [x for value in module_doc.all_values() for x in value.split('|')]
+                [x for value in module_doc.all_values() for x in value.replace(' ', '|').split('|')]
 
         for command in commands:
             if len(command) == 2 and command[0] == '-':

src/modules/gimbal/gimbal.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2024 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
@@ -369,27 +369,77 @@ int gimbal_main(int argc, char *argv[])
 		if (thread_running.load() && g_thread_data && g_thread_data->test_input) {
 
 			if (argc >= 4) {
-				bool found_axis = false;
-				const char *axis_names[3] = {"roll", "pitch", "yaw"};
-				int angles[3] = { 0, 0, 0 };
+
+				float roll_deg = 0.0f;
+				float pitch_deg = 0.0f;
+				float yaw_deg = 0.0f;
+				float rollrate_deg_s = 0.0f;
+				float pitchrate_deg_s = 0.0f;
+				float yawrate_deg_s = 0.0f;
+
+				bool angles_set = false;
+				bool rates_set = false;
 
 				for (int arg_i = 2 ; arg_i < (argc - 1); ++arg_i) {
-					for (int axis_i = 0; axis_i < 3; ++axis_i) {
-						if (!strcmp(argv[arg_i], axis_names[axis_i])) {
-							int angle_deg = (int)strtol(argv[arg_i + 1], nullptr, 0);
-							angles[axis_i] = angle_deg;
-							found_axis = true;
-						}
+
+					if (!strcmp(argv[arg_i], "roll")) {
+						roll_deg = (int)strtof(argv[arg_i + 1], nullptr);
+						angles_set = true;
+
+					} else if (!strcmp(argv[arg_i], "pitch")) {
+						pitch_deg = (int)strtof(argv[arg_i + 1], nullptr);
+						angles_set = true;
+
+					} else if (!strcmp(argv[arg_i], "yaw")) {
+						yaw_deg = (int)strtof(argv[arg_i + 1], nullptr);
+						angles_set = true;
+
+					} else if (!strcmp(argv[arg_i], "rollrate")) {
+						rollrate_deg_s = (int)strtof(argv[arg_i + 1], nullptr);
+						rates_set = true;
+
+					} else if (!strcmp(argv[arg_i], "pitchrate")) {
+						pitchrate_deg_s = (int)strtof(argv[arg_i + 1], nullptr);
+						rates_set = true;
+
+					} else if (!strcmp(argv[arg_i], "yawrate")) {
+						yawrate_deg_s = (int)strtof(argv[arg_i + 1], nullptr);
+						rates_set = true;
+
+					} else {
+						PX4_ERR("Unknown argument: %s", argv[arg_i]);
+						usage();
+						return -1;
 					}
 				}
 
-				if (!found_axis) {
+				if (angles_set && rates_set) {
+					PX4_ERR("This driver doesn't support both, angles and rates, to be set");
 					usage();
 					return -1;
-				}
 
-				g_thread_data->test_input->set_test_input(angles[0], angles[1], angles[2]);
-				return 0;
+				} else if (angles_set) {
+					g_thread_data->test_input->set_test_input_angles(
+						roll_deg,
+						pitch_deg,
+						yaw_deg
+					);
+					return 0;
+
+				} else if (rates_set) {
+
+					g_thread_data->test_input->set_test_input_angle_rates(
+						rollrate_deg_s,
+						pitchrate_deg_s,
+						yawrate_deg_s
+					);
+					return 0;
+
+				} else {
+					PX4_ERR("No angles or angle rates set");
+					usage();
+					return -1;
+				}
 			}
 
 		} else {
@@ -573,5 +623,6 @@ Test the output by setting a angles (all omitted axes are set to 0):
 	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_ARG("rollrate|pitchrate|yawrate <angle rate>", "Specify an axis and an angle rate in degrees / second", false);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }

src/modules/gimbal/input_mavlink.cpp

@@ -882,8 +882,8 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_
 			const matrix::Eulerf euler(0.0f, math::radians(vehicle_command.param1),
 						   math::radians(vehicle_command.param2));
 			const matrix::Quatf q(euler);
-			const matrix::Vector3f angular_velocity(0.0f, vehicle_command.param3,
-								vehicle_command.param4);
+			const matrix::Vector3f angular_velocity(NAN, math::radians(vehicle_command.param3),
+								math::radians(vehicle_command.param4));
 			const uint32_t flags = vehicle_command.param5;
 
 			// TODO: support gimbal device id for multiple gimbals

src/modules/gimbal/input_test.cpp

@@ -55,21 +55,30 @@ InputTest::UpdateResult InputTest::update(unsigned int timeout_ms, ControlData &
 	control_data.type = ControlData::Type::Angle;
 	control_data.timestamp_last_update = hrt_absolute_time();
 
-	control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
-	control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
-	control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
-
-	matrix::Eulerf euler(
-		math::radians((float)_roll_deg),
-		math::radians((float)_pitch_deg),
-		math::radians((float)_yaw_deg));
-	matrix::Quatf q(euler);
-
-	q.copyTo(control_data.type_data.angle.q);
+	if (PX4_ISFINITE(_roll_deg) && PX4_ISFINITE(_pitch_deg) && PX4_ISFINITE(_yaw_deg)) {
+		control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
+		control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
+		control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+		matrix::Eulerf euler(
+			math::radians((float)_roll_deg),
+			math::radians((float)_pitch_deg),
+			math::radians((float)_yaw_deg));
+		matrix::Quatf q(euler);
+		q.copyTo(control_data.type_data.angle.q);
+
+	} else {
+		control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
+		control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
+		control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
+		control_data.type_data.angle.q[0] = NAN;
+		control_data.type_data.angle.q[1] = NAN;
+		control_data.type_data.angle.q[2] = NAN;
+		control_data.type_data.angle.q[3] = NAN;
+		control_data.type_data.angle.angular_velocity[0] = math::radians(_rollrate_deg_s);
+		control_data.type_data.angle.angular_velocity[1] = math::radians(_pitchrate_deg_s);
+		control_data.type_data.angle.angular_velocity[2] = math::radians(_yawrate_deg_s);
+	}
 
-	control_data.type_data.angle.angular_velocity[0] = NAN;
-	control_data.type_data.angle.angular_velocity[1] = NAN;
-	control_data.type_data.angle.angular_velocity[2] = NAN;
 
 	// For testing we mark ourselves as in control.
 	control_data.sysid_primary_control = _parameters.mav_sysid;
@@ -87,17 +96,32 @@ int InputTest::initialize()
 void InputTest::print_status() const
 {
 	PX4_INFO("Input: Test");
-	PX4_INFO_RAW("  roll : % 3d deg\n", _roll_deg);
-	PX4_INFO_RAW("  pitch: % 3d deg\n", _pitch_deg);
-	PX4_INFO_RAW("  yaw  : % 3d deg\n", _yaw_deg);
+	PX4_INFO_RAW("  roll : % .1f deg\n", (double)_roll_deg);
+	PX4_INFO_RAW("  pitch: % .1f deg\n", (double)_pitch_deg);
+	PX4_INFO_RAW("  yaw  : % .1f deg\n", (double)_yaw_deg);
+	PX4_INFO_RAW("  rollrate : % .1f deg/s\n", (double)_rollrate_deg_s);
+	PX4_INFO_RAW("  pitchrate: % .1f deg/s\n", (double)_pitchrate_deg_s);
+	PX4_INFO_RAW("  yawrate  : % .1f deg/s\n", (double)_yawrate_deg_s);
 }
 
-void InputTest::set_test_input(int roll_deg, int pitch_deg, int yaw_deg)
+void InputTest::set_test_input_angles(float roll_deg, float pitch_deg, float yaw_deg)
 {
 	_roll_deg = roll_deg;
 	_pitch_deg = pitch_deg;
 	_yaw_deg = yaw_deg;
-
+	_rollrate_deg_s = NAN;
+	_pitchrate_deg_s = NAN;
+	_yawrate_deg_s = NAN;
+	_has_been_set.store(true);
+}
+void InputTest::set_test_input_angle_rates(float rollrate_deg_s, float pitchrate_deg_s, float yawrate_deg_s)
+{
+	_roll_deg = NAN;
+	_pitch_deg = NAN;
+	_yaw_deg = NAN;
+	_rollrate_deg_s = rollrate_deg_s;
+	_pitchrate_deg_s = pitchrate_deg_s;
+	_yawrate_deg_s = yawrate_deg_s;
 	_has_been_set.store(true);
 }
 

src/modules/gimbal/input_test.h

@@ -50,12 +50,17 @@ class InputTest : public InputBase
 	int initialize() override;
 	void print_status() const override;
 
-	void set_test_input(int roll_deg, int pitch_deg, int yaw_deg);
+	void set_test_input_angles(
+		float roll_deg, float pitch_deg, float yaw_deg);
+	void set_test_input_angle_rates(float rollrate_deg_s, float pitchrate_deg_s, float yawrate_deg_s);
 
 private:
-	int _roll_deg {0};
-	int _pitch_deg {0};
-	int _yaw_deg {0};
+	float _roll_deg {NAN};
+	float _pitch_deg {NAN};
+	float _yaw_deg {NAN};
+	float _rollrate_deg_s {NAN};
+	float _pitchrate_deg_s {NAN};
+	float _yawrate_deg_s {NAN};
 
 	px4::atomic<bool> _has_been_set {false};
 };

src/modules/gimbal/output_rc.cpp

@@ -96,10 +96,19 @@ void OutputRC::_stream_device_attitude_status()
 	attitude_status.timestamp = hrt_absolute_time();
 	attitude_status.target_system = 0;
 	attitude_status.target_component = 0;
-	attitude_status.device_flags = gimbal_device_attitude_status_s::DEVICE_FLAGS_NEUTRAL |
-				       gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK |
-				       gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK |
-				       gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_LOCK;
+	attitude_status.device_flags = 0;
+
+	if (_absolute_angle[0]) {
+		attitude_status.device_flags |= gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK;
+	}
+
+	if (_absolute_angle[1]) {
+		attitude_status.device_flags |= gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK;
+	}
+
+	if (_absolute_angle[2]) {
+		attitude_status.device_flags |= gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_LOCK;
+	}
 
 	matrix::Eulerf euler(_angle_outputs[0], _angle_outputs[1], _angle_outputs[2]);
 	matrix::Quatf q(euler);