Small project to display some OBD values from the Toyota GT86/ Subaru BRZ/ Scion FRS on an OLED display.
Dependencies: Adafruit_GFX MODSERIAL mbed-rtos mbed
Diff: IsoTpHandler.cpp
- Revision:
- 0:6b1f6139fb25
- Child:
- 2:d3d61d9d323e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IsoTpHandler.cpp Tue Apr 22 14:51:04 2014 +0000 @@ -0,0 +1,261 @@ +#include "IsoTpHandler.h" + +enum IsoTpMessageType +{ + SINGLE_FRAME = 0, ///< The single frame transferred contains the complete payload of up to 7 bytes (normal addressing) or 6 bytes (extended addressing) + FIRST_FRAME = 1, ///< The first frame of a longer multi-frame message packet, used when more than 6/7 bytes of data segmented must be communicated. The first frame contains the length of the full packet, along with the initial data. + CONSECUTIVE_FRAME = 2, ///< A frame containing subsequent data for a multi-frame packet + FLOW_CONTOL_FRAME = 3, ///< The response from the receiver, acknowledging a First-frame segment. It lays down the parameters for the transmission of further consecutive frames. +}; + +enum IsoTpFlowType +{ + CLEAR_TO_SEND = 0, + WAIT = 1, + OVERFLOW_ABORT = 2, +}; + +const IsoTpHandler::IdleState IsoTpHandler::idleState; +const IsoTpHandler::ConsequtiveTransferState IsoTpHandler::consequtiveTransferState; + +IsoTpHandler::IdleState::IdleState() +{ +} + +void IsoTpHandler::IdleState::processInput(const CANMessage* message, IsoTpHandler* context) const +{ + if (!IsoTpHandler::isValidIsoTpPacket(message)) + { + return; + } + uint8_t messageType = message->data[0] >> 4; + if (messageType == SINGLE_FRAME) + { + uint8_t messageSize = message->data[0] & 0x0F; + if (messageSize > message->len - 1) + { + printf("Iso tp message is too short: iso len %d vs can len %d\n", messageSize, message->len); + return; + } + context->handle_decoded_packet(&message->data[1], messageSize); + return; + } + if (messageType == FIRST_FRAME) + { + if (message->len != 8) + { + printf("Invalid iso tp message length for FIRST_FRAME, length is %d\n", message->len); + return; + } + + uint16_t messageSize = ((message->data[0] & 0x0F) << 8) | (message->data[1]); + context->init_consequtive_reading(messageSize, &message->data[2]); + context->setState(&consequtiveTransferState); + return; + } + if (messageType == CONSECUTIVE_FRAME) + { + printf("Invalid iso tp message in idle state, because unexpected CONSECUTIVE_FRAME received\n"); + return; + } + if (messageType == FLOW_CONTOL_FRAME) + { + printf("Invalid iso tp message, because unexpected FLOW_CONTOL_FRAME received\n"); + return; + } + + printf("Invalid iso tp message ?!\n"); +} + +void IsoTpHandler::IdleState::onEnter(IsoTpHandler* context) const +{ +} + +void IsoTpHandler::IdleState::onLeave(IsoTpHandler* context) const +{ +} + +IsoTpHandler::ConsequtiveTransferState::ConsequtiveTransferState() +{ +} + +void IsoTpHandler::ConsequtiveTransferState::processInput(const CANMessage* message, IsoTpHandler* context) const +{ + if (!IsoTpHandler::isValidIsoTpPacket(message)) + { + return; + } + uint8_t messageType = message->data[0] >> 4; + if (messageType == SINGLE_FRAME) + { + printf("Received SINGLE_FRAME, expected consequitve frame\n"); + return; + } + if (messageType == FIRST_FRAME) + { + printf("Received FIRST_FRAME, expected consequitve frame\n"); + return; + } + if (messageType == CONSECUTIVE_FRAME) + { + uint8_t index = message->data[0] & 0x0F; + if (index != context->getExpectedIndex()) + { + printf("In consequiive frame, received index %d, expected %d\n", index, context->getExpectedIndex()); + context->setState(&IsoTpHandler::idleState); + return; + + } + + if (context->appendReceivedData(&message->data[1], message->len - 1)) + { + printf("In consequtive frame, change state\n"); + + context->setState(&IsoTpHandler::idleState); + } + return; + } + if (messageType == FLOW_CONTOL_FRAME) + { + printf("Received FLOW_CONTROL_FRAME, expected consequitve frame\n"); + return; + } + + printf("Invalid iso tp message, expected consequitve frame ?!\n"); +} + +void IsoTpHandler::ConsequtiveTransferState::onEnter(IsoTpHandler* context) const +{ +} + +void IsoTpHandler::ConsequtiveTransferState::onLeave(IsoTpHandler* context) const +{ +} + +IsoTpHandler::IsoTpHandler(CAN* canInterface) +: m_state(&idleState) +, m_canInterface (canInterface) +{ + m_state->onEnter(this); +} + +void IsoTpHandler::processCanMessage(const CANMessage* message) +{ + printf("Received new CAN message:\n"); + printf(" ID: 0x%X\n", message->id); + printf(" Len: %d\n", message->len); + printf(" Type: %s\n", (message->type == CANData ? "data" : "remote")); + printf(" Format: %s\n", (message->format == CANStandard ? "standard" : "extended")); + printf( "Data: "); + if (message->len > 8) { + //paranoia + error(" WRONG DATA LEN! "); + return; + } + for (unsigned int i = 0; i < message->len; ++i) { + printf("%X ", message->data[i]); + } + printf("\n"); + m_state->processInput(message, this); +} + +void IsoTpHandler::handle_decoded_packet(const uint8_t* data, uint16_t length) +{ + //todo write into mailbox so another thread can consume this or directly call a callback + printf("New decoded packet: Length: %d\n", length); + printf(" Data: "); + for (uint16_t i = 0; i < length; ++i) + { + printf("%X ", data[i]); + } + printf("\n"); +} + +void IsoTpHandler::init_consequtive_reading(uint16_t messageSize, const uint8_t* data) +{ + char msgContent[8]; + msgContent[0] = (FLOW_CONTOL_FRAME << 4) | CLEAR_TO_SEND; + msgContent[1] = 0; //remaining frames should to be sent without flow control or delay + msgContent[2] = 0; //Separation Time (ST), minimum delay time between frames (end of one frame and the beginning of the other) + //<= 127, separation time in milliseconds. + //0xF1 to 0xF9, 100 to 900 microseconds. + msgContent[3] = 0; + msgContent[4] = 0; + msgContent[5] = 0; + msgContent[6] = 0; + msgContent[7] = 0; + m_canInterface->write(CANMessage(0x7DF, msgContent, sizeof(msgContent))); + + memcpy(m_messageBuffer, data, 6); + m_expectedMessageSize = messageSize; + m_currentMessageSize = 6; + m_expectedIndex = 1; +} + +void IsoTpHandler::setState(const State* state) +{ + if (state == m_state) + { + return; + } + + m_state->onLeave(this); + m_state = state; + m_state->onEnter(this); +} + +bool IsoTpHandler::isValidIsoTpPacket(const CANMessage* message) +{ + if (message->len < 1) + { + printf("Invalid iso tp message, length is zero\n"); + return false; + } + uint8_t messageType = message->data[0] >> 4; + if (messageType > FLOW_CONTOL_FRAME) + { + printf("Invalid iso tp message type %d\n", messageType); + return false; + } + return true; +} + +uint8_t IsoTpHandler::getExpectedIndex() const +{ + return m_expectedIndex; +} + +void IsoTpHandler::incrementExpectedIndex() +{ + ++m_expectedIndex; + if (m_expectedIndex >= 16) + { + m_expectedIndex = 0; + } +} + +bool IsoTpHandler::appendReceivedData(const uint8_t* data, uint8_t length) +{ + if (sizeof(m_messageBuffer) < m_currentMessageSize + length) + { + printf("Buffer in appendReceivedData too small, already got %d bytes, new %d bytes, expected %d bytes.\n", m_currentMessageSize, length, m_expectedMessageSize); + return true; //switch state + } + + if (m_expectedMessageSize < m_currentMessageSize + length) + { + printf("Got too much data in appendReceivedData, already got %d bytes, new %d bytes, expected %d bytes.\n", m_currentMessageSize, length, m_expectedMessageSize); + length = m_expectedMessageSize - m_currentMessageSize; + } + + memcpy(m_messageBuffer + m_currentMessageSize, data, length); + m_currentMessageSize += length; + + if (m_expectedMessageSize == m_currentMessageSize) + { + handle_decoded_packet(m_messageBuffer, m_expectedMessageSize); + return true; //switch state + } + + return false; //do not switch state +} \ No newline at end of file