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

AP_CANManager: fixed slcan receive of CANFD frames

Browse Source
This commit is contained in:
Andrew Tridgell 2022-02-17 21:11:09 +11:00
parent d28cecf25b
commit cf5d94b81f
2 changed files with 9 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_CANManager/AP_SLCANIface.cpp
View File

@ -120,10 +120,11 @@ bool SLCAN::CANIface::push_Frame(AP_HAL::CANFrame &frame)
* <type> <id> <dlc> <data> * <type> <id> <dlc> <data>
* The emitting functions below are highly optimized for speed. * The emitting functions below are highly optimized for speed.
*/ */
bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd) bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd, bool canfd)
{ {
AP_HAL::CANFrame f {}; AP_HAL::CANFrame f {};
hex2nibble_error = false; hex2nibble_error = false;
f.canfd = canfd;
f.id = f.FlagEFF | f.id = f.FlagEFF |
(hex2nibble(cmd[1]) << 28) | (hex2nibble(cmd[1]) << 28) |
(hex2nibble(cmd[2]) << 24) | (hex2nibble(cmd[2]) << 24) |
@ -133,16 +134,14 @@ 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::NonFDCANMaxDataLen)) { f.dlc = hex2nibble(cmd[9]);
return false; if (hex2nibble_error || f.dlc > (canfd?15:8)) {
}
f.dlc = cmd[9] - '0';
if (f.dlc > AP_HAL::CANFrame::NonFDCANMaxDataLen) {
return false; return false;
} }
{ {
const char* p = &cmd[10]; const char* p = &cmd[10];
for (unsigned i = 0; i < f.dlc; i++) { const uint8_t dlen = AP_HAL::CANFrame::dlcToDataLength(f.dlc);
for (unsigned i = 0; i < dlen; i++) {
f.data[i] = (hex2nibble(*p) << 4) | hex2nibble(*(p + 1)); f.data[i] = (hex2nibble(*p) << 4) | hex2nibble(*(p + 1));
p += 2; p += 2;
} }
@ -401,8 +400,8 @@ const char* SLCAN::CANIface::processCommand(char* cmd)
/* /*
* High-traffic SLCAN commands go first * High-traffic SLCAN commands go first
*/ */
if (cmd[0] == 'T') { if (cmd[0] == 'T' || cmd[0] == 'D') {
return handle_FrameDataExt(cmd) ? "Z\r" : "\a"; return handle_FrameDataExt(cmd, cmd[0]=='D') ? "Z\r" : "\a";
} else if (cmd[0] == 't') { } else if (cmd[0] == 't') {
return handle_FrameDataStd(cmd) ? "z\r" : "\a"; return handle_FrameDataStd(cmd) ? "z\r" : "\a";
} else if (cmd[0] == 'R') { } else if (cmd[0] == 'R') {

2
libraries/AP_CANManager/AP_SLCANIface.h
View File

@ -50,7 +50,7 @@ class CANIface: public AP_HAL::CANIface
bool handle_FrameRTRStd(const char* cmd); bool handle_FrameRTRStd(const char* cmd);
bool handle_FrameRTRExt(const char* cmd); bool handle_FrameRTRExt(const char* cmd);
bool handle_FrameDataStd(const char* cmd); bool handle_FrameDataStd(const char* cmd);
bool handle_FrameDataExt(const char* cmd); bool handle_FrameDataExt(const char* cmd, bool canfd);
bool handle_FDFrameDataExt(const char* cmd); bool handle_FDFrameDataExt(const char* cmd);