diff --git a/src/drivers/dshot/CMakeLists.txt b/src/drivers/dshot/CMakeLists.txt index 4e8866e008..757a7a8d24 100644 --- a/src/drivers/dshot/CMakeLists.txt +++ b/src/drivers/dshot/CMakeLists.txt @@ -50,7 +50,6 @@ px4_add_module( arch_dshot mixer mixer_module - output_limit MODULE_CONFIG module.yaml ) diff --git a/src/drivers/linux_pwm_out/CMakeLists.txt b/src/drivers/linux_pwm_out/CMakeLists.txt index 0d91192aba..f6dbb3bbde 100644 --- a/src/drivers/linux_pwm_out/CMakeLists.txt +++ b/src/drivers/linux_pwm_out/CMakeLists.txt @@ -40,6 +40,5 @@ px4_add_module( MODULE_CONFIG module.yaml DEPENDS - output_limit ) diff --git a/src/drivers/pca9685_pwm_out/CMakeLists.txt b/src/drivers/pca9685_pwm_out/CMakeLists.txt index 00f581d1f7..a76fb49bbb 100644 --- a/src/drivers/pca9685_pwm_out/CMakeLists.txt +++ b/src/drivers/pca9685_pwm_out/CMakeLists.txt @@ -42,5 +42,4 @@ px4_add_module( DEPENDS mixer mixer_module - output_limit ) diff --git a/src/drivers/pwm_out/CMakeLists.txt b/src/drivers/pwm_out/CMakeLists.txt index b75b046be7..1e5fa884bc 100644 --- a/src/drivers/pwm_out/CMakeLists.txt +++ b/src/drivers/pwm_out/CMakeLists.txt @@ -51,5 +51,4 @@ px4_add_module( arch_io_pins mixer mixer_module - output_limit ) diff --git a/src/drivers/pwm_out_sim/CMakeLists.txt b/src/drivers/pwm_out_sim/CMakeLists.txt index 0eba6f7116..49e6a87117 100644 --- a/src/drivers/pwm_out_sim/CMakeLists.txt +++ b/src/drivers/pwm_out_sim/CMakeLists.txt @@ -47,5 +47,4 @@ px4_add_module( DEPENDS mixer mixer_module - output_limit ) diff --git a/src/drivers/tap_esc/CMakeLists.txt b/src/drivers/tap_esc/CMakeLists.txt index 7dcdfa6e71..4ef6473932 100644 --- a/src/drivers/tap_esc/CMakeLists.txt +++ b/src/drivers/tap_esc/CMakeLists.txt @@ -47,6 +47,5 @@ px4_add_module( led mixer mixer_module - output_limit tunes ) diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 48bb20a336..74cb613942 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -176,7 +176,6 @@ px4_add_module( led mixer mixer_module - output_limit version git_uavcan diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index fe2d99e65f..0a26be9b32 100644 --- a/src/lib/CMakeLists.txt +++ b/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) diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt index cc6d6106b6..0500e265d7 100644 --- a/src/lib/mixer_module/CMakeLists.txt +++ b/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}) diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index ecd0f9ae8d..4dbfdb6663 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/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) { diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index bebb463be7..c5dfedab4e 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -40,7 +40,6 @@ #include #include #include -#include #include #include #include @@ -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]; diff --git a/src/lib/mixer_module/mixer_module_tests.cpp b/src/lib/mixer_module/mixer_module_tests.cpp index 134faa386c..39ca4ad973 100644 --- a/src/lib/mixer_module/mixer_module_tests.cpp +++ b/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 diff --git a/src/lib/output_limit/CMakeLists.txt b/src/lib/output_limit/CMakeLists.txt deleted file mode 100644 index ac1152a9c3..0000000000 --- a/src/lib/output_limit/CMakeLists.txt +++ /dev/null @@ -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) - diff --git a/src/lib/output_limit/output_limit.cpp b/src/lib/output_limit/output_limit.cpp deleted file mode 100644 index cbff58bbb8..0000000000 --- a/src/lib/output_limit/output_limit.cpp +++ /dev/null @@ -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 -#include -#include -#include - -#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; - } - -} diff --git a/src/lib/output_limit/output_limit.h b/src/lib/output_limit/output_limit.h deleted file mode 100644 index 0984f79db0..0000000000 --- a/src/lib/output_limit/output_limit.h +++ /dev/null @@ -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 - */ - -#pragma once - -#include -#include -#include - -__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 diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index 0f953e6767..9f07fb9a26 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -95,7 +95,6 @@ px4_add_module( SRCS ${srcs} DEPENDS - output_limit version ) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index dc800776d8..7a23a2c8fc 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -45,7 +45,6 @@ #include #include #include -#include #include #include @@ -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) {