gimbal: Add test command for angle rates

This is handy to test angular rate input, not just angles.

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"};
+				float roll_deg = 0.0f;
-				int angles[3] = { 0, 0, 0 };
+				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])) {
+					if (!strcmp(argv[arg_i], "roll")) {
-							int angle_deg = (int)strtol(argv[arg_i + 1], nullptr, 0);
+						roll_deg = (int)strtof(argv[arg_i + 1], nullptr);
-							angles[axis_i] = angle_deg;
+						angles_set = true;
-							found_axis = 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;
+
+				} 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;
 				}
-
-				g_thread_data->test_input->set_test_input(angles[0], angles[1], angles[2]);
-				return 0;
 			}
 
 		} else {
@@ -573,5 +623,6 @@ $ gimbal test pitch -45 yaw 30
 	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_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;
+	if (PX4_ISFINITE(_roll_deg) && PX4_ISFINITE(_pitch_deg) && PX4_ISFINITE(_yaw_deg)) {
-	control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
+		control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
-	control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+		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);
 
-	matrix::Eulerf euler(
+	} else {
-		math::radians((float)_roll_deg),
+		control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
-		math::radians((float)_pitch_deg),
+		control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
-		math::radians((float)_yaw_deg));
+		control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
-	matrix::Quatf q(euler);
+		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);
+	}
 
-	q.copyTo(control_data.type_data.angle.q);
-
-	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("  roll : % .1f deg\n", (double)_roll_deg);
-	PX4_INFO_RAW("  pitch: % 3d deg\n", _pitch_deg);
+	PX4_INFO_RAW("  pitch: % .1f deg\n", (double)_pitch_deg);
-	PX4_INFO_RAW("  yaw  : % 3d deg\n", _yaw_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 @@ public:
 	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};
+	float _roll_deg {NAN};
-	int _pitch_deg {0};
+	float _pitch_deg {NAN};
-	int _yaw_deg {0};
+	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};
 };