From 26648ad0b951412ae834793a60af20c408e55c07 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 2 Sep 2019 20:40:56 +0200 Subject: [PATCH] dshot: add support to send commands via CLI This enables the possibility to permanently reverse motor directions :) --- src/drivers/dshot/dshot.cpp | 149 +++++++++++++++++++++++++++++++++++- 1 file changed, 148 insertions(+), 1 deletion(-) diff --git a/src/drivers/dshot/dshot.cpp b/src/drivers/dshot/dshot.cpp index 2004ed044d..52b761c1f4 100644 --- a/src/drivers/dshot/dshot.cpp +++ b/src/drivers/dshot/dshot.cpp @@ -154,6 +154,15 @@ public: void mixerChanged() override; + /** + * Send a dshot command to one or all motors + * This is expected to be called from another thread. + * @param num_repetitions number of times to repeat, set at least to 1 + * @param motor_index index or -1 for all + * @return 0 on success, <0 error otherwise + */ + int sendCommandThreadSafe(dshot_command_t command, int num_repetitions, int motor_index); + private: enum class DShotConfig { Disabled = 0, @@ -163,6 +172,15 @@ private: DShot1200 = 1200, }; + struct Command { + dshot_command_t command; + int num_repetitions{0}; + uint8_t motor_mask{0xff}; + + bool valid() const { return num_repetitions > 0; } + void clear() { num_repetitions = 0; } + }; + struct Telemetry { DShotTelemetry handler; uORB::PublicationData esc_status_pub{ORB_ID(esc_status)}; @@ -183,6 +201,8 @@ private: uORB::Subscription _param_sub{ORB_ID(parameter_update)}; + Command _current_command; + px4::atomic _new_command{nullptr}; unsigned _num_outputs{0}; int _class_instance{-1}; @@ -578,6 +598,29 @@ void DShotOutput::handleNewTelemetryData(int motor_index, const DShotTelemetry:: _telemetry->last_motor_index = motor_index; } +int DShotOutput::sendCommandThreadSafe(dshot_command_t command, int num_repetitions, int motor_index) +{ + Command cmd; + cmd.command = command; + + if (motor_index == -1) { + cmd.motor_mask = 0xff; + + } else { + cmd.motor_mask = 1 << _mixing_output.reorderedMotorIndex(motor_index); + } + + cmd.num_repetitions = num_repetitions; + _new_command.store(&cmd); + + // wait until main thread processed it + while (_new_command.load()) { + usleep(1000); + } + + return 0; +} + void DShotOutput::mixerChanged() { updateTelemetryNumMotors(); @@ -598,13 +641,28 @@ void DShotOutput::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS if (stop_motors) { + // when motors are stopped we check if we have other commands to send for (int i = 0; i < (int)num_outputs; i++) { - up_dshot_motor_command(i, DShot_cmd_motor_stop, i == requested_telemetry_index); + if (_current_command.valid() && (_current_command.motor_mask & (1 << i))) { + // for some reason we need to always request telemetry when sending a command + up_dshot_motor_command(i, _current_command.command, true); + + } else { + up_dshot_motor_command(i, DShot_cmd_motor_stop, i == requested_telemetry_index); + } } + + if (_current_command.valid()) { + --_current_command.num_repetitions; + } + } 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); } + + // clear commands when motors are running + _current_command.clear(); } if (stop_motors || num_control_groups_updated > 0) { @@ -654,6 +712,16 @@ DShotOutput::Run() _request_telemetry_init.store(false); } + // new command? + if (!_current_command.valid()) { + Command *new_command = _new_command.load(); + + if (new_command) { + _current_command = *new_command; + _new_command.store(nullptr); + } + } + // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) _mixing_output.updateSubscriptions(true); @@ -1235,6 +1303,53 @@ int DShotOutput::custom_command(int argc, char *argv[]) return 0; } + int motor_index = -1; // select motor index, default: -1=all + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "m:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'm': + motor_index = strtol(myoptarg, nullptr, 10) - 1; + break; + + default: + return print_usage("unrecognized flag"); + } + } + + struct Command { + const char *name; + dshot_command_t command; + int num_repetitions; + }; + + constexpr Command commands[] = { + {"reverse", DShot_cmd_spin_direction_reversed, 10}, + {"normal", DShot_cmd_spin_direction_normal, 10}, + {"save", DShot_cmd_save_settings, 10}, + {"3d_on", DShot_cmd_3d_mode_on, 10}, + {"3d_off", DShot_cmd_3d_mode_off, 10}, + {"beep1", DShot_cmd_beacon1, 1}, + {"beep2", DShot_cmd_beacon2, 1}, + {"beep3", DShot_cmd_beacon3, 1}, + {"beep4", DShot_cmd_beacon4, 1}, + {"beep5", DShot_cmd_beacon5, 1}, + }; + + for (unsigned i = 0; i < sizeof(commands) / sizeof(commands[0]); ++i) { + if (!strcmp(verb, commands[i].name)) { + if (!is_running()) { + PX4_ERR("module not running"); + return -1; + } + + return get_instance()->sendCommandThreadSafe(commands[i].command, commands[i].num_repetitions, motor_index); + } + } + + /* start the FMU if not running */ if (!is_running()) { int ret = DShotOutput::task_spawn(argc, argv); @@ -1333,6 +1448,16 @@ int DShotOutput::print_usage(const char *reason) This is the DShot output driver. It is similar to the fmu driver, and can be used as drop-in replacement to use DShot as ESC communication protocol instead of PWM. +It supports: +- DShot150, DShot300, DShot600, DShot1200 +- telemetry via separate UART and publishing as esc_status message +- sending DShot commands via CLI + +### Examples +Permanently reverse motor 1: +$ dshot reverse -m 1 +$ dshot save -m 1 +After saving, the reversed direction will be regarded as the normal one. So to reverse again repeat the same commands. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("dshot", "driver"); @@ -1364,6 +1489,28 @@ to use DShot as ESC communication protocol instead of PWM. PRINT_MODULE_USAGE_COMMAND_DESCR("telemetry", "Enable Telemetry on a UART"); PRINT_MODULE_USAGE_ARG("", "UART device", false); + // DShot commands + PRINT_MODULE_USAGE_COMMAND_DESCR("reverse", "Reverse motor direction"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("normal", "Normal motor direction"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("save", "Save current settings"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("3d_on", "Enable 3D mode"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("3d_off", "Disable 3D mode"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("beep1", "Send Beep pattern 1"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("beep2", "Send Beep pattern 2"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("beep3", "Send Beep pattern 3"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("beep4", "Send Beep pattern 4"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("beep5", "Send Beep pattern 5"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0;