Skip to content

feature: Add support for Hobbywing ESC #26137

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
PavelOlhovikov wants to merge 1 commit into
base: main
Choose a base branch
from
Open

feature: Add support for Hobbywing ESC #26137

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
152 changes: 137 additions & 15 deletions src/drivers/uavcan/actuators/esc.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,28 +50,67 @@ using namespace time_literals;
UavcanEscController::UavcanEscController(uavcan::INode &node) :
_node(node),
_uavcan_pub_raw_cmd(node),
_uavcan_sub_status(node)
_uavcan_pub_hobbywing_raw_cmd(node),
_uavcan_sub_status(node),
_uavcan_hobbywing_status_msg1(node),
_uavcan_hobbywing_status_msg2(node),
_uavcan_hobbywing_status_msg3(node)
{
_uavcan_pub_raw_cmd.setPriority(uavcan::TransferPriority::NumericallyMin); // Highest priority
_uavcan_pub_hobbywing_raw_cmd.setPriority(uavcan::TransferPriority::NumericallyMin);
}

int
UavcanEscController::init()
{
// ESC status subscription
int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb));

if (res < 0) {
PX4_ERR("ESC status sub failed %i", res);
return res;
}

_esc_status_pub.advertise();

int32_t iface_mask{0xFF};
int res = 0;

if (param_get(param_find("UAVCAN_ESC_IFACE"), &iface_mask) == OK && param_get(param_find("UAVCAN_ESC_TYPE"), &_esc_type) == OK) {
switch (_esc_type) {
case 0:
res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb));

if (res < 0) {
PX4_ERR("ESC status sub failed %i", res);
return res;
}

_uavcan_pub_raw_cmd.getTransferSender().setIfaceMask(iface_mask);
break;

case 1:
res = _uavcan_hobbywing_status_msg1.start(StatusMsg1Binder(this, &UavcanEscController::esc_status_msg1_cb));

if (res < 0) {
PX4_ERR("ESC status sub failed %i", res);
return res;
}

res = _uavcan_hobbywing_status_msg2.start(StatusMsg2Binder(this, &UavcanEscController::esc_status_msg2_cb));

if (param_get(param_find("UAVCAN_ESC_IFACE"), &iface_mask) == OK) {
_uavcan_pub_raw_cmd.getTransferSender().setIfaceMask(iface_mask);
if (res < 0) {
PX4_ERR("ESC status sub failed %i", res);
return res;
}

res = _uavcan_hobbywing_status_msg3.start(StatusMsg3Binder(this, &UavcanEscController::esc_status_msg3_cb));

if (res < 0) {
PX4_ERR("ESC status sub failed %i", res);
return res;
}

_uavcan_pub_hobbywing_raw_cmd.getTransferSender().setIfaceMask(iface_mask);
break;

default:
PX4_ERR("ESC type failed");
return res;
break;
}
}

_initialized = true;
Expand All @@ -91,13 +130,35 @@ UavcanEscController::update_outputs(uint16_t outputs[MAX_ACTUATORS], uint8_t out

_prev_cmd_pub = timestamp;

uavcan::equipment::esc::RawCommand msg = {};
switch (_esc_type) {
case 0: {
uavcan::equipment::esc::RawCommand msg = {};

for (unsigned i = 0; i < output_array_size; i++) {
msg.cmd.push_back(static_cast<int>(outputs[i]));
}

_uavcan_pub_raw_cmd.broadcast(msg);
break;
}

case 1: {
com::hobbywing::esc::RawCommand msg = {};

for (unsigned i = 0; i < output_array_size; i++) {
msg.cmd.push_back(static_cast<int>(outputs[i]));
for (unsigned i = 0; i < output_array_size; i++) {
msg.command.push_back(static_cast<int>(outputs[i]));
}

_uavcan_pub_hobbywing_raw_cmd.broadcast(msg);
break;
}

default: {
PX4_ERR("ESC type failed");
break;
}
}

_uavcan_pub_raw_cmd.broadcast(msg);
}

void
Expand Down Expand Up @@ -130,6 +191,67 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavca
}
}

void
UavcanEscController::esc_status_msg1_cb(const uavcan::ReceivedDataStructure<com::hobbywing::esc::StatusMsg1> &msg)
{
if (msg.getSrcNodeID().get() < esc_status_s::CONNECTED_ESC_MAX) {
auto &ref = _esc_status.esc[msg.getSrcNodeID().get() - 1];

ref.timestamp = hrt_absolute_time();
ref.esc_address = msg.getSrcNodeID().get();
ref.esc_rpm = msg.rpm;

_esc_status.esc_count = _rotor_count;
_esc_status.counter += 1;
_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
_esc_status.esc_online_flags = check_escs_status();
_esc_status.esc_armed_flags = (1 << _rotor_count) - 1;
_esc_status.timestamp = hrt_absolute_time();
_esc_status_pub.publish(_esc_status);
}
}

void
UavcanEscController::esc_status_msg2_cb(const uavcan::ReceivedDataStructure<com::hobbywing::esc::StatusMsg2> &msg)
{
if (msg.getSrcNodeID().get() < esc_status_s::CONNECTED_ESC_MAX) {
auto &ref = _esc_status.esc[msg.getSrcNodeID().get() - 1];
Copy link
Contributor

@dakejahl dakejahl Dec 20, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This won't work. The esc_status_s.esc is length 8. Node ID's are typically in the 120s.

Copy link
Author

@PavelOlhovikov PavelOlhovikov Dec 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, I set ESC#1 NodeID = 1, ESC#2 NodeID = 2. If there is a better solution, I will be happy to consider it.

Copy link
Contributor

@dakejahl dakejahl Dec 27, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The ESCs must report from which ESC the data is originating, otherwise mapping to the array correctly is impossible. Let's see what the Hobbywing spec sheet tells us


ref.timestamp = hrt_absolute_time();
ref.esc_address = msg.getSrcNodeID().get();
ref.esc_voltage = msg.input_voltage / 10;
ref.esc_current = msg.current;
ref.esc_temperature = (float)msg.temperature / 100; // In the Mavlink system, the temperature is multiplied by 100.
Copy link
Contributor

@dakejahl dakejahl Dec 20, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
ref.esc_temperature = (float)msg.temperature / 100; // In the Mavlink system, the temperature is multiplied by 100.
ref.esc_temperature = msg.temperature

Both of these fields are degC. Please re-review the message definitions.


_esc_status.esc_count = _rotor_count;
_esc_status.counter += 1;
_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
_esc_status.esc_online_flags = check_escs_status();
_esc_status.esc_armed_flags = (1 << _rotor_count) - 1;
Copy link
Contributor

@dakejahl dakejahl Dec 20, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

armed status can't be known? It's not encoded in StatusMsg1.status?

Copy link
Author

@PavelOlhovikov PavelOlhovikov Dec 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm waiting for a response from Hobbywing support. I don't have a hobbywing message specification

_esc_status.timestamp = hrt_absolute_time();
_esc_status_pub.publish(_esc_status);
}
}

void
UavcanEscController::esc_status_msg3_cb(const uavcan::ReceivedDataStructure<com::hobbywing::esc::StatusMsg3> &msg)
{
if (msg.getSrcNodeID().get() < esc_status_s::CONNECTED_ESC_MAX) {
auto &ref = _esc_status.esc[msg.getSrcNodeID().get() - 1];

ref.timestamp = hrt_absolute_time();
ref.esc_address = msg.getSrcNodeID().get();

_esc_status.esc_count = _rotor_count;
_esc_status.counter += 1;
_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
_esc_status.esc_online_flags = check_escs_status();
_esc_status.esc_armed_flags = (1 << _rotor_count) - 1;
_esc_status.timestamp = hrt_absolute_time();
_esc_status_pub.publish(_esc_status);
}
}

uint8_t
UavcanEscController::check_escs_status()
{
Expand Down
22 changes: 22 additions & 0 deletions src/drivers/uavcan/actuators/esc.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,10 @@
#include <drivers/drv_hrt.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <parameters/param.h>
#include <com/hobbywing/esc/RawCommand.hpp>
#include <com/hobbywing/esc/StatusMsg1.hpp>
#include <com/hobbywing/esc/StatusMsg2.hpp>
#include <com/hobbywing/esc/StatusMsg3.hpp>

class UavcanEscController
{
Expand Down Expand Up @@ -87,6 +91,9 @@ class UavcanEscController
* ESC status message reception will be reported via this callback.
*/
void esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg);
void esc_status_msg1_cb(const uavcan::ReceivedDataStructure<com::hobbywing::esc::StatusMsg1> &msg);
void esc_status_msg2_cb(const uavcan::ReceivedDataStructure<com::hobbywing::esc::StatusMsg2> &msg);
void esc_status_msg3_cb(const uavcan::ReceivedDataStructure<com::hobbywing::esc::StatusMsg3> &msg);

/**
* Checks all the ESCs freshness based on timestamp, if an ESC exceeds the timeout then is flagged offline.
Expand All @@ -99,6 +106,15 @@ class UavcanEscController
typedef uavcan::MethodBinder<UavcanEscController *,
void (UavcanEscController::*)(const uavcan::TimerEvent &)> TimerCbBinder;

typedef uavcan::MethodBinder<UavcanEscController *,
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<com::hobbywing::esc::StatusMsg1>&)> StatusMsg1Binder;

typedef uavcan::MethodBinder<UavcanEscController *,
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<com::hobbywing::esc::StatusMsg2>&)> StatusMsg2Binder;

typedef uavcan::MethodBinder<UavcanEscController *,
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<com::hobbywing::esc::StatusMsg3>&)> StatusMsg3Binder;

bool _initialized{};

esc_status_s _esc_status{};
Expand All @@ -107,11 +123,17 @@ class UavcanEscController

uint8_t _rotor_count{0};

int32_t _esc_type;
Copy link
Contributor

@dakejahl dakejahl Dec 20, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
int32_t _esc_type;
EscInterfaceType _esc_type {};

Let's make this an enum

Copy link
Contributor

@dakejahl dakejahl Dec 20, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
int32_t _esc_type;
EscInterfaceType _esc_type {};

Let's make this an enum


/*
* libuavcan related things
*/
uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting
uavcan::INode &_node;
uavcan::Publisher<uavcan::equipment::esc::RawCommand> _uavcan_pub_raw_cmd;
uavcan::Publisher<com::hobbywing::esc::RawCommand> _uavcan_pub_hobbywing_raw_cmd;
uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status;
uavcan::Subscriber<com::hobbywing::esc::StatusMsg1, StatusMsg1Binder> _uavcan_hobbywing_status_msg1;
uavcan::Subscriber<com::hobbywing::esc::StatusMsg2, StatusMsg2Binder> _uavcan_hobbywing_status_msg2;
uavcan::Subscriber<com::hobbywing::esc::StatusMsg3, StatusMsg3Binder> _uavcan_hobbywing_status_msg3;
};
14 changes: 14 additions & 0 deletions src/drivers/uavcan/module.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,20 @@ parameters:
min: 1
max: 255
reboot_required: true
UAVCAN_ESC_TYPE:
description:
short: Select ESC protocol type.
long: |
Standard UAVCAN ESC protocol
or Hobbywing ESC protocol
type: enum
values:
0: UAVCAN
1: Hobbywing
default: 0
min: 0
max: 1
reboot_required: true
actuator_output:
show_subgroups_if: 'UAVCAN_ENABLE>=3'
config_parameters:
Expand Down