Christian Taedcke / Mbed 2 deprecated ObdDisplay

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@7:a19b63c0a0fa, 2014-05-11 (annotated)

Committer:
chrta
Date:
Sun May 11 09:05:37 2014 +0000
Revision:
7:a19b63c0a0fa
Parent:
4:0e2d6cc31afb 
Changed Adafruit_GFX to my own fork.

Who changed what in which revision?

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