diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 4ac540d62c..565df56730 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -44,6 +44,7 @@ set(msg_files actuator_motors.msg actuator_outputs.msg actuator_servos.msg + actuator_test.msg adc_report.msg airspeed.msg airspeed_validated.msg diff --git a/msg/actuator_test.msg b/msg/actuator_test.msg new file mode 100644 index 0000000000..f198b85124 --- /dev/null +++ b/msg/actuator_test.msg @@ -0,0 +1,22 @@ +uint64 timestamp # time since system start (microseconds) + +# Topic to test individual actuator output functions +# Note: this is only used with SYS_CTRL_ALLOC=1 + +uint8 ACTION_RELEASE_CONTROL = 0 # exit test mode for the given function +uint8 ACTION_DO_CONTROL = 1 # enable actuator test mode + +uint8 FUNCTION_MOTOR1 = 101 +uint8 MAX_NUM_MOTORS = 8 +uint8 FUNCTION_SERVO1 = 201 +uint8 MAX_NUM_SERVOS = 8 + +uint8 action # one of ACTION_* +uint16 function # actuator output function +float32 value # range: [-1, 1], where 1 means maximum positive output, + # 0 to center servos or minimum motor thrust, + # -1 maximum negative (if not supported by the motors, <0 maps to NaN), + # and NaN maps to disarmed (stop the motors) +uint32 timeout_ms # timeout in ms after which to exit test mode (if 0, do not time out) + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt index 50495261a1..79a1e9e50c 100644 --- a/src/lib/mixer_module/CMakeLists.txt +++ b/src/lib/mixer_module/CMakeLists.txt @@ -44,6 +44,7 @@ add_custom_command(OUTPUT ${functions_header} add_custom_target(output_functions_header DEPENDS ${functions_header}) px4_add_library(mixer_module + actuator_test.cpp mixer_module.cpp functions.cpp ) diff --git a/src/lib/mixer_module/actuator_test.cpp b/src/lib/mixer_module/actuator_test.cpp new file mode 100644 index 0000000000..4bbd73dc1f --- /dev/null +++ b/src/lib/mixer_module/actuator_test.cpp @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "actuator_test.hpp" + +using namespace time_literals; + +ActuatorTest::ActuatorTest(const OutputFunction function_assignments[MAX_ACTUATORS]) + : _function_assignments(function_assignments) +{ + reset(); +} + +void ActuatorTest::update(int num_outputs, bool reversible_motors) +{ + const hrt_abstime now = hrt_absolute_time(); + + actuator_test_s actuator_test; + + while (_actuator_test_sub.update(&actuator_test)) { + if (actuator_test.timestamp == 0 || + hrt_elapsed_time(&actuator_test.timestamp) > 100_ms) { + continue; + } + + for (int i = 0; i < num_outputs; ++i) { + if ((int)_function_assignments[i] == actuator_test.function) { + bool in_test_mode = actuator_test.action == actuator_test_s::ACTION_DO_CONTROL; + + // entering test mode? + if (!_in_test_mode && in_test_mode) { + reset(); + _in_test_mode = true; + } + + _output_overridden[i] = in_test_mode; + + if (in_test_mode) { + if (actuator_test.timeout_ms > 0) { + _next_timeout = now + actuator_test.timeout_ms * 1000; + } + + float value = actuator_test.value; + + // handle motors + if (actuator_test.function >= (int)OutputFunction::Motor1 && actuator_test.function <= (int)OutputFunction::MotorMax + && !reversible_motors) { + if (value < -FLT_EPSILON) { + value = NAN; + + } else { + // remap from [0, 1] to [-1, 1] + value = value * 2.f - 1.f; + } + } + + _current_outputs[i] = value; + + } else { + _current_outputs[i] = NAN; + } + } + } + } + + if (_in_test_mode) { + // check if all disabled + bool any_overridden = false; + + for (int i = 0; i < num_outputs; ++i) { + any_overridden |= _output_overridden[i]; + } + + if (!any_overridden) { + _in_test_mode = false; + } + } + + // check for timeout + if (_in_test_mode && _next_timeout != 0 && now > _next_timeout) { + _in_test_mode = false; + } +} + +void ActuatorTest::overrideValues(float outputs[MAX_ACTUATORS], int num_outputs) +{ + if (_in_test_mode) { + for (int i = 0; i < num_outputs; ++i) { + outputs[i] = _current_outputs[i]; + } + } +} + +void ActuatorTest::reset() +{ + _in_test_mode = false; + _next_timeout = 0; + + for (int i = 0; i < MAX_ACTUATORS; ++i) { + _current_outputs[i] = NAN; + _output_overridden[i] = false; + } +} diff --git a/src/lib/mixer_module/actuator_test.hpp b/src/lib/mixer_module/actuator_test.hpp new file mode 100644 index 0000000000..b6617334f5 --- /dev/null +++ b/src/lib/mixer_module/actuator_test.hpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "functions.hpp" + +#include +#include +#include + +static_assert(actuator_test_s::FUNCTION_MOTOR1 == (int)OutputFunction::Motor1, "define mismatch"); +static_assert(actuator_test_s::MAX_NUM_MOTORS == (int)OutputFunction::MotorMax - (int)OutputFunction::Motor1 + 1, + "count mismatch"); +static_assert(actuator_test_s::FUNCTION_SERVO1 == (int)OutputFunction::Servo1, "define mismatch"); +static_assert(actuator_test_s::MAX_NUM_SERVOS == (int)OutputFunction::ServoMax - (int)OutputFunction::Servo1 + 1, + "count mismatch"); + +class ActuatorTest +{ +public: + static constexpr int MAX_ACTUATORS = PWM_OUTPUT_MAX_CHANNELS; + + ActuatorTest(const OutputFunction function_assignments[MAX_ACTUATORS]); + + void reset(); + + void update(int num_outputs, bool reversible_motors); + + void overrideValues(float outputs[MAX_ACTUATORS], int num_outputs); + + bool inTestMode() const { return _in_test_mode; } + +private: + + uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)}; + bool _in_test_mode{false}; + hrt_abstime _next_timeout{0}; + + float _current_outputs[MAX_ACTUATORS]; + bool _output_overridden[MAX_ACTUATORS]; + const OutputFunction *_function_assignments; +}; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 6fab458c3f..76e99e9cbb 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -471,6 +471,8 @@ bool MixingOutput::updateSubscriptionsDynamicMixer(bool allow_wq_switch, bool li setMaxTopicUpdateRate(_max_topic_update_interval_us); _need_function_update = false; + _actuator_test.reset(); + unlock(); _interface.mixerChanged(); @@ -773,10 +775,8 @@ bool MixingOutput::updateDynamicMixer() _interface.ScheduleDelayed(50_ms); } - // check for motor test (after topic updates) - if (!_armed.armed && !_armed.manual_lockdown) { - // TODO - } + // check for actuator test + _actuator_test.update(_max_num_outputs, _reversible_motors); // get output values float outputs[MAX_ACTUATORS]; @@ -799,6 +799,10 @@ bool MixingOutput::updateDynamicMixer() } if (!all_disabled) { + if (!_armed.armed && !_armed.manual_lockdown) { + _actuator_test.overrideValues(outputs, _max_num_outputs); + } + limitAndUpdateOutputs(outputs, has_updates); } @@ -809,8 +813,8 @@ void MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updates) { /* the output limit call takes care of out of band errors, NaN and constrains */ - output_limit_calc(_throttle_armed, armNoThrottle(), _max_num_outputs, _reverse_output_mask, - _disarmed_value, _min_value, _max_value, outputs, _current_output_value, &_output_limit); + output_limit_calc(_throttle_armed || _actuator_test.inTestMode(), armNoThrottle(), _max_num_outputs, + _reverse_output_mask, _disarmed_value, _min_value, _max_value, outputs, _current_output_value, &_output_limit); /* overwrite outputs in case of force_failsafe with _failsafe_value values */ if (_armed.force_failsafe) { @@ -819,7 +823,7 @@ MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updat } } - bool stop_motors = !_throttle_armed; + bool stop_motors = !_throttle_armed && !_actuator_test.inTestMode(); /* overwrite outputs in case of lockdown with disarmed values */ if (_armed.lockdown || _armed.manual_lockdown) { diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 15f6eec50c..27f3ecbbbc 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -33,6 +33,7 @@ #pragma once +#include "actuator_test.hpp" #include "functions.hpp" #include @@ -130,7 +131,9 @@ public: /** * Check if a function is configured, i.e. not set to Disabled and initialized */ - bool isFunctionSet(int index) const { return !_use_dynamic_mixing || _functions[index] != nullptr; }; + bool isFunctionSet(int index) const { return !_use_dynamic_mixing || _functions[index] != nullptr; } + + OutputFunction outputFunction(int index) const { return _function_assignment[index]; } /** * Call this regularly from Run(). It will call interface.updateOutputs(). @@ -340,6 +343,7 @@ private: const char *const _param_prefix; ParamHandles _param_handles[MAX_ACTUATORS]; hrt_abstime _lowrate_schedule_interval{300_ms}; + ActuatorTest _actuator_test{_function_assignment}; uORB::SubscriptionCallbackWorkItem *_subscription_callback{nullptr}; ///< current scheduling callback diff --git a/src/systemcmds/actuator_test/CMakeLists.txt b/src/systemcmds/actuator_test/CMakeLists.txt new file mode 100644 index 0000000000..a71e080b5b --- /dev/null +++ b/src/systemcmds/actuator_test/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__actuator_test + MAIN actuator_test + STACK_MAIN 4096 + SRCS + actuator_test.cpp + ) + diff --git a/src/systemcmds/actuator_test/Kconfig b/src/systemcmds/actuator_test/Kconfig new file mode 100644 index 0000000000..4f4ef5fb99 --- /dev/null +++ b/src/systemcmds/actuator_test/Kconfig @@ -0,0 +1,5 @@ +menuconfig SYSTEMCMDS_ACTUATOR_TEST + bool "actuator_test" + default n + ---help--- + Enable support for actuator_test diff --git a/src/systemcmds/actuator_test/actuator_test.cpp b/src/systemcmds/actuator_test/actuator_test.cpp new file mode 100644 index 0000000000..7a50ae55c4 --- /dev/null +++ b/src/systemcmds/actuator_test/actuator_test.cpp @@ -0,0 +1,194 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file actuator_test.cpp + * + * CLI to publish the actuator_test msg + */ + +#include +#include +#include +#include +#include +#include +#include + +extern "C" __EXPORT int actuator_test_main(int argc, char *argv[]); + +static void actuator_test(int function, float value, int timeout_ms, bool release_control); +static void usage(const char *reason); + +void actuator_test(int function, float value, int timeout_ms, bool release_control) +{ + actuator_test_s actuator_test{}; + actuator_test.timestamp = hrt_absolute_time(); + actuator_test.function = function; + actuator_test.value = value; + actuator_test.action = release_control ? actuator_test_s::ACTION_RELEASE_CONTROL : actuator_test_s::ACTION_DO_CONTROL; + actuator_test.timeout_ms = timeout_ms; + + uORB::Publication actuator_test_pub{ORB_ID(actuator_test)}; + actuator_test_pub.publish(actuator_test); +} + +static void usage(const char *reason) +{ + if (reason != nullptr) { + PX4_WARN("%s", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +Utility to test actuators. + +Note: this is only used in combination with SYS_CTRL_ALLOC=1. + +WARNING: remove all props before using this command. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("actuator_test", "command"); + PRINT_MODULE_USAGE_COMMAND_DESCR("set", "Set an actuator to a specific output value"); + PRINT_MODULE_USAGE_PARAM_COMMENT("The actuator can be specified by motor, servo or function directly:"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 1, 8, "Motor to test (1...8)", true); + PRINT_MODULE_USAGE_PARAM_INT('s', -1, 1, 8, "Servo to test (1...8)", true); + PRINT_MODULE_USAGE_PARAM_INT('f', -1, 1, 8, "Specify function directly", true); + + PRINT_MODULE_USAGE_PARAM_INT('v', 0, 0, 100, "value (-1...1)", false); + PRINT_MODULE_USAGE_PARAM_INT('t', 0, 0, 100, "Timeout in seconds (run interactive if not set)", true); + + PRINT_MODULE_USAGE_COMMAND_DESCR("iterate-motors", "Iterate all motors starting and stopping one after the other"); + PRINT_MODULE_USAGE_COMMAND_DESCR("iterate-servos", "Iterate all servos deflecting one after the other"); +} + +int actuator_test_main(int argc, char *argv[]) +{ + int function = 0; + float value = 10.0f; + int ch; + int timeout_ms = 0; + + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "m:s:f:v:t:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + + case 'm': + function = actuator_test_s::FUNCTION_MOTOR1 + (int)strtol(myoptarg, nullptr, 0) - 1; + break; + + case 's': + function = actuator_test_s::FUNCTION_SERVO1 + (int)strtol(myoptarg, nullptr, 0) - 1; + break; + + case 'f': + function = (int)strtol(myoptarg, nullptr, 0); + break; + + case 'v': + value = strtof(myoptarg, nullptr); + + if (value < -1.f || value > 1.f) { + usage("value invalid"); + return 1; + } + break; + + case 't': + timeout_ms = strtof(myoptarg, nullptr) * 1000.f; + break; + + default: + usage(nullptr); + return 1; + } + } + + + if (myoptind >= 0 && myoptind < argc) { + if (strcmp("set", argv[myoptind]) == 0) { + + if (value > 9.f) { + usage("Missing argument: value"); + return 1; + } + + if (function == 0) { + usage("Missing argument: function"); + return 1; + } + + if (timeout_ms == 0) { + // interactive + actuator_test(function, value, 0, false); + + /* stop on any user request */ + PX4_INFO("Press Enter to stop"); + char c; + ssize_t ret = read(0, &c, 1); + + if (ret < 0) { + PX4_ERR("read failed: %i", errno); + } + + actuator_test(function, NAN, 0, true); + } else { + actuator_test(function, value, timeout_ms, false); + } + return 0; + + } else if (strcmp("iterate-motors", argv[myoptind]) == 0) { + value = 0.15f; + for (int i = 0; i < actuator_test_s::MAX_NUM_MOTORS; ++i) { + PX4_INFO("Motor %i (%.0f%%)", i, (double)(value*100.f)); + actuator_test(actuator_test_s::FUNCTION_MOTOR1+i, value, 400, false); + px4_usleep(600000); + } + return 0; + + } else if (strcmp("iterate-servos", argv[myoptind]) == 0) { + value = 0.3f; + for (int i = 0; i < actuator_test_s::MAX_NUM_SERVOS; ++i) { + PX4_INFO("Servo %i (%.0f%%)", i, (double)(value*100.f)); + actuator_test(actuator_test_s::FUNCTION_SERVO1+i, value, 800, false); + px4_usleep(1000000); + } + return 0; + } + } + + usage(nullptr); + return 0; +}