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;