Beat Küng
3 years ago
committed by
Daniel Agar
10 changed files with 485 additions and 8 deletions
@ -0,0 +1,22 @@
@@ -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 |
@ -0,0 +1,133 @@
@@ -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; |
||||
} |
||||
} |
@ -0,0 +1,73 @@
@@ -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 <drivers/drv_pwm_output.h> |
||||
#include <uORB/topics/actuator_test.h> |
||||
#include <uORB/Subscription.hpp> |
||||
|
||||
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; |
||||
}; |
@ -0,0 +1,40 @@
@@ -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 |
||||
) |
||||
|
@ -0,0 +1,5 @@
@@ -0,0 +1,5 @@
|
||||
menuconfig SYSTEMCMDS_ACTUATOR_TEST |
||||
bool "actuator_test" |
||||
default n |
||||
---help--- |
||||
Enable support for actuator_test |
@ -0,0 +1,194 @@
@@ -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 <drivers/drv_hrt.h> |
||||
#include <px4_platform_common/getopt.h> |
||||
#include <px4_platform_common/log.h> |
||||
#include <px4_platform_common/module.h> |
||||
#include <uORB/Publication.hpp> |
||||
#include <uORB/topics/actuator_test.h> |
||||
#include <math.h> |
||||
|
||||
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_s> 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; |
||||
} |
Loading…
Reference in new issue