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>
* 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 {};
hex2nibble_error = false;
f.canfd = canfd;
f.id = f.FlagEFF |
(hex2nibble(cmd[1]) << 28) |
(hex2nibble(cmd[2]) << 24) |
@ -133,16 +134,14 @@ bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd)
(hex2nibble(cmd[6]) << 8) |
(hex2nibble(cmd[7]) << 4) |
(hex2nibble(cmd[8]) << 0);
if (cmd[9] < '0' || cmd[9] > ('0' + AP_HAL::CANFrame::NonFDCANMaxDataLen)) {
return false;
}
f.dlc = cmd[9] - '0';
if (f.dlc > AP_HAL::CANFrame::NonFDCANMaxDataLen) {
f.dlc = hex2nibble(cmd[9]);
if (hex2nibble_error || f.dlc > (canfd?15:8)) {
return false;
}
{
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));
p += 2;
}
@ -401,8 +400,8 @@ const char* SLCAN::CANIface::processCommand(char* cmd)
/*
* High-traffic SLCAN commands go first
*/
if (cmd[0] == 'T') {
return handle_FrameDataExt(cmd) ? "Z\r" : "\a";
if (cmd[0] == 'T' || cmd[0] == 'D') {
return handle_FrameDataExt(cmd, cmd[0]=='D') ? "Z\r" : "\a";
} else if (cmd[0] == 't') {
return handle_FrameDataStd(cmd) ? "z\r" : "\a";
} 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_FrameRTRExt(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);