mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
dshot telemetry: retrieve & print ESC info with 'dshot esc_info' CLI command
This commit is contained in:
Beat Küng
parent
26648ad0b9
commit
e78250ab8d
@ -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<DShotTelemetry::OutputBuffer *> _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;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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};
|
||||
};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user