lib/mixer_module: consume output_limit library

src/drivers/dshot/CMakeLists.txt

@@ -50,7 +50,6 @@ px4_add_module(
 		arch_dshot
 		mixer
 		mixer_module
-		output_limit
 	MODULE_CONFIG
 		module.yaml
 	)

src/drivers/linux_pwm_out/CMakeLists.txt

@@ -40,6 +40,5 @@ px4_add_module(
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS
-		output_limit
 	)
 

src/drivers/pca9685_pwm_out/CMakeLists.txt

@@ -42,5 +42,4 @@ px4_add_module(
 	DEPENDS
 		mixer
 		mixer_module
-		output_limit
 	)

src/drivers/pwm_out/CMakeLists.txt

@@ -51,5 +51,4 @@ px4_add_module(
 		arch_io_pins
 		mixer
 		mixer_module
-		output_limit
 	)

src/drivers/pwm_out_sim/CMakeLists.txt

@@ -47,5 +47,4 @@ px4_add_module(
 	DEPENDS
 		mixer
 		mixer_module
-		output_limit
 	)

src/drivers/tap_esc/CMakeLists.txt

@@ -47,6 +47,5 @@ px4_add_module(
 		led
 		mixer
 		mixer_module
-		output_limit
 		tunes
 	)

src/drivers/uavcan/CMakeLists.txt

@@ -176,7 +176,6 @@ px4_add_module(
 		led
 		mixer
 		mixer_module
-		output_limit
 		version
 
 		git_uavcan

src/lib/CMakeLists.txt

@@ -54,7 +54,6 @@ add_subdirectory(mathlib)
 add_subdirectory(mixer)
 add_subdirectory(mixer_module)
 add_subdirectory(motion_planning)
-add_subdirectory(output_limit)
 add_subdirectory(perf)
 add_subdirectory(pid)
 add_subdirectory(pid_design)

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,
+			   SchedulingPolicy scheduling_policy, bool support_esc_calibration, bool ramp_up)
-			   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,
+	output_limit_calc(_throttle_armed, mixed_num_outputs, outputs);
-			  _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) {
@@ -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),
+		value = output_limit_calc_single(index, default_failsafe);
-						 _disarmed_value[index], _min_value[index], _max_value[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

src/lib/output_limit/CMakeLists.txt

@@ -1,36 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2018 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.
-#
-############################################################################
-
-add_library(output_limit output_limit.cpp)
-target_link_libraries(output_limit PRIVATE prebuild_targets)
-

src/lib/output_limit/output_limit.cpp

@@ -1,204 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013-2015 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 "output_limit.h"
-
-#include <math.h>
-#include <stdbool.h>
-#include <drivers/drv_hrt.h>
-#include <stdio.h>
-
-#define PROGRESS_INT_SCALING	10000
-
-void output_limit_init(output_limit_t *limit)
-{
-	limit->state = OUTPUT_LIMIT_STATE_INIT;
-	limit->time_armed = 0;
-	limit->ramp_up = true;
-}
-
-void output_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask,
-		       const uint16_t *disarmed_output, const uint16_t *min_output, const uint16_t *max_output,
-		       const float *output, uint16_t *effective_output, output_limit_t *limit)
-{
-
-	/* first evaluate state changes */
-	switch (limit->state) {
-	case OUTPUT_LIMIT_STATE_INIT:
-
-		if (armed) {
-
-			/* set arming time for the first call */
-			if (limit->time_armed == 0) {
-				limit->time_armed = hrt_absolute_time();
-			}
-
-			if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) {
-				limit->state = OUTPUT_LIMIT_STATE_OFF;
-			}
-		}
-
-		break;
-
-	case OUTPUT_LIMIT_STATE_OFF:
-		if (armed) {
-			if (limit->ramp_up) {
-				limit->state = OUTPUT_LIMIT_STATE_RAMP;
-
-			} else {
-				limit->state = OUTPUT_LIMIT_STATE_ON;
-			}
-
-			/* reset arming time, used for ramp timing */
-			limit->time_armed = hrt_absolute_time();
-		}
-
-		break;
-
-	case OUTPUT_LIMIT_STATE_RAMP:
-		if (!armed) {
-			limit->state = OUTPUT_LIMIT_STATE_OFF;
-
-		} else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) {
-			limit->state = OUTPUT_LIMIT_STATE_ON;
-		}
-
-		break;
-
-	case OUTPUT_LIMIT_STATE_ON:
-		if (!armed) {
-			limit->state = OUTPUT_LIMIT_STATE_OFF;
-		}
-
-		break;
-
-	default:
-		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.
-	 */
-
-	unsigned local_limit_state = limit->state;
-
-	if (pre_armed) {
-		local_limit_state = OUTPUT_LIMIT_STATE_ON;
-	}
-
-	unsigned progress;
-
-	/* then set effective_output based on state */
-	switch (local_limit_state) {
-	case OUTPUT_LIMIT_STATE_OFF:
-	case OUTPUT_LIMIT_STATE_INIT:
-		for (unsigned i = 0; i < num_channels; i++) {
-			effective_output[i] = disarmed_output[i];
-		}
-
-		break;
-
-	case OUTPUT_LIMIT_STATE_RAMP: {
-			hrt_abstime diff = hrt_elapsed_time(&limit->time_armed);
-
-			progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US;
-
-			if (progress > PROGRESS_INT_SCALING) {
-				progress = PROGRESS_INT_SCALING;
-			}
-
-			for (unsigned i = 0; i < num_channels; i++) {
-
-				float control_value = output[i];
-
-				/* check for invalid / disabled channels */
-				if (!PX4_ISFINITE(control_value)) {
-					effective_output[i] = disarmed_output[i];
-					continue;
-				}
-
-				uint16_t ramp_min_output;
-
-				/* if a disarmed output value was set, blend between disarmed and min */
-				if (disarmed_output[i] > 0) {
-
-					/* safeguard against overflows */
-					unsigned disarmed = disarmed_output[i];
-
-					if (disarmed > min_output[i]) {
-						disarmed = min_output[i];
-					}
-
-					unsigned disarmed_min_diff = min_output[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_output[i];
-				}
-
-				if (reverse_mask & (1 << i)) {
-					control_value = -1.0f * control_value;
-				}
-
-				effective_output[i] = control_value * (max_output[i] - ramp_min_output) / 2 + (max_output[i] + ramp_min_output) / 2;
-
-				/* last line of defense against invalid inputs */
-				if (effective_output[i] < ramp_min_output) {
-					effective_output[i] = ramp_min_output;
-
-				} else if (effective_output[i] > max_output[i]) {
-					effective_output[i] = max_output[i];
-				}
-			}
-		}
-		break;
-
-	case OUTPUT_LIMIT_STATE_ON:
-
-		for (unsigned i = 0; i < num_channels; i++) {
-			effective_output[i] = output_limit_calc_single(reverse_mask & (1 << i), disarmed_output[i],
-					      min_output[i], max_output[i], output[i]);
-		}
-
-		break;
-
-	default:
-		break;
-	}
-
-}

src/lib/output_limit/output_limit.h

@@ -1,105 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013-2015 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.
- *
- ****************************************************************************/
-
-/**
- * @file output_limit.h
- *
- * Library for output limiting (PWM for example)
- *
- * @author Julian Oes <julian@px4.io>
- */
-
-#pragma once
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <px4_platform_common/defines.h>
-
-__BEGIN_DECLS
-
-/*
- * time for the ESCs to initialize
- * (this is not actually needed if the signal is sent right after boot)
- */
-#define INIT_TIME_US 50000
-/*
- * time to slowly ramp up the ESCs
- */
-#define RAMP_TIME_US 500000
-
-enum output_limit_state {
-	OUTPUT_LIMIT_STATE_OFF = 0,
-	OUTPUT_LIMIT_STATE_INIT,
-	OUTPUT_LIMIT_STATE_RAMP,
-	OUTPUT_LIMIT_STATE_ON
-};
-
-typedef struct {
-	enum output_limit_state state;
-	uint64_t time_armed;
-	bool ramp_up; ///< if true, motors will ramp up from disarmed to min_output after arming
-} output_limit_t;
-
-__EXPORT void output_limit_init(output_limit_t *limit);
-
-__EXPORT void output_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels,
-				const uint16_t reverse_mask, const uint16_t *disarmed_output,
-				const uint16_t *min_output, const uint16_t *max_output,
-				const float *output, uint16_t *effective_output, output_limit_t *limit);
-
-static inline uint16_t output_limit_calc_single(bool reversed, uint16_t disarmed_output,
-		uint16_t min_output, uint16_t max_output, float output)
-{
-	/* check for invalid / disabled channels */
-	if (!PX4_ISFINITE(output)) {
-		return disarmed_output;
-	}
-
-	if (reversed) {
-		output = -1.0f * output;
-	}
-
-	uint16_t effective_output = output * (max_output - min_output) / 2 + (max_output + min_output) / 2;
-
-	/* last line of defense against invalid inputs */
-	if (effective_output < min_output) {
-		effective_output = min_output;
-
-	} else if (effective_output > max_output) {
-		effective_output = max_output;
-	}
-
-	return effective_output;
-}
-
-__END_DECLS

src/systemcmds/tests/CMakeLists.txt

@@ -95,7 +95,6 @@ px4_add_module(
 	SRCS
 		${srcs}
 	DEPENDS
-		output_limit
 		version
 	)
 

src/systemcmds/tests/test_mixer.cpp

@@ -45,7 +45,6 @@
 #include <px4_platform_common/px4_config.h>
 #include <lib/mixer/MixerGroup.hpp>
 #include <lib/mixer/mixer_load.h>
-#include <output_limit/output_limit.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_output.h>
 
@@ -98,7 +97,6 @@ public:
 	MixerTest() = default;
 
 private:
-	bool mixerTest();
 	bool loadIOPass();
 	bool loadVTOL1Test();
 	bool loadVTOL2Test();
@@ -120,7 +118,6 @@ bool MixerTest::run_tests()
 	ut_run_test(loadVTOL2Test);
 	ut_run_test(loadComplexTest);
 	ut_run_test(loadAllTest);
-	ut_run_test(mixerTest);
 
 	return (_tests_failed == 0);
 }
@@ -338,238 +335,6 @@ bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loade
 	return true;
 }
 
-bool MixerTest::mixerTest()
-{
-	/*
-	 * Output limit structure
-	 */
-	output_limit_t output_limit;
-	bool should_arm = false;
-	uint16_t r_page_servo_disarmed[output_max];
-	uint16_t r_page_servo_control_min[output_max];
-	uint16_t r_page_servo_control_max[output_max];
-	uint16_t r_page_servos[output_max];
-	uint16_t servo_predicted[output_max];
-	int16_t reverse_pwm_mask = 0;
-
-	bool load_ok = load_mixer(MIXER_PATH(IO_pass.mix), 8);
-
-	if (!load_ok) {
-		return load_ok;
-	}
-
-	/* execute the mixer */
-
-	float	outputs[output_max];
-	unsigned mixed;
-	const int jmax = 5;
-
-	output_limit_init(&output_limit);
-
-	/* run through arming phase */
-	for (unsigned i = 0; i < output_max; i++) {
-		actuator_controls[i] = 0.1f;
-		r_page_servo_disarmed[i] = PWM_MOTOR_OFF;
-		r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
-		r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
-	}
-
-	//PX4_INFO("PRE-ARM TEST: DISABLING SAFETY");
-
-	/* mix */
-	should_prearm = true;
-	mixed = mixer_group.mix(&outputs[0], output_max);
-
-	output_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
-			  r_page_servo_control_max, outputs, r_page_servos, &output_limit);
-
-	//PX4_INFO("mixed %d outputs (max %d), values:", mixed, output_max);
-
-	for (unsigned i = 0; i < mixed; i++) {
-		//PX4_ERR("pre-arm:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]);
-
-		if (i != actuator_controls_s::INDEX_THROTTLE) {
-			if (r_page_servos[i] < r_page_servo_control_min[i]) {
-				PX4_ERR("active servo < min");
-				return false;
-			}
-
-		} else {
-			if (r_page_servos[i] != r_page_servo_disarmed[i]) {
-				PX4_ERR("throttle output != 0 (this check assumed the IO pass mixer!)");
-				return false;
-			}
-		}
-	}
-
-	should_arm = true;
-	should_prearm = false;
-
-	/* simulate another orb_copy() from actuator controls */
-	for (unsigned i = 0; i < output_max; i++) {
-		actuator_controls[i] = 0.1f;
-	}
-
-	//PX4_INFO("ARMING TEST: STARTING RAMP");
-	unsigned sleep_quantum_us = 10000;
-
-	hrt_abstime starttime = hrt_absolute_time();
-	unsigned sleepcount = 0;
-
-	while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US + 2 * sleep_quantum_us) {
-
-		/* mix */
-		mixed = mixer_group.mix(&outputs[0], output_max);
-
-		output_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
-				  r_page_servo_control_max, outputs, r_page_servos, &output_limit);
-
-		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
-		for (unsigned i = 0; i < mixed; i++) {
-
-			//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
-
-			/* check mixed outputs to be zero during init phase */
-			if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
-			    r_page_servos[i] != r_page_servo_disarmed[i]) {
-				PX4_ERR("disarmed servo value mismatch: %d vs %d", r_page_servos[i], r_page_servo_disarmed[i]);
-				return false;
-			}
-
-			if (hrt_elapsed_time(&starttime) >= INIT_TIME_US &&
-			    r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) {
-				PX4_ERR("ramp servo value mismatch");
-				return false;
-			}
-		}
-
-		px4_usleep(sleep_quantum_us);
-		sleepcount++;
-
-		if (sleepcount % 10 == 0) {
-			fflush(stdout);
-		}
-	}
-
-	//PX4_INFO("ARMING TEST: NORMAL OPERATION");
-
-	for (int j = -jmax; j <= jmax; j++) {
-
-		for (unsigned i = 0; i < output_max; i++) {
-			actuator_controls[i] = j / 10.0f + 0.1f * i;
-			r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
-			r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
-			r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
-		}
-
-		/* mix */
-		mixed = mixer_group.mix(&outputs[0], output_max);
-
-		output_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
-				  r_page_servo_control_max, outputs, r_page_servos, &output_limit);
-
-		//fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max);
-
-		for (unsigned i = 0; i < mixed; i++) {
-			servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
-
-			if (abs(servo_predicted[i] - r_page_servos[i]) > MIXER_DIFFERENCE_THRESHOLD) {
-				fprintf(stderr, "\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i],
-					(int)r_page_servos[i]);
-				PX4_ERR("mixer violated predicted value");
-				return false;
-			}
-		}
-	}
-
-	//PX4_INFO("ARMING TEST: DISARMING");
-
-	starttime = hrt_absolute_time();
-	sleepcount = 0;
-	should_arm = false;
-
-	while (hrt_elapsed_time(&starttime) < 600000) {
-
-		/* mix */
-		mixed = mixer_group.mix(&outputs[0], output_max);
-
-		output_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
-				  r_page_servo_control_max, outputs, r_page_servos, &output_limit);
-
-		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
-		for (unsigned i = 0; i < mixed; i++) {
-
-			//fprintf(stderr, "disarmed:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
-
-			/* check mixed outputs to be zero during init phase */
-			if (r_page_servos[i] != r_page_servo_disarmed[i]) {
-				PX4_ERR("disarmed servo value mismatch");
-				return false;
-			}
-		}
-
-		px4_usleep(sleep_quantum_us);
-		sleepcount++;
-
-		if (sleepcount % 10 == 0) {
-			//printf(".");
-			//fflush(stdout);
-		}
-	}
-
-	//printf("\n");
-
-	//PX4_INFO("ARMING TEST: REARMING: STARTING RAMP");
-
-	starttime = hrt_absolute_time();
-	sleepcount = 0;
-	should_arm = true;
-
-	while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) {
-
-		/* mix */
-		mixed = mixer_group.mix(&outputs[0], output_max);
-
-		output_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
-				  r_page_servo_control_max, outputs, r_page_servos, &output_limit);
-
-		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
-		for (unsigned i = 0; i < mixed; i++) {
-			/* predict value */
-			servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
-
-			/* check ramp */
-
-			//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
-
-			if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
-			    (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
-			     r_page_servos[i] > servo_predicted[i])) {
-				PX4_ERR("ramp servo value mismatch");
-				return false;
-			}
-
-			/* check post ramp phase */
-			if (hrt_elapsed_time(&starttime) > RAMP_TIME_US &&
-			    abs(servo_predicted[i] - r_page_servos[i]) > 2) {
-				printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]);
-				PX4_ERR("mixer violated predicted value");
-				return false;
-			}
-		}
-
-		px4_usleep(sleep_quantum_us);
-		sleepcount++;
-
-		if (sleepcount % 10 == 0) {
-			//	printf(".");
-			//	fflush(stdout);
-		}
-	}
-
-	return true;
-}
-
 static int
 mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control)
 {