commander: add support for DO_MOTOR_TEST

- add an optional timeout to test_motor
- enforce a timeout when receiving DO_MOTOR_TEST
- limitation: DO_MOTOR_TEST can only control the MAIN outputs

src/lib/mixer_module/mixer_module.cpp

@@ -253,9 +253,21 @@ unsigned MixingOutput::motorTest()
 			int idx = test_motor.motor_number;
 
 			if (idx < MAX_ACTUATORS) {
-				_current_output_value[reorderedMotorIndex(idx)] =
-					math::constrain<uint16_t>(_min_value[idx] + (uint16_t)((_max_value[idx] - _min_value[idx]) * test_motor.value),
-								  _min_value[idx], _max_value[idx]);
+				if (test_motor.value < 0.f) {
+					_current_output_value[reorderedMotorIndex(idx)] = _disarmed_value[idx];
+
+				} else {
+					_current_output_value[reorderedMotorIndex(idx)] =
+						math::constrain<uint16_t>(_min_value[idx] + (uint16_t)((_max_value[idx] - _min_value[idx]) * test_motor.value),
+									  _min_value[idx], _max_value[idx]);
+				}
+			}
+
+			if (test_motor.timeout_ms > 0) {
+				_motor_test.timeout = test_motor.timestamp + test_motor.timeout_ms * 1000;
+
+			} else {
+				_motor_test.timeout = 0;
 			}
 		}
 
@@ -263,6 +275,18 @@ unsigned MixingOutput::motorTest()
 		had_update = true;
 	}
 
+	// check for timeouts
+	if (_motor_test.timeout != 0 && hrt_absolute_time() > _motor_test.timeout) {
+		_motor_test.in_test_mode = false;
+		_motor_test.timeout = 0;
+
+		for (int i = 0; i < MAX_ACTUATORS; ++i) {
+			_current_output_value[i] = _disarmed_value[i];
+		}
+
+		had_update = true;
+	}
+
 	return (_motor_test.in_test_mode || had_update) ? _max_num_outputs : 0;
 }
 

src/lib/mixer_module/mixer_module.hpp

@@ -264,6 +264,7 @@ private:
 	struct MotorTest {
 		uORB::Subscription test_motor_sub{ORB_ID(test_motor)};
 		bool in_test_mode{false};
+		hrt_abstime timeout{0};
 	};
 	MotorTest _motor_test;
 

src/modules/commander/Commander.cpp

@@ -1080,6 +1080,10 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
 		main_state_transition(*status_local, commander_state_s::MAIN_STATE_ORBIT, status_flags, &internal_state);
 		break;
 
+	case vehicle_command_s::VEHICLE_CMD_DO_MOTOR_TEST:
+		cmd_result = handle_command_motor_test(cmd);
+		break;
+
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
@@ -1129,6 +1133,41 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
 	return true;
 }
 
+unsigned
+Commander::handle_command_motor_test(const vehicle_command_s &cmd)
+{
+	if (armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
+		return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
+	}
+
+	test_motor_s test_motor{};
+	test_motor.timestamp = hrt_absolute_time();
+	test_motor.motor_number = (int)(cmd.param1 + 0.5f) - 1;
+	int throttle_type = (int)(cmd.param2 + 0.5f);
+	if (throttle_type != 0) { // 0: MOTOR_TEST_THROTTLE_PERCENT
+		return vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+	}
+	int motor_count = (int) (cmd.param5 + 0.5);
+	if (motor_count > 1) {
+		return vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+	}
+	test_motor.action = test_motor_s::ACTION_RUN;
+	test_motor.value = math::constrain(cmd.param3 / 100.f, 0.f, 1.f);
+	if (test_motor.value < FLT_EPSILON) {
+		// the message spec is not clear on whether 0 means stop, but it should be closer to what a user expects
+		test_motor.value = -1.f;
+	}
+	test_motor.timeout_ms = (int)(cmd.param4 * 1000.f + 0.5f);
+	// enforce a timeout and a maximum limit
+	if (test_motor.timeout_ms == 0 || test_motor.timeout_ms > 3000) {
+		test_motor.timeout_ms = 3000;
+	}
+	test_motor.driver_instance = 0; // the mavlink command does not allow to specify the instance, so set to 0 for now
+	_test_motor_pub.publish(test_motor);
+
+	return vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+}
+
 /**
 * @brief This function initializes the home position an altitude of the vehicle. This happens first time we get a good GPS fix and each
 *		 time the vehicle is armed with a good GPS fix.

src/modules/commander/Commander.hpp

@@ -52,6 +52,7 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_status_flags.h>
+#include <uORB/topics/test_motor.h>
 
 // subscriptions
 #include <uORB/Subscription.hpp>
@@ -205,6 +206,8 @@ private:
 	bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed,
 			    uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub, bool *changed);
 
+	unsigned handle_command_motor_test(const vehicle_command_s &cmd);
+
 	bool set_home_position();
 	bool set_home_position_alt_only();
 
@@ -291,6 +294,7 @@ private:
 	uORB::Publication<actuator_armed_s>			_armed_pub{ORB_ID(actuator_armed)};
 	uORB::Publication<commander_state_s>			_commander_state_pub{ORB_ID(commander_state)};
 	uORB::Publication<vehicle_status_flags_s>		_vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
+	uORB::Publication<test_motor_s>				_test_motor_pub{ORB_ID(test_motor)};
 
 	uORB::PublicationData<home_position_s>			_home_pub{ORB_ID(home_position)};
 

src/systemcmds/motor_test/motor_test.cpp

@@ -48,10 +48,10 @@
 
 extern "C" __EXPORT int motor_test_main(int argc, char *argv[]);
 
-static void motor_test(unsigned channel, float value, uint8_t driver_instance);
+static void motor_test(unsigned channel, float value, uint8_t driver_instance, int timeout_ms);
 static void usage(const char *reason);
 
-void motor_test(unsigned channel, float value, uint8_t driver_instance)
+void motor_test(unsigned channel, float value, uint8_t driver_instance, int timeout_ms)
 {
 	test_motor_s test_motor{};
 	test_motor.timestamp = hrt_absolute_time();
@@ -59,6 +59,7 @@ void motor_test(unsigned channel, float value, uint8_t driver_instance)
 	test_motor.value = value;
 	test_motor.action = value >= 0.f ? test_motor_s::ACTION_RUN : test_motor_s::ACTION_STOP;
 	test_motor.driver_instance = driver_instance;
+	test_motor.timeout_ms = timeout_ms;
 
 	uORB::PublicationQueued<test_motor_s> test_motor_pub{ORB_ID(test_motor)};
 	test_motor_pub.publish(test_motor);
@@ -90,6 +91,7 @@ Note: this can only be used for drivers which support the motor_test uorb topic
 	PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Set motor(s) to a specific output value");
 	PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 7, "Motor to test (0...7, all if not specified)", true);
 	PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 100, "Power (0...100)", true);
+	PRINT_MODULE_USAGE_PARAM_INT('t', 0, 0, 100, "Timeout in seconds (default=no timeout)", true);
 	PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 4, "driver instance", true);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop all motors");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("iterate", "Iterate all motors starting and stopping one after the other");
@@ -103,11 +105,12 @@ int motor_test_main(int argc, char *argv[])
 	float value = 0.0f;
 	uint8_t driver_instance = 0;
 	int ch;
+	int timeout_ms = 0;
 
 	int myoptind = 1;
 	const char *myoptarg = NULL;
 
-	while ((ch = px4_getopt(argc, argv, "i:m:p:", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "i:m:p:t:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 
 		case 'i':
@@ -131,6 +134,10 @@ int motor_test_main(int argc, char *argv[])
 			value = ((float)lval) / 100.f;
 			break;
 
+		case 't':
+			timeout_ms = strtol(myoptarg, NULL, 0) * 1000;
+			break;
+
 		default:
 			usage(NULL);
 			return 1;
@@ -148,9 +155,9 @@ int motor_test_main(int argc, char *argv[])
 			value = 0.15f;
 
 			for (int i = 0; i < 8; ++i) {
-				motor_test(i, value, driver_instance);
+				motor_test(i, value, driver_instance, 0);
 				px4_usleep(500000);
-				motor_test(i, -1.f, driver_instance);
+				motor_test(i, -1.f, driver_instance, 0);
 				px4_usleep(10000);
 			}
 
@@ -171,12 +178,12 @@ int motor_test_main(int argc, char *argv[])
 	if (run_test) {
 		if (channel < 0) {
 			for (int i = 0; i < 8; ++i) {
-				motor_test(i, value, driver_instance);
+				motor_test(i, value, driver_instance, timeout_ms);
 				px4_usleep(10000);
 			}
 
 		} else {
-			motor_test(channel, value, driver_instance);
+			motor_test(channel, value, driver_instance, timeout_ms);
 		}
 	}