dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mixer_module: add support for test_motor (motor_test CLI command)

This commit is contained in:
Beat Küng 2019-10-15 18:56:52 +02:00
parent 349469cf75
commit 0871802568
9 changed files with 170 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
boards/px4/sitl/default.cmake
View File

@ -56,6 +56,7 @@ px4_add_board(
led_control
mixer
motor_ramp
motor_test
#mtd
#nshterm
param

6
msg/test_motor.msg
View File

@ -1,7 +1,13 @@
uint64 timestamp # time since system start (microseconds)
uint8 NUM_MOTOR_OUTPUTS = 8
uint8 ACTION_STOP = 0 # stop motors (disable motor test mode)
uint8 ACTION_RUN = 1 # run motors (enable motor test mode)
uint8 action # one of ACTION_* (applies to all motors)
uint32 motor_number # number of motor to spin
float32 value # output power, range [0..1]
uint8 driver_instance # select output driver (for boards with multiple outputs, like IO+FMU)
uint8 ORB_QUEUE_LENGTH = 4

19
src/drivers/dshot/dshot.cpp
View File

@ -165,6 +165,8 @@ public:
bool telemetryEnabled() const { return _telemetry != nullptr; }
private:
static constexpr uint16_t DISARMED_VALUE = 0;
enum class DShotConfig {
Disabled = 0,
DShot150 = 150,
@ -248,8 +250,8 @@ DShotOutput::DShotOutput() :
_cycle_perf(perf_alloc(PC_ELAPSED, "dshot: cycle")),
_cycle_interval_perf(perf_alloc(PC_INTERVAL, "dshot: cycle interval"))
{
_mixing_output.setAllDisarmedValues(0);
_mixing_output.setAllMinValues(1);
_mixing_output.setAllDisarmedValues(DISARMED_VALUE);
_mixing_output.setAllMinValues(DISARMED_VALUE + 1);
_mixing_output.setAllMaxValues(DSHOT_MAX_THROTTLE);
}
@ -286,6 +288,8 @@ DShotOutput::init()
PX4_ERR("FAILED registering class device");
}
_mixing_output.setDriverInstance(_class_instance);
// Getting initial parameter values
update_params();
@ -721,7 +725,12 @@ void DShotOutput::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS
} else {
for (int i = 0; i < (int)num_outputs; i++) {
up_dshot_motor_data_set(i, math::min(outputs[i], (uint16_t)DSHOT_MAX_THROTTLE), i == requested_telemetry_index);
if (outputs[i] == DISARMED_VALUE) {
up_dshot_motor_command(i, DShot_cmd_motor_stop, i == requested_telemetry_index);
} else {
up_dshot_motor_data_set(i, math::min(outputs[i], (uint16_t)DSHOT_MAX_THROTTLE), i == requested_telemetry_index);
}
}
// clear commands when motors are running
@ -805,8 +814,8 @@ void DShotOutput::update_params()
updateParams();
// we use a minimum value of 1, since 0 is for disarmed
_mixing_output.setAllMinValues(math::constrain((int)(_param_dshot_min.get() * (float)DSHOT_MAX_THROTTLE), 1,
DSHOT_MAX_THROTTLE));
_mixing_output.setAllMinValues(math::constrain((int)(_param_dshot_min.get() * (float)DSHOT_MAX_THROTTLE),
DISARMED_VALUE + 1, DSHOT_MAX_THROTTLE));
}

2
src/drivers/px4fmu/fmu.cpp
View File

@ -253,6 +253,8 @@ PX4FMU::init()
PX4_ERR("FAILED registering class device");
}
_mixing_output.setDriverInstance(_class_instance);
/* force a reset of the update rate */
_current_update_rate = 0;

9
src/drivers/tap_esc/tap_esc.cpp
View File

@ -496,7 +496,14 @@ void TAP_ESC::cycle()
if (test_motor_updated) {
struct test_motor_s test_motor;
orb_copy(ORB_ID(test_motor), _test_motor_sub, &test_motor);
_outputs.output[test_motor.motor_number] = RPMSTOPPED + ((RPMMAX - RPMSTOPPED) * test_motor.value);
if (test_motor.action == test_motor_s::ACTION_STOP) {
_outputs.output[test_motor.motor_number] = RPMSTOPPED;
} else {
_outputs.output[test_motor.motor_number] = RPMSTOPPED + ((RPMMAX - RPMSTOPPED) * test_motor.value);
}
PX4_INFO("setting motor %i to %.1lf", test_motor.motor_number,
(double)_outputs.output[test_motor.motor_number]);
}

2
src/drivers/uavcan/uavcan_main.cpp
View File

@ -971,7 +971,7 @@ int UavcanNode::run()
orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor);
// Update the test status and check that we're not locked down
_test_in_progress = (_test_motor.value > 0);
_test_in_progress = (_test_motor.action == test_motor_s::ACTION_RUN);
_esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress);
}

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

@ -34,6 +34,7 @@
#include "mixer_module.hpp"
#include <lib/circuit_breaker/circuit_breaker.h>
#include <uORB/PublicationQueued.hpp>
#include <px4_log.h>
using namespace time_literals;
@ -72,6 +73,12 @@ _control_latency_perf(perf_alloc(PC_ELAPSED, "control latency"))
#endif
px4_sem_init(&_lock, 0, 1);
// Enforce the existence of the test_motor topic, so we won't miss initial publications
test_motor_s test{};
uORB::PublicationQueued<test_motor_s> test_motor_pub{ORB_ID(test_motor)};
test_motor_pub.publish(test);
_motor_test.test_motor_sub.subscribe();
}
MixingOutput::~MixingOutput()
@ -86,12 +93,14 @@ void MixingOutput::printStatus() const
perf_print_counter(_control_latency_perf);
PX4_INFO("Switched to rate_ctrl work queue: %i", (int)_wq_switched);
PX4_INFO("Mixer loaded: %s", _mixers ? "yes" : "no");
PX4_INFO("Driver instance: %i", _driver_instance);
PX4_INFO("Channel Configuration:");
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
PX4_INFO("Channel %i: failsafe: %d, disarmed: %d, min: %d, max: %d", i, _failsafe_value[i], _disarmed_value[i],
_min_value[i], _max_value[i]);
for (unsigned i = 0; i < _max_num_outputs; i++) {
int reordered_i = reorderedMotorIndex(i);
PX4_INFO("Channel %i: value: %i, failsafe: %d, disarmed: %d, min: %d, max: %d", reordered_i, _current_output_value[i],
_failsafe_value[reordered_i], _disarmed_value[reordered_i], _min_value[reordered_i], _max_value[reordered_i]);
}
}
@ -231,6 +240,44 @@ void MixingOutput::updateOutputSlewrate()
_mixers->set_max_delta_out_once(delta_out_max);
}
unsigned MixingOutput::motorTest()
{
test_motor_s test_motor;
bool had_update = false;
while (_motor_test.test_motor_sub.update(&test_motor)) {
if (test_motor.driver_instance != _driver_instance ||
hrt_elapsed_time(&test_motor.timestamp) > 100_ms) {
continue;
}
bool in_test_mode = test_motor.action == test_motor_s::ACTION_RUN;
if (in_test_mode != _motor_test.in_test_mode) {
// reset all outputs to disarmed on state change
for (int i = 0; i < MAX_ACTUATORS; ++i) {
_current_output_value[i] = _disarmed_value[i];
}
}
if (in_test_mode) {
int idx = test_motor.motor_number;
if (idx < MAX_ACTUATORS) {
_current_output_value[reorderedMotorIndex(idx)] =
math::constrain<uint16_t>(_min_value[idx] + (uint16_t)((_max_value[idx] - _min_value[idx]) * test_motor.value),
_min_value[idx], _max_value[idx]);
}
}
_motor_test.in_test_mode = in_test_mode;
had_update = true;
}
return (_motor_test.in_test_mode || had_update) ? _max_num_outputs : 0;
}
bool MixingOutput::update()
{
if (!_mixers) {
@ -250,6 +297,24 @@ bool MixingOutput::update()
/* Update the armed status and check that we're not locked down.
* We also need to arm throttle for the ESC calibration. */
_throttle_armed = (_safety_off && _armed.armed && !_armed.lockdown) || (_safety_off && _armed.in_esc_calibration_mode);
if (_armed.armed) {
_motor_test.in_test_mode = false;
}
}
// check for motor test
if (!_armed.armed) {
unsigned num_motor_test = motorTest();
if (num_motor_test > 0) {
_interface.updateOutputs(false, _current_output_value, num_motor_test, 1);
actuator_outputs_s actuator_outputs{};
setAndPublishActuatorOutputs(num_motor_test, actuator_outputs);
checkSafetyButton();
handleCommands();
return true;
}
}
if (_param_mot_slew_max.get() > FLT_EPSILON) {
@ -285,15 +350,13 @@ bool MixingOutput::update()
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 */
uint16_t output_limited[MAX_ACTUATORS] {};
output_limit_calc(_throttle_armed, armNoThrottle(), mixed_num_outputs, _reverse_output_mask,
_disarmed_value, _min_value, _max_value, outputs, output_limited, &_output_limit);
_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 < mixed_num_outputs; i++) {
output_limited[i] = _failsafe_value[i];
_current_output_value[i] = _failsafe_value[i];
}
}
@ -302,28 +365,21 @@ bool MixingOutput::update()
/* overwrite outputs in case of lockdown or parachute triggering with disarmed values */
if (_armed.lockdown || _armed.manual_lockdown) {
for (size_t i = 0; i < mixed_num_outputs; i++) {
output_limited[i] = _disarmed_value[i];
_current_output_value[i] = _disarmed_value[i];
}
stop_motors = true;
}
/* apply _param_mot_ordering */
reorderOutputs(output_limited);
reorderOutputs(_current_output_value);
/* now return the outputs to the driver */
_interface.updateOutputs(stop_motors, output_limited, mixed_num_outputs, n_updates);
_interface.updateOutputs(stop_motors, _current_output_value, mixed_num_outputs, n_updates);
actuator_outputs_s actuator_outputs{};
actuator_outputs.noutputs = mixed_num_outputs;
for (size_t i = 0; i < mixed_num_outputs; ++i) {
actuator_outputs.output[i] = output_limited[i];
}
actuator_outputs.timestamp = hrt_absolute_time();
_outputs_pub.publish(actuator_outputs);
setAndPublishActuatorOutputs(mixed_num_outputs, actuator_outputs);
publishMixerStatus(actuator_outputs);
updateLatencyPerfCounter(actuator_outputs);
@ -346,6 +402,19 @@ MixingOutput::checkSafetyButton()
}
}
void
MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs)
{
actuator_outputs.noutputs = num_outputs;
for (size_t i = 0; i < num_outputs; ++i) {
actuator_outputs.output[i] = _current_output_value[i];
}
actuator_outputs.timestamp = hrt_absolute_time();
_outputs_pub.publish(actuator_outputs);
}
void
MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs)
{
@ -404,7 +473,7 @@ MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS])
*/
}
int MixingOutput::reorderedMotorIndex(int index)
int MixingOutput::reorderedMotorIndex(int index) const
{
if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) {
switch (index) {

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

@ -51,6 +51,7 @@
#include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/test_motor.h>
/**
@ -102,6 +103,8 @@ public:
~MixingOutput();
void setDriverInstance(uint8_t instance) { _driver_instance = instance; }
void printStatus() const;
/**
@ -163,7 +166,7 @@ public:
* @param index motor index in [0, num_motors-1]
* @return reordered motor index. When out of range, the input index is returned
*/
int reorderedMotorIndex(int index);
int reorderedMotorIndex(int index) const;
void setIgnoreLockdown(bool ignore_lockdown) { _ignore_lockdown = ignore_lockdown; }
@ -178,8 +181,11 @@ private:
return (_armed.prearmed && !_armed.armed) || _armed.in_esc_calibration_mode;
}
unsigned motorTest();
void updateOutputSlewrate();
void checkSafetyButton();
void setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs);
void publishMixerStatus(const actuator_outputs_s &actuator_outputs);
void updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs);
@ -218,6 +224,7 @@ private:
uint16_t _disarmed_value[MAX_ACTUATORS] {};
uint16_t _min_value[MAX_ACTUATORS] {};
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;
@ -246,8 +253,15 @@ private:
const bool _support_esc_calibration;
bool _wq_switched{false};
uint8_t _driver_instance{0}; ///< for boards that supports multiple outputs (e.g. PX4IO + FMU)
const uint8_t _max_num_outputs;
struct MotorTest {
uORB::Subscription test_motor_sub{ORB_ID(test_motor)};
bool in_test_mode{false};
};
MotorTest _motor_test;
OutputModuleInterface &_interface;
perf_counter_t _control_latency_perf;

53
src/systemcmds/motor_test/motor_test.cpp
View File

@ -48,20 +48,27 @@
extern "C" __EXPORT int motor_test_main(int argc, char *argv[]);
static void motor_test(unsigned channel, float value);
static void motor_test(unsigned channel, float value, uint8_t driver_instance);
static void usage(const char *reason);
void motor_test(unsigned channel, float value)
void motor_test(unsigned channel, float value, uint8_t driver_instance)
{
test_motor_s test_motor{};
test_motor.timestamp = hrt_absolute_time();
test_motor.motor_number = channel;
test_motor.value = value;
test_motor.action = value >= 0.f ? test_motor_s::ACTION_RUN : test_motor_s::ACTION_STOP;
test_motor.driver_instance = driver_instance;
uORB::PublicationQueued<test_motor_s> test_motor_pub{ORB_ID(test_motor)};
test_motor_pub.publish(test_motor);
PX4_INFO("motor %d set to %.2f", channel, (double)value);
if (test_motor.action == test_motor_s::ACTION_STOP) {
PX4_INFO("motors stop command sent");
} else {
PX4_INFO("motor %d set to %.2f", channel, (double)value);
}
}
static void usage(const char *reason)
@ -70,15 +77,20 @@ static void usage(const char *reason)
PX4_WARN("%s", reason);
}
PRINT_MODULE_DESCRIPTION("Utility to test motors.\n"
"\n"
"Note: this can only be used for drivers which support the motor_test uorb topic (currently uavcan and tap_esc)\n"
);
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
Utility to test motors.
WARNING: remove all props before using this command.
Note: this can only be used for drivers which support the motor_test uorb topic (not px4io).
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("motor_test", "command");
PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Set motor(s) to a specific output value");
PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 7, "Motor to test (0...7, all if not specified)", true);
PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 100, "Power (0...100)", true);
PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 4, "driver instance", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop all motors");
PRINT_MODULE_USAGE_COMMAND_DESCR("iterate", "Iterate all motors starting and stopping one after the other");
@ -89,14 +101,19 @@ int motor_test_main(int argc, char *argv[])
int channel = -1; //default to all channels
unsigned long lval;
float value = 0.0f;
uint8_t driver_instance = 0;
int ch;
int myoptind = 1;
const char *myoptarg = NULL;
while ((ch = px4_getopt(argc, argv, "m:p:", &myoptind, &myoptarg)) != EOF) {
while ((ch = px4_getopt(argc, argv, "i:m:p:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'i':
driver_instance = (uint8_t)strtol(myoptarg, NULL, 0);
break;
case 'm':
/* Read in motor number */
channel = (int)strtol(myoptarg, NULL, 0);
@ -124,17 +141,17 @@ int motor_test_main(int argc, char *argv[])
if (myoptind >= 0 && myoptind < argc) {
if (strcmp("stop", argv[myoptind]) == 0) {
channel = -1;
value = 0.f;
channel = 0;
value = -1.f;
} else if (strcmp("iterate", argv[myoptind]) == 0) {
value = 0.3f;
value = 0.15f;
for (int i = 0; i < 8; ++i) {
motor_test(i, value);
usleep(500000);
motor_test(i, 0.f);
usleep(10000);
motor_test(i, value, driver_instance);
px4_usleep(500000);
motor_test(i, -1.f, driver_instance);
px4_usleep(10000);
}
run_test = false;
@ -154,12 +171,12 @@ int motor_test_main(int argc, char *argv[])
if (run_test) {
if (channel < 0) {
for (int i = 0; i < 8; ++i) {
motor_test(i, value);
usleep(10000);
motor_test(i, value, driver_instance);
px4_usleep(10000);
}
} else {
motor_test(channel, value);
motor_test(channel, value, driver_instance);
}
}