Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL: add support for taking in canfd bitrate in caniface

This commit is contained in:
bugobliterator 2022-02-10 13:10:44 +05:30 committed by Andrew Tridgell
parent 79f55e7aad
commit 1f43c79bfd
2 changed files with 87 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
libraries/AP_HAL/CANIface.cpp
View File

@ -93,3 +93,83 @@ bool AP_HAL::CANIface::register_frame_callback(FrameCb cb)
frame_callback = cb;
return true;
}
AP_HAL::CANFrame::CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame) :
id(can_id),
canfd(canfd_frame)
{
if ((can_data == nullptr) || (data_len == 0) || (data_len > MaxDataLen)) {
return;
}
memcpy(this->data, can_data, data_len);
if (data_len <= 8) {
dlc = data_len;
} else if (!canfd) {
dlc = 8;
} else {
/*
Data Length Code 9 10 11 12 13 14 15
Number of data bytes 12 16 20 24 32 48 64
*/
if (data_len <= 12) {
dlc = 9;
} else if (data_len <= 16) {
dlc = 10;
} else if (data_len <= 20) {
dlc = 11;
} else if (data_len <= 24) {
dlc = 12;
} else if (data_len <= 32) {
dlc = 13;
} else if (data_len <= 48) {
dlc = 14;
} else if (data_len <= 64) {
dlc = 15;
}
}
}
uint8_t AP_HAL::CANFrame::dataLengthToDlc(uint8_t data_length)
{
if (data_length <= 8) {
return data_length;
} else if (data_length <= 12) {
return 9;
} else if (data_length <= 16) {
return 10;
} else if (data_length <= 20) {
return 11;
} else if (data_length <= 24) {
return 12;
} else if (data_length <= 32) {
return 13;
} else if (data_length <= 48) {
return 14;
}
return 15;
}
uint8_t AP_HAL::CANFrame::dlcToDataLength(uint8_t dlc)
{
/*
Data Length Code 9 10 11 12 13 14 15
Number of data bytes 12 16 20 24 32 48 64
*/
if (dlc <= 8) {
return dlc;
} else if (dlc == 9) {
return 12;
} else if (dlc == 10) {
return 16;
} else if (dlc == 11) {
return 20;
} else if (dlc == 12) {
return 24;
} else if (dlc == 13) {
return 32;
} else if (dlc == 14) {
return 48;
}
return 64;
}

81
libraries/AP_HAL/CANIface.h
View File

@ -56,40 +56,7 @@ struct AP_HAL::CANFrame {
memset(data,0, MaxDataLen);
}
CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame = false) :
id(can_id),
canfd(canfd_frame)
{
if ((can_data == nullptr) || (data_len == 0) || (data_len > MaxDataLen)) {
return;
}
memcpy(this->data, can_data, data_len);
if (data_len <= 8) {
dlc = data_len;
} else if (!canfd) {
dlc = 8;
} else {
/*
Data Length Code 9 10 11 12 13 14 15
Number of data bytes 12 16 20 24 32 48 64
*/
if (data_len <= 12) {
dlc = 9;
} else if (data_len <= 16) {
dlc = 10;
} else if (data_len <= 20) {
dlc = 11;
} else if (data_len <= 24) {
dlc = 12;
} else if (data_len <= 32) {
dlc = 13;
} else if (data_len <= 48) {
dlc = 14;
} else if (data_len <= 64) {
dlc = 15;
}
}
}
CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame = false);
bool operator!=(const CANFrame& rhs) const
{
@ -122,47 +89,9 @@ struct AP_HAL::CANFrame {
return canfd;
}
static uint8_t dlcToDataLength(uint8_t dlc) {
/*
Data Length Code 9 10 11 12 13 14 15
Number of data bytes 12 16 20 24 32 48 64
*/
if (dlc <= 8) {
return dlc;
} else if (dlc == 9) {
return 12;
} else if (dlc == 10) {
return 16;
} else if (dlc == 11) {
return 20;
} else if (dlc == 12) {
return 24;
} else if (dlc == 13) {
return 32;
} else if (dlc == 14) {
return 48;
}
return 64;
}
static uint8_t dlcToDataLength(uint8_t dlc);
static uint8_t dataLengthToDlc(uint8_t data_length) {
if (data_length <= 8) {
return data_length;
} else if (data_length <= 12) {
return 9;
} else if (data_length <= 16) {
return 10;
} else if (data_length <= 20) {
return 11;
} else if (data_length <= 24) {
return 12;
} else if (data_length <= 32) {
return 13;
} else if (data_length <= 48) {
return 14;
}
return 15;
}
static uint8_t dataLengthToDlc(uint8_t data_length);
/**
* CAN frame arbitration rules, particularly STD vs EXT:
* Marco Di Natale - "Understanding and using the Controller Area Network"
@ -234,6 +163,10 @@ public:
}
};
virtual bool init(const uint32_t bitrate, const OperatingMode mode, const uint32_t fdbitrate) {
return init(bitrate, mode);
}
// Initialise the interface with hardware configuration required to start comms.
virtual bool init(const uint32_t bitrate, const OperatingMode mode) = 0;