75 lines
2.1 KiB
C++
75 lines
2.1 KiB
C++
#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;
|
|
} |