API tools faq
paste
Advertisement
VaZZoN

Arduino iso 9141-2 problem

VaZZoN
Mar 24th, 2019
465
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 39.76 KB | None | 0 0
raw download clone embed print report
  1. #include <avr/pgmspace.h>
  2. #include <AltSoftSerial.h>
  3. #include <RBD_Timer.h>
  4. #include <SPI.h>
  5. #include <Wire.h>
  6. #include <Adafruit_GFX.h>
  7. #include <Adafruit_SSD1306.h>
  8.  
  9. #define rxPin 8
  10. #define txPin 9
  11.  
  12. AltSoftSerial softSerial;
  13. #define MAX_RESPONSES 15
  14. uint32_t gPidSupport = 0;
  15. uint8_t responseBuffer[MAX_RESPONSES][11] = {0};
  16. #define BUFLEN 64
  17. char buffer[BUFLEN] = {0};
  18. RBD::Timer timerKeepAlive;
  19.  
  20. //OLED
  21. //Software SPI
  22. #define OLED_MOSI  3
  23. #define OLED_CLK   2
  24. #define OLED_DC    5
  25. #define OLED_CS    6
  26. #define OLED_RESET 4
  27. Adafruit_SSD1306 display(OLED_MOSI, OLED_CLK, OLED_DC, OLED_RESET, OLED_CS);
  28.  
  29. #define NUMFLAKES 10
  30. #define XPOS 0
  31. #define YPOS 1
  32. #define DELTAY 2
  33.  
  34. void init_5baud()
  35. {
  36.   pinMode( txPin, OUTPUT );
  37.   //2600ms
  38.   digitalWrite( txPin, HIGH );
  39.   //Init
  40.   Serial.println( "Starting Init" );
  41.   //Start bit
  42.   digitalWrite( txPin,LOW );
  43.   delay( 200 );
  44.   //Transmitting from right to left (LSB)
  45.   //0x33 = 00110011
  46.   digitalWrite( txPin, HIGH ); //11
  47.   delay( 400 );
  48.   digitalWrite( txPin, LOW ); //00
  49.   delay( 400 );
  50.   digitalWrite( txPin, HIGH ); //11
  51.   delay( 400 );
  52.   digitalWrite( txPin, LOW ); //00
  53.   delay( 400 );
  54.   //Stop bit
  55.   digitalWrite( txPin, HIGH );
  56.   delay( 200 );
  57.  
  58.   //Init softSerial at 10400 baudrate
  59.   softSerial.begin( 10400 );
  60.   bool bInitError = false;  // If there is an error, set bInitError to true
  61.   String strError = "";     // and store the error description in strError.
  62.   bool iso9141 = false;     // Will either be ISO 9141 or ISO 14230
  63.   // Wait for SYNC byte (0x55)
  64.   uint8_t sync;
  65.   if( !getSoftSerial( sync, 300 ) ) {
  66.     //DEBUG
  67.     snprintf_P( buffer, BUFLEN, PSTR("SYNC Byte: %2X"), sync );
  68.     Serial.println( buffer );
  69.  
  70.     // Continue Init if we got the correct SYNC Byte
  71.     if( sync == 0x55 ) {
  72.       uint8_t key1;
  73.       uint8_t key2;
  74.       uint8_t invKey2;
  75.       uint8_t invAddress;
  76.       // Get key1
  77.       if( !getSoftSerial( key1, 20 ) ) {
  78.         // Get key2
  79.         if( !getSoftSerial( key2, 20 ) ) {
  80.           //DEBUG
  81.           snprintf_P( buffer, BUFLEN, PSTR("KEY1: %X    KEY2: %X"), key1, key2 );
  82.           Serial.println( buffer );
  83.  
  84.           delay( 25 );  // 25ms <= W4 <= 50ms (Time between finished reciept of key2 and start send of inverted key2)
  85.           //DEBUG
  86.           invKey2 = ~key2;
  87.           snprintf_P( buffer, BUFLEN, PSTR("Sending ~KEY2: %X"), invKey2 );
  88.           Serial.println( buffer );
  89.  
  90.           //TODO: Use sendSoftSerial instead so we don't need to duplicate the junk read-back
  91.           softSerial.write( invKey2 );    // Send back inverted key2
  92.           uint8_t junk;
  93.           getSoftSerial( junk, 1 ); // TX and RX share the same line so we recieve back the byte we just sent
  94.           //DEBUG
  95.           snprintf_P( buffer, BUFLEN, PSTR("Received ~KEY2: %X"), junk );
  96.           Serial.println( buffer );
  97.  
  98.           if( !getSoftSerial( invAddress, 50 ) ) {
  99.             //DEBUG
  100.             snprintf_P( buffer, BUFLEN, PSTR("Inverted Address: %X"), invAddress );
  101.             Serial.println( buffer );
  102.  
  103.             //HACK: Arduino has a BUG where it cannot compare a char with a constant if
  104.             //      the char is negative (e.g. 0xCC becomes 0xFFCC)
  105.             //TODO: Check that this is 0xCC. If we use anything other than char Arduino
  106.             //      uses significantly more SRAM and we are unable to compile for the
  107.             //      2K available to use in a Pro Mini
  108.             if( invAddress == 0xCC ) {
  109.               // Check if we are using ISO 9141-2 (otherwise it is ISO 14230)
  110.               if( ( key1 == 0x08 ) && ( key2 == 0x08 ) ) {
  111.                 iso9141 = true;
  112.                 Serial.println( F("Protocol: ISO 9141-2") );
  113.                 Serial.println( F("5-baud Init SUCCESS") );
  114.               } else {
  115.                 //ERROR: Only ISO 9141-2 is supported
  116.                 bInitError = true;
  117.                 strError = F("ISO 14230-4 is not supported");
  118.               }
  119.             } else {    // if( invAddress == 0xCC ) {
  120.               //ERROR: Unexpected Inverted Address received
  121.               bInitError = true;
  122.               strError = F("Bad Inverted Address Byte");
  123.             }
  124.           } else {    // if( !getSoftSerial( invAddress, 50 ) ) {
  125.             //ERROR: invAddress timeout
  126.             bInitError = true;
  127.             strError = F("Did not receive invAddress before timeout occured");
  128.           }
  129.         } else {    // if( !getSoftSerial( key2, 20 ) ) {
  130.           //ERROR: key2 timeout
  131.           bInitError = true;
  132.           strError = F("Did not receive key1 before timeout occured");
  133.         }
  134.       } else {    // if( !getSoftSerial( key1, 20 ) ) {
  135.         //ERROR: key1 timeout
  136.         bInitError = true;
  137.         strError = F("Did not receive key1 before timeout occured");
  138.       }
  139.     } else {    // if( sync == 0x55 ) {
  140.       //ERROR: SYNC Byte is not correct
  141.       bInitError = true;
  142.       strError = F("Bad SYNC Byte");
  143.     }
  144.   } else {    // if( !getSoftSerial( sync, 300 ) ) {
  145.     //ERROR: sync timeout
  146.     bInitError = true;
  147.     display.println("SYNC BYTE ERROR");
  148.     display.display();
  149.     strError = F("Did not receive SYNC byte before Timeout occured");
  150.   }
  151.  
  152.   // Report any errors that occured during Init and enter inf loop
  153.   if( bInitError ) {
  154.     Serial.println( F("5-baud Init FAILED") );
  155.     Serial.println( strError );
  156.     Serial.println( F("RESET to Retry") );
  157.     display.println( "ERROR" );
  158.     // Go into inf loop to prevent WDT from being setup
  159.     while( true ) {}
  160.   }
  161.  
  162.   delay( 55 );  // 55ms <= P3 (wait between messages) <= 5s
  163.   // Init is over. Start reading data from the ECU
  164.  
  165.   // 55ms has passed since the last message. We now have (5sec - 55ms) to send another
  166.   timerKeepAlive.restart();
  167. }
  168.  
  169. //Po co? Dlaczego?
  170. boolean getSoftSerial( uint8_t& retVal, uint32_t timeout )
  171. {
  172.   uint32_t start = millis();
  173.   while( !softSerial.available() ) {
  174.     if( ( millis() - start ) > timeout ) {
  175.       return true;
  176.     }
  177.   }
  178.   // Return the (16-bit) int as a single (8-bit) byte
  179.   // We always check for data first with Serial.available()
  180.   // so no need to check for an empty buffer (e.g. 0xFFFF)
  181.   retVal = softSerial.read();
  182.   return false;
  183. }
  184.  
  185. //Tu rozumiem
  186. void sendSoftSerial( uint8_t* bytes, unsigned size )
  187. {
  188.   for( int i = 0; i < size; i++ ) {
  189.     // Send the next byte
  190.     softSerial.write( bytes[i] );
  191.     // TX and RX share the same line so we recieve back the byte we just sent
  192.     uint8_t trash;
  193.     getSoftSerial( trash, 1 );
  194.     //TODO: This can be adjusted depending on how long the above
  195.     //      function takes to execute. 5ms <= P4 <= 20ms
  196.     delay( 5 ); // P4 (Inter-byte spacing)
  197.   }
  198. }
  199.  
  200. //A co to się tu stanęło?
  201. //==================================================================
  202. //==================================================================
  203. //==================================================================
  204. //==================================================================
  205.  
  206. uint8_t sendPid( uint8_t mode, uint8_t pid = 0, boolean ping = false ) {
  207.   // Message format: Header1, Header2, Source Address, Mode, (PID), Checksum   (see SAE J1979)
  208.   uint8_t request[6] = {0x68, 0x6A, 0xF1, mode, 0x00, 0x00};
  209.   uint8_t crc = ( 0x68 + 0x6A + 0xF1 + mode );
  210.  
  211.   // Format the request based on which mode we are requesting
  212.   if( ( mode == 0x03 ) || ( mode == 0x04 ) ) {
  213.     // Mode 3 and 4 require no PID
  214.     request[4] = crc;
  215.     // request[5] is unused, leave it as 0x00
  216.     sendSoftSerial( request, 5 );   // Send the request
  217.   } else {
  218.     // Modes 1, 2, 5, and 9 require a PID
  219.     crc += pid;
  220.     request[4] = pid;
  221.     request[5] = crc;
  222.     sendSoftSerial( request, 6 );   // Send the request
  223.   }
  224.   // The ECU(s) will now wait 50ms (P2) to ensure that we are done transmitting before responding
  225.   delay( 50 );
  226.  
  227.   // Get all responses and store them in the responseBuffer
  228.   // Start loop to get a response from each ECU that wants to reply
  229.   boolean bMsgTimeout = false;
  230.   uint8_t messageNum = 0;
  231.  
  232.   while( !bMsgTimeout ) {
  233.     boolean bByteTimeout = false;
  234.     uint8_t trash;
  235.  
  236.     //TODO: Define constants for timeouts
  237.     uint8_t byteNum = 0;
  238.     while( !bByteTimeout ) {
  239.       if( ping ) {
  240.         bByteTimeout = getSoftSerial( trash, 20 ); // Store response
  241.       } else {
  242.         bByteTimeout = getSoftSerial( responseBuffer[messageNum][byteNum], 20 ); // Store response
  243.         if( bByteTimeout ) {
  244.           // Set the current byte to 0 if the timeout occured before it could be written
  245.           responseBuffer[messageNum][byteNum] = 0;
  246.         }
  247.       }
  248.  
  249.       byteNum++;
  250.     }
  251.  
  252.  
  253.     // Wait for another ECU to respond or for message timeout to occur
  254.     // 55ms must pass before we know that no other ECU will respond
  255.     // The previous message timed out for the last byte recieved so add
  256.     // the byteTimeout to the start time, and wait for messageTimeout
  257.     unsigned long startTime = millis() - 20;
  258.  
  259.     while( !softSerial.available() ) {
  260.       // Timeout is 55ms
  261.       if( ( millis() - startTime ) >= 55 ) {
  262.         bMsgTimeout = true;   // Leave the outter 'while' loop
  263.         break;  // Leave this 'while' loop
  264.       }
  265.     }
  266.  
  267.     // Fill the rest of the current message with zeros if this is not a ping
  268.     if( !ping ) {
  269.       for( int i = byteNum; byteNum < 11; byteNum++ ) {
  270.         responseBuffer[messageNum][byteNum] = 0;
  271.       }
  272.     }
  273.  
  274.     // Move to the next message if timeout was BETWEEN 20ms and 55ms
  275.     messageNum++;
  276.   }
  277.  
  278.   // 55ms has passed since the last message. We now have (5sec - 55ms) to send another
  279.   timerKeepAlive.restart();
  280.  
  281.   return messageNum;
  282. }
  283.  
  284. void dumpRawData( uint8_t numMsg )
  285. {
  286.   Serial.print( "Raw Data: " );
  287.   for( uint8_t i = 0; i < numMsg; i++ ) {
  288.     for( uint8_t j = 0; j < 11; j++ ) {
  289.       snprintf( buffer, BUFLEN, "%02X ", responseBuffer[i][j] );
  290.       Serial.print( buffer );
  291.     }
  292.     Serial.println();
  293.   }
  294. }
  295.  
  296. uint8_t firstDTCChar( uint16_t dtc )
  297. {
  298.   // Get the first two bits of the high byte
  299.   uint8_t hidtc = highByte( dtc & 0xC000 );
  300.   switch( hidtc ) {
  301.     case 0x00:
  302.       return 'P';
  303.     case 0x40:
  304.       return 'C';
  305.     case 0x80:
  306.       return 'B';
  307.     case 0xC0:
  308.       return 'U';
  309.   }
  310.   // ERROR - Bad input
  311.   return 0;
  312. }
  313.  
  314.  
  315. /*
  316.    Function to display a list of PIDs which may or may not be supported
  317.    chPid determines which PIDs are displayed (e.g. PID 0x20 will display 0x21 through 0x40)
  318. */
  319. void displaySupportedPids( uint8_t chMode, uint8_t chPid, uint8_t numEcu ) {
  320.   gPidSupport = 0;     // Reset global PID support
  321.   // Create a list of globally supported PIDs supported by any ECU in the system
  322.   for( uint8_t curEcu = 0; curEcu < numEcu; curEcu++ ) {
  323.     // Store the PIDs that are supported for this specific ECU packed into 4 bytes
  324.     uint32_t ecuPidSupport = ( responseBuffer[curEcu][5] << 24 ) | ( responseBuffer[curEcu][6] << 16 ) | ( responseBuffer[curEcu][7] << 8 ) | responseBuffer[curEcu][8];
  325.     // OR support with the list of globally supported PIDs for this system
  326.     gPidSupport = gPidSupport | ecuPidSupport;
  327.  
  328.     //DEBUG: Print which PIDs are supported for this ECU
  329.     //snprintf(buffer, BUFLEN, "ECU %2X: %8X", responseBuffer[ecuNum][2], ecuPidSupport);
  330.     //Serial.println(buffer);
  331.   }
  332.  
  333.   // Display a second Menu with available PIDs
  334.   snprintf_P( buffer, BUFLEN, PSTR("Mode %X PIDs Supported:"), chMode );
  335.   Serial.println( buffer );
  336.  
  337.   // Show 0x20 PIDs in 2 rows. First PID determined by chPid (e.g. chPid + 1)
  338.   for( uint8_t row = 0; row < 2; row++ ) {
  339.     Serial.print( F( "  " ) );
  340.     for( uint8_t col = 1; col <= 16; col++ ) {
  341.       snprintf_P( buffer, BUFLEN, PSTR("%02X "), (16 * row) + col + chPid);
  342.       Serial.print( buffer );
  343.     }
  344.     // Move below the numbers to print support (Y, N, or ?)
  345.     Serial.println();
  346.     Serial.print( F( "  " ) );
  347.  
  348.     // Now print if the previous PIDs are supported or not
  349.     for( uint8_t bitNum = (16 * row); bitNum < (16 * row) + 16; bitNum++ ) {
  350.       // Invert the bits to get supported PIDs in numerical order
  351.       if( ( gPidSupport >> ( 31 - bitNum ) ) & 0x01 ) {
  352.         Serial.print( F( " Y " ) );
  353.       } else {
  354.         // If there was no response then we don't know if there
  355.         // is support for PIDs with 0s. If there was a reply
  356.         // then 0s in gPidSupport means that PID is unsupported.
  357.         if( numEcu > 0 ) {
  358.           Serial.print( F(" N ") );
  359.         } else {
  360.           Serial.print( F(" ? ") );
  361.         }
  362.       }
  363.     }
  364.     Serial.println();
  365.  
  366.   }
  367.  
  368.   // Print the number of ECUs that responded to the request
  369.   Serial.print( F("Number of responding ECUs: ") );
  370.   Serial.println( numEcu, DEC );
  371.  
  372. }
  373.  
  374.  
  375.  
  376.  
  377.  
  378. //==================================================================
  379. //==================================================================
  380. //==================================================================
  381. //==================================================================
  382.  
  383. void formatResponse( uint8_t chMode, uint8_t chPid, uint8_t numMsg ) {
  384.   //DEBUG
  385.   dumpRawData(numMsg);
  386.  
  387.   if( chMode == 0x01 ) {
  388.     // Mode 1 - Show Current Data
  389.     switch( chPid ) {
  390.       case 0x00:
  391.       {
  392.         displaySupportedPids( chMode, chPid, numMsg );
  393.         break;
  394.       }
  395.       case 0x01:  // Monitor status since DTCs cleared
  396.       {
  397.         uint8_t numDTCs = 0;
  398.         for( uint8_t i = 0; i < numMsg; i++ ) {
  399.           // Display ECU address
  400.           snprintf_P( buffer, BUFLEN, PSTR("ECU %2X "), responseBuffer[i][2] );
  401.           Serial.println( buffer );
  402.  
  403.           // Display the MIL indicator status
  404.           if( responseBuffer[i][5] & 0x80 ) {
  405.             Serial.println( F("  MIL: ON") );
  406.           } else {
  407.             Serial.println( F("  MIL: OFF") );
  408.           }
  409.  
  410.           // Display the number of DTCs returned
  411.           Serial.print( F("  DTC_CNT: ") );
  412.           Serial.println( responseBuffer[i][5] & 0x7F, DEC );
  413.  
  414.           numDTCs +=  responseBuffer[i][5] & 0x7F;
  415.         }
  416.  
  417.         // Print the total number of DTCs
  418.         Serial.print( F("Total DTCs: ") );
  419.         Serial.println( numDTCs, DEC );
  420.         break;
  421.       }
  422.       case 0x02:  // DTC that caused required freeze frame data storage
  423.       {
  424.         uint16_t dtc = ( responseBuffer[0][5] << 8 ) | responseBuffer[0][6];
  425.         if( dtc == 0 ) {
  426.           Serial.println( F("DTCFRZF: No Freeze Frame Data") );
  427.         } else {
  428.           snprintf_P( buffer, BUFLEN, PSTR("DTCFRZF: %c%04X"), firstDTCChar( dtc & 0xC0 ), ( dtc & 0x3F ) );
  429.           Serial.println( buffer );
  430.         }
  431.         break;
  432.       }
  433.       case 0x03:  // Fuel system 1/2 status
  434.       {
  435.         for( int i = 5; i <= 6; i++ ) {
  436.           snprintf_P( buffer, BUFLEN, PSTR("FUELSYS%d "), i - 4 );
  437.           Serial.println( buffer );
  438.           switch( responseBuffer[0][i] ) {
  439.             case 0x01:
  440.               Serial.println( F("Open loop - has not yet satisfied conditions to go closed loop") );
  441.               break;
  442.             case 0x02:
  443.               Serial.println( F("Closed loop - using oxygen sensor(s) as feedback for fuel control") );
  444.               break;
  445.             case 0x04:
  446.               Serial.println( F("Open loop due to driving conditions (e.g. power enrichment, deceleration enleanment)") );
  447.               break;
  448.             case 0x08:
  449.               Serial.println( F("Open loop - due to detected system fault") );
  450.               break;
  451.             case 0x10:
  452.               Serial.println( F("Closed loop, but fault with at least one oxygen sensor - may be using single oxygen sensor for fuel control") );
  453.               break;
  454.           }
  455.         }
  456.         break;
  457.       }
  458.       case 0x04:  // Calculated LOAD Value
  459.       {
  460.         uint32_t load = map( responseBuffer[0][5], 0, 255, -40, 215 );
  461.         snprintf_P( buffer, BUFLEN, PSTR("LOAD_PCT: %d%%"), load );
  462.         Serial.println( buffer );
  463.         break;
  464.       }
  465.       case 0x05:  // Engine Coolant Temperature
  466.       {
  467.         uint32_t temp = map( responseBuffer[0][5], 0, 255, 0, 100 );
  468.         //snprintf_P( buffer, BUFLEN, PSTR("ECT: %d C"), temp );
  469.         //Serial.println( buffer );
  470.         Serial.println( "Temp: " + temp-48 );
  471.         display.println( temp-48 );
  472.         break;
  473.       }
  474.       case 0x06:
  475.       {
  476.         uint32_t fuel = map( responseBuffer[0][5], 0, 255, -100, 99.22 );
  477.         snprintf_P( buffer, BUFLEN, PSTR("SHRTFT1: %d%%"), fuel );
  478.         Serial.println( buffer );
  479.         fuel = map( responseBuffer[0][6], 0, 255, -100, 99.22 );
  480.         snprintf_P( buffer, BUFLEN, PSTR("SHRTFT3: %d%%"), fuel );
  481.         Serial.println( buffer );
  482.         break;
  483.       }
  484.       case 0x07:
  485.       {
  486.         uint32_t fuel = map( responseBuffer[0][5], 0, 255, -100, 99.22 );
  487.         snprintf_P( buffer, BUFLEN, PSTR("LONGFT1: %d%%"), fuel );
  488.         Serial.println( buffer );
  489.         fuel = map( responseBuffer[0][6], 0, 255, -100, 99.22 );
  490.         snprintf_P( buffer, BUFLEN, PSTR("LONGFT3: %d%%"), fuel );
  491.         Serial.println( buffer );
  492.         break;
  493.       }
  494.       case 0x08:
  495.       {
  496.         uint32_t fuel = map( responseBuffer[0][5], 0, 255, -100, 99.22 );
  497.         snprintf_P( buffer, BUFLEN, PSTR("SHRTFT2: %d%%"), fuel );
  498.         Serial.println( buffer );
  499.         fuel = map( responseBuffer[0][6], 0, 255, -100, 99.22 );
  500.         snprintf_P( buffer, BUFLEN, PSTR("SHRTFT4: %d%%"), fuel );
  501.         Serial.println( buffer );
  502.         break;
  503.       }
  504.       case 0x09:
  505.       {
  506.         uint32_t fuel = map( responseBuffer[0][5], 0, 255, -100, 99.22 );
  507.         snprintf_P( buffer, BUFLEN, PSTR("LONGFT2: %d%%"), fuel );
  508.         Serial.println( buffer );
  509.         fuel = map( responseBuffer[0][6], 0, 255, -100, 99.22 );
  510.         snprintf_P( buffer, BUFLEN, PSTR("LONGFT4: %d%%"), fuel );
  511.         Serial.println( buffer );
  512.         break;
  513.       }
  514.       case 0x0A:  // Fuel Rail Pressure (gauge)
  515.       {
  516.         uint32_t kpa = map( responseBuffer[0][5], 0, 255, 0, 765 );
  517.         snprintf_P( buffer, BUFLEN, PSTR("FRP: %d kPa"), kpa );
  518.         Serial.println( buffer );
  519.         break;
  520.       }
  521.       case 0x0B:  // Intake Manifold Absolute Pressure
  522.       {
  523.         uint32_t kpa = map( responseBuffer[0][5], 0, 255, 0, 255 );
  524.         snprintf_P( buffer, BUFLEN, PSTR("MAP: %d kPa"), kpa );
  525.         Serial.println( buffer );
  526.         break;
  527.       }
  528.       case 0x0C:  // Engine RPM
  529.       {
  530.         uint32_t rpm = map( ( responseBuffer[0][5] << 8 ) | responseBuffer[0][6], 0, 65535, 0, 16383.75 );
  531.         snprintf_P( buffer, BUFLEN, PSTR("RPM: %d/min"), rpm );
  532.         //Serial.println( buffer );
  533.         Serial.println( "RPM: " + rpm );
  534.         display.println( rpm );
  535.         break;
  536.       }
  537.       case 0x0D:  // Vehicle Speed Sensor
  538.       {
  539.         uint32_t vss = map( responseBuffer[0][5], 0, 255, 0, 255 );
  540.         snprintf_P( buffer, BUFLEN, PSTR("VSS: %d kPa"), vss );
  541.         //Serial.println( buffer );
  542.        
  543.         Serial.println( "VSS: " + vss );
  544.         display.println( vss );
  545.         break;
  546.       }
  547.       case 0x0E:  // Ignition Timing Advance for #1 Cylinder
  548.       {
  549.         uint32_t adv = map( responseBuffer[0][5], 0, 255, -64, 63.5 );
  550.         snprintf_P( buffer, BUFLEN, PSTR("SPARKADV: %d deg"), adv );
  551.         Serial.println( buffer );
  552.         break;
  553.       }
  554.       case 0x0F:  // Intake Air Temperature
  555.       {
  556.         uint32_t iat = map( responseBuffer[0][5], 0, 255, -40, 215 );
  557.         snprintf_P( buffer, BUFLEN, PSTR("IAT: %d C"), iat );
  558.         //Serial.println( buffer );
  559.        
  560.         Serial.println( "IAT: " + iat );
  561.         display.println( iat );
  562.         break;
  563.       }
  564.       case 0x10:  // Air Flow Rate from Mass Air Flow Sensor
  565.       {
  566.         uint32_t maf = map( ( responseBuffer[0][5] << 8 ) | responseBuffer[0][6], 0, 65535, 0, 655.35 );
  567.         snprintf_P( buffer, BUFLEN, PSTR("MAF: %d g/s"), maf );
  568.         Serial.println( buffer );
  569.         break;
  570.       }
  571.       case 0x11:  // Absolute Throttle Position
  572.       {
  573.         uint32_t tp = map( responseBuffer[0][5], 0, 255, 0, 100 );
  574.         snprintf_P( buffer, BUFLEN, PSTR("TP: %d%%"), tp );
  575.         Serial.println( buffer );
  576.         break;
  577.       }
  578.       case 0x12:  // Commanded Secondary Air Status
  579.       {
  580.         switch( responseBuffer[0][5] ) {
  581.           case 0x01:
  582.             Serial.println( F("AIR_STAT: UPS (upstream of first catalytic converter)") );
  583.             break;
  584.           case 0x02:
  585.             Serial.println( F("AIR_STAT: DNS (downstream of first catalytic converter inlet)") );
  586.             break;
  587.           case 0x04:
  588.             Serial.println( F("AIR_STAT: OFF (atmosphere / off)") );
  589.             break;
  590.         }
  591.         break;
  592.       }
  593.       case 0x13:  // Location of Oxygen Sensors
  594.         // PID 0x13 shall only be supported by a given vehicle if PID 0x1D is not supported.
  595.       {
  596.         uint8_t o2loc = responseBuffer[0][5];
  597.         Serial.print( F("O2SLOC: ") );
  598.         if( o2loc & 0x01 ) {
  599.           Serial.print( F("O2S11 ") );
  600.         }
  601.         if( o2loc & 0x02 ) {
  602.           Serial.print( F("O2S12 ") );
  603.         }
  604.         if( o2loc & 0x04 ) {
  605.           Serial.print( F("O2S13 ") );
  606.         }
  607.         if( o2loc & 0x08 ) {
  608.           Serial.print( F("O2S14 ") );
  609.         }
  610.         if( o2loc & 0x10 ) {
  611.           Serial.print( F("O2S21 ") );
  612.         }
  613.         if( o2loc & 0x20 ) {
  614.           Serial.print( F("O2S22 ") );
  615.         }
  616.         if( o2loc & 0x40 ) {
  617.           Serial.print( F("O2S23 ") );
  618.         }
  619.         if( o2loc & 0x80 ) {
  620.           Serial.print( F("O2S24 ") );
  621.         }
  622.         Serial.println();
  623.         break;
  624.       }
  625.       case 0x14:  // [0x14 - 0x1B] The Bank/Sensor number depends on if PID 0x13 or 0x1D is supported.
  626.       case 0x15:  // Oxygen Sensor Output Voltage
  627.       case 0x16:  // and Short Term Fuel Trim
  628.       case 0x17:  // for each sensor in the
  629.       case 0x18:  // 4 banks (2 sensors each)
  630.       case 0x19:
  631.       case 0x1A:
  632.       case 0x1B:
  633.       {
  634.         // Get which Bank and Sensor we are reading based on the PID
  635.         char bank;
  636.         char sensor;
  637.         // First check whether PID 0x13 or 0x1D is supported to
  638.         // determine how many banks/sensors are used/supported
  639.         if( gPidSupport & 0x2000 ) {
  640.           // PID 0x13 uses 2 banks with 4 sensor positions each
  641.           if( chPid < 0x18 ) {
  642.             bank = 1;
  643.           } else {
  644.             bank = 2;
  645.           }
  646.           sensor = ( chPid % 4 ) + 1;
  647.         } else { // else i gPidSupport[0] & 0x0008 )
  648.           // PID 0x1D uses 4 banks with 2 sensor positions each
  649.           if( chPid < 0x16 ) {
  650.             bank = 1;
  651.           } else if( chPid < 0x18 ) {
  652.             bank = 2;
  653.           } else if( chPid < 0x1A ) {
  654.             bank = 3;
  655.           } else {
  656.             bank = 4;
  657.           }
  658.           sensor = ( chPid % 2 ) + 1;
  659.         }
  660.  
  661.         // Display O2 sensor voltage
  662.         // Arduino has a hard time printing double/float so do the math by hand
  663.         // Scale = 0.005V per bit
  664.         uint16_t o2 = responseBuffer[0][5] * 5;   // number in millivolts
  665.         snprintf_P( buffer, BUFLEN, PSTR("O2S%d%d: %d.%dV"), bank, sensor, o2 / 1000, o2 % 1000 );
  666.         Serial.println( buffer );
  667.  
  668.         // Display fuel trim
  669.         uint32_t fuel = map( responseBuffer[0][5], 0, 255, -100, 99.22 );
  670.         snprintf_P( buffer, BUFLEN, PSTR("SHRTFT%d%d: %d%%"), bank, sensor, fuel );
  671.         Serial.println( buffer );
  672.         break;
  673.       }
  674.       case 0x1C:  // OBD requirements to which vehicle is designed
  675.       {
  676.         uint8_t obd = responseBuffer[0][5];
  677.         Serial.print( F("OBDSUP: ") );
  678.         if( obd == 0x01 ) {
  679.           Serial.println( F("OBD II") );
  680.         } else if( obd == 0x02 ) {
  681.           Serial.println( F("OBD") );
  682.         } else if( obd == 0x03 ) {
  683.           Serial.println( F("OBD and OBD II") );
  684.         } else if( obd == 0x04 ) {
  685.           Serial.println( F("OBD I") );
  686.         } else if( obd == 0x05 ) {
  687.           Serial.println( F("NO OBD") );
  688.         } else if( obd == 0x06 ) {
  689.           Serial.println( F("EOBD") );
  690.         } else if( obd == 0x07 ) {
  691.           Serial.println( F("EOBD and OBD II") );
  692.         } else if( obd == 0x08 ) {
  693.           Serial.println( F("EOBD and OBD") );
  694.         } else if( obd == 0x09 ) {
  695.           Serial.println( F("EOBD, OBD and OBD II") );
  696.         } else if( obd == 0x0A ) {
  697.           Serial.println( F("JOBD") );
  698.         } else if( obd == 0x0B ) {
  699.           Serial.println( F("JOBD and OBD II") );
  700.         } else if( obd == 0x0C ) {
  701.           Serial.println( F("JOBD and EOBD") );
  702.         } else if( obd == 0x0D ) {
  703.           Serial.println( F("JOBD, EOBD, and OBD II") );
  704.         } else if( obd == 0x0E ) {
  705.           Serial.println( F("EURO IV B1") );
  706.         } else if( obd == 0x0F ) {
  707.           Serial.println( F("EURO V B2") );
  708.         } else if( obd == 0x10 ) {
  709.           Serial.println( F("EURO C") );
  710.         } else if( obd == 0x11 ) {
  711.           Serial.println( F("EMD") );
  712.         } else if( obd <= 0xFA ) {
  713.           Serial.println( F("ISO/SAE reserved") );
  714.         } else {
  715.           Serial.println( F("SAE J1939 special meaning") );
  716.         }
  717.         break;
  718.       }
  719.       case 0x1D:  // Location of Oxygen Sensors
  720.         // PID 0x1D shall only be supported by a given vehicle if PID 0x13 is not supported.
  721.       {
  722.         uint8_t o2loc = responseBuffer[0][5];
  723.         Serial.print( F("O2SLOC: ") );
  724.         if( o2loc & 0x01 ) {
  725.           Serial.print( F("O2S11 ") );
  726.         }
  727.         if( o2loc & 0x02 ) {
  728.           Serial.print( F("O2S12 ") );
  729.         }
  730.         if( o2loc & 0x04 ) {
  731.           Serial.print( F("O2S21 ") );
  732.         }
  733.         if( o2loc & 0x08 ) {
  734.           Serial.print( F("O2S22 ") );
  735.         }
  736.         if( o2loc & 0x10 ) {
  737.           Serial.print( F("O2S31 ") );
  738.         }
  739.         if( o2loc & 0x20 ) {
  740.           Serial.print( F("O2S32 ") );
  741.         }
  742.         if( o2loc & 0x40 ) {
  743.           Serial.print( F("O2S41 ") );
  744.         }
  745.         if( o2loc & 0x80 ) {
  746.           Serial.print( F("O2S42 ") );
  747.         }
  748.         Serial.println();
  749.         break;
  750.       }
  751.       case 0x1E:  // Auxiliary Input Status
  752.       {
  753.         if( responseBuffer[0][5] & 0x01 ) {
  754.           Serial.println( F("PTO_STAT: ON") );
  755.         } else {
  756.           Serial.println( F("PTO_STAT: OFF") );
  757.         }
  758.         // Bits 1-7 are Reserved and should be reported as '0'
  759.       }
  760.       case 0x1F:  // Time Since Engine Start
  761.       {
  762.         uint16_t uptime = ( responseBuffer[0][5] << 8 ) | responseBuffer[0][6];
  763.         snprintf_P( buffer, BUFLEN, PSTR("RUNTM: %d sec."), uptime );
  764.         Serial.println( buffer );
  765.         break;
  766.       }
  767.       case 0x20:  // PIDs supported [21 - 40] (same format as PID 0x00)
  768.       {
  769.         displaySupportedPids( chMode, chPid, numMsg );
  770.         break;
  771.       }
  772.       default:    // PIDs 0x84-0xFF ISO/SAE Reserved
  773.         dumpRawData( numMsg );
  774.         break;
  775.     }
  776.   } else if( chMode == 0x02 ) {
  777.     //TODO: Mode 2 - Show Freeze Frame Data (Not Yet Implemented. Similar to Mode 1)
  778.   } else if( chMode == 0x05 ) {
  779.     //TODO: Mode 5 - (Non-CAN) Test Results (Not Yet Implemented)
  780.   } else if( chMode == 0x08 ) {
  781.     //TODO: Mode 8 (SMOG Testing?) Probably Won't Be Implemented
  782.   } else if( chMode == 0x09 ) {
  783.     // Mode 9 - Vehicle Information
  784.     switch( chPid ) {
  785.       case 0x00:
  786.       {
  787.         displaySupportedPids( chMode, chPid, numMsg );
  788.         break;
  789.       }
  790.       case 0x01:  // MessageCount VIN (Mode 9 PID 2)
  791.       {
  792.         snprintf_P( buffer, BUFLEN, PSTR("MC_VIN: %d"), responseBuffer[0][5] );
  793.         Serial.println( buffer );
  794.         break;
  795.       }
  796.       case 0x02:  // Vehicle Identification Number (VIN)
  797.       {
  798.         if( numMsg == 5 ) {
  799.           char vin[18]; // 17 ASCII chars + '\0'
  800.           // Nested loops were hard to read, so just copy the chars over one by one
  801.           vin[0]  = responseBuffer[0][8];  vin[1]  = responseBuffer[1][5];  vin[2]  = responseBuffer[1][6];
  802.           vin[3]  = responseBuffer[1][7];  vin[4]  = responseBuffer[1][8];  vin[5]  = responseBuffer[2][5];
  803.           vin[6]  = responseBuffer[2][6];  vin[7]  = responseBuffer[2][7];  vin[8]  = responseBuffer[2][8];
  804.           vin[9]  = responseBuffer[3][5];  vin[10] = responseBuffer[3][6];  vin[11] = responseBuffer[3][7];
  805.           vin[12] = responseBuffer[3][8];  vin[13] = responseBuffer[4][5];  vin[14] = responseBuffer[4][6];
  806.           vin[15] = responseBuffer[4][7];  vin[16] = responseBuffer[4][8];  vin[17] = '\0';
  807.           // Print the VIN "string"
  808.           snprintf_P( buffer, BUFLEN, PSTR("VIN: %s"), vin );
  809.           Serial.println( buffer );
  810.         } else {
  811.           snprintf_P( buffer, BUFLEN, PSTR("ERROR: Expecting 5 messages and received %d"), numMsg );
  812.           Serial.println( buffer );
  813.         }
  814.         break;
  815.       }
  816.       case 0x03:  // MessageCount CALID
  817.       {
  818.         snprintf_P( buffer, BUFLEN, PSTR("MC_CALID: %d"), responseBuffer[0][5] );
  819.         Serial.println( buffer );
  820.         if( ( responseBuffer[0][5] % 4 ) != 0 ) {
  821.           Serial.println( F("ERROR: MC_CALID was not a multiple of 4") );
  822.         }
  823.         break;
  824.       }
  825.       case 0x04:  // Calibration Identifications
  826.       {
  827.         for( int calNum = 0; calNum < numMsg; calNum += 4 ) {
  828.           char calid[17]; // 16 ASCII chars + '\0'
  829.           // Nested loops were hard to read, so just copy the chars over one by one
  830.           calid[0]  = responseBuffer[calNum][5];    calid[1]  = responseBuffer[calNum][6];    calid[2]  = responseBuffer[calNum][7];
  831.           calid[3]  = responseBuffer[calNum][8];    calid[4]  = responseBuffer[calNum + 1][5];  calid[5]  = responseBuffer[calNum + 1][6];
  832.           calid[6]  = responseBuffer[calNum + 1][7];  calid[7]  = responseBuffer[calNum + 1][8];  calid[8]  = responseBuffer[calNum + 2][5];
  833.           calid[9]  = responseBuffer[calNum + 2][6];  calid[10] = responseBuffer[calNum + 2][7];  calid[11] = responseBuffer[calNum + 2][8];
  834.           calid[12] = responseBuffer[calNum + 3][5];  calid[13] = responseBuffer[calNum + 3][6];  calid[14] = responseBuffer[calNum + 3][7];
  835.           calid[15] = responseBuffer[calNum + 3][8];  calid[16] = '\0';
  836.           // Print the CALID "string"
  837.           snprintf_P( buffer, BUFLEN, PSTR("CALID: %s"), calid );
  838.           Serial.println( buffer );
  839.         }
  840.         break;
  841.       }
  842.       case 0x05:  // MessageCount CVN
  843.       {
  844.         snprintf_P( buffer, BUFLEN, PSTR("MC_CVN: %d"), responseBuffer[0][5] );
  845.         Serial.println( buffer );
  846.         break;
  847.       }
  848.       case 0x06:  // Calibration Verification Numbers
  849.       {
  850.         for( int numCvn = 0; numCvn < numMsg; numCvn++ ) {
  851.           // CVN uses 4 bytes of HEX data
  852.           snprintf_P( buffer, BUFLEN, PSTR("CVN: %X%X%X%X"), responseBuffer[numCvn][5], responseBuffer[numCvn][6], responseBuffer[numCvn][7], responseBuffer[numCvn][8] );
  853.           Serial.println( buffer );
  854.         }
  855.         break;
  856.       }
  857.       case 0x07:  // MessageCount IPT
  858.       {
  859.         snprintf_P( buffer, BUFLEN, PSTR("MC_IPT: %d"), responseBuffer[0][5] );
  860.         Serial.println( buffer );
  861.         break;
  862.       }
  863.       case 0x08:  // In-use Performance Tracking
  864.       {
  865.         // We are expecting either 16 or 20 messages (4 data bytes each)
  866.         // in the specific order definded in SAE 1979/ISO 9141-2
  867.         Serial.println( F("IPT: ") );
  868.         for( int ipt = 0; ipt < numMsg; ipt++ ) {
  869.           uint16_t counts = ( responseBuffer[0][5] << 8 ) | responseBuffer[0][6];
  870.           // Format the data depending on the message number we are on
  871.           if( ipt == 0 ) {
  872.             snprintf_P( buffer, BUFLEN, PSTR("OBDCOND: %d cnts"), counts );
  873.           } else if( ipt == 1 ) {
  874.             snprintf_P( buffer, BUFLEN, PSTR("IGNCNTR: %d cnts"), counts );
  875.           } else if( ipt == 2 ) {
  876.             snprintf_P( buffer, BUFLEN, PSTR("CATCOMP1: %d cnts"), counts );
  877.           } else if( ipt == 3 ) {
  878.             snprintf_P( buffer, BUFLEN, PSTR("CATCOND1: %d cnts"), counts );
  879.           } else if( ipt == 4 ) {
  880.             snprintf_P( buffer, BUFLEN, PSTR("CATCOMP2: %d cnts"), counts );
  881.           } else if( ipt == 5 ) {
  882.             snprintf_P( buffer, BUFLEN, PSTR("CATCOND2: %d cnts"), counts );
  883.           } else if( ipt == 6 ) {
  884.             snprintf_P( buffer, BUFLEN, PSTR("O2SCOMP1: %d cnts"), counts );
  885.           } else if( ipt == 7 ) {
  886.             snprintf_P( buffer, BUFLEN, PSTR("O2SCOND1: %d cnts"), counts );
  887.           } else if( ipt == 8 ) {
  888.             snprintf_P( buffer, BUFLEN, PSTR("O2SCOMP2: %d cnts"), counts );
  889.           } else if( ipt == 9 ) {
  890.             snprintf_P( buffer, BUFLEN, PSTR("O2SCOND2: %d cnts"), counts );
  891.           } else if( ipt == 10 ) {
  892.             snprintf_P( buffer, BUFLEN, PSTR("EGRCOMP: %d cnts"), counts );
  893.           } else if( ipt == 11 ) {
  894.             snprintf_P( buffer, BUFLEN, PSTR("EGRCOND: %d cnts"), counts );
  895.           } else if( ipt == 12 ) {
  896.             snprintf_P( buffer, BUFLEN, PSTR("AIRCOMP: %d cnts"), counts );
  897.           } else if( ipt == 13 ) {
  898.             snprintf_P( buffer, BUFLEN, PSTR("AIRCOND: %d cnts"), counts );
  899.           } else if( ipt == 14 ) {
  900.             snprintf_P( buffer, BUFLEN, PSTR("EVAPCOMP: %d cnts"), counts );
  901.           } else if( ipt == 15 ) {
  902.             snprintf_P( buffer, BUFLEN, PSTR("EVAPCOND: %d cnts"), counts );
  903.           } else if( ipt == 16 ) {
  904.             snprintf_P( buffer, BUFLEN, PSTR("SO2SCOMP1: %d cnts"), counts );
  905.           } else if( ipt == 17 ) {
  906.             snprintf_P( buffer, BUFLEN, PSTR("SO2SCOND1: %d cnts"), counts );
  907.           } else if( ipt == 18 ) {
  908.             snprintf_P( buffer, BUFLEN, PSTR("SO2SCOMP2: %d cnts"), counts );
  909.           } else if( ipt == 19 ) {
  910.             snprintf_P( buffer, BUFLEN, PSTR("SO2SCOND2: %d cnts"), counts );
  911.           } /* else ERROR */
  912.           // Print the data we formatted
  913.           Serial.println( buffer );
  914.         }
  915.         break;
  916.       }
  917.       case 0x09:  // MessageCount ECUNAME
  918.       {
  919.         // For ISO 9141-2 the message count in the response should be 0x05
  920.         snprintf_P( buffer, BUFLEN, PSTR("MC_ECUNM: %d"), responseBuffer[0][5] );
  921.         Serial.println( buffer );
  922.         break;
  923.       }
  924.       case 0x0A:  // ECUNAME
  925.       {
  926.         for( int ecuNum = 0; ecuNum < numMsg; ecuNum += 5 ) {
  927.           char ecuAbbrv[5]; // 4 ASCII chars + '\0'
  928.           char ecuFull[16]; // 15 ASCII chars + '\0'
  929.           // Nested loops were hard to read, so just copy the chars over one by one
  930.           ecuAbbrv[0]  = responseBuffer[ecuNum][5];    ecuAbbrv[1]  = responseBuffer[ecuNum][6];    ecuAbbrv[2]  = responseBuffer[ecuNum][7];
  931.           ecuAbbrv[3]  = responseBuffer[ecuNum][8];    ecuAbbrv[4] = '\0';
  932.  
  933.           ecuFull[0]  = responseBuffer[ecuNum + 1][6];  ecuFull[1]  = responseBuffer[ecuNum + 1][7];  ecuFull[2]  = responseBuffer[ecuNum + 1][8];
  934.           ecuFull[3]  = responseBuffer[ecuNum + 2][5];  ecuFull[4]  = responseBuffer[ecuNum + 2][6];  ecuFull[5]  = responseBuffer[ecuNum + 2][7];
  935.           ecuFull[6]  = responseBuffer[ecuNum + 2][8];  ecuFull[7]  = responseBuffer[ecuNum + 3][5];  ecuFull[8]  = responseBuffer[ecuNum + 3][6];
  936.           ecuFull[9]  = responseBuffer[ecuNum + 3][7];  ecuFull[10] = responseBuffer[ecuNum + 3][8];  ecuFull[11] = responseBuffer[ecuNum + 4][5];
  937.           ecuFull[12] = responseBuffer[ecuNum + 4][6];  ecuFull[13] = responseBuffer[ecuNum + 4][7];  ecuFull[14] = responseBuffer[ecuNum + 4][8];
  938.           ecuFull[15] = '\0';
  939.           // Print the CALID "string"
  940.           snprintf_P( buffer, BUFLEN, PSTR("ECU: %s"), ecuAbbrv );
  941.           Serial.println( buffer );
  942.           snprintf_P( buffer, BUFLEN, PSTR("ECUNAME: %s"), ecuFull );
  943.           Serial.println( buffer );
  944.         }
  945.         break;
  946.       }
  947.       case 0x0B:  // In-use Performance Tracking
  948.       {
  949.         // We are expecting either 16 (4 data bytes each)
  950.         // in the specific order definded in SAE 1979/ISO 9141-2
  951.         Serial.println( F("IPT: ") );
  952.         for( int ipt = 0; ipt < numMsg; ipt++ ) {
  953.           uint16_t counts = ( responseBuffer[0][5] << 8 ) | responseBuffer[0][6];
  954.           // Format the data depending on the message number we are on
  955.           if( ipt == 0 ) {
  956.             snprintf_P( buffer, BUFLEN, PSTR("OBDCOND: %d cnts"), counts );
  957.           } else if( ipt == 1 ) {
  958.             snprintf_P( buffer, BUFLEN, PSTR("IGNCNTR: %d cnts"), counts );
  959.           } else if( ipt == 2 ) {
  960.             snprintf_P( buffer, BUFLEN, PSTR("HCCATCOMP: %d cnts"), counts );
  961.           } else if( ipt == 3 ) {
  962.             snprintf_P( buffer, BUFLEN, PSTR("HCCATCOND: %d cnts"), counts );
  963.           } else if( ipt == 4 ) {
  964.             snprintf_P( buffer, BUFLEN, PSTR("NCATCOMP: %d cnts"), counts );
  965.           } else if( ipt == 5 ) {
  966.             snprintf_P( buffer, BUFLEN, PSTR("NCATCOND: %d cnts"), counts );
  967.           } else if( ipt == 6 ) {
  968.             snprintf_P( buffer, BUFLEN, PSTR("NADSCOMP: %d cnts"), counts );
  969.           } else if( ipt == 7 ) {
  970.             snprintf_P( buffer, BUFLEN, PSTR("NADSCOND: %d cnts"), counts );
  971.           } else if( ipt == 8 ) {
  972.             snprintf_P( buffer, BUFLEN, PSTR("PMCOMP: %d cnts"), counts );
  973.           } else if( ipt == 9 ) {
  974.             snprintf_P( buffer, BUFLEN, PSTR("PMCOND: %d cnts"), counts );
  975.           } else if( ipt == 10 ) {
  976.             snprintf_P( buffer, BUFLEN, PSTR("EGSCOMP: %d cnts"), counts );
  977.           } else if( ipt == 11 ) {
  978.             snprintf_P( buffer, BUFLEN, PSTR("EGSCOND: %d cnts"), counts );
  979.           } else if( ipt == 12 ) {
  980.             snprintf_P( buffer, BUFLEN, PSTR("EGRCOMP: %d cnts"), counts );
  981.           } else if( ipt == 13 ) {
  982.             snprintf_P( buffer, BUFLEN, PSTR("EGRCOND: %d cnts"), counts );
  983.           } else if( ipt == 14 ) {
  984.             snprintf_P( buffer, BUFLEN, PSTR("BPCOMP: %d cnts"), counts );
  985.           } else if( ipt == 15 ) {
  986.             snprintf_P( buffer, BUFLEN, PSTR("BPCOND: %d cnts"), counts );
  987.           } /* else ERROR */
  988.           // Print the data we formatted
  989.           Serial.println( buffer );
  990.         }
  991.         break;
  992.       }
  993.       default:    // PIDs 0x0C-0xFF ISO/SAE Reserved
  994.         dumpRawData( numMsg );
  995.         break;
  996.     }
  997.   }
  998.  
  999. }
  1000.  
  1001.  
  1002. void ui()
  1003. {
  1004.   /*display.setCursor(0, 0);
  1005.   display.println("LOAD");*/
  1006.   display.setCursor(0, 0);
  1007.   display.println("TEMP");
  1008.   display.setCursor(0, 10);
  1009.   display.println("RPM");
  1010.   display.setCursor(0, 20);
  1011.   display.println("VSS");
  1012.  
  1013.   display.setCursor(65, 0);
  1014.   display.println("IAT");
  1015. }
  1016.  
  1017. void disp(uint8_t chMode)
  1018. {
  1019.   uint8_t numMsg;
  1020.   //load
  1021.   /*display.setCursor( 32, 0 );
  1022.   display.println( formatResponse( 0x04 ) );*/
  1023.   //engine temp
  1024.   display.setCursor( 32, 0 );
  1025.   numMsg = sendPid( chMode, 0x05 );
  1026.   formatResponse( chMode, 0x05, numMsg );
  1027.   delay(100);
  1028.   //rpm
  1029.   display.setCursor( 32, 10 );
  1030.   numMsg = sendPid( chMode, 0x0C );
  1031.   formatResponse( chMode, 0x0C, numMsg );
  1032.   delay(100);
  1033.   //speed
  1034.   display.setCursor(32, 20 );
  1035.   numMsg = sendPid( chMode, 0x0D );
  1036.   formatResponse( chMode, 0x0D, numMsg );
  1037.   delay(100);
  1038.   //intake air
  1039.   display.setCursor(97, 0);
  1040.   numMsg = sendPid( chMode, 0x0F );
  1041.   formatResponse( chMode, 0x0F, numMsg );
  1042. }
  1043.  
  1044. void setup()
  1045. {
  1046.   //OLED
  1047.   display.begin(SSD1306_SWITCHCAPVCC);
  1048.  
  1049.   display.setTextSize(1);
  1050.   display.setTextColor(WHITE);
  1051.  
  1052.   display.display();
  1053.   display.clearDisplay();
  1054.   display.display();
  1055.   display.println("TEST");
  1056.   display.display();
  1057.   delay(2000);
  1058.  
  1059.   //Set timer
  1060.   timerKeepAlive.setTimeout(4000);
  1061.  
  1062.   //Set usb serial
  1063.   Serial.begin( 57600 );
  1064.   Serial.println( "Init score" );
  1065.   init_5baud();
  1066. }
  1067.  
  1068. void loop()
  1069. {
  1070.   uint8_t chMode = 0x01;
  1071.  
  1072.   uint8_t numMsg = sendPid( chMode, 0 );
  1073.  
  1074.   uint8_t chPid;
  1075.  
  1076.   // Send the actual request
  1077.   snprintf_P( buffer, BUFLEN, PSTR("Sending Mode %X PID %02X Request"), chMode, chPid );
  1078.   //Serial.println( buffer );
  1079.   numMsg = sendPid( chMode, chPid );
  1080.   // Format the response and send to Serial
  1081.   //formatResponse( chMode, chPid, numMsg );
  1082.   //formatResponse( 0x04, 0x00 );
  1083.   display.clearDisplay();
  1084.   ui();
  1085.   disp(chMode);
  1086.   display.display();
  1087.   delay(100);
  1088. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!