mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-29 11:44:06 +08:00
Added all changes to DShot driver, needed to work with ESC Flasher module.
This commit is contained in:
MattKow-Firefly
parent
a7e93d1145
commit
8eb753068a
1
.vscode/settings.json
vendored
1
.vscode/settings.json
vendored
@ -129,4 +129,5 @@
|
||||
"yaml.schemas": {
|
||||
"${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
|
||||
},
|
||||
"C_Cpp.errorSquiggles": "disabled",
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -42,6 +42,11 @@
|
||||
|
||||
#include "DShotTelemetry.h"
|
||||
|
||||
#include <uORB/topics/esc_flasher_request.h>
|
||||
#include <uORB/topics/esc_flasher_request_ack.h>
|
||||
#include <uORB/topics/esc_flasher_status.h>
|
||||
#include <uORB/topics/esc_flasher_versions.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
#if !defined(DIRECT_PWM_OUTPUT_CHANNELS)
|
||||
@ -76,6 +81,8 @@ public:
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
void retrieve_and_print_esc_info_thread_safe(const int motor_index);
|
||||
|
||||
/**
|
||||
* Send a dshot command to one or all motors
|
||||
* This is expected to be called from another thread.
|
||||
@ -93,6 +100,9 @@ public:
|
||||
bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated) override;
|
||||
|
||||
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated);// override;
|
||||
|
||||
private:
|
||||
|
||||
/** Disallow copy construction and move assignment. */
|
||||
@ -121,7 +131,7 @@ private:
|
||||
|
||||
void enable_dshot_outputs(const bool enabled);
|
||||
|
||||
void init_telemetry(const char *device, bool swap_rxtx);
|
||||
void init_telemetry(const char *device);
|
||||
|
||||
int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data, bool ignore_rpm);
|
||||
|
||||
@ -129,6 +139,8 @@ private:
|
||||
|
||||
int handle_new_bdshot_erpm(void);
|
||||
|
||||
int request_esc_info();
|
||||
|
||||
void Run() override;
|
||||
|
||||
void update_params();
|
||||
@ -137,7 +149,13 @@ private:
|
||||
|
||||
void handle_vehicle_commands();
|
||||
|
||||
uint16_t convert_output_to_3d_scaling(uint16_t output);
|
||||
void handle_esc_flasher_requests();
|
||||
|
||||
void run_get_esc_info();
|
||||
|
||||
int retrieve_and_print_esc_info_non_blocking(const int motor_index);
|
||||
|
||||
int retrieve_and_print_esc_info_check_result(uint8_t* fw_ver_major, uint8_t* fw_ver_minor);
|
||||
|
||||
MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
uint32_t _reversible_outputs{};
|
||||
@ -147,31 +165,78 @@ private:
|
||||
uORB::PublicationMultiData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
|
||||
|
||||
static char _telemetry_device[20];
|
||||
static bool _telemetry_swap_rxtx;
|
||||
static px4::atomic_bool _request_telemetry_init;
|
||||
|
||||
px4::atomic<Command *> _new_command{nullptr};
|
||||
|
||||
px4::atomic<DShotTelemetry::OutputBuffer *> _request_esc_info{nullptr};
|
||||
|
||||
bool _outputs_initialized{false};
|
||||
bool _outputs_on{false};
|
||||
bool _waiting_for_esc_info{false};
|
||||
bool _bidirectional_dshot_enabled{false};
|
||||
|
||||
const char *esc_type_unknown = "Unknown";
|
||||
const char *esc_type_am32 = "AM32";
|
||||
const char *esc_type_blheli32 = "BLHeli32";
|
||||
const char *esc_type_am32_old = "AM32_Old";
|
||||
const char *esc_type_bluejay = "BlueJay";
|
||||
|
||||
const char* esc_types_strings[5] = {esc_type_unknown, esc_type_am32, esc_type_blheli32, esc_type_am32_old, esc_type_bluejay};
|
||||
|
||||
enum class ESCType {
|
||||
Unknown = 0,
|
||||
AM32 = 1,
|
||||
BLHELI32 = 2,
|
||||
AM32_Old = 3,
|
||||
BlueJay = 4
|
||||
};
|
||||
ESCType _esc_type{ESCType::Unknown};
|
||||
ESCType _esc_type_temp{ESCType::Unknown};
|
||||
|
||||
esc_flasher_request_s _esc_flasher_request{0};
|
||||
esc_flasher_request_ack_s _esc_flasher_response{0};
|
||||
uint32_t esc_flasher_esc_info_state{0};
|
||||
uint32_t esc_flasher_esc_info_motor_index;
|
||||
uint32_t esc_flasher_flashing_state{0};
|
||||
DShotTelemetry::OutputBuffer esc_flasher_output_buffer{};
|
||||
|
||||
static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS};
|
||||
uint32_t _output_mask{0};
|
||||
|
||||
int _num_motors{0};
|
||||
uint32_t _broken_esc{0};
|
||||
bool _broken_esc_flag{0};
|
||||
|
||||
// run_get_esc_info variables
|
||||
struct EscInfoSave {
|
||||
ESCType type;
|
||||
uint8_t fw_major;
|
||||
uint8_t fw_minor;
|
||||
};
|
||||
|
||||
EscInfoSave _esc_info_save[esc_status_s::CONNECTED_ESC_MAX];
|
||||
|
||||
uint64_t get_esc_info_time{0};
|
||||
uint32_t get_esc_info_state{0};
|
||||
uint32_t get_esc_info_motor_index{0};
|
||||
uint32_t get_esc_info_start{0};
|
||||
uint32_t get_esc_info_boot{0};
|
||||
uint32_t get_esc_info_tries{0};
|
||||
//uint32_t get_esc_info_debug{0};
|
||||
|
||||
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
perf_counter_t _bdshot_rpm_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot rpm")};
|
||||
perf_counter_t _dshot_telem_perf{perf_alloc(PC_COUNT, MODULE_NAME": dshot telem")};
|
||||
|
||||
Command _current_command{};
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||
uORB::Subscription _esc_flasher_request_sub{ORB_ID(esc_flasher_request)};
|
||||
uORB::Subscription _esc_flasher_status_sub{ORB_ID(esc_flasher_status)};
|
||||
uORB::Publication<esc_flasher_request_ack_s> _esc_flasher_request_ack_pub{ORB_ID(esc_flasher_request_ack)};
|
||||
uint16_t _esc_status_counter{0};
|
||||
uORB::Publication<esc_flasher_versions_s> _esc_flasher_versions_pub{ORB_ID(esc_flasher_versions)};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::DSHOT_MIN>) _param_dshot_min,
|
||||
@ -179,6 +244,7 @@ private:
|
||||
(ParamInt<px4::params::DSHOT_3D_DEAD_H>) _param_dshot_3d_dead_h,
|
||||
(ParamInt<px4::params::DSHOT_3D_DEAD_L>) _param_dshot_3d_dead_l,
|
||||
(ParamInt<px4::params::MOT_POLE_COUNT>) _param_mot_pole_count,
|
||||
(ParamBool<px4::params::DSHOT_BIDIR_EN>) _param_bidirectional_enable
|
||||
(ParamBool<px4::params::DSHOT_BIDIR_EN>) _param_bidirectional_enable,
|
||||
(ParamInt<px4::params::ESC_TYPE>) _param_esc_type
|
||||
)
|
||||
};
|
||||
|
||||
@ -46,38 +46,53 @@ using namespace time_literals;
|
||||
|
||||
DShotTelemetry::~DShotTelemetry()
|
||||
{
|
||||
_uart.close();
|
||||
deinit();
|
||||
}
|
||||
|
||||
int DShotTelemetry::init(const char *port, bool swap_rxtx)
|
||||
int DShotTelemetry::init(const char *uart_device)
|
||||
{
|
||||
if (!_uart.setPort(port)) {
|
||||
PX4_ERR("Error configuring port %s", port);
|
||||
return PX4_ERROR;
|
||||
deinit();
|
||||
_uart_fd = ::open(uart_device, O_RDONLY | O_NOCTTY);
|
||||
|
||||
if (_uart_fd < 0) {
|
||||
PX4_ERR("failed to open serial port: %s err: %d", uart_device, errno);
|
||||
return -errno;
|
||||
}
|
||||
|
||||
if (!_uart.setBaudrate(DSHOT_TELEMETRY_UART_BAUDRATE)) {
|
||||
PX4_ERR("Error setting baudrate on %s", port);
|
||||
return PX4_ERROR;
|
||||
_num_timeouts = 0;
|
||||
_num_successful_responses = 0;
|
||||
_current_motor_index_request = -1;
|
||||
return setBaudrate(DSHOT_TELEMETRY_UART_BAUDRATE);
|
||||
}
|
||||
|
||||
void DShotTelemetry::deinit()
|
||||
{
|
||||
if (_uart_fd >= 0) {
|
||||
close(_uart_fd);
|
||||
_uart_fd = -1;
|
||||
}
|
||||
}
|
||||
|
||||
int DShotTelemetry::redirectOutput(OutputBuffer &buffer)
|
||||
{
|
||||
if (expectingData()) {
|
||||
// Error: cannot override while we already expect data
|
||||
return -EBUSY;
|
||||
}
|
||||
|
||||
if (swap_rxtx) {
|
||||
if (!_uart.setSwapRxTxMode()) {
|
||||
PX4_ERR("Error swapping TX/RX");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
if (! _uart.open()) {
|
||||
PX4_ERR("Error opening %s", port);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
_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(int num_motors)
|
||||
{
|
||||
if (_uart_fd < 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (_current_motor_index_request == -1) {
|
||||
// nothing in progress, start a request
|
||||
_current_motor_index_request = 0;
|
||||
@ -87,9 +102,10 @@ int DShotTelemetry::update(int num_motors)
|
||||
}
|
||||
|
||||
// read from the uart. This must be non-blocking, so check first if there is data available
|
||||
int bytes_available = _uart.bytesAvailable();
|
||||
int bytes_available = 0;
|
||||
int ret = ioctl(_uart_fd, FIONREAD, (unsigned long)&bytes_available);
|
||||
|
||||
if (bytes_available <= 0) {
|
||||
if (ret != 0 || bytes_available <= 0) {
|
||||
// no data available. Check for a timeout
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
@ -111,12 +127,13 @@ int DShotTelemetry::update(int num_motors)
|
||||
return -1;
|
||||
}
|
||||
|
||||
uint8_t buf[ESC_FRAME_SIZE];
|
||||
int bytes = _uart.read(buf, sizeof(buf));
|
||||
const int buf_length = ESC_FRAME_SIZE;
|
||||
uint8_t buf[buf_length];
|
||||
|
||||
int ret = -1;
|
||||
int num_read = read(_uart_fd, buf, buf_length);
|
||||
ret = -1;
|
||||
|
||||
for (int i = 0; i < bytes && ret == -1; ++i) {
|
||||
for (int i = 0; i < num_read && ret == -1; ++i) {
|
||||
if (_redirect_output) {
|
||||
_redirect_output->buffer[_redirect_output->buf_pos++] = buf[i];
|
||||
|
||||
@ -166,9 +183,6 @@ bool DShotTelemetry::decodeByte(uint8_t byte, bool &successful_decoding)
|
||||
_latest_data.erpm);
|
||||
++_num_successful_responses;
|
||||
successful_decoding = true;
|
||||
|
||||
} else {
|
||||
++_num_checksum_errors;
|
||||
}
|
||||
|
||||
return true;
|
||||
@ -181,30 +195,32 @@ void DShotTelemetry::printStatus() const
|
||||
{
|
||||
PX4_INFO("Number of successful ESC frames: %i", _num_successful_responses);
|
||||
PX4_INFO("Number of timeouts: %i", _num_timeouts);
|
||||
PX4_INFO("Number of CRC errors: %i", _num_checksum_errors);
|
||||
}
|
||||
|
||||
uint8_t DShotTelemetry::updateCrc8(uint8_t crc, uint8_t crc_seed)
|
||||
{
|
||||
uint8_t crc_u = crc ^ crc_seed;
|
||||
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
|
||||
}
|
||||
|
||||
return crc_u;
|
||||
}
|
||||
|
||||
|
||||
uint8_t DShotTelemetry::crc8(const uint8_t *buf, uint8_t len)
|
||||
{
|
||||
auto update_crc8 = [](uint8_t crc, uint8_t crc_seed) {
|
||||
uint8_t crc_u = crc ^ crc_seed;
|
||||
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
|
||||
}
|
||||
|
||||
return crc_u;
|
||||
};
|
||||
|
||||
uint8_t crc = 0;
|
||||
|
||||
for (int i = 0; i < len; ++i) {
|
||||
crc = update_crc8(buf[i], crc);
|
||||
crc = updateCrc8(buf[i], crc);
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
|
||||
void DShotTelemetry::requestNextMotor(int num_motors)
|
||||
{
|
||||
_current_motor_index_request = (_current_motor_index_request + 1) % num_motors;
|
||||
@ -237,6 +253,7 @@ void DShotTelemetry::decodeAndPrintEscInfoPacket(const OutputBuffer &buffer)
|
||||
BLHELI32,
|
||||
KissV1,
|
||||
KissV2,
|
||||
AM32
|
||||
};
|
||||
ESCVersionInfo version;
|
||||
int packet_length;
|
||||
@ -250,8 +267,21 @@ void DShotTelemetry::decodeAndPrintEscInfoPacket(const OutputBuffer &buffer)
|
||||
packet_length = esc_info_size_kiss_v2;
|
||||
|
||||
} else {
|
||||
version = ESCVersionInfo::KissV1;
|
||||
packet_length = esc_info_size_kiss_v1;
|
||||
if (buffer.buf_pos > esc_info_size_kiss_v1) {
|
||||
version = ESCVersionInfo::AM32;
|
||||
packet_length = esc_info_size_am32;
|
||||
}
|
||||
else {
|
||||
version = ESCVersionInfo::KissV1;
|
||||
packet_length = esc_info_size_kiss_v1;
|
||||
}
|
||||
}
|
||||
|
||||
// Check for AM32 but with bad data
|
||||
if (version != ESCVersionInfo::AM32 && buffer.buf_pos == esc_info_size_am32) {
|
||||
version = ESCVersionInfo::AM32;
|
||||
packet_length = esc_info_size_am32;
|
||||
PX4_WARN("Packet length mismatch (%i != %i), reverting to AM32 decoding", buffer.buf_pos, packet_length);
|
||||
}
|
||||
|
||||
if (buffer.buf_pos != packet_length) {
|
||||
@ -264,116 +294,337 @@ void DShotTelemetry::decodeAndPrintEscInfoPacket(const OutputBuffer &buffer)
|
||||
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;
|
||||
if (version == ESCVersionInfo::AM32) {
|
||||
// AM32 2.18+ ESC_INFO packet sends bytes 0-47 of its EEPROM struct
|
||||
if (buffer.buf_pos < packet_length) {
|
||||
PX4_ERR("Not enough data received");
|
||||
return;
|
||||
}
|
||||
|
||||
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;
|
||||
uint8_t eeprom_version = data[1];
|
||||
if (eeprom_version != 2) {
|
||||
PX4_ERR("Unsupported EEPROM version from ESC");
|
||||
return;
|
||||
}
|
||||
|
||||
setting = data[19];
|
||||
AM32_EEprom_st* esc_info = (AM32_EEprom_st*)buffer.buffer;
|
||||
|
||||
switch (setting) {
|
||||
case 0:
|
||||
PX4_INFO("Current Limit: off");
|
||||
char firmware_name[sizeof(esc_info->firmware_name) + 1];
|
||||
memcpy(firmware_name, esc_info->firmware_name, sizeof(firmware_name));
|
||||
firmware_name[sizeof(esc_info->firmware_name)] = 0;
|
||||
|
||||
PX4_INFO("Received ESC_INFO packet from motor %d:", buffer.motor_index + 1);
|
||||
|
||||
PX4_INFO(" ESC firmware version: %d.%d - ESC firmware name: %s", esc_info->version.major, esc_info->version.minor, firmware_name);
|
||||
|
||||
if (esc_info->stuck_rotor_protection) PX4_INFO(" Stuck rotor protection is ENABLED");
|
||||
else PX4_INFO(" Stuck rotor protection is DISABLED");
|
||||
|
||||
if (esc_info->telemetry_on_interval) PX4_INFO(" Auto-Telemetry interval is ENABLED");
|
||||
else PX4_INFO(" Auto-Telemetry interval is DISABLED");
|
||||
|
||||
if (esc_info->bi_direction) PX4_INFO(" Bi-Directional DShot is ENABLED");
|
||||
else PX4_INFO(" Bi-Directional DShot is DISABLED");
|
||||
}
|
||||
else {
|
||||
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 255:
|
||||
PX4_INFO("Current Limit: unsupported");
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_INFO("Current Limit: %d A", setting);
|
||||
case ESCVersionInfo::KissV2:
|
||||
case ESCVersionInfo::BLHELI32:
|
||||
esc_firmware_version = data[13];
|
||||
esc_firmware_subversion = data[14];
|
||||
esc_type = data[15];
|
||||
break;
|
||||
case ESCVersionInfo::AM32:
|
||||
// AM32 is handled above, should not get to this case
|
||||
return;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
setting = data[i + 20];
|
||||
PX4_INFO("LED %d: %s", i, setting ? (setting == 255 ? "unsupported" : "on") : "off");
|
||||
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;
|
||||
case ESCVersionInfo::AM32:
|
||||
// AM32 is handled above, should not get to this case
|
||||
return;
|
||||
}
|
||||
|
||||
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;
|
||||
case ESCVersionInfo::AM32:
|
||||
// AM32 is handled above, should not get to this case
|
||||
return;
|
||||
}
|
||||
|
||||
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::decodeEscInfoPacketFwVersion(const OutputBuffer &buffer, uint8_t* fw_version_major, uint8_t* fw_version_minor)
|
||||
{
|
||||
// Return 0 for AM32 success
|
||||
// Return -1 for not enough data (unknown)
|
||||
// Return -2 for BLHeli success
|
||||
|
||||
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 -1;
|
||||
}
|
||||
|
||||
enum class ESCVersionInfo {
|
||||
BLHELI32,
|
||||
KissV1,
|
||||
KissV2,
|
||||
AM32
|
||||
};
|
||||
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 {
|
||||
if (buffer.buf_pos > esc_info_size_kiss_v1) {
|
||||
version = ESCVersionInfo::AM32;
|
||||
packet_length = esc_info_size_am32;
|
||||
}
|
||||
else {
|
||||
version = ESCVersionInfo::KissV1;
|
||||
packet_length = esc_info_size_kiss_v1;
|
||||
}
|
||||
}
|
||||
|
||||
// Check for AM32 but with bad data
|
||||
if (version != ESCVersionInfo::AM32 && buffer.buf_pos == esc_info_size_am32) {
|
||||
version = ESCVersionInfo::AM32;
|
||||
packet_length = esc_info_size_am32;
|
||||
PX4_WARN("Packet length mismatch (%i != %i), reverting to AM32 decoding", buffer.buf_pos, packet_length);
|
||||
}
|
||||
|
||||
if (buffer.buf_pos != packet_length) {
|
||||
PX4_ERR("Packet length mismatch (%i != %i)", buffer.buf_pos, packet_length);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (DShotTelemetry::crc8(data, packet_length - 1) != data[packet_length - 1]) {
|
||||
PX4_ERR("Checksum mismatch");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (version == ESCVersionInfo::AM32) {
|
||||
// AM32 2.18+ ESC_INFO packet sends bytes 0-47 of its EEPROM struct
|
||||
if (buffer.buf_pos < packet_length) {
|
||||
PX4_ERR("Not enough data received");
|
||||
return -1;
|
||||
}
|
||||
|
||||
uint8_t eeprom_version = data[1];
|
||||
if (eeprom_version != 2) {
|
||||
PX4_ERR("Unsupported EEPROM version from ESC");
|
||||
return -1;
|
||||
}
|
||||
|
||||
AM32_EEprom_st* esc_info = (AM32_EEprom_st*)buffer.buffer;
|
||||
|
||||
*fw_version_major = esc_info->version.major;
|
||||
*fw_version_minor = esc_info->version.minor;
|
||||
return 0;
|
||||
}
|
||||
else {
|
||||
//PX4_ERR("Unsupported ESC type, this function only supports AM32");
|
||||
|
||||
switch (version) {
|
||||
case ESCVersionInfo::KissV1:
|
||||
*fw_version_major = data[12];
|
||||
*fw_version_minor = (data[13] & 0x1f) + 97;
|
||||
break;
|
||||
case ESCVersionInfo::KissV2:
|
||||
case ESCVersionInfo::BLHELI32:
|
||||
*fw_version_major = data[13];
|
||||
*fw_version_minor = data[14];
|
||||
break;
|
||||
case ESCVersionInfo::AM32:
|
||||
// AM32 is handled above, should not get to this case
|
||||
return -1;
|
||||
}
|
||||
|
||||
return -2;
|
||||
}
|
||||
}
|
||||
|
||||
int DShotTelemetry::setBaudrate(unsigned baud)
|
||||
{
|
||||
int speed;
|
||||
|
||||
switch (baud) {
|
||||
case 9600: speed = B9600; break;
|
||||
|
||||
case 19200: speed = B19200; break;
|
||||
|
||||
case 38400: speed = B38400; break;
|
||||
|
||||
case 57600: speed = B57600; break;
|
||||
|
||||
case 115200: speed = B115200; break;
|
||||
|
||||
case 230400: speed = B230400; break;
|
||||
|
||||
default:
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
struct termios uart_config;
|
||||
|
||||
int termios_state;
|
||||
|
||||
/* fill the struct for the new configuration */
|
||||
tcgetattr(_uart_fd, &uart_config);
|
||||
|
||||
//
|
||||
// Input flags - Turn off input processing
|
||||
//
|
||||
// convert break to null byte, no CR to NL translation,
|
||||
// no NL to CR translation, don't mark parity errors or breaks
|
||||
// no input parity check, don't strip high bit off,
|
||||
// no XON/XOFF software flow control
|
||||
//
|
||||
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
|
||||
INLCR | PARMRK | INPCK | ISTRIP | IXON);
|
||||
//
|
||||
// Output flags - Turn off output processing
|
||||
//
|
||||
// no CR to NL translation, no NL to CR-NL translation,
|
||||
// no NL to CR translation, no column 0 CR suppression,
|
||||
// no Ctrl-D suppression, no fill characters, no case mapping,
|
||||
// no local output processing
|
||||
//
|
||||
// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
|
||||
// ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
|
||||
uart_config.c_oflag = 0;
|
||||
|
||||
//
|
||||
// No line processing
|
||||
//
|
||||
// echo off, echo newline off, canonical mode off,
|
||||
// extended input processing off, signal chars off
|
||||
//
|
||||
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
|
||||
|
||||
/* no parity, one stop bit, disable flow control */
|
||||
uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
|
||||
|
||||
/* set baud rate */
|
||||
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@ -33,9 +33,55 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/Serial.hpp>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
typedef struct {
|
||||
uint8_t reserved_0; //0
|
||||
uint8_t eeprom_version; //1
|
||||
uint8_t reserved_1; //2
|
||||
struct {
|
||||
uint8_t major; //3
|
||||
uint8_t minor; //4
|
||||
} version;
|
||||
char firmware_name[12]; //5-16
|
||||
uint8_t dir_reversed; // 17
|
||||
uint8_t bi_direction; // 18
|
||||
uint8_t use_sine_start; // 19
|
||||
uint8_t comp_pwm; // 20
|
||||
uint8_t variable_pwm; // 21
|
||||
uint8_t stuck_rotor_protection; // 22
|
||||
uint8_t advance_level; // 23
|
||||
uint8_t pwm_frequency; // 24
|
||||
uint8_t startup_power; // 25
|
||||
uint8_t motor_kv; // 26
|
||||
uint8_t motor_poles; // 27
|
||||
uint8_t brake_on_stop; // 28
|
||||
uint8_t stall_protection; // 29
|
||||
uint8_t beep_volume; // 30
|
||||
uint8_t telemetry_on_interval; // 31
|
||||
struct {
|
||||
uint8_t low_threshold; // 32
|
||||
uint8_t high_threshold; // 33
|
||||
uint8_t neutral; // 34
|
||||
uint8_t dead_band; // 35
|
||||
} servo;
|
||||
uint8_t low_voltage_cut_off; // 36
|
||||
uint8_t low_cell_volt_cutoff; // 37
|
||||
uint8_t rc_car_reverse; // 38
|
||||
uint8_t use_hall_sensors; // 39
|
||||
uint8_t sine_mode_changeover_thottle_level; // 40
|
||||
uint8_t drag_brake_strength; // 41
|
||||
uint8_t driving_brake_strength; // 42
|
||||
struct {
|
||||
uint8_t temperature; // 43
|
||||
uint8_t current; // 44
|
||||
} limits;
|
||||
uint8_t sine_mode_power; // 45
|
||||
uint8_t input_type; // 46
|
||||
uint8_t auto_advance; // 47
|
||||
uint8_t crc; // 48
|
||||
} AM32_EEprom_st;
|
||||
|
||||
class DShotTelemetry
|
||||
{
|
||||
public:
|
||||
@ -51,6 +97,7 @@ public:
|
||||
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 esc_info_size_am32 = 49;
|
||||
static constexpr int max_esc_info_size = esc_info_size_blheli32;
|
||||
|
||||
struct OutputBuffer {
|
||||
@ -61,7 +108,9 @@ public:
|
||||
|
||||
~DShotTelemetry();
|
||||
|
||||
int init(const char *uart_device, bool swap_rxtx);
|
||||
int init(const char *uart_device);
|
||||
|
||||
void deinit();
|
||||
|
||||
/**
|
||||
* Read telemetry from the UART (non-blocking) and handle timeouts.
|
||||
@ -70,6 +119,16 @@ public:
|
||||
*/
|
||||
int update(int num_motors);
|
||||
|
||||
/**
|
||||
* 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; }
|
||||
|
||||
/**
|
||||
@ -89,9 +148,18 @@ public:
|
||||
|
||||
static void decodeAndPrintEscInfoPacket(const OutputBuffer &buffer);
|
||||
|
||||
static int decodeEscInfoPacketFwVersion(const OutputBuffer &buffer, uint8_t* fw_version_major, uint8_t* fw_version_minor);
|
||||
|
||||
private:
|
||||
static constexpr int ESC_FRAME_SIZE = 10;
|
||||
|
||||
/**
|
||||
* set the Baudrate
|
||||
* @param baud
|
||||
* @return 0 on success, <0 on error
|
||||
*/
|
||||
int setBaudrate(unsigned baud);
|
||||
|
||||
void requestNextMotor(int num_motors);
|
||||
|
||||
/**
|
||||
@ -102,10 +170,10 @@ private:
|
||||
*/
|
||||
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);
|
||||
|
||||
device::Serial _uart {};
|
||||
|
||||
int _uart_fd{-1};
|
||||
uint8_t _frame_buffer[ESC_FRAME_SIZE];
|
||||
int _frame_position{0};
|
||||
|
||||
@ -119,5 +187,4 @@ private:
|
||||
// statistics
|
||||
int _num_timeouts{0};
|
||||
int _num_successful_responses{0};
|
||||
int _num_checksum_errors{0};
|
||||
};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user