Skip to content

Commit

Permalink
Add XC Technology DroneCan ESC Support
Browse files Browse the repository at this point in the history
  • Loading branch information
@lcago
lcago committed Nov 27, 2024
1 parent 83520d6 commit b1165d7
Show file tree
Hide file tree
Showing 3 changed files with 209 additions and 1 deletion.
1 change: 1 addition & 0 deletions Tools/scripts/build_options.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -412,6 +412,7 @@ def config_option(self):
Feature('Actuators', 'KDECAN', 'AP_KDECAN_ENABLED', 'KDE Direct KDECAN ESC', 0, None),
Feature('Actuators', 'HimarkServo', 'AP_DRONECAN_HIMARK_SERVO_SUPPORT', 'Enable Himark DroneCAN servos', 0, "DroneCAN"),
Feature('Actuators', 'HobbywingESC', 'AP_DRONECAN_HOBBYWING_ESC_SUPPORT', 'Enable Hobbywing DroneCAN ESCs', 0, "DroneCAN"),
Feature('Actuators', 'XiongCaiESC', 'AP_DRONECAN_XCKJ_ESC_SUPPORT','Enable XCKJ DroneCAN ESCs', 0,"DroneCAN"),

Feature('Precision Landing', 'PrecLand', 'AC_PRECLAND_ENABLED', 'Enable Precision landing support', 0, None),
Feature('Precision Landing', 'PrecLand - Companion', 'AC_PRECLAND_COMPANION_ENABLED', 'Enable Companion computer precision landing ', 0, "PrecLand"), # noqa
Expand Down
175 changes: 174 additions & 1 deletion libraries/AP_DroneCAN/AP_DroneCAN.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,7 +128,8 @@ const AP_Param::GroupInfo AP_DroneCAN::var_info[] = {
// @Param: OPTION
// @DisplayName: DroneCAN options
// @Description: Option flags
// @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS,6:UseHimarkServo,7:HobbyWingESC,8:EnableStats,9:EnableFlexDebug
// @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS,6:UseHimarkServo,7:HobbyWingESC,
// 8:EnableStats,9:EnableFlexDebug,10:XiongCaiESC
// @User: Advanced
AP_GROUPINFO("OPTION", 5, AP_DroneCAN, _options, 0),

Expand Down Expand Up @@ -424,6 +425,13 @@ void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters)
esc_hobbywing_raw.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
#endif

#if AP_DRONECAN_XCKJ_ESC_SUPPORT
esc_xckj_throtgroup1.set_timeout_ms(2);
esc_xckj_throtgroup1.set_priority(CANARD_TRANSFER_PRIORITY_HIGH - 1);
esc_xckj_throtgroup2.set_timeout_ms(2);
esc_xckj_throtgroup2.set_priority(CANARD_TRANSFER_PRIORITY_HIGH - 1);
#endif

#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
himark_out.set_timeout_ms(2);
himark_out.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
Expand Down Expand Up @@ -550,6 +558,9 @@ void AP_DroneCAN::loop(void)
logging();
#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
hobbywing_ESC_update();
#endif
#if AP_DRONECAN_XCKJ_ESC_SUPPORT
xckj_ESC_update();
#endif
if (_SRV_armed_mask != 0) {
// we have active servos
Expand Down Expand Up @@ -661,6 +672,91 @@ void AP_DroneCAN::handle_hobbywing_StatusMsg2(const CanardRxTransfer& transfer,
}
#endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT

#if AP_DRONECAN_XCKJ_ESC_SUPPORT
void AP_DroneCAN::xckj_ESC_update(void)
{
if (hal.util->get_soft_armed()) {
// don't update ID database while disarmed, as it can cause some ESCs to stutter
return;
}
uint32_t now = AP_HAL::millis();
if (now - xckj.last_GetId_Time_Ms >= 1000U) {
xckj.last_GetId_Time_Ms = now;
com_xckj_esc_OperateId msg;
msg.payload.len = 3;
msg.payload.data[0] = 2
msg.payload.data[1] = 0;
msg.payload.data[2] = 0;
esc_xckj_OperateId.broadcast(msg);
}
}

/*
handle xckj OperateId reply. This gets us the mapping between CAN NodeID and throttle channel
*/
void AP_DroneCAN::handle_xckj_OperateId(const CanardRxTransfer& transfer, const com_xckj_esc_OperateId& msg)
{
if ((msg.payload.len = 3) && (msg.payload.data[0] == 3) &&
(msg.payload.data[1] == transfer.source_node_id)) {
// throttle channel is 2nd payload byte
const uint8_t thr_channel = msg.payload.data[2];
if (thr_channel > 0 && thr_channel <= XCKJ_MAX_ESC) {
xckj.throt_chan[thr_channel - 1] = transfer.source_node_id;
}
}
}

/*
find the ESC index given a CAN node ID
*/
bool AP_DroneCAN::xckj_find_esc_index(uint8_t node_id, uint8_t& esc_index) const
{
for (uint8_t i = 0; i < XCKJ_MAX_ESC; i++) {
if (xckj.throt_chan[i] == node_id) {
const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
esc_index = i + esc_offset;
return true;
}
}
return false;
}

/*
handle xckj AutoUpMsg1 reply
*/
void AP_DroneCAN::handle_xckj_AutoUpMsg1(const CanardRxTransfer& transfer, const com_xckj_esc_AutoUpMsg1& msg)
{
uint8_t esc_index;
if (xckj_find_esc_index(transfer.source_node_id, esc_index)) {
update_rpm(esc_index, msg.rpm * 0.1f);
TelemetryData t{
.current = msg.current * 0.1f,
};
update_telem_data(esc_index, t, AP_ESC_Telem_Backend::TelemetryType::CURRENT);
}
}

/*
handle xckj StatusMsg2 reply
*/
void AP_DroneCAN::handle_xckj_AutoUpMsg2(const CanardRxTransfer& transfer, const com_xckj_esc_AutoUpMsg2& msg)
{
uint8_t esc_index;
if (xckj_find_esc_index(transfer.source_node_id, esc_index)) {
TelemetryData t{
.voltage = msg.voltage * 0.1f,
.temperature_cdeg = int16_t(msg.MOS_T - 40),
.motor_temp_cdeg = int16_t(msg.Motor_T - 40),
};
update_telem_data(esc_index, t,
AP_ESC_Telem_Backend::TelemetryType::VOLTAGE |
AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE |
AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE);
}

}
#endif // AP_DRONECAN_XCKJ_ESC_SUPPORT

void AP_DroneCAN::send_node_status(void)
{
const uint32_t now = AP_HAL::millis();
Expand Down Expand Up @@ -925,6 +1021,77 @@ void AP_DroneCAN::SRV_send_esc_hobbywing(void)
}
#endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT

#if AP_DRONECAN_XCKJ_ESC_SUPPORT
/*
support for xckj DroneCAN ESCs
*/
void AP_DroneCAN::SRV_send_esc_xckj(void)
{
com_xckj_esc_ThrotGroup1 throtgroup1_msg;
com_xckj_esc_ThrotGroup2 throtgroup2_msg;

uint8_t active_esc_num = 0, max_esc_num = 0;
uint8_t k = 0;

// esc offset allows for efficient packing of higher ESC numbers in RawCommand
const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);

// find out how many esc we have enabled and if they are active at all
for (uint8_t i = esc_offset; i < DRONECAN_SRV_NUMBER; i++) {
if ((((uint32_t)1) << i) & _ESC_armed_mask) {
max_esc_num = i + 1;
if (_SRV_conf[i].esc_pending) {
active_esc_num++;
}
}
}

// if at least one is active (update) we need to send to all
if (active_esc_num > 0) {
k = 0;
const bool armed = hal.util->get_soft_armed();
for (uint8_t i = esc_offset; k < 8; i++) {
if (k < 4) {
if (armed && ((((uint32_t)1U) << i) & _ESC_armed_mask)) {
throtgroup1_msg.command.data[k] = scale_esc_output(i);
}
else {
throtgroup1_msg.command.data[k] = static_cast<unsigned>(0);
}
}
else if (max_esc_num > esc_offset + 4) {
if (armed && ((((uint32_t)1U) << i) & _ESC_armed_mask)) {
throtgroup2_msg_msg.command.data[k - 4] = scale_esc_output(i);
}
else {
throtgroup2_msg.command.data[k - 4] = static_cast<unsigned>(0);
}
}
k++;
}

throtgroup1_msg.command.len = 4;
if (esc_xckj_throtgroup1.broadcast(esc_throtgroup1)) {
_esc_send_count++;
}
else {
_fail_send_count++;
}
if (max_esc_num > esc_offset + 4) {
throtgroup2_msg.command.len = 4;
if (esc_xckj_throtgroup2.broadcast(esc_throtgroup2)) {
_esc_send_count++;
}
else {
_fail_send_count++;
}
}
// immediately push data to CAN bus
canard_iface.processTx(true);
}
}
#endif // AP_DRONECAN_XCKJ_ESC_SUPPORT

void AP_DroneCAN::SRV_push_servos()
{
WITH_SEMAPHORE(SRV_sem);
Expand Down Expand Up @@ -961,6 +1128,12 @@ void AP_DroneCAN::SRV_push_servos()
if (option_is_set(Options::USE_HOBBYWING_ESC)) {
SRV_send_esc_hobbywing();
} else
#endif
#if AP_DRONECAN_XCKJ_ESC_SUPPORT
if (option_is_set(Options::USE_XCKJ_ESC))
{
SRV_send_esc_xckj();
} else
#endif
{
SRV_send_esc();
Expand Down
34 changes: 34 additions & 0 deletions libraries/AP_DroneCAN/AP_DroneCAN.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,10 @@
#define AP_DRONECAN_HOBBYWING_ESC_SUPPORT (BOARD_FLASH_SIZE>1024)
#endif

#ifndef AP_DRONECAN_XCKJ_ESC_SUPPORT
#define AP_DRONECAN_XCKJ_ESC_SUPPORT (BOARD_FLASH_SIZE>1024)
#endif

#ifndef AP_DRONECAN_HIMARK_SERVO_SUPPORT
#define AP_DRONECAN_HIMARK_SERVO_SUPPORT (BOARD_FLASH_SIZE>1024)
#endif
Expand Down Expand Up @@ -149,6 +153,7 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
USE_HOBBYWING_ESC = (1U<<7),
ENABLE_STATS = (1U<<8),
ENABLE_FLEX_DEBUG = (1U<<9),
USE_XCKJ_ESC = (1U<<10),
};

// check if a option is set
Expand Down Expand Up @@ -401,6 +406,35 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
void handle_hobbywing_StatusMsg2(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg2& msg);
#endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT

#if AP_DRONECAN_XCKJ_ESC_SUPPORT
#define XCKJ_MAX_ESC 8
struct {
uint32_t last_GetId_Time_Ms;
uint8_t throt_chan[XCKJ_MAX_ESC];
}xckj;
void xckj_ESC_update();


void SRV_send_esc_xckj();
Canard::Publisher<com_xckj_esc_ThortGroup1> esc_xckj_throtgroup1{ canard_iface };
Canard::Publisher<com_xckj_esc_ThortGroup2> esc_xckj_throtgroup2{ canard_iface };

Canard::Publisher<com_xckj_esc_OperateId> esc_xckj_OperateId{ canard_iface };
Canard::ObjCallback<AP_DroneCAN, com_xckj_esc_OperateId> esc_xckj_OperateId_cb{ this, &AP_DroneCAN::handle_xckj_OperateId };
Canard::Subscriber<com_xckj_esc_OperateId> esc_xckj_OperateId_listener{ handle_xckj_OperateId_cb, _driver_index };

Canard::ObjCallback<AP_DroneCAN, com_xckj_esc_AutoUpMsg1> esc_xckj_AutoUpMsg1_cb{ this, &AP_DroneCAN::handle_xckj_AutoUpMsg1 };
Canard::Subscriber<com_xckj_esc_AutoUpMsg1> esc_xckj_AutoUpMsg1_listener{ esc_xckj_AutoUpMsg1_cb, _driver_index };

Canard::ObjCallback<AP_DroneCAN, com_xckj_esc_AutoUpMsg2> esc_xckj_AutoUpMsg2_cb{ this, &AP_DroneCAN::handle_xckj_AutoUpMsg2 };
Canard::Subscriber<com_xckj_esc_AutoUpMsg2> esc_xckj_AutoUpMsg2_listener{ esc_xckj_AutoUpMsg2_cb, _driver_index };

bool xckj_find_esc_index(uint8_t node_id, uint8_t& esc_index) const;
void handle_xckj_OperateId(const CanardRxTransfer& transfer, const com_xckj_esc_OperateId& msg);
void handle_xckj_AutoUpMsg1(const CanardRxTransfer& transfer, const com_xckj_esc_AutoUpMsg1& msg);
void handle_xckj_AutoUpMsg2(const CanardRxTransfer& transfer, const com_xckj_esc_AutoUpMsg2& msg);
#endif // AP_DRONECAN_XCKJ_ESC_SUPPORT

#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
void handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg);
#endif
Expand Down

0 comments on commit b1165d7

Please sign in to comment.