Diff: IsoTpHandler.cpp

Revision:

4:0e2d6cc31afb

Parent:

3:eb807d330292

--- a/IsoTpHandler.cpp	Sun Apr 27 17:11:32 2014 +0000
+++ b/IsoTpHandler.cpp	Sun Apr 27 17:42:32 2014 +0000
@@ -4,6 +4,12 @@
 #include "MODSERIAL.h"
 extern MODSERIAL pc;
 
+#ifdef ACTIVATE_DEBUG_OUTPUT
+#define DEBUG_PRINT(format, ...) pc.printf(format, ##__VA_ARGS__) 
+#else
+#define DEBUG_PRINT(format, ...)
+#endif
+
 static PidDecoder pidDecoder;
 
 enum IsoTpMessageType
@@ -29,7 +35,7 @@
 }
 
 void IsoTpHandler::IdleState::processInput(const CANMessage* message, IsoTpHandler* context) const
-{        
+{
     if (!IsoTpHandler::isValidIsoTpPacket(message))
     {
         return;
@@ -40,7 +46,7 @@
         uint8_t messageSize = message->data[0] & 0x0F;
         if (messageSize > message->len - 1)
         {
-            pc.printf("Iso tp message is too short: iso len %d vs can len %d\r\n", messageSize, message->len);
+            DEBUG_PRINT("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);
@@ -50,7 +56,7 @@
     {
         if (message->len != 8)
         {
-            pc.printf("Invalid iso tp message length for FIRST_FRAME, length is %d\r\n", message->len);
+            DEBUG_PRINT("Invalid iso tp message length for FIRST_FRAME, length is %d\r\n", message->len);
             return;
         }
         
@@ -61,16 +67,16 @@
     }
     if (messageType == CONSECUTIVE_FRAME)
     {
-        pc.printf("Invalid iso tp message in idle state, because unexpected CONSECUTIVE_FRAME received\r\n");
+        DEBUG_PRINT("Invalid iso tp message in idle state, because unexpected CONSECUTIVE_FRAME received\r\n");
         return;
     }
     if (messageType == FLOW_CONTOL_FRAME)
     {
-        pc.printf("Invalid iso tp message, because unexpected FLOW_CONTOL_FRAME received\r\n");
+        DEBUG_PRINT("Invalid iso tp message, because unexpected FLOW_CONTOL_FRAME received\r\n");
         return;
     }
     
-    pc.printf("Invalid iso tp message ?!\r\n");
+    DEBUG_PRINT("Invalid iso tp message ?!\r\n");
 }
 
 void IsoTpHandler::IdleState::onEnter(IsoTpHandler* context) const
@@ -94,12 +100,12 @@
     uint8_t messageType = message->data[0] >> 4;
     if (messageType == SINGLE_FRAME)
     {
-            pc.printf("Received SINGLE_FRAME, expected consequitve frame\r\n");
+            DEBUG_PRINT("Received SINGLE_FRAME, expected consequitve frame\r\n");
             return;
     }
     if (messageType == FIRST_FRAME)
     {
-        pc.printf("Received FIRST_FRAME, expected consequitve frame\r\n");
+        DEBUG_PRINT("Received FIRST_FRAME, expected consequitve frame\r\n");
         return;
     }
     if (messageType == CONSECUTIVE_FRAME)
@@ -107,7 +113,7 @@
         uint8_t index = message->data[0] & 0x0F;
         if (index != context->getExpectedIndex())
         {
-            pc.printf("In consequiive frame, received index %d, expected %d\r\n", index, context->getExpectedIndex());
+            DEBUG_PRINT("In consequiive frame, received index %d, expected %d\r\n", index, context->getExpectedIndex());
             context->setState(&IsoTpHandler::idleState);
             return;
     
@@ -115,7 +121,7 @@
         
         if (context->appendReceivedData(&message->data[1], message->len - 1))
         {
-            pc.printf("In consequtive frame, change state\r\n");
+            DEBUG_PRINT("In consequtive frame, change state\r\n");
             
             context->setState(&IsoTpHandler::idleState);
         }
@@ -124,11 +130,11 @@
     }
     if (messageType == FLOW_CONTOL_FRAME)
     {
-        pc.printf("Received FLOW_CONTROL_FRAME, expected consequitve frame\r\n");
+        DEBUG_PRINT("Received FLOW_CONTROL_FRAME, expected consequitve frame\r\n");
         return;
     }
     
-    pc.printf("Invalid iso tp message, expected consequitve frame ?!\r\n");
+    DEBUG_PRINT("Invalid iso tp message, expected consequitve frame ?!\r\n");
 }
 
 void IsoTpHandler::ConsequtiveTransferState::onEnter(IsoTpHandler* context) const
@@ -148,34 +154,34 @@
 
 void IsoTpHandler::processCanMessage(const CANMessage* message)
 {
-    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: ");
+    DEBUG_PRINT("Received new CAN message:\r\n");
+    DEBUG_PRINT(" ID: 0x%X\r\n", message->id);
+    DEBUG_PRINT(" Len: %d\r\n", message->len);
+    DEBUG_PRINT(" Type: %s\r\n", (message->type == CANData ? "data" : "remote"));
+    DEBUG_PRINT(" Format: %s\r\n", (message->format == CANStandard ? "standard" : "extended"));
+    DEBUG_PRINT( "Data: ");
     if (message->len > 8) {
         //paranoia
         error(" WRONG DATA LEN! ");
         return;
     }
     for (unsigned int i = 0; i < message->len; ++i) {
-        pc.printf("%X ", message->data[i]);
+        DEBUG_PRINT("%X ", message->data[i]);
     }
-    pc.printf("\r\n");
+    DEBUG_PRINT("\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
-    pc.printf("New decoded packet: Length: %d\r\n", length);
-    pc.printf(" Data: ");
+    DEBUG_PRINT("New decoded packet: Length: %d\r\n", length);
+    DEBUG_PRINT(" Data: ");
     for (uint16_t i = 0; i < length; ++i)
     {
-        pc.printf("%X ", data[i]);
+        DEBUG_PRINT("%X ", data[i]);
     }
-    pc.printf("\r\n");
+    DEBUG_PRINT("\r\n");
     
     pidDecoder.decode(data, length);
 }
@@ -217,13 +223,13 @@
 {
     if (message->len < 1)
     {
-        pc.printf("Invalid iso tp message, length is zero\r\n");
+        DEBUG_PRINT("Invalid iso tp message, length is zero\r\n");
         return false;
     } 
     uint8_t messageType = message->data[0] >> 4;
     if (messageType > FLOW_CONTOL_FRAME)
     {
-        pc.printf("Invalid iso tp message type %d\r\n", messageType);
+        DEBUG_PRINT("Invalid iso tp message type %d\r\n", messageType);
         return false;
     }
     return true;
@@ -247,13 +253,13 @@
 {
     if (sizeof(m_messageBuffer) < m_currentMessageSize + length)
     {
-        pc.printf("Buffer in appendReceivedData too small, already got %d bytes, new %d bytes, expected %d bytes.\r\n", m_currentMessageSize, length, m_expectedMessageSize);
+        DEBUG_PRINT("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)
     {
-        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);
+        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);
         length = m_expectedMessageSize - m_currentMessageSize;
     }