Gobot RPC Stack

2024-11-27 23:29:05 +01:00
parent c1dc89eb58
commit bdec2db94d
8 changed files with 1407 additions and 36 deletions
--- a/can-interface/components/CAN-Protocol-Stack/canTP.cpp
+++ b/can-interface/components/CAN-Protocol-Stack/canTP.cpp
@@ -0,0 +1,75 @@
+#include "canTP.hpp"
+#include <math.h>
+#include <cstring>
+
+void onRxHandlerWrapper(void *arg) {
+    SocketCANTP *socket = (SocketCANTP *)arg;
+    socket->onRxHandler();
+}
+
+void SocketCANTP::onRxHandler() {
+    uint8_t tmpBuffer[8];
+    rx_info info = mcp2521->get_rx_id(MCP2521_RX_BUFFER::RXB0);
+    mcp2521->read_rx_buf(MCP2521_RX_BUFFER::RXB0, MCP2521_BUFFER_TYPE::DATA, tmpBuffer, info.length);
+
+    CANTP_FRAME_TYPE ft = static_cast<CANTP_FRAME_TYPE>((tmpBuffer[0] & 0xF0) >> 4);
+
+    switch (ft) {
+        case CANTP_FRAME_TYPE::SINGLE_FRAME:
+            if (userHandler) {
+                userHandler(tmpBuffer + 1, info.length - 1, userArg);
+            }
+            break;
+
+        case CANTP_FRAME_TYPE::FIRST_FRAME:
+            int sizeTotal = ((tmpBuffer[0] & 0x0F) << 8 | tmpBuffer[1]) - 6;
+            consecitonFrameCounter =  ceil(((float)(sizeTotal)) / 7.0);
+            memcpy(rxBuffer, tmpBuffer + 1, 6);
+
+            sendFlowControl(CANTP_FLOW_STATUS::CLEAR_TO_SEND, blockSizes, 0);
+
+            break;
+        
+        case CANTP_FRAME_TYPE::CONSECUTIVE_FRAME:
+
+
+            consecitonFrameCounter--;
+            if(consecitonFrameCounter == 0) {
+                if (userHandler) {
+                    userHandler(rxBuffer, rxLength, userArg);
+                }
+            }
+            break;
+        
+        case CANTP_FRAME_TYPE::FLOW_CONTROL:
+            break;
+    }
+}
+
+
+SocketCANTP::SocketCANTP(
+    MCP2521 *mcp2521, 
+    uint32_t address, 
+    bool is_extended, 
+    uint8_t blockSizes
+) {
+    this->mcp2521 = mcp2521;
+    this->address = address;
+    this->is_extended = is_extended;
+    this->rxLength = 0;
+    this->consecitonFrameCounter = 0;
+    this->blockSizes = 0;
+
+    mcp2521->register_rx0_handler(onRxHandlerWrapper, this);
+}
+
+void SocketCANTP::send(uint8_t *data, uint8_t length) {
+    mcp2521->prepare_tx(
+        MCP2521_TX_BUFFER::TXB0, address, data, length, is_extended, false);
+    mcp2521->request_to_send(MCP2521_TX_BUFFER::TXB0);
+}
+
+void SocketCANTP::register_rx_handler(rx_handler handler, void *arg) {
+    userHandler = handler;
+    userArg = arg;
+}