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
IsoTpHandler.cpp@3:eb807d330292, 2014-04-27 (annotated)
- Committer:
- chrta
- Date:
- Sun Apr 27 17:11:32 2014 +0000
- Revision:
- 3:eb807d330292
- Parent:
- 2:d3d61d9d323e
- Child:
- 4:0e2d6cc31afb
Working pid decoder
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
chrta | 0:6b1f6139fb25 | 1 | #include "IsoTpHandler.h" |
chrta | 3:eb807d330292 | 2 | #include "PidDecoder.h" |
chrta | 3:eb807d330292 | 3 | |
chrta | 2:d3d61d9d323e | 4 | #include "MODSERIAL.h" |
chrta | 2:d3d61d9d323e | 5 | extern MODSERIAL pc; |
chrta | 0:6b1f6139fb25 | 6 | |
chrta | 3:eb807d330292 | 7 | static PidDecoder pidDecoder; |
chrta | 3:eb807d330292 | 8 | |
chrta | 0:6b1f6139fb25 | 9 | enum IsoTpMessageType |
chrta | 0:6b1f6139fb25 | 10 | { |
chrta | 0:6b1f6139fb25 | 11 | SINGLE_FRAME = 0, ///< The single frame transferred contains the complete payload of up to 7 bytes (normal addressing) or 6 bytes (extended addressing) |
chrta | 0:6b1f6139fb25 | 12 | 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. |
chrta | 0:6b1f6139fb25 | 13 | CONSECUTIVE_FRAME = 2, ///< A frame containing subsequent data for a multi-frame packet |
chrta | 0:6b1f6139fb25 | 14 | 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. |
chrta | 0:6b1f6139fb25 | 15 | }; |
chrta | 0:6b1f6139fb25 | 16 | |
chrta | 0:6b1f6139fb25 | 17 | enum IsoTpFlowType |
chrta | 0:6b1f6139fb25 | 18 | { |
chrta | 0:6b1f6139fb25 | 19 | CLEAR_TO_SEND = 0, |
chrta | 0:6b1f6139fb25 | 20 | WAIT = 1, |
chrta | 0:6b1f6139fb25 | 21 | OVERFLOW_ABORT = 2, |
chrta | 0:6b1f6139fb25 | 22 | }; |
chrta | 0:6b1f6139fb25 | 23 | |
chrta | 0:6b1f6139fb25 | 24 | const IsoTpHandler::IdleState IsoTpHandler::idleState; |
chrta | 0:6b1f6139fb25 | 25 | const IsoTpHandler::ConsequtiveTransferState IsoTpHandler::consequtiveTransferState; |
chrta | 0:6b1f6139fb25 | 26 | |
chrta | 0:6b1f6139fb25 | 27 | IsoTpHandler::IdleState::IdleState() |
chrta | 0:6b1f6139fb25 | 28 | { |
chrta | 0:6b1f6139fb25 | 29 | } |
chrta | 0:6b1f6139fb25 | 30 | |
chrta | 0:6b1f6139fb25 | 31 | void IsoTpHandler::IdleState::processInput(const CANMessage* message, IsoTpHandler* context) const |
chrta | 0:6b1f6139fb25 | 32 | { |
chrta | 0:6b1f6139fb25 | 33 | if (!IsoTpHandler::isValidIsoTpPacket(message)) |
chrta | 0:6b1f6139fb25 | 34 | { |
chrta | 0:6b1f6139fb25 | 35 | return; |
chrta | 0:6b1f6139fb25 | 36 | } |
chrta | 0:6b1f6139fb25 | 37 | uint8_t messageType = message->data[0] >> 4; |
chrta | 0:6b1f6139fb25 | 38 | if (messageType == SINGLE_FRAME) |
chrta | 0:6b1f6139fb25 | 39 | { |
chrta | 0:6b1f6139fb25 | 40 | uint8_t messageSize = message->data[0] & 0x0F; |
chrta | 0:6b1f6139fb25 | 41 | if (messageSize > message->len - 1) |
chrta | 0:6b1f6139fb25 | 42 | { |
chrta | 2:d3d61d9d323e | 43 | pc.printf("Iso tp message is too short: iso len %d vs can len %d\r\n", messageSize, message->len); |
chrta | 0:6b1f6139fb25 | 44 | return; |
chrta | 0:6b1f6139fb25 | 45 | } |
chrta | 0:6b1f6139fb25 | 46 | context->handle_decoded_packet(&message->data[1], messageSize); |
chrta | 0:6b1f6139fb25 | 47 | return; |
chrta | 0:6b1f6139fb25 | 48 | } |
chrta | 0:6b1f6139fb25 | 49 | if (messageType == FIRST_FRAME) |
chrta | 0:6b1f6139fb25 | 50 | { |
chrta | 0:6b1f6139fb25 | 51 | if (message->len != 8) |
chrta | 0:6b1f6139fb25 | 52 | { |
chrta | 2:d3d61d9d323e | 53 | pc.printf("Invalid iso tp message length for FIRST_FRAME, length is %d\r\n", message->len); |
chrta | 0:6b1f6139fb25 | 54 | return; |
chrta | 0:6b1f6139fb25 | 55 | } |
chrta | 0:6b1f6139fb25 | 56 | |
chrta | 0:6b1f6139fb25 | 57 | uint16_t messageSize = ((message->data[0] & 0x0F) << 8) | (message->data[1]); |
chrta | 0:6b1f6139fb25 | 58 | context->init_consequtive_reading(messageSize, &message->data[2]); |
chrta | 0:6b1f6139fb25 | 59 | context->setState(&consequtiveTransferState); |
chrta | 0:6b1f6139fb25 | 60 | return; |
chrta | 0:6b1f6139fb25 | 61 | } |
chrta | 0:6b1f6139fb25 | 62 | if (messageType == CONSECUTIVE_FRAME) |
chrta | 0:6b1f6139fb25 | 63 | { |
chrta | 2:d3d61d9d323e | 64 | pc.printf("Invalid iso tp message in idle state, because unexpected CONSECUTIVE_FRAME received\r\n"); |
chrta | 0:6b1f6139fb25 | 65 | return; |
chrta | 0:6b1f6139fb25 | 66 | } |
chrta | 0:6b1f6139fb25 | 67 | if (messageType == FLOW_CONTOL_FRAME) |
chrta | 0:6b1f6139fb25 | 68 | { |
chrta | 2:d3d61d9d323e | 69 | pc.printf("Invalid iso tp message, because unexpected FLOW_CONTOL_FRAME received\r\n"); |
chrta | 0:6b1f6139fb25 | 70 | return; |
chrta | 0:6b1f6139fb25 | 71 | } |
chrta | 0:6b1f6139fb25 | 72 | |
chrta | 2:d3d61d9d323e | 73 | pc.printf("Invalid iso tp message ?!\r\n"); |
chrta | 0:6b1f6139fb25 | 74 | } |
chrta | 0:6b1f6139fb25 | 75 | |
chrta | 0:6b1f6139fb25 | 76 | void IsoTpHandler::IdleState::onEnter(IsoTpHandler* context) const |
chrta | 0:6b1f6139fb25 | 77 | { |
chrta | 0:6b1f6139fb25 | 78 | } |
chrta | 0:6b1f6139fb25 | 79 | |
chrta | 0:6b1f6139fb25 | 80 | void IsoTpHandler::IdleState::onLeave(IsoTpHandler* context) const |
chrta | 0:6b1f6139fb25 | 81 | { |
chrta | 0:6b1f6139fb25 | 82 | } |
chrta | 0:6b1f6139fb25 | 83 | |
chrta | 0:6b1f6139fb25 | 84 | IsoTpHandler::ConsequtiveTransferState::ConsequtiveTransferState() |
chrta | 0:6b1f6139fb25 | 85 | { |
chrta | 0:6b1f6139fb25 | 86 | } |
chrta | 0:6b1f6139fb25 | 87 | |
chrta | 0:6b1f6139fb25 | 88 | void IsoTpHandler::ConsequtiveTransferState::processInput(const CANMessage* message, IsoTpHandler* context) const |
chrta | 0:6b1f6139fb25 | 89 | { |
chrta | 0:6b1f6139fb25 | 90 | if (!IsoTpHandler::isValidIsoTpPacket(message)) |
chrta | 0:6b1f6139fb25 | 91 | { |
chrta | 0:6b1f6139fb25 | 92 | return; |
chrta | 0:6b1f6139fb25 | 93 | } |
chrta | 0:6b1f6139fb25 | 94 | uint8_t messageType = message->data[0] >> 4; |
chrta | 0:6b1f6139fb25 | 95 | if (messageType == SINGLE_FRAME) |
chrta | 0:6b1f6139fb25 | 96 | { |
chrta | 2:d3d61d9d323e | 97 | pc.printf("Received SINGLE_FRAME, expected consequitve frame\r\n"); |
chrta | 0:6b1f6139fb25 | 98 | return; |
chrta | 0:6b1f6139fb25 | 99 | } |
chrta | 0:6b1f6139fb25 | 100 | if (messageType == FIRST_FRAME) |
chrta | 0:6b1f6139fb25 | 101 | { |
chrta | 2:d3d61d9d323e | 102 | pc.printf("Received FIRST_FRAME, expected consequitve frame\r\n"); |
chrta | 0:6b1f6139fb25 | 103 | return; |
chrta | 0:6b1f6139fb25 | 104 | } |
chrta | 0:6b1f6139fb25 | 105 | if (messageType == CONSECUTIVE_FRAME) |
chrta | 0:6b1f6139fb25 | 106 | { |
chrta | 0:6b1f6139fb25 | 107 | uint8_t index = message->data[0] & 0x0F; |
chrta | 0:6b1f6139fb25 | 108 | if (index != context->getExpectedIndex()) |
chrta | 0:6b1f6139fb25 | 109 | { |
chrta | 2:d3d61d9d323e | 110 | pc.printf("In consequiive frame, received index %d, expected %d\r\n", index, context->getExpectedIndex()); |
chrta | 0:6b1f6139fb25 | 111 | context->setState(&IsoTpHandler::idleState); |
chrta | 0:6b1f6139fb25 | 112 | return; |
chrta | 0:6b1f6139fb25 | 113 | |
chrta | 0:6b1f6139fb25 | 114 | } |
chrta | 0:6b1f6139fb25 | 115 | |
chrta | 0:6b1f6139fb25 | 116 | if (context->appendReceivedData(&message->data[1], message->len - 1)) |
chrta | 0:6b1f6139fb25 | 117 | { |
chrta | 2:d3d61d9d323e | 118 | pc.printf("In consequtive frame, change state\r\n"); |
chrta | 0:6b1f6139fb25 | 119 | |
chrta | 0:6b1f6139fb25 | 120 | context->setState(&IsoTpHandler::idleState); |
chrta | 0:6b1f6139fb25 | 121 | } |
chrta | 2:d3d61d9d323e | 122 | context->incrementExpectedIndex(); |
chrta | 0:6b1f6139fb25 | 123 | return; |
chrta | 0:6b1f6139fb25 | 124 | } |
chrta | 0:6b1f6139fb25 | 125 | if (messageType == FLOW_CONTOL_FRAME) |
chrta | 0:6b1f6139fb25 | 126 | { |
chrta | 2:d3d61d9d323e | 127 | pc.printf("Received FLOW_CONTROL_FRAME, expected consequitve frame\r\n"); |
chrta | 0:6b1f6139fb25 | 128 | return; |
chrta | 0:6b1f6139fb25 | 129 | } |
chrta | 0:6b1f6139fb25 | 130 | |
chrta | 2:d3d61d9d323e | 131 | pc.printf("Invalid iso tp message, expected consequitve frame ?!\r\n"); |
chrta | 0:6b1f6139fb25 | 132 | } |
chrta | 0:6b1f6139fb25 | 133 | |
chrta | 0:6b1f6139fb25 | 134 | void IsoTpHandler::ConsequtiveTransferState::onEnter(IsoTpHandler* context) const |
chrta | 0:6b1f6139fb25 | 135 | { |
chrta | 0:6b1f6139fb25 | 136 | } |
chrta | 0:6b1f6139fb25 | 137 | |
chrta | 0:6b1f6139fb25 | 138 | void IsoTpHandler::ConsequtiveTransferState::onLeave(IsoTpHandler* context) const |
chrta | 0:6b1f6139fb25 | 139 | { |
chrta | 0:6b1f6139fb25 | 140 | } |
chrta | 0:6b1f6139fb25 | 141 | |
chrta | 0:6b1f6139fb25 | 142 | IsoTpHandler::IsoTpHandler(CAN* canInterface) |
chrta | 0:6b1f6139fb25 | 143 | : m_state(&idleState) |
chrta | 0:6b1f6139fb25 | 144 | , m_canInterface (canInterface) |
chrta | 0:6b1f6139fb25 | 145 | { |
chrta | 0:6b1f6139fb25 | 146 | m_state->onEnter(this); |
chrta | 0:6b1f6139fb25 | 147 | } |
chrta | 0:6b1f6139fb25 | 148 | |
chrta | 0:6b1f6139fb25 | 149 | void IsoTpHandler::processCanMessage(const CANMessage* message) |
chrta | 0:6b1f6139fb25 | 150 | { |
chrta | 2:d3d61d9d323e | 151 | pc.printf("Received new CAN message:\r\n"); |
chrta | 2:d3d61d9d323e | 152 | pc.printf(" ID: 0x%X\r\n", message->id); |
chrta | 2:d3d61d9d323e | 153 | pc.printf(" Len: %d\r\n", message->len); |
chrta | 2:d3d61d9d323e | 154 | pc.printf(" Type: %s\r\n", (message->type == CANData ? "data" : "remote")); |
chrta | 2:d3d61d9d323e | 155 | pc.printf(" Format: %s\r\n", (message->format == CANStandard ? "standard" : "extended")); |
chrta | 2:d3d61d9d323e | 156 | pc.printf( "Data: "); |
chrta | 0:6b1f6139fb25 | 157 | if (message->len > 8) { |
chrta | 0:6b1f6139fb25 | 158 | //paranoia |
chrta | 0:6b1f6139fb25 | 159 | error(" WRONG DATA LEN! "); |
chrta | 0:6b1f6139fb25 | 160 | return; |
chrta | 0:6b1f6139fb25 | 161 | } |
chrta | 0:6b1f6139fb25 | 162 | for (unsigned int i = 0; i < message->len; ++i) { |
chrta | 2:d3d61d9d323e | 163 | pc.printf("%X ", message->data[i]); |
chrta | 0:6b1f6139fb25 | 164 | } |
chrta | 2:d3d61d9d323e | 165 | pc.printf("\r\n"); |
chrta | 0:6b1f6139fb25 | 166 | m_state->processInput(message, this); |
chrta | 0:6b1f6139fb25 | 167 | } |
chrta | 0:6b1f6139fb25 | 168 | |
chrta | 0:6b1f6139fb25 | 169 | void IsoTpHandler::handle_decoded_packet(const uint8_t* data, uint16_t length) |
chrta | 0:6b1f6139fb25 | 170 | { |
chrta | 0:6b1f6139fb25 | 171 | //todo write into mailbox so another thread can consume this or directly call a callback |
chrta | 2:d3d61d9d323e | 172 | pc.printf("New decoded packet: Length: %d\r\n", length); |
chrta | 2:d3d61d9d323e | 173 | pc.printf(" Data: "); |
chrta | 0:6b1f6139fb25 | 174 | for (uint16_t i = 0; i < length; ++i) |
chrta | 0:6b1f6139fb25 | 175 | { |
chrta | 2:d3d61d9d323e | 176 | pc.printf("%X ", data[i]); |
chrta | 0:6b1f6139fb25 | 177 | } |
chrta | 2:d3d61d9d323e | 178 | pc.printf("\r\n"); |
chrta | 3:eb807d330292 | 179 | |
chrta | 3:eb807d330292 | 180 | pidDecoder.decode(data, length); |
chrta | 0:6b1f6139fb25 | 181 | } |
chrta | 0:6b1f6139fb25 | 182 | |
chrta | 0:6b1f6139fb25 | 183 | void IsoTpHandler::init_consequtive_reading(uint16_t messageSize, const uint8_t* data) |
chrta | 0:6b1f6139fb25 | 184 | { |
chrta | 0:6b1f6139fb25 | 185 | char msgContent[8]; |
chrta | 0:6b1f6139fb25 | 186 | msgContent[0] = (FLOW_CONTOL_FRAME << 4) | CLEAR_TO_SEND; |
chrta | 0:6b1f6139fb25 | 187 | msgContent[1] = 0; //remaining frames should to be sent without flow control or delay |
chrta | 0:6b1f6139fb25 | 188 | msgContent[2] = 0; //Separation Time (ST), minimum delay time between frames (end of one frame and the beginning of the other) |
chrta | 0:6b1f6139fb25 | 189 | //<= 127, separation time in milliseconds. |
chrta | 0:6b1f6139fb25 | 190 | //0xF1 to 0xF9, 100 to 900 microseconds. |
chrta | 0:6b1f6139fb25 | 191 | msgContent[3] = 0; |
chrta | 0:6b1f6139fb25 | 192 | msgContent[4] = 0; |
chrta | 0:6b1f6139fb25 | 193 | msgContent[5] = 0; |
chrta | 0:6b1f6139fb25 | 194 | msgContent[6] = 0; |
chrta | 0:6b1f6139fb25 | 195 | msgContent[7] = 0; |
chrta | 2:d3d61d9d323e | 196 | m_canInterface->write(CANMessage(0x7E0, msgContent, sizeof(msgContent))); //or 7DF? |
chrta | 0:6b1f6139fb25 | 197 | |
chrta | 0:6b1f6139fb25 | 198 | memcpy(m_messageBuffer, data, 6); |
chrta | 0:6b1f6139fb25 | 199 | m_expectedMessageSize = messageSize; |
chrta | 0:6b1f6139fb25 | 200 | m_currentMessageSize = 6; |
chrta | 0:6b1f6139fb25 | 201 | m_expectedIndex = 1; |
chrta | 0:6b1f6139fb25 | 202 | } |
chrta | 0:6b1f6139fb25 | 203 | |
chrta | 0:6b1f6139fb25 | 204 | void IsoTpHandler::setState(const State* state) |
chrta | 0:6b1f6139fb25 | 205 | { |
chrta | 0:6b1f6139fb25 | 206 | if (state == m_state) |
chrta | 0:6b1f6139fb25 | 207 | { |
chrta | 0:6b1f6139fb25 | 208 | return; |
chrta | 0:6b1f6139fb25 | 209 | } |
chrta | 0:6b1f6139fb25 | 210 | |
chrta | 0:6b1f6139fb25 | 211 | m_state->onLeave(this); |
chrta | 0:6b1f6139fb25 | 212 | m_state = state; |
chrta | 0:6b1f6139fb25 | 213 | m_state->onEnter(this); |
chrta | 0:6b1f6139fb25 | 214 | } |
chrta | 0:6b1f6139fb25 | 215 | |
chrta | 0:6b1f6139fb25 | 216 | bool IsoTpHandler::isValidIsoTpPacket(const CANMessage* message) |
chrta | 0:6b1f6139fb25 | 217 | { |
chrta | 0:6b1f6139fb25 | 218 | if (message->len < 1) |
chrta | 0:6b1f6139fb25 | 219 | { |
chrta | 2:d3d61d9d323e | 220 | pc.printf("Invalid iso tp message, length is zero\r\n"); |
chrta | 0:6b1f6139fb25 | 221 | return false; |
chrta | 0:6b1f6139fb25 | 222 | } |
chrta | 0:6b1f6139fb25 | 223 | uint8_t messageType = message->data[0] >> 4; |
chrta | 0:6b1f6139fb25 | 224 | if (messageType > FLOW_CONTOL_FRAME) |
chrta | 0:6b1f6139fb25 | 225 | { |
chrta | 2:d3d61d9d323e | 226 | pc.printf("Invalid iso tp message type %d\r\n", messageType); |
chrta | 0:6b1f6139fb25 | 227 | return false; |
chrta | 0:6b1f6139fb25 | 228 | } |
chrta | 0:6b1f6139fb25 | 229 | return true; |
chrta | 0:6b1f6139fb25 | 230 | } |
chrta | 0:6b1f6139fb25 | 231 | |
chrta | 0:6b1f6139fb25 | 232 | uint8_t IsoTpHandler::getExpectedIndex() const |
chrta | 0:6b1f6139fb25 | 233 | { |
chrta | 0:6b1f6139fb25 | 234 | return m_expectedIndex; |
chrta | 0:6b1f6139fb25 | 235 | } |
chrta | 0:6b1f6139fb25 | 236 | |
chrta | 0:6b1f6139fb25 | 237 | void IsoTpHandler::incrementExpectedIndex() |
chrta | 0:6b1f6139fb25 | 238 | { |
chrta | 0:6b1f6139fb25 | 239 | ++m_expectedIndex; |
chrta | 0:6b1f6139fb25 | 240 | if (m_expectedIndex >= 16) |
chrta | 0:6b1f6139fb25 | 241 | { |
chrta | 0:6b1f6139fb25 | 242 | m_expectedIndex = 0; |
chrta | 0:6b1f6139fb25 | 243 | } |
chrta | 0:6b1f6139fb25 | 244 | } |
chrta | 0:6b1f6139fb25 | 245 | |
chrta | 0:6b1f6139fb25 | 246 | bool IsoTpHandler::appendReceivedData(const uint8_t* data, uint8_t length) |
chrta | 0:6b1f6139fb25 | 247 | { |
chrta | 0:6b1f6139fb25 | 248 | if (sizeof(m_messageBuffer) < m_currentMessageSize + length) |
chrta | 0:6b1f6139fb25 | 249 | { |
chrta | 2:d3d61d9d323e | 250 | pc.printf("Buffer in appendReceivedData too small, already got %d bytes, new %d bytes, expected %d bytes.\r\n", m_currentMessageSize, length, m_expectedMessageSize); |
chrta | 0:6b1f6139fb25 | 251 | return true; //switch state |
chrta | 0:6b1f6139fb25 | 252 | } |
chrta | 0:6b1f6139fb25 | 253 | |
chrta | 0:6b1f6139fb25 | 254 | if (m_expectedMessageSize < m_currentMessageSize + length) |
chrta | 0:6b1f6139fb25 | 255 | { |
chrta | 2:d3d61d9d323e | 256 | pc.printf("Got too much data in appendReceivedData, already got %d bytes, new %d bytes, expected %d bytes.\r\n", m_currentMessageSize, length, m_expectedMessageSize); |
chrta | 0:6b1f6139fb25 | 257 | length = m_expectedMessageSize - m_currentMessageSize; |
chrta | 0:6b1f6139fb25 | 258 | } |
chrta | 0:6b1f6139fb25 | 259 | |
chrta | 0:6b1f6139fb25 | 260 | memcpy(m_messageBuffer + m_currentMessageSize, data, length); |
chrta | 0:6b1f6139fb25 | 261 | m_currentMessageSize += length; |
chrta | 0:6b1f6139fb25 | 262 | |
chrta | 0:6b1f6139fb25 | 263 | if (m_expectedMessageSize == m_currentMessageSize) |
chrta | 0:6b1f6139fb25 | 264 | { |
chrta | 0:6b1f6139fb25 | 265 | handle_decoded_packet(m_messageBuffer, m_expectedMessageSize); |
chrta | 0:6b1f6139fb25 | 266 | return true; //switch state |
chrta | 0:6b1f6139fb25 | 267 | } |
chrta | 0:6b1f6139fb25 | 268 | |
chrta | 0:6b1f6139fb25 | 269 | return false; //do not switch state |
chrta | 0:6b1f6139fb25 | 270 | } |