diff --git a/src/iflytop/core/driver/socketcan/socket_can.cpp b/src/iflytop/core/driver/socketcan/socket_can.cpp index 27960c4..3ef7b13 100644 --- a/src/iflytop/core/driver/socketcan/socket_can.cpp +++ b/src/iflytop/core/driver/socketcan/socket_can.cpp @@ -13,6 +13,10 @@ using namespace iflytop; } while (0) SocketCan::SocketCanError_t SocketCan::sendFrame(shared_ptr frame) { + if (!m_canBusIsReady) { + return kDeviceBusy; + } + if (frame == nullptr) { logger->error("frame is null"); return kParamError; @@ -27,6 +31,10 @@ SocketCan::SocketCanError_t SocketCan::sendFrame(shared_ptr fram return writeFrame(canframe); } SocketCan::SocketCanError_t SocketCan::writeFrame(const canfd_frame_t &frame) { + if (!m_canBusIsReady) { + return kDeviceBusy; + } + setTxStateToTxing(frame); int ret = write(m_socketCanFd, &frame, frame.len + 8); if (ret != (frame.len + 8)) { @@ -39,6 +47,15 @@ SocketCan::SocketCanError_t SocketCan::writeFrame(const canfd_frame_t &frame) { SocketCan::SocketCanError_t SocketCan::sendFrame(shared_ptr frame, int overtime) { tp_steady start = tu_steady().now(); + + while (tu_steady().elapsedTimeMs(start) < overtime && !m_canBusIsReady) { + usleep(100); + } + + if (!m_canBusIsReady) { + return kOvertime; + } + while (tu_steady().elapsedTimeMs(start) < overtime && isTxing()) { usleep(100); } @@ -61,6 +78,10 @@ bool SocketCan::isTxFrame(const canfd_frame_t &frame) { } SocketCan::SocketCanError_t SocketCan::sendFrame(string framestr) { + if (!m_canBusIsReady) { + return kOvertime; + } + struct canfd_frame frame = {0}; int required_mtu = 0; @@ -117,6 +138,9 @@ void SocketCan::startListenThread() { */ socketCanReadThreadLoop(); })); + m_canBusIsReady = true; + + m_autoRestartThread.reset(new Thread("SocketCanAutoRestartThread", [this]() { monitorLoop(); })); } void SocketCan::endListenThread() { logger->info("endListenThread"); @@ -386,8 +410,7 @@ void SocketCan::socketCanReadThreadLoop() { // /usr/include/linux/can/error.h // x can-bus error event 20000004,0004000000000086 logger->error("rx can-bus error event {:x},{}", canframe.can_id, StringUtils().bytesToString(canframe.data, canframe.len)); - // m_canTriggerError = true; - // socketcanInitialize(); + unsetTxStateToTxing(); // exit(-1); break; @@ -400,4 +423,48 @@ void SocketCan::socketCanReadThreadLoop() { } } } + + m_canTriggerError = true; + m_canBusIsReady = false; +} + +void SocketCan::monitorLoop() { + ThisThread thisThread; + while (!thisThread.getExitFlag()) { + if (m_canTriggerError) { + // 尝试恢复CAN总线 + logger->warn("try to recover can bus............................................"); + if (m_thread) { + m_thread->join(); + m_thread = nullptr; + } + logger->warn("close can bus fd:{}", m_socketCanFd); + + close(m_socketCanFd); + m_socketCanFd = -1; + + { + lock_guard lock(m_txState.lock); + memset(&m_txState.txbuf, 0x00, sizeof(m_txState.txbuf)); + m_txState.txState = TxState_t::kTxIdle; + m_txState.txAckCount = 0; + m_txState.txLoopMessageRxTicket = zsteady_clock().now(); + } + + logger->warn("re init can bus"); + socketcanInitialize(); + + m_thread.reset(new Thread("SocketCanThread", [this]() { + usleep(10 * 1000); + socketCanReadThreadLoop(); + })); + + m_canTriggerError = false; + m_canBusIsReady = true; + + logger->warn("recover can bus ok............................................"); + } + + thisThread.sleepForMs(30); + } } diff --git a/src/iflytop/core/driver/socketcan/socket_can.hpp b/src/iflytop/core/driver/socketcan/socket_can.hpp index 26f0144..80b4294 100644 --- a/src/iflytop/core/driver/socketcan/socket_can.hpp +++ b/src/iflytop/core/driver/socketcan/socket_can.hpp @@ -75,6 +75,7 @@ class SocketCan : public enable_shared_from_this { private: unique_ptr m_thread; + unique_ptr m_autoRestartThread; int m_socketCanFd = -1; bool m_startListen = false; SocketCanUtils m_socketCanUtils; @@ -82,7 +83,8 @@ class SocketCan : public enable_shared_from_this { shared_ptr m_socketCanConfig; TxState m_txState; - bool m_canTriggerError; + bool m_canTriggerError = false; + bool m_canBusIsReady = false; public: typedef enum { @@ -125,7 +127,7 @@ class SocketCan : public enable_shared_from_this { ~SocketCan(); void initialize(shared_ptr socketCanConfig); - bool isError() { return m_canTriggerError; } + bool isError() { return false; } void startListen(); @@ -161,6 +163,9 @@ class SocketCan : public enable_shared_from_this { void endListenThread(); void socketCanReadThreadLoop(); + void monitorLoop(); + + private: void dosystem(string cmd); void setCanBitrate(string canName, int bitrate);