mixer_module: add testing for SYS_CTRL_ALLOC=1 with actuator_test cmd+uorb msg

src/lib/mixer_module/CMakeLists.txt

@@ -44,6 +44,7 @@ add_custom_command(OUTPUT ${functions_header}
 add_custom_target(output_functions_header DEPENDS ${functions_header})
 
 px4_add_library(mixer_module
+	actuator_test.cpp
 	mixer_module.cpp
 	functions.cpp
 	)

src/lib/mixer_module/actuator_test.cpp

@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "actuator_test.hpp"
+
+using namespace time_literals;
+
+ActuatorTest::ActuatorTest(const OutputFunction function_assignments[MAX_ACTUATORS])
+	: _function_assignments(function_assignments)
+{
+	reset();
+}
+
+void ActuatorTest::update(int num_outputs, bool reversible_motors)
+{
+	const hrt_abstime now = hrt_absolute_time();
+
+	actuator_test_s actuator_test;
+
+	while (_actuator_test_sub.update(&actuator_test)) {
+		if (actuator_test.timestamp == 0 ||
+		    hrt_elapsed_time(&actuator_test.timestamp) > 100_ms) {
+			continue;
+		}
+
+		for (int i = 0; i < num_outputs; ++i) {
+			if ((int)_function_assignments[i] == actuator_test.function) {
+				bool in_test_mode = actuator_test.action == actuator_test_s::ACTION_DO_CONTROL;
+
+				// entering test mode?
+				if (!_in_test_mode && in_test_mode) {
+					reset();
+					_in_test_mode = true;
+				}
+
+				_output_overridden[i] = in_test_mode;
+
+				if (in_test_mode) {
+					if (actuator_test.timeout_ms > 0) {
+						_next_timeout = now + actuator_test.timeout_ms * 1000;
+					}
+
+					float value = actuator_test.value;
+
+					// handle motors
+					if (actuator_test.function >= (int)OutputFunction::Motor1 && actuator_test.function <= (int)OutputFunction::MotorMax
+					    && !reversible_motors) {
+						if (value < -FLT_EPSILON) {
+							value = NAN;
+
+						} else {
+							// remap from [0, 1] to [-1, 1]
+							value = value * 2.f - 1.f;
+						}
+					}
+
+					_current_outputs[i] = value;
+
+				} else {
+					_current_outputs[i] = NAN;
+				}
+			}
+		}
+	}
+
+	if (_in_test_mode) {
+		// check if all disabled
+		bool any_overridden = false;
+
+		for (int i = 0; i < num_outputs; ++i) {
+			any_overridden |= _output_overridden[i];
+		}
+
+		if (!any_overridden) {
+			_in_test_mode = false;
+		}
+	}
+
+	// check for timeout
+	if (_in_test_mode && _next_timeout != 0 && now > _next_timeout) {
+		_in_test_mode = false;
+	}
+}
+
+void ActuatorTest::overrideValues(float outputs[MAX_ACTUATORS], int num_outputs)
+{
+	if (_in_test_mode) {
+		for (int i = 0; i < num_outputs; ++i) {
+			outputs[i] = _current_outputs[i];
+		}
+	}
+}
+
+void ActuatorTest::reset()
+{
+	_in_test_mode = false;
+	_next_timeout = 0;
+
+	for (int i = 0; i < MAX_ACTUATORS; ++i) {
+		_current_outputs[i] = NAN;
+		_output_overridden[i] = false;
+	}
+}

src/lib/mixer_module/actuator_test.hpp

@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "functions.hpp"
+
+#include <drivers/drv_pwm_output.h>
+#include <uORB/topics/actuator_test.h>
+#include <uORB/Subscription.hpp>
+
+static_assert(actuator_test_s::FUNCTION_MOTOR1 == (int)OutputFunction::Motor1, "define mismatch");
+static_assert(actuator_test_s::MAX_NUM_MOTORS == (int)OutputFunction::MotorMax - (int)OutputFunction::Motor1 + 1,
+	      "count mismatch");
+static_assert(actuator_test_s::FUNCTION_SERVO1 == (int)OutputFunction::Servo1, "define mismatch");
+static_assert(actuator_test_s::MAX_NUM_SERVOS == (int)OutputFunction::ServoMax - (int)OutputFunction::Servo1 + 1,
+	      "count mismatch");
+
+class ActuatorTest
+{
+public:
+	static constexpr int MAX_ACTUATORS = PWM_OUTPUT_MAX_CHANNELS;
+
+	ActuatorTest(const OutputFunction function_assignments[MAX_ACTUATORS]);
+
+	void reset();
+
+	void update(int num_outputs, bool reversible_motors);
+
+	void overrideValues(float outputs[MAX_ACTUATORS], int num_outputs);
+
+	bool inTestMode() const { return _in_test_mode; }
+
+private:
+
+	uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)};
+	bool _in_test_mode{false};
+	hrt_abstime _next_timeout{0};
+
+	float _current_outputs[MAX_ACTUATORS];
+	bool _output_overridden[MAX_ACTUATORS];
+	const OutputFunction *_function_assignments;
+};

src/lib/mixer_module/mixer_module.cpp

@@ -471,6 +471,8 @@ bool MixingOutput::updateSubscriptionsDynamicMixer(bool allow_wq_switch, bool li
 	setMaxTopicUpdateRate(_max_topic_update_interval_us);
 	_need_function_update = false;
 
+	_actuator_test.reset();
+
 	unlock();
 
 	_interface.mixerChanged();
@@ -773,10 +775,8 @@ bool MixingOutput::updateDynamicMixer()
 		_interface.ScheduleDelayed(50_ms);
 	}
 
-	// check for motor test (after topic updates)
-	if (!_armed.armed && !_armed.manual_lockdown) {
-		// TODO
-	}
+	// check for actuator test
+	_actuator_test.update(_max_num_outputs, _reversible_motors);
 
 	// get output values
 	float outputs[MAX_ACTUATORS];
@@ -799,6 +799,10 @@ bool MixingOutput::updateDynamicMixer()
 	}
 
 	if (!all_disabled) {
+		if (!_armed.armed && !_armed.manual_lockdown) {
+			_actuator_test.overrideValues(outputs, _max_num_outputs);
+		}
+
 		limitAndUpdateOutputs(outputs, has_updates);
 	}
 
@@ -809,8 +813,8 @@ void
 MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updates)
 {
 	/* the output limit call takes care of out of band errors, NaN and constrains */
-	output_limit_calc(_throttle_armed, armNoThrottle(), _max_num_outputs, _reverse_output_mask,
-			  _disarmed_value, _min_value, _max_value, outputs, _current_output_value, &_output_limit);
+	output_limit_calc(_throttle_armed || _actuator_test.inTestMode(), armNoThrottle(), _max_num_outputs,
+			  _reverse_output_mask, _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) {
@@ -819,7 +823,7 @@ MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updat
 		}
 	}
 
-	bool stop_motors = !_throttle_armed;
+	bool stop_motors = !_throttle_armed && !_actuator_test.inTestMode();
 
 	/* overwrite outputs in case of lockdown with disarmed values */
 	if (_armed.lockdown || _armed.manual_lockdown) {

src/lib/mixer_module/mixer_module.hpp

@@ -33,6 +33,7 @@
 
 #pragma once
 
+#include "actuator_test.hpp"
 #include "functions.hpp"
 
 #include <board_config.h>
@@ -130,7 +131,9 @@ public:
 	/**
 	 * Check if a function is configured, i.e. not set to Disabled and initialized
 	 */
-	bool isFunctionSet(int index) const { return !_use_dynamic_mixing || _functions[index] != nullptr; };
+	bool isFunctionSet(int index) const { return !_use_dynamic_mixing || _functions[index] != nullptr; }
+
+	OutputFunction outputFunction(int index) const { return _function_assignment[index]; }
 
 	/**
 	 * Call this regularly from Run(). It will call interface.updateOutputs().
@@ -340,6 +343,7 @@ private:
 	const char *const _param_prefix;
 	ParamHandles _param_handles[MAX_ACTUATORS];
 	hrt_abstime _lowrate_schedule_interval{300_ms};
+	ActuatorTest _actuator_test{_function_assignment};
 
 	uORB::SubscriptionCallbackWorkItem *_subscription_callback{nullptr}; ///< current scheduling callback