Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_CANManager: add support for trx CANFD frames over SLCAN

Browse Source
This commit is contained in:
Siddharth Purohit 2021-05-17 23:03:48 +05:30 committed by Andrew Tridgell
parent 5e54871d82
commit 563e69e64c
3 changed files with 86 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_CANManager/AP_CANTester.cpp
View File

@ -442,7 +442,7 @@ bool CANTester::test_uavcan_dna()
return false; return false;
} }
auto *node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator, false); auto *node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator);
if (!node) { if (!node) {
return false; return false;
} }
@ -777,7 +777,7 @@ bool CANTester::test_uavcan_esc(bool enable_canfd)
uavcan::Node<0> *node = nullptr; uavcan::Node<0> *node = nullptr;
{ {
node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator, false); node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator);
if (node == nullptr) { if (node == nullptr) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to allocate ESC Node"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to allocate ESC Node");
ret = false; ret = false;
@ -854,13 +854,19 @@ bool CANTester::test_uavcan_esc(bool enable_canfd)
goto exit; goto exit;
} }
esc_status_publisher = new uavcan::Publisher<uavcan::equipment::esc::Status>(*node, enable_canfd); esc_status_publisher = new uavcan::Publisher<uavcan::equipment::esc::Status>(*node);
if (esc_status_publisher == nullptr) { if (esc_status_publisher == nullptr) {
ret = false; ret = false;
goto exit; goto exit;
} }
esc_status_publisher->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); esc_status_publisher->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
esc_status_publisher->setPriority(uavcan::TransferPriority::OneLowerThanHighest); esc_status_publisher->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
if (enable_canfd) {
node->enableCanFd();
} else {
node->disableCanFd();
}
node->setNodeID(client.getAllocatedNodeID()); node->setNodeID(client.getAllocatedNodeID());
node->setModeOperational(); node->setModeOperational();
} }

90
libraries/AP_CANManager/AP_SLCANIface.cpp
View File

@ -122,7 +122,7 @@ bool SLCAN::CANIface::push_Frame(AP_HAL::CANFrame &frame)
*/ */
bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd) bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd)
{ {
AP_HAL::CANFrame f; AP_HAL::CANFrame f {};
hex2nibble_error = false; hex2nibble_error = false;
f.id = f.FlagEFF | f.id = f.FlagEFF |
(hex2nibble(cmd[1]) << 28) | (hex2nibble(cmd[1]) << 28) |
@ -133,11 +133,11 @@ bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd)
(hex2nibble(cmd[6]) << 8) | (hex2nibble(cmd[6]) << 8) |
(hex2nibble(cmd[7]) << 4) | (hex2nibble(cmd[7]) << 4) |
(hex2nibble(cmd[8]) << 0); (hex2nibble(cmd[8]) << 0);
if (cmd[9] < '0' || cmd[9] > ('0' + AP_HAL::CANFrame::MaxDataLen)) { if (cmd[9] < '0' || cmd[9] > ('0' + AP_HAL::CANFrame::NonFDCANMaxDataLen)) {
return false; return false;
} }
f.dlc = cmd[9] - '0'; f.dlc = cmd[9] - '0';
if (f.dlc > AP_HAL::CANFrame::MaxDataLen) { if (f.dlc > AP_HAL::CANFrame::NonFDCANMaxDataLen) {
return false; return false;
} }
{ {
@ -153,18 +153,56 @@ bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd)
return push_Frame(f); return push_Frame(f);
} }
#if HAL_CANFD_SUPPORTED
/**
* General frame format:
* <type> <id> <dlc> <data>
* The emitting functions below are highly optimized for speed.
*/
bool SLCAN::CANIface::handle_FDFrameDataExt(const char* cmd)
{
AP_HAL::CANFrame f {};
hex2nibble_error = false;
f.canfd = true;
f.id = f.FlagEFF |
(hex2nibble(cmd[1]) << 28) |
(hex2nibble(cmd[2]) << 24) |
(hex2nibble(cmd[3]) << 20) |
(hex2nibble(cmd[4]) << 16) |
(hex2nibble(cmd[5]) << 12) |
(hex2nibble(cmd[6]) << 8) |
(hex2nibble(cmd[7]) << 4) |
(hex2nibble(cmd[8]) << 0);
f.dlc = hex2nibble(cmd[9]);
if (f.dlc > AP_HAL::CANFrame::dataLengthToDlc(AP_HAL::CANFrame::MaxDataLen)) {
return false;
}
{
const char* p = &cmd[10];
for (unsigned i = 0; i < AP_HAL::CANFrame::dlcToDataLength(f.dlc); i++) {
f.data[i] = (hex2nibble(*p) << 4) | hex2nibble(*(p + 1));
p += 2;
}
}
if (hex2nibble_error) {
return false;
}
return push_Frame(f);
}
#endif //#if HAL_CANFD_SUPPORTED
bool SLCAN::CANIface::handle_FrameDataStd(const char* cmd) bool SLCAN::CANIface::handle_FrameDataStd(const char* cmd)
{ {
AP_HAL::CANFrame f; AP_HAL::CANFrame f {};
hex2nibble_error = false; hex2nibble_error = false;
f.id = (hex2nibble(cmd[1]) << 8) | f.id = (hex2nibble(cmd[1]) << 8) |
(hex2nibble(cmd[2]) << 4) | (hex2nibble(cmd[2]) << 4) |
(hex2nibble(cmd[3]) << 0); (hex2nibble(cmd[3]) << 0);
if (cmd[4] < '0' || cmd[4] > ('0' + AP_HAL::CANFrame::MaxDataLen)) { if (cmd[4] < '0' || cmd[4] > ('0' + AP_HAL::CANFrame::NonFDCANMaxDataLen)) {
return false; return false;
} }
f.dlc = cmd[4] - '0'; f.dlc = cmd[4] - '0';
if (f.dlc > AP_HAL::CANFrame::MaxDataLen) { if (f.dlc > AP_HAL::CANFrame::NonFDCANMaxDataLen) {
return false; return false;
} }
{ {
@ -182,7 +220,7 @@ bool SLCAN::CANIface::handle_FrameDataStd(const char* cmd)
bool SLCAN::CANIface::handle_FrameRTRExt(const char* cmd) bool SLCAN::CANIface::handle_FrameRTRExt(const char* cmd)
{ {
AP_HAL::CANFrame f; AP_HAL::CANFrame f {};
hex2nibble_error = false; hex2nibble_error = false;
f.id = f.FlagEFF | f.FlagRTR | f.id = f.FlagEFF | f.FlagRTR |
(hex2nibble(cmd[1]) << 28) | (hex2nibble(cmd[1]) << 28) |
@ -193,12 +231,12 @@ bool SLCAN::CANIface::handle_FrameRTRExt(const char* cmd)
(hex2nibble(cmd[6]) << 8) | (hex2nibble(cmd[6]) << 8) |
(hex2nibble(cmd[7]) << 4) | (hex2nibble(cmd[7]) << 4) |
(hex2nibble(cmd[8]) << 0); (hex2nibble(cmd[8]) << 0);
if (cmd[9] < '0' || cmd[9] > ('0' + AP_HAL::CANFrame::MaxDataLen)) { if (cmd[9] < '0' || cmd[9] > ('0' + AP_HAL::CANFrame::NonFDCANMaxDataLen)) {
return false; return false;
} }
f.dlc = cmd[9] - '0'; f.dlc = cmd[9] - '0';
if (f.dlc > AP_HAL::CANFrame::MaxDataLen) { if (f.dlc > AP_HAL::CANFrame::NonFDCANMaxDataLen) {
return false; return false;
} }
if (hex2nibble_error) { if (hex2nibble_error) {
@ -209,17 +247,17 @@ bool SLCAN::CANIface::handle_FrameRTRExt(const char* cmd)
bool SLCAN::CANIface::handle_FrameRTRStd(const char* cmd) bool SLCAN::CANIface::handle_FrameRTRStd(const char* cmd)
{ {
AP_HAL::CANFrame f; AP_HAL::CANFrame f {};
hex2nibble_error = false; hex2nibble_error = false;
f.id = f.FlagRTR | f.id = f.FlagRTR |
(hex2nibble(cmd[1]) << 8) | (hex2nibble(cmd[1]) << 8) |
(hex2nibble(cmd[2]) << 4) | (hex2nibble(cmd[2]) << 4) |
(hex2nibble(cmd[3]) << 0); (hex2nibble(cmd[3]) << 0);
if (cmd[4] < '0' || cmd[4] > ('0' + AP_HAL::CANFrame::MaxDataLen)) { if (cmd[4] < '0' || cmd[4] > ('0' + AP_HAL::CANFrame::NonFDCANMaxDataLen)) {
return false; return false;
} }
f.dlc = cmd[4] - '0'; f.dlc = cmd[4] - '0';
if (f.dlc <= AP_HAL::CANFrame::MaxDataLen) { if (f.dlc <= AP_HAL::CANFrame::NonFDCANMaxDataLen) {
return false; return false;
} }
if (hex2nibble_error) { if (hex2nibble_error) {
@ -267,7 +305,11 @@ int16_t SLCAN::CANIface::reportFrame(const AP_HAL::CANFrame& frame, uint64_t tim
if (_port == nullptr) { if (_port == nullptr) {
return -1; return -1;
} }
#if HAL_CANFD_SUPPORTED
constexpr unsigned SLCANMaxFrameSize = 200;
#else
constexpr unsigned SLCANMaxFrameSize = 40; constexpr unsigned SLCANMaxFrameSize = 40;
#endif
uint8_t buffer[SLCANMaxFrameSize] = {'\0'}; uint8_t buffer[SLCANMaxFrameSize] = {'\0'};
uint8_t* p = &buffer[0]; uint8_t* p = &buffer[0];
/* /*
@ -277,7 +319,13 @@ int16_t SLCAN::CANIface::reportFrame(const AP_HAL::CANFrame& frame, uint64_t tim
*p++ = frame.isExtended() ? 'R' : 'r'; *p++ = frame.isExtended() ? 'R' : 'r';
} else if (frame.isErrorFrame()) { } else if (frame.isErrorFrame()) {
return -1; // Not supported return -1; // Not supported
} else { }
#if HAL_CANFD_SUPPORTED
else if (frame.canfd) {
*p++ = frame.isExtended() ? 'D' : 'd';
}
#endif
else {
*p++ = frame.isExtended() ? 'T' : 't'; *p++ = frame.isExtended() ? 'T' : 't';
} }
@ -301,12 +349,12 @@ int16_t SLCAN::CANIface::reportFrame(const AP_HAL::CANFrame& frame, uint64_t tim
/* /*
* DLC * DLC
*/ */
*p++ = char('0' + frame.dlc); *p++ = nibble2hex(frame.dlc);
/* /*
* Data * Data
*/ */
for (unsigned i = 0; i < frame.dlc; i++) { for (unsigned i = 0; i < AP_HAL::CANFrame::dlcToDataLength(frame.dlc); i++) {
const uint8_t byte = frame.data[i]; const uint8_t byte = frame.data[i];
*p++ = nibble2hex(byte >> 4); *p++ = nibble2hex(byte >> 4);
*p++ = nibble2hex(byte); *p++ = nibble2hex(byte);
@ -361,6 +409,12 @@ const char* SLCAN::CANIface::processCommand(char* cmd)
// See long commands below // See long commands below
return handle_FrameRTRStd(cmd) ? "z\r" : "\a"; return handle_FrameRTRStd(cmd) ? "z\r" : "\a";
} }
#if HAL_CANFD_SUPPORTED
else if (cmd[0] == 'D') {
return handle_FDFrameDataExt(cmd) ? "Z\r" : "\a";
}
#endif
uint8_t resp_bytes[40]; uint8_t resp_bytes[40];
uint16_t resp_len; uint16_t resp_len;
/* /*
@ -622,7 +676,11 @@ int16_t SLCAN::CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadlin
return ret; return ret;
} }
if (frame.isErrorFrame() || frame.dlc > 8) { if (frame.isErrorFrame()
#if !HAL_CANFD_SUPPORTED
|| frame.dlc > 8
#endif
) {
return ret; return ret;
} }
reportFrame(frame, AP_HAL::native_micros64()); reportFrame(frame, AP_HAL::native_micros64());

3
libraries/AP_CANManager/AP_SLCANIface.h
View File

@ -52,6 +52,9 @@ class CANIface: public AP_HAL::CANIface
bool handle_FrameDataStd(const char* cmd); bool handle_FrameDataStd(const char* cmd);
bool handle_FrameDataExt(const char* cmd); bool handle_FrameDataExt(const char* cmd);
#if HAL_CANFD_SUPPORTED
bool handle_FDFrameDataExt(const char* cmd);
#endif
// Parsing bytes received on the serial port // Parsing bytes received on the serial port
inline void addByte(const uint8_t byte); inline void addByte(const uint8_t byte);