lib/mixer_module: consume output_limit library

src/lib/mixer_module/CMakeLists.txt

@@ -50,7 +50,6 @@ px4_add_library(mixer_module
 	)
 
 add_dependencies(mixer_module output_functions_header)
-target_link_libraries(mixer_module PRIVATE output_limit)
 target_compile_options(mixer_module PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 
 

src/lib/mixer_module/mixer_module.cpp

@@ -67,9 +67,9 @@ static const FunctionProvider all_function_providers[] = {
 };
 
 MixingOutput::MixingOutput(const char *param_prefix, uint8_t max_num_outputs, OutputModuleInterface &interface,
-			   SchedulingPolicy scheduling_policy,
-			   bool support_esc_calibration, bool ramp_up)
+			   SchedulingPolicy scheduling_policy, bool support_esc_calibration, bool ramp_up)
 	: ModuleParams(&interface),
+	  _output_ramp_up(ramp_up),
 	  _control_subs{
 	{&interface, ORB_ID(actuator_controls_0)},
 	{&interface, ORB_ID(actuator_controls_1)},
@@ -83,9 +83,6 @@ _interface(interface),
 _control_latency_perf(perf_alloc(PC_ELAPSED, "control latency")),
 _param_prefix(param_prefix)
 {
-	output_limit_init(&_output_limit);
-	_output_limit.ramp_up = ramp_up;
-
 	/* Safely initialize armed flags */
 	_armed.armed = false;
 	_armed.prearmed = false;
@@ -694,7 +691,7 @@ bool MixingOutput::updateStaticMixer()
 				_controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f;
 
 				/* Switch off the output limit ramp for the calibration. */
-				_output_limit.state = OUTPUT_LIMIT_STATE_ON;
+				_output_state = OutputLimitState::ON;
 			}
 		}
 	}
@@ -719,8 +716,7 @@ bool MixingOutput::updateStaticMixer()
 	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 */
-	output_limit_calc(_throttle_armed, armNoThrottle(), mixed_num_outputs, _reverse_output_mask,
-			  _disarmed_value, _min_value, _max_value, outputs, _current_output_value, &_output_limit);
+	output_limit_calc(_throttle_armed, mixed_num_outputs, outputs);
 
 	/* overwrite outputs in case of force_failsafe with _failsafe_value values */
 	if (_armed.force_failsafe) {
@@ -824,26 +820,25 @@ bool MixingOutput::updateDynamicMixer()
 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 || _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) {
-		for (size_t i = 0; i < _max_num_outputs; i++) {
-			_current_output_value[i] = actualFailsafeValue(i);
-		}
-	}
-
 	bool stop_motors = !_throttle_armed && !_actuator_test.inTestMode();
 
-	/* overwrite outputs in case of lockdown with disarmed values */
 	if (_armed.lockdown || _armed.manual_lockdown) {
+		// overwrite outputs in case of lockdown with disarmed values
 		for (size_t i = 0; i < _max_num_outputs; i++) {
 			_current_output_value[i] = _disarmed_value[i];
 		}
 
 		stop_motors = true;
+
+	} else if (_armed.force_failsafe) {
+		// overwrite outputs in case of force_failsafe with _failsafe_value values
+		for (size_t i = 0; i < _max_num_outputs; i++) {
+			_current_output_value[i] = actualFailsafeValue(i);
+		}
+
+	} else {
+		// the output limit call takes care of out of band errors, NaN and constrains
+		output_limit_calc(_throttle_armed || _actuator_test.inTestMode(), _max_num_outputs, outputs);
 	}
 
 	/* now return the outputs to the driver */
@@ -855,6 +850,165 @@ MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updat
 	}
 }
 
+uint16_t MixingOutput::output_limit_calc_single(int i, float value) const
+{
+	// check for invalid / disabled channels
+	if (!PX4_ISFINITE(value)) {
+		return _disarmed_value[i];
+	}
+
+	if (_reverse_output_mask & (1 << i)) {
+		value = -1.f * value;
+	}
+
+	uint16_t effective_output = value * (_max_value[i] - _min_value[i]) / 2 + (_max_value[i] + _min_value[i]) / 2;
+
+	// last line of defense against invalid inputs
+	return math::constrain(effective_output, _min_value[i], _max_value[i]);
+}
+
+void
+MixingOutput::output_limit_calc(const bool armed, const int num_channels, const float output[MAX_ACTUATORS])
+{
+	const bool pre_armed = armNoThrottle();
+
+	// time to slowly ramp up the ESCs
+	static constexpr hrt_abstime RAMP_TIME_US = 500_ms;
+
+	/* first evaluate state changes */
+	switch (_output_state) {
+	case OutputLimitState::INIT:
+		if (armed) {
+			// set arming time for the first call
+			if (_output_time_armed == 0) {
+				_output_time_armed = hrt_absolute_time();
+			}
+
+			// time for the ESCs to initialize (this is not actually needed if the signal is sent right after boot)
+			if (hrt_elapsed_time(&_output_time_armed) >= 50_ms) {
+				_output_state = OutputLimitState::OFF;
+			}
+		}
+
+		break;
+
+	case OutputLimitState::OFF:
+		if (armed) {
+			if (_output_ramp_up) {
+				_output_state = OutputLimitState::RAMP;
+
+			} else {
+				_output_state = OutputLimitState::ON;
+			}
+
+			// reset arming time, used for ramp timing
+			_output_time_armed = hrt_absolute_time();
+		}
+
+		break;
+
+	case OutputLimitState::RAMP:
+		if (!armed) {
+			_output_state = OutputLimitState::OFF;
+
+		} else if (hrt_elapsed_time(&_output_time_armed) >= RAMP_TIME_US) {
+			_output_state = OutputLimitState::ON;
+		}
+
+		break;
+
+	case OutputLimitState::ON:
+		if (!armed) {
+			_output_state = OutputLimitState::OFF;
+		}
+
+		break;
+	}
+
+	/* if the system is pre-armed, the limit state is temporarily on,
+	 * as some outputs are valid and the non-valid outputs have been
+	 * set to NaN. This is not stored in the state machine though,
+	 * as the throttle channels need to go through the ramp at
+	 * regular arming time.
+	 */
+	auto local_limit_state = _output_state;
+
+	if (pre_armed) {
+		local_limit_state = OutputLimitState::ON;
+	}
+
+	// then set _current_output_value based on state
+	switch (local_limit_state) {
+	case OutputLimitState::OFF:
+	case OutputLimitState::INIT:
+		for (int i = 0; i < num_channels; i++) {
+			_current_output_value[i] = _disarmed_value[i];
+		}
+
+		break;
+
+	case OutputLimitState::RAMP: {
+			hrt_abstime diff = hrt_elapsed_time(&_output_time_armed);
+
+			static constexpr int PROGRESS_INT_SCALING = 10000;
+			int progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US;
+
+			if (progress > PROGRESS_INT_SCALING) {
+				progress = PROGRESS_INT_SCALING;
+			}
+
+			for (int i = 0; i < num_channels; i++) {
+
+				float control_value = output[i];
+
+				/* check for invalid / disabled channels */
+				if (!PX4_ISFINITE(control_value)) {
+					_current_output_value[i] = _disarmed_value[i];
+					continue;
+				}
+
+				uint16_t ramp_min_output;
+
+				/* if a disarmed output value was set, blend between disarmed and min */
+				if (_disarmed_value[i] > 0) {
+
+					/* safeguard against overflows */
+					auto disarmed = _disarmed_value[i];
+
+					if (disarmed > _min_value[i]) {
+						disarmed = _min_value[i];
+					}
+
+					int disarmed_min_diff = _min_value[i] - disarmed;
+					ramp_min_output = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING;
+
+				} else {
+					/* no disarmed output value set, choose min output */
+					ramp_min_output = _min_value[i];
+				}
+
+				if (_reverse_output_mask & (1 << i)) {
+					control_value = -1.f * control_value;
+				}
+
+				_current_output_value[i] = control_value * (_max_value[i] - ramp_min_output) / 2 + (_max_value[i] + ramp_min_output) /
+							   2;
+
+				/* last line of defense against invalid inputs */
+				_current_output_value[i] = math::constrain(_current_output_value[i], ramp_min_output, _max_value[i]);
+			}
+		}
+		break;
+
+	case OutputLimitState::ON:
+		for (int i = 0; i < num_channels; i++) {
+			_current_output_value[i] = output_limit_calc_single(i, output[i]);
+		}
+
+		break;
+	}
+}
+
 void
 MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs)
 {
@@ -966,8 +1120,7 @@ MixingOutput::actualFailsafeValue(int index)
 			default_failsafe = _functions[index]->defaultFailsafeValue(_function_assignment[index]);
 		}
 
-		value = output_limit_calc_single(_reverse_output_mask & (1 << index),
-						 _disarmed_value[index], _min_value[index], _max_value[index], default_failsafe);
+		value = output_limit_calc_single(index, default_failsafe);
 
 	} else {
 		value = _failsafe_value[index];
@@ -1031,7 +1184,7 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8
 	input = math::constrain(input, -1.f, 1.f);
 
 	/* motor spinup phase - lock throttle to zero */
-	if (output->_output_limit.state == OUTPUT_LIMIT_STATE_RAMP) {
+	if (output->_output_state == OutputLimitState::RAMP) {
 		if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
 		     control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
 		    control_index == actuator_controls_s::INDEX_THROTTLE) {

src/lib/mixer_module/mixer_module.hpp

@@ -40,7 +40,6 @@
 #include <drivers/drv_pwm_output.h>
 #include <lib/mixer/MixerGroup.hpp>
 #include <lib/perf/perf_counter.h>
-#include <lib/output_limit/output_limit.h>
 #include <px4_platform_common/atomic.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
@@ -254,6 +253,10 @@ private:
 
 	void limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updates);
 
+	uint16_t output_limit_calc_single(int i, float value) const;
+
+	void output_limit_calc(const bool armed, const int num_channels, const float outputs[MAX_ACTUATORS]);
+
 	struct ParamHandles {
 		param_t function{PARAM_INVALID};
 		param_t disarmed{PARAM_INVALID};
@@ -297,7 +300,16 @@ private:
 	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;
+
+	enum class OutputLimitState {
+		OFF = 0,
+		INIT,
+		RAMP,
+		ON
+	} _output_state{OutputLimitState::INIT};
+
+	hrt_abstime _output_time_armed{0};
+	const bool _output_ramp_up; ///< if true, motors will ramp up from disarmed to min_output after arming
 
 	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
 	uORB::SubscriptionCallbackWorkItem _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];

src/lib/mixer_module/mixer_module_tests.cpp

@@ -72,7 +72,7 @@ public:
 	{
 		mixing_output.update();
 		// make sure output_limit switches to ON (if outputs enabled)
-		px4_usleep(INIT_TIME_US * 2);
+		px4_usleep(50000 * 2);
 		mixing_output.update();
 		mixing_output.update();
 		return 3; // expected number of output updates