p_transmit_disinfection
/
iflytoplinuxsdk_ptdv1
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

udpate socket can

disinfection_machine
zhaohe 2 years ago
parent
6c0265c3c5
commit
f186de7700
2 changed files with 277 additions and 27 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 228
      core/driver/socketcan/socket_can.cpp
  2. 58
      core/driver/socketcan/socket_can.hpp

228
core/driver/socketcan/socket_can.cpp
View File

@ -1,4 +1,8 @@
#include "socket_can.hpp" #include "socket_can.hpp"
#include <linux/can/error.h>
#include "iflytopcpp/core/components/stringutils.hpp"
using namespace iflytop; using namespace iflytop;
#define DO(exptr) \ #define DO(exptr) \
do { \ do { \
@ -14,19 +18,57 @@ SocketCan::SocketCanError_t SocketCan::sendFrame(shared_ptr<SocketCanFrame> fram
return kParamError; return kParamError;
} }
canfd_frame_t canframe = frame->getCanFrame();
if (isTxing()) {
logger->error("txing, can't send frame");
return kDeviceBusy;
}
int ret = write(m_socketCanFd, &canframe, canframe.len + 8);
if (ret != (canframe.len + 8)) {
canfd_frame_t canframe = frame->getCanFrame();
return writeFrame(canframe);
}
SocketCan::SocketCanError_t SocketCan::writeFrame(const canfd_frame_t &frame) {
setTxStateToTxing(frame);
int ret = write(m_socketCanFd, &frame, frame.len + 8);
if (ret != (frame.len + 8)) {
logger->error("write fail,{}", strerror(errno)); logger->error("write fail,{}", strerror(errno));
unsetTxStateToTxing();
return kWriteError; return kWriteError;
} }
return kSuccess; return kSuccess;
} }
SocketCan::SocketCanError_t SocketCan::sendFrame(shared_ptr<SocketCanFrame> frame, int overtime) {
tp_steady start = tu_steady().now();
while (tu_steady().elapsedTimeMs(start) < overtime && isTxing()) {
usleep(100);
}
if (isTxing()) {
logger->error("send frame overtime");
return kOvertime;
}
return sendFrame(frame);
}
bool SocketCan::isTxing() { return m_txState.txState == kTxing; }
bool SocketCan::isTxFrame(const canfd_frame_t &frame) {
lock_guard<recursive_mutex> lock(m_txState.lock);
if (m_txState.txbuf.can_id == frame.can_id && m_txState.txbuf.len == frame.len && memcmp(m_txState.txbuf.data, frame.data, frame.len) == 0) {
return true;
}
return false;
}
SocketCan::SocketCanError_t SocketCan::sendFrame(string framestr) { SocketCan::SocketCanError_t SocketCan::sendFrame(string framestr) {
struct canfd_frame frame = {0}; struct canfd_frame frame = {0};
int required_mtu = 0; int required_mtu = 0;
if (isTxing()) {
logger->error("txing, can't send frame");
return kDeviceBusy;
}
required_mtu = m_socketCanUtils.parse_canframe((char *)framestr.c_str(), &frame); required_mtu = m_socketCanUtils.parse_canframe((char *)framestr.c_str(), &frame);
if (!required_mtu) { if (!required_mtu) {
logger->error("Wrong CAN-frame format!"); logger->error("Wrong CAN-frame format!");
@ -38,12 +80,8 @@ SocketCan::SocketCanError_t SocketCan::sendFrame(string framestr) {
return kCANFDNotSupported; return kCANFDNotSupported;
} }
logger->info("send frame:{} {} {} {} {} {}", required_mtu, frame.can_id, frame.len, frame.flags, frame.__res0, frame.__res1);
if (write(m_socketCanFd, &frame, required_mtu) != required_mtu) {
logger->error("write failed,{}", strerror(errno));
return kWriteError;
}
return kSuccess;
// logger->info("send frame:{} {} {} {} {} {}", required_mtu, frame.can_id, frame.len, frame.flags, frame.__res0, frame.__res1);
return writeFrame(frame);
} }
void SocketCan::initialize(shared_ptr<SocketCanConfig> socketCanConfig) { // void SocketCan::initialize(shared_ptr<SocketCanConfig> socketCanConfig) { //
@ -86,7 +124,9 @@ void SocketCan::socketcanInitialize() {
struct sockaddr_can addr = {0}; struct sockaddr_can addr = {0};
can_filter_t zerofilter = {0}; can_filter_t zerofilter = {0};
int recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */ int recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */
int localloop = 0;
vector<can_filter_t> filters = m_socketCanConfig->m_canfilters; vector<can_filter_t> filters = m_socketCanConfig->m_canfilters;
can_err_mask_t err_mask = 0;
if (!findCan(m_socketCanConfig->m_canName)) { if (!findCan(m_socketCanConfig->m_canName)) {
logger->error("can:{} not found", m_socketCanConfig->m_canName); logger->error("can:{} not found", m_socketCanConfig->m_canName);
@ -117,15 +157,34 @@ void SocketCan::socketcanInitialize() {
addr.can_ifindex = ifr.ifr_ifindex; addr.can_ifindex = ifr.ifr_ifindex;
DO(bind(m_socketCanFd, (struct sockaddr *)&addr, sizeof(addr))); DO(bind(m_socketCanFd, (struct sockaddr *)&addr, sizeof(addr)));
// 设置过滤器
if (filters.empty()) {
filters.push_back(zerofilter);
}
// 设置过滤器,默认不过滤
DO(setsockopt(m_socketCanFd, SOL_CAN_RAW, CAN_RAW_FILTER, &zerofilter, sizeof(zerofilter))); DO(setsockopt(m_socketCanFd, SOL_CAN_RAW, CAN_RAW_FILTER, &zerofilter, sizeof(zerofilter)));
if (m_socketCanConfig->enablLoopback) {
localloop = 1; /* 0 = disabled (default), 1 = enabled */
DO(setsockopt(m_socketCanFd, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &localloop, sizeof(localloop)));
recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
DO(setsockopt(m_socketCanFd, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &recv_own_msgs, sizeof(recv_own_msgs)));
} else {
localloop = 0; /* 0 = disabled (default), 1 = enabled */
DO(setsockopt(m_socketCanFd, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &localloop, sizeof(localloop)));
recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */ recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */
DO(setsockopt(m_socketCanFd, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &recv_own_msgs, sizeof(recv_own_msgs))); DO(setsockopt(m_socketCanFd, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &recv_own_msgs, sizeof(recv_own_msgs)));
}
err_mask = CAN_ERR_TX_TIMEOUT | //
CAN_ERR_LOSTARB | //
CAN_ERR_CRTL | //
CAN_ERR_PROT | //
CAN_ERR_TRX | //
CAN_ERR_ACK | //
CAN_ERR_BUSOFF | //
CAN_ERR_BUSERROR | //
CAN_ERR_RESTARTED;
setsockopt(m_socketCanFd, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &err_mask, sizeof(err_mask));
logger->info("socketcan init success"); logger->info("socketcan init success");
return; return;
@ -153,6 +212,142 @@ void SocketCan::dosystem(string cmd) {
logger->info("do cmd:{}", cmd); logger->info("do cmd:{}", cmd);
system(cmd.c_str()); system(cmd.c_str());
} }
bool SocketCan::tryUpdateTxState(canfd_frame_t &rxloopmsg) {
lock_guard<recursive_mutex> lock(m_txState.lock);
if (m_txState.txState == kTxing && isTxFrame(rxloopmsg)) {
unsetTxStateToTxing();
return true;
}
return false;
}
void SocketCan::setTxStateToTxing(const canfd_frame_t &txpacket) {
lock_guard<recursive_mutex> lock(m_txState.lock);
m_txState.txState = kTxing;
m_txState.txbuf = txpacket;
}
void SocketCan::unsetTxStateToTxing() {
lock_guard<recursive_mutex> lock(m_txState.lock);
m_txState.txState = kTxIdle;
}
#define TEST_CANDRIVER_DO(exptr) \
do { \
if ((exptr) != 0) { \
logger->error("do {} failed,{}", #exptr, strerror(errno)); \
if (socketId > 0) { \
close(socketId); \
socketId = -1; \
return -1; \
} \
} \
} while (0)
int SocketCan::dumpCanDriverInfo(string canDevice, int baudrate) {
int socketId = -1;
struct ifreq ifr = {0};
struct sockaddr_can addr = {0};
can_filter_t zerofilter = {0};
int recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */
int localloop = 0;
size_t optlen;
can_err_mask_t err_mask = 0;
int ret = 0;
fd_set readfds, readFileDescriptors;
timeval waitTime;
int loopTime = 0;
logger_t logger = GET_LOGGER(SocketCan);
if (!findCan(canDevice)) {
logger->error("can:{} not found", canDevice);
return -1;
}
// set bitrate
system(fmt::format("ip link set {} down", canDevice).c_str());
system(fmt::format("ip link set {} type can bitrate {}", canDevice, baudrate).c_str());
system(fmt::format("ip link set {} up", canDevice).c_str());
socketId = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (socketId < 0) {
logger->error("socket failed,{}", strerror(errno));
return -1;
}
// 设置依靠名字绑定CAN设备
strcpy(ifr.ifr_name, canDevice.c_str());
TEST_CANDRIVER_DO(ioctl(socketId, SIOCGIFINDEX, &ifr));
// BIND
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
TEST_CANDRIVER_DO(bind(socketId, (struct sockaddr *)&addr, sizeof(addr)));
// 设置过滤器
TEST_CANDRIVER_DO(setsockopt(socketId, SOL_CAN_RAW, CAN_RAW_FILTER, &zerofilter, sizeof(zerofilter)));
// 打开回环
localloop = 1; /* 0 = disabled (default), 1 = enabled */
TEST_CANDRIVER_DO(setsockopt(socketId, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &localloop, sizeof(localloop)));
recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
TEST_CANDRIVER_DO(setsockopt(socketId, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &recv_own_msgs, sizeof(recv_own_msgs)));
// 发送一条消息
canfd_frame_t txframe = {0};
canfd_frame_t rxcanframe = {0};
int rxloopcount = 0;
txframe.can_id = 0x12;
txframe.len = 8;
txframe.data[0] = 0x01;
logger->info("write one frame");
ret = write(socketId, &txframe, txframe.len + 8);
if (ret < 0) {
logger->error("write failed,{}", strerror(errno));
close(socketId);
return -1;
}
FD_ZERO(&readFileDescriptors);
FD_SET(socketId, &readFileDescriptors);
while (loopTime < 10) {
loopTime++;
waitTime.tv_sec = 0;
waitTime.tv_usec = 100000; // 100ms
readfds = readFileDescriptors;
int n = select(socketId + 1, &readfds, 0, 0, &waitTime);
if (n <= 0) continue;
int readsize = read(socketId, &rxcanframe, sizeof(rxcanframe));
if (readsize < 0) {
logger->error("read failed,{}", strerror(errno));
close(socketId);
return -1;
}
if (rxcanframe.can_id == txframe.can_id && //
rxcanframe.data[0] == txframe.data[0] && //
rxcanframe.len == txframe.len) {
rxloopcount++;
logger->info("rx loop frame {}", rxloopcount);
}
}
logger->info("Test Can Driver OK!");
logger->info("=================Can Driver Info =================");
logger->info("canDevice :{}", canDevice);
logger->info("baudrate :{}", baudrate);
logger->info("loop count :{}", rxloopcount);
logger->info("shouldEnableLoop:{}", rxloopcount == 1);
logger->info("==================================================");
close(socketId);
return 0;
}
void SocketCan::socketCanReadThreadLoop() { void SocketCan::socketCanReadThreadLoop() {
ThisThread thisThread; ThisThread thisThread;
@ -180,8 +375,15 @@ void SocketCan::socketCanReadThreadLoop() {
continue; continue;
} }
if (canframe.can_id & CAN_ERR_FLAG) {
logger->error("error frame {:x},{}", canframe.can_id, StringUtils().bytesToString(canframe.data, canframe.len));
} else {
// logger->info("TX:{} {} {}", canframe.can_id, canframe.__res0, canframe.__res1);
if (!tryUpdateTxState(canframe)) {
shared_ptr<SocketCanFrame> socketCanFrame = SocketCanFrame::createFrame(canframe); shared_ptr<SocketCanFrame> socketCanFrame = SocketCanFrame::createFrame(canframe);
logger->debug("RX:{}", socketCanFrame->toString()); logger->debug("RX:{}", socketCanFrame->toString());
onSocketCanFrame(socketCanFrame); onSocketCanFrame(socketCanFrame);
} }
} }
}
}

58
core/driver/socketcan/socket_can.hpp
View File

@ -8,11 +8,15 @@
#include <list> #include <list>
#include <map> #include <map>
#include <memory> #include <memory>
#include <mutex>
#include <queue>
#include <set> #include <set>
#include <sstream> #include <sstream>
#include <string> #include <string>
#include <vector> #include <vector>
#include "iflytopcpp/core/basic/concurrentqueue/concurrentqueue.h"
#include "iflytopcpp/core/components/timeutils.hpp"
#include "iflytopcpp/core/spdlogfactory/logger.hpp" #include "iflytopcpp/core/spdlogfactory/logger.hpp"
#include "iflytopcpp/core/thread/thread.hpp" #include "iflytopcpp/core/thread/thread.hpp"
#include "socket_can_frame.hpp" #include "socket_can_frame.hpp"
@ -33,21 +37,43 @@
namespace iflytop { namespace iflytop {
using namespace std; using namespace std;
using namespace core; using namespace core;
using namespace moodycamel;
class SocketCanConfig { class SocketCanConfig {
public: public:
string m_canName; string m_canName;
int m_canBaudrate; int m_canBaudrate;
vector<can_filter_t> m_canfilters;
};
class SocketCanDeviceState {
public:
/**
* @brief
* SocketCan服务依赖于loopback功能，一般来说是需要使能的。
* 但有些can驱动自动开启了这个功能，这时候就需要禁用，否则就会收到两个相同的帧。
* 通过testCanDriver可以测试是否需要禁用loopback功能。
*/
bool enablLoopback = false;
vector<can_filter_t> m_canfilters; // 应用层过滤，这里不要配置
}; };
class SocketCan : public enable_shared_from_this<SocketCan> { class SocketCan : public enable_shared_from_this<SocketCan> {
ENABLE_LOGGER(SocketCan); ENABLE_LOGGER(SocketCan);
public:
typedef enum {
kTxIdle,
kTxing,
} TxState_t;
class TxState {
public:
canfd_frame_t txbuf;
TxState_t txState = kTxIdle;
int txAckCount = 0;
zsteady_tp txLoopMessageRxTicket;
recursive_mutex lock;
};
private:
unique_ptr<Thread> m_thread; unique_ptr<Thread> m_thread;
unique_ptr<Thread> m_monitorThread; unique_ptr<Thread> m_monitorThread;
int m_socketCanFd = -1; int m_socketCanFd = -1;
@ -56,6 +82,8 @@ class SocketCan : public enable_shared_from_this<SocketCan> {
shared_ptr<SocketCanConfig> m_socketCanConfig; shared_ptr<SocketCanConfig> m_socketCanConfig;
TxState m_txState;
public: public:
typedef enum { typedef enum {
kSuccess, kSuccess,
@ -65,6 +93,8 @@ class SocketCan : public enable_shared_from_this<SocketCan> {
kCANFDNotSupported, kCANFDNotSupported,
kWriteError, kWriteError,
kParamError, kParamError,
kDeviceBusy,
kOvertime,
} SocketCanError_t; } SocketCanError_t;
public: public:
@ -76,6 +106,7 @@ class SocketCan : public enable_shared_from_this<SocketCan> {
void startListen(); void startListen();
SocketCanError_t sendFrame(shared_ptr<SocketCanFrame> frame, int overtime);
SocketCanError_t sendFrame(shared_ptr<SocketCanFrame> frame); SocketCanError_t sendFrame(shared_ptr<SocketCanFrame> frame);
/** /**
* @brief 发送can帧 * @brief 发送can帧
@ -88,6 +119,15 @@ class SocketCan : public enable_shared_from_this<SocketCan> {
* @return int * @return int
*/ */
SocketCanError_t sendFrame(string frame); SocketCanError_t sendFrame(string frame);
/**
* @brief 使用这个方法测试目标设备CAN驱动的行为，需要CAN总线上，除了目标设备外，至少有一个正常工作的节点。
*
* @param canDevice
* @param baudrate
* @return int
*/
static int dumpCanDriverInfo(string canDevice, int baudrate);
bool isTxing();
private: private:
void socketcanInitialize(); void socketcanInitialize();
@ -101,9 +141,17 @@ class SocketCan : public enable_shared_from_this<SocketCan> {
private: private:
void dosystem(string cmd); void dosystem(string cmd);
void setCanBitrate(string canName, int bitrate); void setCanBitrate(string canName, int bitrate);
bool findCan(string canName);
static bool findCan(string canName);
SocketCanError_t writeFrame(const canfd_frame_t &frame);
private: private:
void canReadThread(); void canReadThread();
bool tryUpdateTxState(canfd_frame_t &rxloopmsg);
void setTxStateToTxing(const canfd_frame_t &txpacket);
void unsetTxStateToTxing();
bool isTxFrame(const canfd_frame_t &frame);
}; };
} // namespace iflytop } // namespace iflytop
Write Preview
Loading…
Cancel
Save