forked from Archive/PX4-Autopilot
mixer_module: add testing for SYS_CTRL_ALLOC=1 with actuator_test cmd+uorb msg
This commit is contained in:
Beat Küng
Daniel Agar
parent
36296794c8
commit
6fdcc43ea8
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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;
|
||||
};
|
||||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -33,6 +33,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "actuator_test.hpp"
|
||||
#include "functions.hpp"
|
||||
|
||||
#include <board_config.h>
|
||||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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 @@
|
|||
menuconfig SYSTEMCMDS_ACTUATOR_TEST
|
||||
bool "actuator_test"
|
||||
default n
|
||||
---help---
|
||||
Enable support for actuator_test
|
||||
|
|
@ -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