main.cpp

00001 #include "mbed.h"
00002 #include <vector>
00003 #include <algorithm>
00004 
00005 DigitalOut rx_led(LED1);
00006 DigitalOut filter_led(LED4);
00007 Serial pc(USBTX, USBRX);
00008 CAN gmlan(p30, p29);
00009 Timer uptime;
00010 bool filterPackets = false;
00011 vector<int> filteredPackets;
00012 extern "C" void mbed_reset();
00013 
00014 namespace mbed {
00015     class CANHeader {
00016         // Example header packet for Steering Wheel Switches:
00017         // Hexadecimal:    0x10     0x0D     0x00     0x60
00018         // Binary:       00010000 00001101 00000000 01100000
00019         // Priority:        ---
00020         // Arbitration:        -- -------- ---
00021         // Sending ECU:                       ----- --------
00022 
00023         private:
00024             int priorityID, arbitrationID, senderID;
00025         
00026         public:
00027             int priority(void) { return priorityID; }
00028             void priority(int _priority) { priorityID = _priority; }
00029             int arbitration(void) { return arbitrationID; }
00030             void arbitration(int _arbitration) { arbitrationID = _arbitration; }
00031             int sender(void) { return senderID; }
00032             void sender(int _sender) { senderID = _sender; }
00033             
00034             void decode(int _header) {
00035                 priorityID = (_header >> 26) & 0x7;
00036                 arbitrationID = (_header >> 13) & 0x1FFF;
00037                 senderID = (_header >> 0)  & 0x1FFF;
00038             }
00039             int encode(void) {
00040                 long int buffer = 0;
00041                 buffer = (buffer << 3) | 0x0; // 3 bit padding
00042                 buffer = (buffer << 3) | priorityID;
00043                 buffer = (buffer << 13) | arbitrationID;
00044                 buffer = (buffer << 13) | senderID;
00045                 return buffer;
00046             }
00047     };
00048 }
00049 
00050 void processMessage()
00051 {
00052     // Turn on rx_led to indicate we are receiving data
00053     rx_led = 1;
00054     
00055     // Create a CANMessage object and read the buffer into it
00056     CANMessage rxmsg;
00057     gmlan.read(rxmsg);
00058     
00059     // Check to see if our header is in the filtered packet list or we are filtering, if so we skip over
00060     bool packetFound = false;
00061     if (find(filteredPackets.begin(), filteredPackets.end(), rxmsg.id) != filteredPackets.end())
00062         packetFound = true;
00063         
00064     // If we are filtering packets and this one isn't found, add it to the list
00065     if (filterPackets == true && packetFound == false)
00066     {
00067         filteredPackets.push_back(rxmsg.id);
00068         pc.printf("Added 0x%08X to filter at position %u\r\n", rxmsg.id, filteredPackets.size());
00069     }
00070     else if (filterPackets == false && packetFound == false) {
00071         CANHeader rxHeader;
00072         rxHeader.decode(rxmsg.id);
00073         
00074         // Print these results to the serial terminal using printf to format
00075         // Ref: http://www.cplusplus.com/reference/cstdio/printf/
00076         pc.printf("[%10.2f] ", uptime.read());            // Seconds (to 2 decimal places) since start
00077         pc.printf("[0x%1X] ", rxHeader.priority());       // Packet priority (0-7, 0 being highest)
00078         pc.printf("[0x%4X] ", rxHeader.arbitration());    // Arbitration ID (0x0 to 0x1FFF)
00079         pc.printf("[0x%4X] ", rxHeader.sender());         // ECU ID (0x0 to 0x1FFF)
00080         pc.printf("[0x%08X] ", rxmsg.id);                 // Full header for legacy reasons (4 bytes)
00081         pc.printf("[%d] ", rxmsg.len);                    // Length of message (0-8 bytes typically)
00082         
00083         // Process actual message here, only run this if we have a message
00084         if (rxmsg.len > 0)
00085         {
00086             pc.printf("[%02X", rxmsg.data[0]);  // Print first byte
00087             for (unsigned int i = 1; i < rxmsg.len; i++)
00088             pc.printf(" %02X", rxmsg.data[i]);  // Print additional byte(s) with a preceeding space
00089             pc.printf("]");                     // Print closing bracket
00090         }
00091         pc.printf("\r\n");                      // Print carriage return and newline
00092     }
00093         
00094     // Turn off our rx_led as we have finished reading data
00095     rx_led = 0;
00096 }
00097 
00098 void clearAndHome()
00099 {
00100     pc.printf("%c", 27);    // ESC
00101     pc.printf("[2J");       // clear screen
00102     pc.printf("%c", 27);    // ESC
00103     pc.printf("[H");        // cursor to home
00104 }
00105 
00106 int main() {
00107     // Set serial baud rate to 115kbit
00108     pc.baud(115200);
00109     
00110     // Set CANBUS to 33.3kbit and put into monitor mode (does not ACK packets, aka stealth mode)
00111     int baudrate = 33333;
00112     gmlan.frequency(baudrate);
00113     gmlan.monitor(true);
00114     
00115     // Clear serial terminal
00116     clearAndHome();
00117     pc.printf("Keys:\r\nF = start/stop filter capture\r\nD = display filtered headers\r\nC = clear filter\r\nR = restart mbed\r\n");
00118     pc.printf("Starting packet capture at %i bps\r\n\r\n", baudrate);
00119     
00120     // Start capturing packets
00121     uptime.start();
00122     gmlan.attach(&processMessage);
00123     
00124     while(1) {
00125         // Poll serial for keypresses for certain tasks
00126         if (pc.readable())
00127         {
00128             switch (pc.getc())
00129             {
00130                 case 'f':
00131                 case 'F':
00132                 {
00133                     // Toggle filter
00134                     filterPackets = !filterPackets;
00135                     filter_led = (int)filterPackets;
00136                     break;
00137                 }
00138                 case 'c':
00139                 case 'C':
00140                 {
00141                     // Clear filter
00142                     filteredPackets.clear();
00143                     pc.printf("Packet filter cleared\r\n");
00144                     break;
00145                 }
00146                 case 'd':
00147                 case 'D':
00148                 {
00149                     // Show filter
00150                     clearAndHome();
00151                     pc.printf("[%u] entries in filter:\r\n", filteredPackets.size());
00152                     for (int i=0; i < filteredPackets.size(); i++)
00153                         pc.printf("%u: 0x%08X ", i+1, filteredPackets[i]);
00154                     pc.printf("\r\n");
00155                     break;
00156                 }
00157                 case 'r':
00158                 case 'R':
00159                 {
00160                     // Restart mbed
00161                     pc.printf("Restarting mbed...\r\n");
00162                     mbed_reset();
00163                     break;
00164                 }
00165                 default:
00166                     pc.printf("Unknown keypress!\r\n");
00167                     break;
00168             }
00169         }       
00170         // Sleep for 20ms repeatedly, as all messages are handled by an interrupt, this prevents
00171         // keeping the mbed at full load
00172         wait(0.02);
00173     }
00174 }