diff --git a/src/drivers/dshot/dshot.cpp b/src/drivers/dshot/dshot.cpp index 52b761c1f4..ee6105e465 100644 --- a/src/drivers/dshot/dshot.cpp +++ b/src/drivers/dshot/dshot.cpp @@ -163,6 +163,10 @@ public: */ int sendCommandThreadSafe(dshot_command_t command, int num_repetitions, int motor_index); + void retrieveAndPrintESCInfoThreadSafe(int motor_index); + + bool telemetryEnabled() const { return _telemetry != nullptr; } + private: enum class DShotConfig { Disabled = 0, @@ -191,12 +195,17 @@ private: void initTelemetry(const char *device); void handleNewTelemetryData(int motor_index, const DShotTelemetry::EscData &data); + int requestESCInfo(); + MixingOutput _mixing_output{*this, MixingOutput::SchedulingPolicy::Auto, false, false}; Telemetry *_telemetry{nullptr}; static char _telemetry_device[20]; static px4::atomic_bool _request_telemetry_init; + px4::atomic _request_esc_info{nullptr}; + bool _waiting_for_esc_info{false}; + Mode _mode{MODE_NONE}; uORB::Subscription _param_sub{ORB_ID(parameter_update)}; @@ -615,12 +624,53 @@ int DShotOutput::sendCommandThreadSafe(dshot_command_t command, int num_repetiti // wait until main thread processed it while (_new_command.load()) { - usleep(1000); + px4_usleep(1000); } return 0; } +void DShotOutput::retrieveAndPrintESCInfoThreadSafe(int motor_index) +{ + if (_request_esc_info.load() != nullptr) { + // already in progress (not expected to ever happen) + return; + } + + DShotTelemetry::OutputBuffer output_buffer; + output_buffer.motor_index = motor_index; + // start the request + _request_esc_info.store(&output_buffer); + + // wait until processed + int max_time = 1000; + + while (_request_esc_info.load() != nullptr && max_time-- > 0) { + px4_usleep(1000); + } + + _request_esc_info.store(nullptr); // just in case we time out... + + if (output_buffer.buf_pos == 0) { + PX4_ERR("No data received. If telemetry is setup correctly, try again"); + return; + } + + DShotTelemetry::decodeAndPrintEscInfoPacket(output_buffer); +} + +int DShotOutput::requestESCInfo() +{ + _telemetry->handler.redirectOutput(*_request_esc_info.load()); + _waiting_for_esc_info = true; + int motor_index = _mixing_output.reorderedMotorIndex(_request_esc_info.load()->motor_index); + _current_command.motor_mask = 1 << motor_index; + _current_command.num_repetitions = 1; + _current_command.command = DShot_cmd_esc_info; + PX4_DEBUG("Requesting ESC info for motor %i", motor_index); + return motor_index; +} + void DShotOutput::mixerChanged() { updateTelemetryNumMotors(); @@ -636,7 +686,14 @@ void DShotOutput::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS int requested_telemetry_index = -1; if (_telemetry) { - requested_telemetry_index = _mixing_output.reorderedMotorIndex(_telemetry->handler.getRequestMotorIndex()); + // check for an ESC info request. We only process it when we're not expecting other telemetry data + if (_request_esc_info.load() != nullptr && !_waiting_for_esc_info && stop_motors + && !_telemetry->handler.expectingData() && !_current_command.valid()) { + requested_telemetry_index = requestESCInfo(); + + } else { + requested_telemetry_index = _mixing_output.reorderedMotorIndex(_telemetry->handler.getRequestMotorIndex()); + } } if (stop_motors) { @@ -697,7 +754,14 @@ DShotOutput::Run() if (_telemetry) { int telem_update = _telemetry->handler.update(); - if (telem_update >= 0) { + // Are we waiting for ESC info? + if (_waiting_for_esc_info) { + if (telem_update != -1) { + _request_esc_info.store(nullptr); + _waiting_for_esc_info = false; + } + + } else if (telem_update >= 0) { handleNewTelemetryData(telem_update, _telemetry->handler.latestESCData()); } } @@ -1349,8 +1413,27 @@ int DShotOutput::custom_command(int argc, char *argv[]) } } + if (!strcmp(verb, "esc_info")) { + if (!is_running()) { + PX4_ERR("module not running"); + return -1; + } + + if (motor_index == -1) { + PX4_ERR("No motor index specified"); + return -1; + } + + if (!get_instance()->telemetryEnabled()) { + PX4_ERR("Telemetry is not enabled, but required to get ESC info"); + return -1; + } + + get_instance()->retrieveAndPrintESCInfoThreadSafe(motor_index); + return 0; + } + - /* start the FMU if not running */ if (!is_running()) { int ret = DShotOutput::task_spawn(argc, argv); @@ -1511,6 +1594,9 @@ After saving, the reversed direction will be regarded as the normal one. So to r 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_COMMAND_DESCR("esc_info", "Request ESC information"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based)", false); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; @@ -1569,6 +1655,7 @@ int DShotOutput::print_status() _mixing_output.printStatus(); if (_telemetry) { PX4_INFO("telemetry on: %s", _telemetry_device); + _telemetry->handler.printStatus(); } return 0; diff --git a/src/drivers/dshot/telemetry.cpp b/src/drivers/dshot/telemetry.cpp index 50823a318f..c263bb3301 100644 --- a/src/drivers/dshot/telemetry.cpp +++ b/src/drivers/dshot/telemetry.cpp @@ -59,6 +59,8 @@ int DShotTelemetry::init(const char *uart_device) return -errno; } + _num_timeouts = 0; + _num_successful_responses = 0; _current_motor_index_request = -1; return setBaudrate(DSHOT_TELEMETRY_UART_BAUDRATE); } @@ -71,6 +73,20 @@ void DShotTelemetry::deinit() } } +int DShotTelemetry::redirectOutput(OutputBuffer &buffer) +{ + if (expectingData()) { + // Error: cannot override while we already expect data + return -EBUSY; + } + + _current_motor_index_request = buffer.motor_index; + _current_request_start = hrt_absolute_time(); + _redirect_output = &buffer; + _redirect_output->buf_pos = 0; + return 0; +} + int DShotTelemetry::update() { if (_uart_fd < 0) { @@ -94,8 +110,18 @@ int DShotTelemetry::update() const hrt_abstime now = hrt_absolute_time(); if (_current_request_start > 0 && now - _current_request_start > 30_ms) { - PX4_DEBUG("ESC telemetry timeout for motor %i (frame pos=%i)", _current_motor_index_request, _frame_position); + if (_redirect_output) { + // clear and go back to internal buffer + _redirect_output = nullptr; + _current_motor_index_request = -1; + + } else { + PX4_DEBUG("ESC telemetry timeout for motor %i (frame pos=%i)", _current_motor_index_request, _frame_position); + ++_num_timeouts; + } + requestNextMotor(); + return -2; } return -1; @@ -107,19 +133,38 @@ int DShotTelemetry::update() int num_read = read(_uart_fd, buf, buf_length); ret = -1; - for (int i = 0; i < num_read; ++i) { - if (decodeByte(buf[i])) { - ret = _current_motor_index_request; - requestNextMotor(); + for (int i = 0; i < num_read && ret == -1; ++i) { + if (_redirect_output) { + _redirect_output->buffer[_redirect_output->buf_pos++] = buf[i]; + + if (_redirect_output->buf_pos == sizeof(_redirect_output->buffer)) { + // buffer full: return & go back to internal buffer + _redirect_output = nullptr; + ret = _current_motor_index_request; + _current_motor_index_request = -1; + requestNextMotor(); + } + + } else { + bool successful_decoding; + + if (decodeByte(buf[i], successful_decoding)) { + if (successful_decoding) { + ret = _current_motor_index_request; + } + + requestNextMotor(); + } } } return ret; } -bool DShotTelemetry::decodeByte(uint8_t byte) +bool DShotTelemetry::decodeByte(uint8_t byte, bool &successful_decoding) { _frame_buffer[_frame_position++] = byte; + successful_decoding = false; if (_frame_position == ESC_FRAME_SIZE) { PX4_DEBUG("got ESC frame for motor %i", _current_motor_index_request); @@ -136,6 +181,8 @@ bool DShotTelemetry::decodeByte(uint8_t byte) PX4_DEBUG("Motor %i: temp=%i, V=%i, cur=%i, consumpt=%i, rpm=%i", _current_motor_index_request, _latest_data.temperature, _latest_data.voltage, _latest_data.current, _latest_data.consumption, _latest_data.erpm); + ++_num_successful_responses; + successful_decoding = true; } return true; @@ -144,6 +191,12 @@ bool DShotTelemetry::decodeByte(uint8_t byte) return false; } +void DShotTelemetry::printStatus() const +{ + PX4_INFO("Number of successful ESC frames: %i", _num_successful_responses); + PX4_INFO("Number of timeouts: %i", _num_timeouts); +} + uint8_t DShotTelemetry::updateCrc8(uint8_t crc, uint8_t crc_seed) { uint8_t crc_u = crc ^ crc_seed; @@ -186,6 +239,163 @@ int DShotTelemetry::getRequestMotorIndex() return _current_motor_index_request; } +void DShotTelemetry::decodeAndPrintEscInfoPacket(const OutputBuffer &buffer) +{ + static constexpr int version_position = 12; + const uint8_t *data = buffer.buffer; + + if (buffer.buf_pos < version_position) { + PX4_ERR("Not enough data received"); + return; + } + + enum class ESCVersionInfo { + BLHELI32, + KissV1, + KissV2, + }; + ESCVersionInfo version; + int packet_length; + + if (data[version_position] == 254) { + version = ESCVersionInfo::BLHELI32; + packet_length = esc_info_size_blheli32; + + } else if (data[version_position] == 255) { + version = ESCVersionInfo::KissV2; + packet_length = esc_info_size_kiss_v2; + + } else { + version = ESCVersionInfo::KissV1; + packet_length = esc_info_size_kiss_v1; + } + + if (buffer.buf_pos != packet_length) { + PX4_ERR("Packet length mismatch (%i != %i)", buffer.buf_pos, packet_length); + return; + } + + if (DShotTelemetry::crc8(data, packet_length - 1) != data[packet_length - 1]) { + PX4_ERR("Checksum mismatch"); + return; + } + + uint8_t esc_firmware_version = 0; + uint8_t esc_firmware_subversion = 0; + uint8_t esc_type = 0; + + switch (version) { + case ESCVersionInfo::KissV1: + esc_firmware_version = data[12]; + esc_firmware_subversion = (data[13] & 0x1f) + 97; + esc_type = (data[13] & 0xe0) >> 5; + break; + + case ESCVersionInfo::KissV2: + case ESCVersionInfo::BLHELI32: + esc_firmware_version = data[13]; + esc_firmware_subversion = data[14]; + esc_type = data[15]; + break; + } + + const char *esc_type_str = ""; + + switch (version) { + case ESCVersionInfo::KissV1: + case ESCVersionInfo::KissV2: + switch (esc_type) { + case 1: esc_type_str = "KISS8A"; + break; + + case 2: esc_type_str = "KISS16A"; + break; + + case 3: esc_type_str = "KISS24A"; + break; + + case 5: esc_type_str = "KISS Ultralite"; + break; + + default: esc_type_str = "KISS (unknown)"; + break; + } + + break; + + case ESCVersionInfo::BLHELI32: { + char *esc_type_mutable = (char *)(data + 31); + esc_type_mutable[32] = 0; + esc_type_str = esc_type_mutable; + } + break; + } + + PX4_INFO("ESC Type: %s", esc_type_str); + + PX4_INFO("MCU Serial Number: %02x%02x%02x-%02x%02x%02x-%02x%02x%02x-%02x%02x%02x", + data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7], data[8], + data[9], data[10], data[11]); + + switch (version) { + case ESCVersionInfo::KissV1: + case ESCVersionInfo::KissV2: + PX4_INFO("Firmware version: %d.%d%c", esc_firmware_version / 100, esc_firmware_version % 100, + (char)esc_firmware_subversion); + break; + + case ESCVersionInfo::BLHELI32: + PX4_INFO("Firmware version: %d.%d", esc_firmware_version, esc_firmware_subversion); + break; + } + + if (version == ESCVersionInfo::KissV2 || version == ESCVersionInfo::BLHELI32) { + PX4_INFO("Rotation Direction: %s", data[16] ? "reversed" : "normal"); + PX4_INFO("3D Mode: %s", data[17] ? "on" : "off"); + } + + if (version == ESCVersionInfo::BLHELI32) { + uint8_t setting = data[18]; + + switch (setting) { + case 0: + PX4_INFO("Low voltage Limit: off"); + break; + + case 255: + PX4_INFO("Low voltage Limit: unsupported"); + break; + + default: + PX4_INFO("Low voltage Limit: %d.%01d V", setting / 10, setting % 10); + break; + } + + setting = data[19]; + + switch (setting) { + case 0: + PX4_INFO("Current Limit: off"); + break; + + case 255: + PX4_INFO("Current Limit: unsupported"); + break; + + default: + PX4_INFO("Current Limit: %d A", setting); + break; + } + + for (int i = 0; i < 4; ++i) { + setting = data[i + 20]; + PX4_INFO("LED %d: %s", i, setting ? (setting == 255 ? "unsupported" : "on") : "off"); + } + } + + +} + int DShotTelemetry::setBaudrate(unsigned baud) { int speed; diff --git a/src/drivers/dshot/telemetry.h b/src/drivers/dshot/telemetry.h index 4373a8079d..a5c549c476 100644 --- a/src/drivers/dshot/telemetry.h +++ b/src/drivers/dshot/telemetry.h @@ -47,6 +47,17 @@ public: int16_t erpm; ///< [100ERPM] }; + static constexpr int esc_info_size_blheli32 = 64; + static constexpr int esc_info_size_kiss_v1 = 15; + static constexpr int esc_info_size_kiss_v2 = 21; + static constexpr int max_esc_info_size = esc_info_size_blheli32; + + struct OutputBuffer { + uint8_t buffer[max_esc_info_size]; + int buf_pos{0}; + int motor_index; + }; + ~DShotTelemetry(); int init(const char *uart_device); @@ -58,10 +69,22 @@ public: /** * Read telemetry from the UART (non-blocking) and handle timeouts. - * @return -1 if no update, >= 0 for the motor index. Use @latestESCData() to get the data. + * @return -1 if no update, -2 timeout, >= 0 for the motor index. Use @latestESCData() to get the data. */ int update(); + /** + * Redirect everything that is read into a different buffer. + * Future calls to @update will write to that instead of an internal buffer, until @update returns + * a value different from -1. No decoding is done. + * The caller must ensure the buffer exists until that point. + * @param buffer + * @return 0 on success <0 on error + */ + int redirectOutput(OutputBuffer &buffer); + + bool redirectActive() const { return _redirect_output != nullptr; } + /** * Get the motor index for which telemetry should be requested. * @return -1 if no request should be made, motor index otherwise @@ -70,6 +93,15 @@ public: const EscData &latestESCData() const { return _latest_data; } + /** + * Check whether we are currently expecting to read new data from an ESC + */ + bool expectingData() const { return _current_request_start != 0; } + + void printStatus() const; + + static void decodeAndPrintEscInfoPacket(const OutputBuffer &buffer); + private: static constexpr int ESC_FRAME_SIZE = 10; @@ -82,7 +114,13 @@ private: void requestNextMotor(); - bool decodeByte(uint8_t byte); + /** + * Decode a single byte from an ESC feedback frame + * @param byte + * @param successful_decoding set to true if checksum matches + * @return true if received the expected amount of bytes and the next motor can be requested + */ + bool decodeByte(uint8_t byte, bool &successful_decoding); static inline uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed); static uint8_t crc8(const uint8_t *buf, uint8_t len); @@ -96,4 +134,10 @@ private: int _current_motor_index_request{-1}; hrt_abstime _current_request_start{0}; + + OutputBuffer *_redirect_output{nullptr}; ///< if set, all read bytes are stored here instead of the internal buffer + + // statistics + int _num_timeouts{0}; + int _num_successful_responses{0}; };