mixer_module: add support for test_motor (motor_test CLI command)

boards/px4/sitl/default.cmake

@@ -56,6 +56,7 @@ px4_add_board(
 		led_control
 		mixer
 		motor_ramp
+		motor_test
 		#mtd
 		#nshterm
 		param

msg/test_motor.msg

@@ -1,7 +1,13 @@
 uint64 timestamp				# time since system start (microseconds)
 uint8 NUM_MOTOR_OUTPUTS = 8
 
+uint8 ACTION_STOP = 0				# stop motors (disable motor test mode)
+uint8 ACTION_RUN = 1				# run motors (enable motor test mode)
+
+uint8 action					# one of ACTION_* (applies to all motors)
 uint32 motor_number				# number of motor to spin
 float32 value					# output power, range [0..1]
 
+uint8 driver_instance				# select output driver (for boards with multiple outputs, like IO+FMU)
+
 uint8 ORB_QUEUE_LENGTH = 4

src/drivers/dshot/dshot.cpp

@@ -165,6 +165,8 @@ public:
 	bool telemetryEnabled() const { return _telemetry != nullptr; }
 
 private:
+	static constexpr uint16_t DISARMED_VALUE = 0;
+
 	enum class DShotConfig {
 		Disabled = 0,
 		DShot150 = 150,
@@ -248,8 +250,8 @@ DShotOutput::DShotOutput() :
 	_cycle_perf(perf_alloc(PC_ELAPSED, "dshot: cycle")),
 	_cycle_interval_perf(perf_alloc(PC_INTERVAL, "dshot: cycle interval"))
 {
-	_mixing_output.setAllDisarmedValues(0);
+	_mixing_output.setAllDisarmedValues(DISARMED_VALUE);
-	_mixing_output.setAllMinValues(1);
+	_mixing_output.setAllMinValues(DISARMED_VALUE + 1);
 	_mixing_output.setAllMaxValues(DSHOT_MAX_THROTTLE);
 
 }
@@ -286,6 +288,8 @@ DShotOutput::init()
 		PX4_ERR("FAILED registering class device");
 	}
 
+	_mixing_output.setDriverInstance(_class_instance);
+
 	// Getting initial parameter values
 	update_params();
 
@@ -721,7 +725,12 @@ void DShotOutput::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS
 
 	} else {
 		for (int i = 0; i < (int)num_outputs; i++) {
-			up_dshot_motor_data_set(i, math::min(outputs[i], (uint16_t)DSHOT_MAX_THROTTLE), i == requested_telemetry_index);
+			if (outputs[i] == DISARMED_VALUE) {
+				up_dshot_motor_command(i, DShot_cmd_motor_stop, i == requested_telemetry_index);
+
+			} else {
+				up_dshot_motor_data_set(i, math::min(outputs[i], (uint16_t)DSHOT_MAX_THROTTLE), i == requested_telemetry_index);
+			}
 		}
 
 		// clear commands when motors are running
@@ -805,8 +814,8 @@ void DShotOutput::update_params()
 	updateParams();
 
 	// we use a minimum value of 1, since 0 is for disarmed
-	_mixing_output.setAllMinValues(math::constrain((int)(_param_dshot_min.get() * (float)DSHOT_MAX_THROTTLE), 1,
+	_mixing_output.setAllMinValues(math::constrain((int)(_param_dshot_min.get() * (float)DSHOT_MAX_THROTTLE),
-				       DSHOT_MAX_THROTTLE));
+				       DISARMED_VALUE + 1, DSHOT_MAX_THROTTLE));
 }
 
 

src/drivers/px4fmu/fmu.cpp

@@ -253,6 +253,8 @@ PX4FMU::init()
 		PX4_ERR("FAILED registering class device");
 	}
 
+	_mixing_output.setDriverInstance(_class_instance);
+
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 

src/drivers/tap_esc/tap_esc.cpp

@@ -496,7 +496,14 @@ void TAP_ESC::cycle()
 			if (test_motor_updated) {
 				struct test_motor_s test_motor;
 				orb_copy(ORB_ID(test_motor), _test_motor_sub, &test_motor);
-				_outputs.output[test_motor.motor_number] = RPMSTOPPED + ((RPMMAX - RPMSTOPPED) * test_motor.value);
+
+				if (test_motor.action == test_motor_s::ACTION_STOP) {
+					_outputs.output[test_motor.motor_number] = RPMSTOPPED;
+
+				} else {
+					_outputs.output[test_motor.motor_number] = RPMSTOPPED + ((RPMMAX - RPMSTOPPED) * test_motor.value);
+				}
+
 				PX4_INFO("setting motor %i to %.1lf", test_motor.motor_number,
 					 (double)_outputs.output[test_motor.motor_number]);
 			}

src/drivers/uavcan/uavcan_main.cpp

@@ -971,7 +971,7 @@ int UavcanNode::run()
 			orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor);
 
 			// Update the test status and check that we're not locked down
-			_test_in_progress = (_test_motor.value > 0);
+			_test_in_progress = (_test_motor.action == test_motor_s::ACTION_RUN);
 			_esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress);
 		}
 

src/lib/mixer_module/mixer_module.cpp

@@ -34,6 +34,7 @@
 #include "mixer_module.hpp"
 
 #include <lib/circuit_breaker/circuit_breaker.h>
+#include <uORB/PublicationQueued.hpp>
 #include <px4_log.h>
 
 using namespace time_literals;
@@ -72,6 +73,12 @@ _control_latency_perf(perf_alloc(PC_ELAPSED, "control latency"))
 #endif
 
 	px4_sem_init(&_lock, 0, 1);
+
+	// Enforce the existence of the test_motor topic, so we won't miss initial publications
+	test_motor_s test{};
+	uORB::PublicationQueued<test_motor_s> test_motor_pub{ORB_ID(test_motor)};
+	test_motor_pub.publish(test);
+	_motor_test.test_motor_sub.subscribe();
 }
 
 MixingOutput::~MixingOutput()
@@ -86,12 +93,14 @@ void MixingOutput::printStatus() const
 	perf_print_counter(_control_latency_perf);
 	PX4_INFO("Switched to rate_ctrl work queue: %i", (int)_wq_switched);
 	PX4_INFO("Mixer loaded: %s", _mixers ? "yes" : "no");
+	PX4_INFO("Driver instance: %i", _driver_instance);
 
 	PX4_INFO("Channel Configuration:");
 
-	for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
+	for (unsigned i = 0; i < _max_num_outputs; i++) {
-		PX4_INFO("Channel %i: failsafe: %d, disarmed: %d, min: %d, max: %d", i, _failsafe_value[i], _disarmed_value[i],
+		int reordered_i = reorderedMotorIndex(i);
-			 _min_value[i], _max_value[i]);
+		PX4_INFO("Channel %i: value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d", reordered_i, _current_output_value[i],
+			 _failsafe_value[reordered_i], _disarmed_value[reordered_i], _min_value[reordered_i], _max_value[reordered_i]);
 	}
 }
 
@@ -231,6 +240,44 @@ void MixingOutput::updateOutputSlewrate()
 	_mixers->set_max_delta_out_once(delta_out_max);
 }
 
+
+unsigned MixingOutput::motorTest()
+{
+	test_motor_s test_motor;
+	bool had_update = false;
+
+	while (_motor_test.test_motor_sub.update(&test_motor)) {
+		if (test_motor.driver_instance != _driver_instance ||
+		    hrt_elapsed_time(&test_motor.timestamp) > 100_ms) {
+			continue;
+		}
+
+		bool in_test_mode = test_motor.action == test_motor_s::ACTION_RUN;
+
+		if (in_test_mode != _motor_test.in_test_mode) {
+			// reset all outputs to disarmed on state change
+			for (int i = 0; i < MAX_ACTUATORS; ++i) {
+				_current_output_value[i] = _disarmed_value[i];
+			}
+		}
+
+		if (in_test_mode) {
+			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]);
+			}
+		}
+
+		_motor_test.in_test_mode = in_test_mode;
+		had_update = true;
+	}
+
+	return (_motor_test.in_test_mode || had_update) ? _max_num_outputs : 0;
+}
+
 bool MixingOutput::update()
 {
 	if (!_mixers) {
@@ -250,6 +297,24 @@ bool MixingOutput::update()
 		/* Update the armed status and check that we're not locked down.
 		 * We also need to arm throttle for the ESC calibration. */
 		_throttle_armed = (_safety_off && _armed.armed && !_armed.lockdown) || (_safety_off && _armed.in_esc_calibration_mode);
+
+		if (_armed.armed) {
+			_motor_test.in_test_mode = false;
+		}
+	}
+
+	// check for motor test
+	if (!_armed.armed) {
+		unsigned num_motor_test = motorTest();
+
+		if (num_motor_test > 0) {
+			_interface.updateOutputs(false, _current_output_value, num_motor_test, 1);
+			actuator_outputs_s actuator_outputs{};
+			setAndPublishActuatorOutputs(num_motor_test, actuator_outputs);
+			checkSafetyButton();
+			handleCommands();
+			return true;
+		}
 	}
 
 	if (_param_mot_slew_max.get() > FLT_EPSILON) {
@@ -285,15 +350,13 @@ bool MixingOutput::update()
 	const unsigned mixed_num_outputs = _mixers->mix(outputs, _max_num_outputs);
 
 	/* the output limit call takes care of out of band errors, NaN and constrains */
-	uint16_t output_limited[MAX_ACTUATORS] {};
-
 	output_limit_calc(_throttle_armed, armNoThrottle(), mixed_num_outputs, _reverse_output_mask,
-			  _disarmed_value, _min_value, _max_value, outputs, output_limited, &_output_limit);
+			  _disarmed_value, _min_value, _max_value, outputs, _current_output_value, &_output_limit);
 
 	/* overwrite outputs in case of force_failsafe with _failsafe_value values */
 	if (_armed.force_failsafe) {
 		for (size_t i = 0; i < mixed_num_outputs; i++) {
-			output_limited[i] = _failsafe_value[i];
+			_current_output_value[i] = _failsafe_value[i];
 		}
 	}
 
@@ -302,28 +365,21 @@ bool MixingOutput::update()
 	/* overwrite outputs in case of lockdown or parachute triggering with disarmed values */
 	if (_armed.lockdown || _armed.manual_lockdown) {
 		for (size_t i = 0; i < mixed_num_outputs; i++) {
-			output_limited[i] = _disarmed_value[i];
+			_current_output_value[i] = _disarmed_value[i];
 		}
 
 		stop_motors = true;
 	}
 
 	/* apply _param_mot_ordering */
-	reorderOutputs(output_limited);
+	reorderOutputs(_current_output_value);
 
 	/* now return the outputs to the driver */
-	_interface.updateOutputs(stop_motors, output_limited, mixed_num_outputs, n_updates);
+	_interface.updateOutputs(stop_motors, _current_output_value, mixed_num_outputs, n_updates);
 
 
 	actuator_outputs_s actuator_outputs{};
-	actuator_outputs.noutputs = mixed_num_outputs;
+	setAndPublishActuatorOutputs(mixed_num_outputs, actuator_outputs);
-
-	for (size_t i = 0; i < mixed_num_outputs; ++i) {
-		actuator_outputs.output[i] = output_limited[i];
-	}
-
-	actuator_outputs.timestamp = hrt_absolute_time();
-	_outputs_pub.publish(actuator_outputs);
 
 	publishMixerStatus(actuator_outputs);
 	updateLatencyPerfCounter(actuator_outputs);
@@ -346,6 +402,19 @@ MixingOutput::checkSafetyButton()
 	}
 }
 
+void
+MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs)
+{
+	actuator_outputs.noutputs = num_outputs;
+
+	for (size_t i = 0; i < num_outputs; ++i) {
+		actuator_outputs.output[i] = _current_output_value[i];
+	}
+
+	actuator_outputs.timestamp = hrt_absolute_time();
+	_outputs_pub.publish(actuator_outputs);
+}
+
 void
 MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs)
 {
@@ -404,7 +473,7 @@ MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS])
 	 */
 }
 
-int MixingOutput::reorderedMotorIndex(int index)
+int MixingOutput::reorderedMotorIndex(int index) const
 {
 	if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) {
 		switch (index) {

src/lib/mixer_module/mixer_module.hpp

@@ -51,6 +51,7 @@
 #include <uORB/topics/multirotor_motor_limits.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/safety.h>
+#include <uORB/topics/test_motor.h>
 
 
 /**
@@ -102,6 +103,8 @@ public:
 
 	~MixingOutput();
 
+	void setDriverInstance(uint8_t instance) { _driver_instance = instance; }
+
 	void printStatus() const;
 
 	/**
@@ -163,7 +166,7 @@ public:
 	 * @param index motor index in [0, num_motors-1]
 	 * @return reordered motor index. When out of range, the input index is returned
 	 */
-	int reorderedMotorIndex(int index);
+	int reorderedMotorIndex(int index) const;
 
 	void setIgnoreLockdown(bool ignore_lockdown) { _ignore_lockdown = ignore_lockdown; }
 
@@ -178,8 +181,11 @@ private:
 		return (_armed.prearmed && !_armed.armed) || _armed.in_esc_calibration_mode;
 	}
 
+	unsigned motorTest();
+
 	void updateOutputSlewrate();
 	void checkSafetyButton();
+	void setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs);
 	void publishMixerStatus(const actuator_outputs_s &actuator_outputs);
 	void updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs);
 
@@ -218,6 +224,7 @@ private:
 	uint16_t _disarmed_value[MAX_ACTUATORS] {};
 	uint16_t _min_value[MAX_ACTUATORS] {};
 	uint16_t _max_value[MAX_ACTUATORS] {};
+	uint16_t _current_output_value[MAX_ACTUATORS] {}; ///< current output values (reordered)
 	uint16_t _reverse_output_mask{0}; ///< reverses the interval [min, max] -> [max, min], NOT motor direction
 	output_limit_t _output_limit;
 
@@ -246,8 +253,15 @@ private:
 	const bool _support_esc_calibration;
 
 	bool _wq_switched{false};
+	uint8_t _driver_instance{0}; ///< for boards that supports multiple outputs (e.g. PX4IO + FMU)
 	const uint8_t _max_num_outputs;
 
+	struct MotorTest {
+		uORB::Subscription test_motor_sub{ORB_ID(test_motor)};
+		bool in_test_mode{false};
+	};
+	MotorTest _motor_test;
+
 	OutputModuleInterface &_interface;
 
 	perf_counter_t _control_latency_perf;

src/systemcmds/motor_test/motor_test.cpp

@@ -48,20 +48,27 @@
 
 extern "C" __EXPORT int motor_test_main(int argc, char *argv[]);
 
-static void motor_test(unsigned channel, float value);
+static void motor_test(unsigned channel, float value, uint8_t driver_instance);
 static void usage(const char *reason);
 
-void motor_test(unsigned channel, float value)
+void motor_test(unsigned channel, float value, uint8_t driver_instance)
 {
 	test_motor_s test_motor{};
 	test_motor.timestamp = hrt_absolute_time();
 	test_motor.motor_number = channel;
 	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;
 
 	uORB::PublicationQueued<test_motor_s> test_motor_pub{ORB_ID(test_motor)};
 	test_motor_pub.publish(test_motor);
 
-	PX4_INFO("motor %d set to %.2f", channel, (double)value);
+	if (test_motor.action == test_motor_s::ACTION_STOP) {
+		PX4_INFO("motors stop command sent");
+
+	} else {
+		PX4_INFO("motor %d set to %.2f", channel, (double)value);
+	}
 }
 
 static void usage(const char *reason)
@@ -70,15 +77,20 @@ static void usage(const char *reason)
 		PX4_WARN("%s", reason);
 	}
 
-	PRINT_MODULE_DESCRIPTION("Utility to test motors.\n"
+	PRINT_MODULE_DESCRIPTION(
-				 "\n"
+		R"DESCR_STR(
-				 "Note: this can only be used for drivers which support the motor_test uorb topic (currently uavcan and tap_esc)\n"
+Utility to test motors.
-				);
+
+WARNING: remove all props before using this command.
+
+Note: this can only be used for drivers which support the motor_test uorb topic (not px4io).
+)DESCR_STR");
 
 	PRINT_MODULE_USAGE_NAME("motor_test", "command");
 	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('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");
 
@@ -89,14 +101,19 @@ int motor_test_main(int argc, char *argv[])
 	int channel = -1; //default to all channels
 	unsigned long lval;
 	float value = 0.0f;
+	uint8_t driver_instance = 0;
 	int ch;
 
 	int myoptind = 1;
 	const char *myoptarg = NULL;
 
-	while ((ch = px4_getopt(argc, argv, "m:p:", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "i:m:p:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 
+		case 'i':
+			driver_instance = (uint8_t)strtol(myoptarg, NULL, 0);
+			break;
+
 		case 'm':
 			/* Read in motor number */
 			channel = (int)strtol(myoptarg, NULL, 0);
@@ -124,17 +141,17 @@ int motor_test_main(int argc, char *argv[])
 
 	if (myoptind >= 0 && myoptind < argc) {
 		if (strcmp("stop", argv[myoptind]) == 0) {
-			channel = -1;
+			channel = 0;
-			value = 0.f;
+			value = -1.f;
 
 		} else if (strcmp("iterate", argv[myoptind]) == 0) {
-			value = 0.3f;
+			value = 0.15f;
 
 			for (int i = 0; i < 8; ++i) {
-				motor_test(i, value);
+				motor_test(i, value, driver_instance);
-				usleep(500000);
+				px4_usleep(500000);
-				motor_test(i, 0.f);
+				motor_test(i, -1.f, driver_instance);
-				usleep(10000);
+				px4_usleep(10000);
 			}
 
 			run_test = false;
@@ -154,12 +171,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);
+				motor_test(i, value, driver_instance);
-				usleep(10000);
+				px4_usleep(10000);
 			}
 
 		} else {
-			motor_test(channel, value);
+			motor_test(channel, value, driver_instance);
 		}
 	}