fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

lib/mixer_module: consume output_limit library

Browse Source
This commit is contained in:
Daniel Agar 2021-10-25 23:04:05 -04:00 committed by Beat Küng
parent 71d8b15b73
commit 78b3d22471
17 changed files with 191 additions and 616 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/drivers/dshot/CMakeLists.txt
View File

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

1
src/drivers/linux_pwm_out/CMakeLists.txt
View File

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

1
src/drivers/pca9685_pwm_out/CMakeLists.txt
View File

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

1
src/drivers/pwm_out/CMakeLists.txt
View File

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

1
src/drivers/pwm_out_sim/CMakeLists.txt
View File

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

1
src/drivers/tap_esc/CMakeLists.txt
View File

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

1
src/drivers/uavcan/CMakeLists.txt
View File

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

1
src/lib/CMakeLists.txt
View File

@ -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)

1
src/lib/mixer_module/CMakeLists.txt
View File

@ -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})

199
src/lib/mixer_module/mixer_module.cpp
View File

@ -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) {

16
src/lib/mixer_module/mixer_module.hpp
View File

@ -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];

2
src/lib/mixer_module/mixer_module_tests.cpp
View File

@ -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

36
src/lib/output_limit/CMakeLists.txt
View File

@ -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)

204
src/lib/output_limit/output_limit.cpp
View File

@ -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;
}
}

105
src/lib/output_limit/output_limit.h
View File

@ -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

1
src/systemcmds/tests/CMakeLists.txt
View File

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

235
src/systemcmds/tests/test_mixer.cpp
View File

@ -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)
{