From 782513c7119d2a300d95d39c09e8fb49b23d89b3 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Tue, 25 Apr 2023 16:06:35 +0800 Subject: [PATCH] update --- src/test/socketcan.cpp | 190 +++++++++++++++++++++++++++++++++++++++++-------- src/test/socketcan.hpp | 22 ++++-- src/test/test_can.cpp | 66 +++++++++++++++-- 3 files changed, 238 insertions(+), 40 deletions(-) diff --git a/src/test/socketcan.cpp b/src/test/socketcan.cpp index ef73d48..3dbc564 100644 --- a/src/test/socketcan.cpp +++ b/src/test/socketcan.cpp @@ -1,8 +1,23 @@ #include "socketcan.hpp" +#include +#include + #include "iflytopcpp/core/components/stringutils.hpp" // using namespace iflytop; +#define CANID_DELIM '#' +#define CC_DLC_DELIM '_' +#define DATA_SEPERATOR '.' +#define CAN_MAX_DLC 8 +#define CAN_MAX_RAW_DLC 15 +#define CAN_MAX_DLEN 8 +unsigned char asc2nibble(char c) { + if ((c >= '0') && (c <= '9')) return c - '0'; + if ((c >= 'A') && (c <= 'F')) return c - 'A' + 10; + if ((c >= 'a') && (c <= 'f')) return c - 'a' + 10; + return 16; /* error */ +} /******************************************************************************* * can_frame_t * @@ -30,20 +45,20 @@ shared_ptr CanFrame::createStdDataFrame(uint32_t id, uint8_t *data, si return canframe; } -shared_ptr CanFrame::createExtRemoteFrame(uint32_t id, uint8_t dlc) { +shared_ptr CanFrame::createExtRemoteFrame(uint32_t id) { shared_ptr canframe = make_shared(); canframe->m_id = id; canframe->m_canIdentifier = kextFrame; canframe->m_canFrameType = kremoteframe; - canframe->m_dlc = dlc; + canframe->m_dlc = 0; return canframe; } -shared_ptr CanFrame::createStdRemoteFrame(uint32_t id, uint8_t dlc) { +shared_ptr CanFrame::createStdRemoteFrame(uint32_t id) { shared_ptr canframe = make_shared(); canframe->m_id = id; canframe->m_canIdentifier = kstdFrame; canframe->m_canFrameType = kremoteframe; - canframe->m_dlc = dlc; + canframe->m_dlc = 0; return canframe; } void CanFrame::setCanIdentifier(can_identifier_t canIdentifier) { this->m_canIdentifier = canIdentifier; } @@ -54,26 +69,6 @@ uint32_t CanFrame::getId() { return m_id; } can_identifier_t CanFrame::getCanIdentifier() { return m_canIdentifier; } can_frame_type_t CanFrame::getFanFrameType() { return m_canFrameType; } -can_frame_t CanFrame::getCanFrame() { - can_frame_t frame; - memset(&frame, 0, sizeof(frame)); - - if (m_canIdentifier == kstdFrame) { - frame.can_id = (m_id & CAN_SFF_MASK); - } else { - frame.can_id = (m_id & CAN_EFF_MASK) | CAN_EFF_FLAG; - } - - if (m_canFrameType == kremoteframe) { - frame.can_id |= CAN_RTR_FLAG; - frame.can_dlc = m_dlc; - } else { - frame.can_dlc = m_dlc; - memcpy(frame.data, m_data, m_dlc); - } - return frame; -} - /******************************************************************************* * SocketCan * *******************************************************************************/ @@ -177,10 +172,10 @@ int SocketCan::sendFrame(shared_ptr frame) { return -1; } - can_frame_t canframe = frame->getCanFrame(); + canfd_frame_t canframe = frame->getCanFrame(); - int ret = write(m_canfd, &canframe, sizeof(canframe)); - if (ret != sizeof(canframe)) { + int ret = write(m_canfd, &canframe, canframe.len + 8); + if (ret != (canframe.len + 8)) { logger->error("write fail,{}", strerror(errno)); return -1; } @@ -201,7 +196,7 @@ void SocketCan::startListen() { FD_SET(m_canfd, &readFileDescriptors); while (!thisThread.getExitFlag()) { - can_frame_t canframe; + canfd_frame_t canframe; logger->info("select...."); size_t n = select(m_canfd + 1, &readFileDescriptors, 0, 0, &waitTime); @@ -220,6 +215,145 @@ void SocketCan::startListen() { } })); } +canfd_frame_t CanFrame::getCanFrame() { + canfd_frame_t frame; +#if 1 + memset(&frame, 0, sizeof(frame)); + + if (m_canIdentifier == kstdFrame) { + frame.can_id = (m_id & CAN_SFF_MASK); + } else { + frame.can_id = (m_id & CAN_EFF_MASK) | CAN_EFF_FLAG; + } + + if (m_canFrameType == kremoteframe) { + frame.can_id |= CAN_RTR_FLAG; + frame.len = m_dlc; + } else { + frame.len = m_dlc; + memcpy(frame.data, m_data, m_dlc); + } +#endif + return frame; +} + +int SocketCan::parse_canframe(char *cs, struct canfd_frame *cf) { + /* documentation see lib.h */ + + int i, idx, dlen, len; + int maxdlen = CAN_MAX_DLEN; + int ret = CAN_MTU; + canid_t tmp; + + len = strlen(cs); + // printf("'%s' len %d\n", cs, len); + + memset(cf, 0, sizeof(*cf)); /* init CAN FD frame, e.g. LEN = 0 */ + + if (len < 4) return 0; + + if (cs[3] == CANID_DELIM) { /* 3 digits */ + + idx = 4; + for (i = 0; i < 3; i++) { + if ((tmp = asc2nibble(cs[i])) > 0x0F) return 0; + cf->can_id |= (tmp << (2 - i) * 4); + } + + } else if (cs[8] == CANID_DELIM) { /* 8 digits */ + + idx = 9; + for (i = 0; i < 8; i++) { + if ((tmp = asc2nibble(cs[i])) > 0x0F) return 0; + cf->can_id |= (tmp << (7 - i) * 4); + } + if (!(cf->can_id & CAN_ERR_FLAG)) /* 8 digits but no errorframe? */ + cf->can_id |= CAN_EFF_FLAG; /* then it is an extended frame */ + + } else + return 0; + + if ((cs[idx] == 'R') || (cs[idx] == 'r')) { /* RTR frame */ + cf->can_id |= CAN_RTR_FLAG; + + /* check for optional DLC value for CAN 2.0B frames */ + if (cs[++idx] && (tmp = asc2nibble(cs[idx++])) <= CAN_MAX_DLEN) { + cf->len = tmp; + + /* check for optional raw DLC value for CAN 2.0B frames */ + if ((tmp == CAN_MAX_DLEN) && (cs[idx++] == CC_DLC_DELIM)) { + tmp = asc2nibble(cs[idx]); + if ((tmp > CAN_MAX_DLEN) && (tmp <= CAN_MAX_RAW_DLC)) { + struct can_frame *ccf = (struct can_frame *)cf; + + ccf->__res1 = tmp; + } + } + } + return ret; + } + + if (cs[idx] == CANID_DELIM) { /* CAN FD frame escape char '##' */ + + maxdlen = CANFD_MAX_DLEN; + ret = CANFD_MTU; + + /* CAN FD frame ##* */ + if ((tmp = asc2nibble(cs[idx + 1])) > 0x0F) return 0; + + cf->flags = tmp; + idx += 2; + } + + for (i = 0, dlen = 0; i < maxdlen; i++) { + if (cs[idx] == DATA_SEPERATOR) /* skip (optional) separator */ + idx++; + + if (idx >= len) /* end of string => end of data */ + break; + + if ((tmp = asc2nibble(cs[idx++])) > 0x0F) return 0; + cf->data[i] = (tmp << 4); + if ((tmp = asc2nibble(cs[idx++])) > 0x0F) return 0; + cf->data[i] |= tmp; + dlen++; + } + cf->len = dlen; + + /* check for extra DLC when having a Classic CAN with 8 bytes payload */ + if ((maxdlen == CAN_MAX_DLEN) && (dlen == CAN_MAX_DLEN) && (cs[idx++] == CC_DLC_DELIM)) { + unsigned char dlc = asc2nibble(cs[idx]); + + if ((dlc > CAN_MAX_DLEN) && (dlc <= CAN_MAX_RAW_DLC)) { + struct can_frame *ccf = (struct can_frame *)cf; + + ccf->__res1 = dlc; + } + } + + return ret; +} + +int SocketCan::sendFrame(string framestr) { + struct canfd_frame frame; + int required_mtu; + required_mtu = parse_canframe((char *)framestr.c_str(), &frame); + if (!required_mtu) { + logger->error("Wrong CAN-frame format!"); + return -1; + } + + if (required_mtu > (int)CAN_MTU) { + logger->error("CAN-FD frames not supported!"); + return -1; + } + logger->info("send frame:{} {} {} {} {} {}", required_mtu, frame.can_id, frame.len, frame.flags, frame.__res0, frame.__res1); + if (write(m_canfd, &frame, required_mtu) != required_mtu) { + logger->error("CAN-FD frames not supported!"); + return -1; + } + return 0; +} void SocketCan::dosystem(string cmd) { logger->info("do cmd:{}", cmd); diff --git a/src/test/socketcan.hpp b/src/test/socketcan.hpp index 3e24a33..0198485 100644 --- a/src/test/socketcan.hpp +++ b/src/test/socketcan.hpp @@ -47,8 +47,8 @@ using namespace core; * CAN_FRAME * *******************************************************************************/ -typedef struct can_filter can_filter_t; -typedef struct can_frame can_frame_t; +typedef struct can_filter can_filter_t; +typedef struct canfd_frame canfd_frame_t; typedef enum { kstdFrame, kextFrame, @@ -70,8 +70,8 @@ class CanFrame { public: static shared_ptr createExtDataFrame(uint32_t id, uint8_t *data, size_t len); static shared_ptr createStdDataFrame(uint32_t id, uint8_t *data, size_t len); - static shared_ptr createExtRemoteFrame(uint32_t id, uint8_t dlc); - static shared_ptr createStdRemoteFrame(uint32_t id, uint8_t dlc); + static shared_ptr createExtRemoteFrame(uint32_t id); + static shared_ptr createStdRemoteFrame(uint32_t id); void setId(uint32_t id); uint32_t getId(); @@ -80,7 +80,7 @@ class CanFrame { can_identifier_t getCanIdentifier(); void setCanIdentifier(can_identifier_t canIdentifier); - can_frame_t getCanFrame(); + canfd_frame_t getCanFrame(); }; /******************************************************************************* @@ -102,6 +102,17 @@ class SocketCan { SocketCan(){}; bool initialize(string canName, int bitrate, vector filters); int sendFrame(shared_ptr frame); + /** + * @brief 发送can帧 + * + * @param frame + * frame="11111111#1122334455667788" ID=0x11111111,帧类型=数据帧,帧格式=扩展帧,数据长度=8,数据=1122334455667788 + * frame="111#1122334455667788" ID=0x111, 帧类型=数据帧,帧格式=标准帧,数据长度=8,数据=1122334455667788 + * frame="111#R" ID=0x111, 帧类型=远程帧,帧格式=标准帧,数据长度=0 + * frame="11111111#R" ID=0x11111111,帧类型=远程帧,帧格式=扩展帧,数据长度=0 + * @return int + */ + int sendFrame(string frame); void startListen(); ~SocketCan() { if (m_canfd > 0) { @@ -113,5 +124,6 @@ class SocketCan { void dosystem(string cmd); void setCanBitrate(string canName, int bitrate); bool findCan(string canName); + int parse_canframe(char *cs, struct canfd_frame *cf); }; } // namespace iflytop \ No newline at end of file diff --git a/src/test/test_can.cpp b/src/test/test_can.cpp index a053359..fc1670d 100644 --- a/src/test/test_can.cpp +++ b/src/test/test_can.cpp @@ -57,6 +57,50 @@ class Main { } void onSIGINT() { exit(0); } int main(int argCount, char* argValues[]) { +#if 0 + int s, nbytes; + struct sockaddr_can addr; + struct ifreq ifr; + struct can_frame frame[2] = {{0}}; + s = socket(PF_CAN, SOCK_RAW, CAN_RAW); + printf("send_s = %d\n", s); + strcpy(ifr.ifr_name, "can0"); + ioctl(s, SIOCGIFINDEX, &ifr); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + bind(s, (struct sockaddr*)&addr, sizeof(addr)); + + setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0); + + frame[0].can_id = 0x11; + frame[0].can_dlc = 1; + frame[0].data[0] = 'Y'; + frame[1].can_id = 0x410; + frame[1].can_dlc = 1; + frame[1].data[0] = 'N'; + + while (1) { + nbytes = write(s, &frame[0], sizeof(frame[0])); + if (nbytes != sizeof(frame[0])) { + printf("Send Error frame[0]!\n"); + break; + } + + sleep(1); + + nbytes = write(s, &frame[1], sizeof(frame[1])); + if (nbytes != sizeof(frame[1])) { + printf("Send Error frame[1]!\n"); + break; + } + + sleep(1); + } + + close(s); + while(1) sleep(1000); +#endif + socketCan.reset(new SocketCan()); socketCan->initialize("can0", 500000, {}); @@ -70,15 +114,23 @@ class Main { ThisThread thisThread; int i = 0; while (!thisThread.getExitFlag()) { - thisThread.sleepForMs(1000); - uint8_t data[8] = {0}; - data[0]++; - shared_ptr canframe = CanFrame::createExtDataFrame(i++, data, 8); - int ret = socketCan->sendFrame(canframe); - if (ret < 0) { - logger->error("sendFrame failed"); + thisThread.sleepForMs(10); + uint8_t data[8] = {1, 2, 3, 4, 5, 6, 7, 8}; + shared_ptr canframe = CanFrame::createExtDataFrame(0x11111111, data, 8); + logger->info("TX Frame {}", i); + socketCan->sendFrame(canframe); + i++; + if (i == 10) { + break; } + +#if 0 + socketCan->sendFrame("11111111#1122334455667788"); logger->info("TX Frame"); +#endif + } + while (true) { + sleep(1000); } return 0;