From 0871802568cffe7ee05b4243110a97c53c98b44a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 15 Oct 2019 18:56:52 +0200 Subject: [PATCH] mixer_module: add support for test_motor (motor_test CLI command) --- boards/px4/sitl/default.cmake | 1 + msg/test_motor.msg | 6 ++ src/drivers/dshot/dshot.cpp | 19 ++-- src/drivers/px4fmu/fmu.cpp | 2 + src/drivers/tap_esc/tap_esc.cpp | 9 +- src/drivers/uavcan/uavcan_main.cpp | 2 +- src/lib/mixer_module/mixer_module.cpp | 107 +++++++++++++++++++---- src/lib/mixer_module/mixer_module.hpp | 16 +++- src/systemcmds/motor_test/motor_test.cpp | 53 +++++++---- 9 files changed, 170 insertions(+), 45 deletions(-) diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 98b058455e..8cade4ed94 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -56,6 +56,7 @@ px4_add_board( led_control mixer motor_ramp + motor_test #mtd #nshterm param diff --git a/msg/test_motor.msg b/msg/test_motor.msg index 55bfd3f8f2..133d97a901 100644 --- a/msg/test_motor.msg +++ b/msg/test_motor.msg @@ -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 diff --git a/src/drivers/dshot/dshot.cpp b/src/drivers/dshot/dshot.cpp index 1b32267270..90f0dd13f4 100644 --- a/src/drivers/dshot/dshot.cpp +++ b/src/drivers/dshot/dshot.cpp @@ -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)); } diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 90582eb142..1166855248 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -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; diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp index c5c1068baf..e738a1afc9 100644 --- a/src/drivers/tap_esc/tap_esc.cpp +++ b/src/drivers/tap_esc/tap_esc.cpp @@ -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]); } diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 42320559ae..6297468f56 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -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); } diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 6ca7595f29..5f4a3de00e 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -34,6 +34,7 @@ #include "mixer_module.hpp" #include +#include #include 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_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(_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) { diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 31f04ceca0..cc2039e298 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -51,6 +51,7 @@ #include #include #include +#include /** @@ -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; diff --git a/src/systemcmds/motor_test/motor_test.cpp b/src/systemcmds/motor_test/motor_test.cpp index 0426316212..8060b33a5e 100644 --- a/src/systemcmds/motor_test/motor_test.cpp +++ b/src/systemcmds/motor_test/motor_test.cpp @@ -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_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); } }