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
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 238
      core/driver/socketcan/socket_can.cpp
  2. 66
      core/driver/socketcan/socket_can.hpp

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

@ -1,4 +1,8 @@
#include "socket_can.hpp"
#include <linux/can/error.h>
#include "iflytopcpp/core/components/stringutils.hpp"
using namespace iflytop;
#define DO(exptr) \
do { \
@ -14,19 +18,57 @@ SocketCan::SocketCanError_t SocketCan::sendFrame(shared_ptr<SocketCanFrame> fram
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));
unsetTxStateToTxing();
return kWriteError;
}
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) {
struct canfd_frame frame = {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);
if (!required_mtu) {
logger->error("Wrong CAN-frame format!");
@ -38,12 +80,8 @@ SocketCan::SocketCanError_t SocketCan::sendFrame(string framestr) {
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) { //
@ -86,7 +124,9 @@ void SocketCan::socketcanInitialize() {
struct sockaddr_can addr = {0};
can_filter_t zerofilter = {0};
int recv_own_msgs = 0; /* 0 = disabled (default), 1 = enabled */
int localloop = 0;
vector<can_filter_t> filters = m_socketCanConfig->m_canfilters;
can_err_mask_t err_mask = 0;
if (!findCan(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;
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)));
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)));
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 */
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");
return;
@ -153,6 +212,142 @@ void SocketCan::dosystem(string cmd) {
logger->info("do cmd:{}", cmd);
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() {
ThisThread thisThread;
@ -180,8 +375,15 @@ void SocketCan::socketCanReadThreadLoop() {
continue;
}
shared_ptr<SocketCanFrame> socketCanFrame = SocketCanFrame::createFrame(canframe);
logger->debug("RX:{}", socketCanFrame->toString());
onSocketCanFrame(socketCanFrame);
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);
logger->debug("RX:{}", socketCanFrame->toString());
onSocketCanFrame(socketCanFrame);
}
}
}
}

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

@ -8,11 +8,15 @@
#include <list>
#include <map>
#include <memory>
#include <mutex>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <vector>
#include "iflytopcpp/core/basic/concurrentqueue/concurrentqueue.h"
#include "iflytopcpp/core/components/timeutils.hpp"
#include "iflytopcpp/core/spdlogfactory/logger.hpp"
#include "iflytopcpp/core/thread/thread.hpp"
#include "socket_can_frame.hpp"
@ -33,21 +37,43 @@
namespace iflytop {
using namespace std;
using namespace core;
using namespace moodycamel;
class SocketCanConfig {
public:
string m_canName;
int m_canBaudrate;
vector<can_filter_t> m_canfilters;
};
string m_canName;
int m_canBaudrate;
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> {
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_monitorThread;
int m_socketCanFd = -1;
@ -56,6 +82,8 @@ class SocketCan : public enable_shared_from_this<SocketCan> {
shared_ptr<SocketCanConfig> m_socketCanConfig;
TxState m_txState;
public:
typedef enum {
kSuccess,
@ -65,6 +93,8 @@ class SocketCan : public enable_shared_from_this<SocketCan> {
kCANFDNotSupported,
kWriteError,
kParamError,
kDeviceBusy,
kOvertime,
} SocketCanError_t;
public:
@ -76,6 +106,7 @@ class SocketCan : public enable_shared_from_this<SocketCan> {
void startListen();
SocketCanError_t sendFrame(shared_ptr<SocketCanFrame> frame, int overtime);
SocketCanError_t sendFrame(shared_ptr<SocketCanFrame> frame);
/**
* @brief 发送can帧
@ -88,6 +119,15 @@ class SocketCan : public enable_shared_from_this<SocketCan> {
* @return int
*/
SocketCanError_t sendFrame(string frame);
/**
* @brief 使用这个方法测试目标设备CAN驱动的行为，需要CAN总线上，除了目标设备外，至少有一个正常工作的节点。
*
* @param canDevice
* @param baudrate
* @return int
*/
static int dumpCanDriverInfo(string canDevice, int baudrate);
bool isTxing();
private:
void socketcanInitialize();
@ -99,11 +139,19 @@ class SocketCan : public enable_shared_from_this<SocketCan> {
void socketCanReadThreadLoop();
private:
void dosystem(string cmd);
void setCanBitrate(string canName, int bitrate);
bool findCan(string canName);
void dosystem(string cmd);
void setCanBitrate(string canName, int bitrate);
static bool findCan(string canName);
SocketCanError_t writeFrame(const canfd_frame_t &frame);
private:
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
Write Preview
Loading…
Cancel
Save