Diff: IsoTpHandler.cpp

Revision:

2:d3d61d9d323e

Parent:

0:6b1f6139fb25

Child:

3:eb807d330292

--- a/IsoTpHandler.cpp	Tue Apr 22 15:32:49 2014 +0000
+++ b/IsoTpHandler.cpp	Sun Apr 27 14:50:13 2014 +0000
@@ -1,4 +1,6 @@
 #include "IsoTpHandler.h"
+#include "MODSERIAL.h"
+extern MODSERIAL pc;
 
 enum IsoTpMessageType
 {
@@ -34,7 +36,7 @@
         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);
+            pc.printf("Iso tp message is too short: iso len %d vs can len %d\r\n", messageSize, message->len);
             return;
         }
         context->handle_decoded_packet(&message->data[1], messageSize);
@@ -44,7 +46,7 @@
     {
         if (message->len != 8)
         {
-            printf("Invalid iso tp message length for FIRST_FRAME, length is %d\n", message->len);
+            pc.printf("Invalid iso tp message length for FIRST_FRAME, length is %d\r\n", message->len);
             return;
         }
         
@@ -55,16 +57,16 @@
     }
     if (messageType == CONSECUTIVE_FRAME)
     {
-        printf("Invalid iso tp message in idle state, because unexpected CONSECUTIVE_FRAME received\n");
+        pc.printf("Invalid iso tp message in idle state, because unexpected CONSECUTIVE_FRAME received\r\n");
         return;
     }
     if (messageType == FLOW_CONTOL_FRAME)
     {
-        printf("Invalid iso tp message, because unexpected FLOW_CONTOL_FRAME received\n");
+        pc.printf("Invalid iso tp message, because unexpected FLOW_CONTOL_FRAME received\r\n");
         return;
     }
     
-    printf("Invalid iso tp message ?!\n");
+    pc.printf("Invalid iso tp message ?!\r\n");
 }
 
 void IsoTpHandler::IdleState::onEnter(IsoTpHandler* context) const
@@ -88,12 +90,12 @@
     uint8_t messageType = message->data[0] >> 4;
     if (messageType == SINGLE_FRAME)
     {
-            printf("Received SINGLE_FRAME, expected consequitve frame\n");
+            pc.printf("Received SINGLE_FRAME, expected consequitve frame\r\n");
             return;
     }
     if (messageType == FIRST_FRAME)
     {
-        printf("Received FIRST_FRAME, expected consequitve frame\n");
+        pc.printf("Received FIRST_FRAME, expected consequitve frame\r\n");
         return;
     }
     if (messageType == CONSECUTIVE_FRAME)
@@ -101,7 +103,7 @@
         uint8_t index = message->data[0] & 0x0F;
         if (index != context->getExpectedIndex())
         {
-            printf("In consequiive frame, received index %d, expected %d\n", index, context->getExpectedIndex());
+            pc.printf("In consequiive frame, received index %d, expected %d\r\n", index, context->getExpectedIndex());
             context->setState(&IsoTpHandler::idleState);
             return;
     
@@ -109,19 +111,20 @@
         
         if (context->appendReceivedData(&message->data[1], message->len - 1))
         {
-            printf("In consequtive frame, change state\n");
+            pc.printf("In consequtive frame, change state\r\n");
             
             context->setState(&IsoTpHandler::idleState);
         }
+        context->incrementExpectedIndex();
         return;
     }
     if (messageType == FLOW_CONTOL_FRAME)
     {
-        printf("Received FLOW_CONTROL_FRAME, expected consequitve frame\n");
+        pc.printf("Received FLOW_CONTROL_FRAME, expected consequitve frame\r\n");
         return;
     }
     
-    printf("Invalid iso tp message, expected consequitve frame ?!\n");
+    pc.printf("Invalid iso tp message, expected consequitve frame ?!\r\n");
 }
 
 void IsoTpHandler::ConsequtiveTransferState::onEnter(IsoTpHandler* context) const
@@ -141,34 +144,34 @@
 
 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: ");
+    pc.printf("Received new CAN message:\r\n");
+    pc.printf(" ID: 0x%X\r\n", message->id);
+    pc.printf(" Len: %d\r\n", message->len);
+    pc.printf(" Type: %s\r\n", (message->type == CANData ? "data" : "remote"));
+    pc.printf(" Format: %s\r\n", (message->format == CANStandard ? "standard" : "extended"));
+    pc.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]);
+        pc.printf("%X ", message->data[i]);
     }
-    printf("\n");
+    pc.printf("\r\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: ");
+    pc.printf("New decoded packet: Length: %d\r\n", length);
+    pc.printf(" Data: ");
     for (uint16_t i = 0; i < length; ++i)
     {
-        printf("%X ", data[i]);
+        pc.printf("%X ", data[i]);
     }
-    printf("\n");
+    pc.printf("\r\n");
 }
 
 void IsoTpHandler::init_consequtive_reading(uint16_t messageSize, const uint8_t* data)
@@ -184,7 +187,7 @@
     msgContent[5] = 0;
     msgContent[6] = 0;
     msgContent[7] = 0;
-    m_canInterface->write(CANMessage(0x7DF, msgContent, sizeof(msgContent)));
+    m_canInterface->write(CANMessage(0x7E0, msgContent, sizeof(msgContent)));  //or 7DF?
     
     memcpy(m_messageBuffer, data, 6);
     m_expectedMessageSize = messageSize;
@@ -208,13 +211,13 @@
 {
     if (message->len < 1)
     {
-        printf("Invalid iso tp message, length is zero\n");
+        pc.printf("Invalid iso tp message, length is zero\r\n");
         return false;
     } 
     uint8_t messageType = message->data[0] >> 4;
     if (messageType > FLOW_CONTOL_FRAME)
     {
-        printf("Invalid iso tp message type %d\n", messageType);
+        pc.printf("Invalid iso tp message type %d\r\n", messageType);
         return false;
     }
     return true;
@@ -238,13 +241,13 @@
 {
     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);
+        pc.printf("Buffer in appendReceivedData too small, already got %d bytes, new %d bytes, expected %d bytes.\r\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);
+        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);
         length = m_expectedMessageSize - m_currentMessageSize;
     }