aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_DroneCAN: push ESC data out immediately and ensure high priority

Browse Source
This commit is contained in:
bugobliterator 2023-05-24 16:01:38 +10:00 committed by Andrew Tridgell
parent f38041567f
commit efe5fb7e69
3 changed files with 40 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_DroneCAN/AP_Canard_iface.cpp
View File

@ -5,12 +5,14 @@
#include <canard/handler_list.h>
#include <canard/transfer_object.h>
#include <AP_Math/AP_Math.h>
#include <dronecan_msgs.h>
extern const AP_HAL::HAL& hal;
#define LOG_TAG "DroneCANIface"
#define DEBUG_PKTS 0
#define CANARD_MSG_TYPE_FROM_ID(x) ((uint16_t)(((x) >> 8U) & 0xFFFFU))
DEFINE_HANDLER_LIST_HEADS();
DEFINE_HANDLER_LIST_SEMAPHORES();
@ -161,7 +163,7 @@ void CanardInterface::processTestRx() {
}
#endif
void CanardInterface::processTx() {
void CanardInterface::processTx(bool raw_commands_only = false) {
WITH_SEMAPHORE(_sem);
for (uint8_t iface = 0; iface < num_ifaces; iface++) {
@ -176,6 +178,16 @@ void CanardInterface::processTx() {
// scan through list of pending transfers
while (true) {
auto txf = &txq->frame;
if (raw_commands_only &&
CANARD_MSG_TYPE_FROM_ID(txf->id) != UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID &&
CANARD_MSG_TYPE_FROM_ID(txf->id) != COM_HOBBYWING_ESC_RAWCOMMAND_ID) {
// look at next transfer
txq = txq->next;
if (txq == nullptr) {
break;
}
continue;
}
txmsg.dlc = AP_HAL::CANFrame::dataLengthToDlc(txf->data_len);
memcpy(txmsg.data, txf->data, txf->data_len);
txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF);

2
libraries/AP_DroneCAN/AP_Canard_iface.h
View File

@ -33,7 +33,7 @@ public:
/// @return true if response was added to the queue
bool respond(uint8_t destination_node_id, const Canard::Transfer &res_transfer) override;
void processTx();
void processTx(bool raw_commands_only);
void processRx();
void process(uint32_t duration);

50
libraries/AP_DroneCAN/AP_DroneCAN.cpp
View File

@ -253,11 +253,12 @@ void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters)
AP_Proximity_DroneCAN::subscribe_msgs(this);
#endif
act_out_array.set_timeout_ms(2);
act_out_array.set_timeout_ms(5);
act_out_array.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
esc_raw.set_timeout_ms(2);
esc_raw.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
// esc_raw is one higher than high priority to ensure that it is given higher priority over act_out_array
esc_raw.set_priority(CANARD_TRANSFER_PRIORITY_HIGH - 1);
#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
esc_hobbywing_raw.set_timeout_ms(2);
@ -361,8 +362,6 @@ void AP_DroneCAN::loop(void)
hobbywing_ESC_update();
#endif
if (_SRV_armed) {
bool sent_servos = false;
if (_servo_bm > 0) {
// if we have any Servos in bitmask
uint32_t now = AP_HAL::native_micros();
@ -374,28 +373,11 @@ void AP_DroneCAN::loop(void)
} else {
SRV_send_actuator();
}
sent_servos = true;
for (uint8_t i = 0; i < DRONECAN_SRV_NUMBER; i++) {
_SRV_conf[i].servo_pending = false;
}
}
}
// if we have any ESC's in bitmask
if (_esc_bm > 0 && !sent_servos) {
#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
if (option_is_set(Options::USE_HOBBYWING_ESC)) {
SRV_send_esc_hobbywing();
} else
#endif
{
SRV_send_esc();
}
}
for (uint8_t i = 0; i < DRONECAN_SRV_NUMBER; i++) {
_SRV_conf[i].esc_pending = false;
}
}
}
}
@ -593,8 +575,6 @@ void AP_DroneCAN::SRV_send_esc(void)
uint8_t active_esc_num = 0, max_esc_num = 0;
uint8_t k = 0;
WITH_SEMAPHORE(SRV_sem);
// 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);
@ -633,6 +613,12 @@ void AP_DroneCAN::SRV_send_esc(void)
} else {
_fail_send_count++;
}
// immediately push data to CAN bus
canard_iface.processTx(true);
}
for (uint8_t i = 0; i < DRONECAN_SRV_NUMBER; i++) {
_SRV_conf[i].esc_pending = false;
}
}
@ -648,8 +634,6 @@ void AP_DroneCAN::SRV_send_esc_hobbywing(void)
uint8_t active_esc_num = 0, max_esc_num = 0;
uint8_t k = 0;
WITH_SEMAPHORE(SRV_sem);
// 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);
@ -688,6 +672,8 @@ void AP_DroneCAN::SRV_send_esc_hobbywing(void)
} else {
_fail_send_count++;
}
// immediately push data to CAN bus
canard_iface.processTx(true);
}
}
#endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT
@ -706,6 +692,20 @@ void AP_DroneCAN::SRV_push_servos()
}
_SRV_armed = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;
if (_SRV_armed) {
if (_esc_bm > 0) {
// push ESCs as fast as we can
#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
if (option_is_set(Options::USE_HOBBYWING_ESC)) {
SRV_send_esc_hobbywing();
} else
#endif
{
SRV_send_esc();
}
}
}
}
// notify state send