Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
main.cpp@0:cbd88c574455, 2012-12-11 (annotated)
- Committer:
- foxdie
- Date:
- Tue Dec 11 21:31:57 2012 +0000
- Revision:
- 0:cbd88c574455
First attempt to send DIC message, known to not work at this stage.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
foxdie | 0:cbd88c574455 | 1 | #include "mbed.h" |
foxdie | 0:cbd88c574455 | 2 | #include <vector> |
foxdie | 0:cbd88c574455 | 3 | #include <algorithm> |
foxdie | 0:cbd88c574455 | 4 | #include <queue> |
foxdie | 0:cbd88c574455 | 5 | #include <string> |
foxdie | 0:cbd88c574455 | 6 | |
foxdie | 0:cbd88c574455 | 7 | DigitalOut rx_led(LED1); |
foxdie | 0:cbd88c574455 | 8 | DigitalOut tx_led(LED2); |
foxdie | 0:cbd88c574455 | 9 | DigitalOut filter_led(LED3); |
foxdie | 0:cbd88c574455 | 10 | DigitalOut notify_led(LED4); |
foxdie | 0:cbd88c574455 | 11 | Serial pc(USBTX, USBRX); |
foxdie | 0:cbd88c574455 | 12 | CAN gmlan(p30, p29); |
foxdie | 0:cbd88c574455 | 13 | Timer uptime; |
foxdie | 0:cbd88c574455 | 14 | Timeout notifyHandler; |
foxdie | 0:cbd88c574455 | 15 | bool filterPackets = false; |
foxdie | 0:cbd88c574455 | 16 | vector<int> filteredPackets; |
foxdie | 0:cbd88c574455 | 17 | extern "C" void mbed_reset(); |
foxdie | 0:cbd88c574455 | 18 | |
foxdie | 0:cbd88c574455 | 19 | namespace mbed { |
foxdie | 0:cbd88c574455 | 20 | class CANHeader { |
foxdie | 0:cbd88c574455 | 21 | // Example header packet for Steering Wheel Switches: |
foxdie | 0:cbd88c574455 | 22 | // Hexadecimal: 0x10 0x0D 0x00 0x60 |
foxdie | 0:cbd88c574455 | 23 | // Binary: 00010000 00001101 00000000 01100000 |
foxdie | 0:cbd88c574455 | 24 | // Priority: --- |
foxdie | 0:cbd88c574455 | 25 | // Arbitration: -- -------- --- |
foxdie | 0:cbd88c574455 | 26 | // Sending ECU: ----- -------- |
foxdie | 0:cbd88c574455 | 27 | |
foxdie | 0:cbd88c574455 | 28 | private: |
foxdie | 0:cbd88c574455 | 29 | int priorityID, arbitrationID, senderID; |
foxdie | 0:cbd88c574455 | 30 | |
foxdie | 0:cbd88c574455 | 31 | public: |
foxdie | 0:cbd88c574455 | 32 | int priority(void) { return priorityID; } |
foxdie | 0:cbd88c574455 | 33 | void priority(int _priority) { priorityID = _priority; } |
foxdie | 0:cbd88c574455 | 34 | int arbitration(void) { return arbitrationID; } |
foxdie | 0:cbd88c574455 | 35 | void arbitration(int _arbitration) { arbitrationID = _arbitration; } |
foxdie | 0:cbd88c574455 | 36 | int sender(void) { return senderID; } |
foxdie | 0:cbd88c574455 | 37 | void sender(int _sender) { senderID = _sender; } |
foxdie | 0:cbd88c574455 | 38 | |
foxdie | 0:cbd88c574455 | 39 | void decode(int _header) { |
foxdie | 0:cbd88c574455 | 40 | priorityID = (_header >> 26) & 0x7; |
foxdie | 0:cbd88c574455 | 41 | arbitrationID = (_header >> 13) & 0x1FFF; |
foxdie | 0:cbd88c574455 | 42 | senderID = (_header >> 0) & 0x1FFF; |
foxdie | 0:cbd88c574455 | 43 | } |
foxdie | 0:cbd88c574455 | 44 | int encode(void) { |
foxdie | 0:cbd88c574455 | 45 | long int buffer = 0; |
foxdie | 0:cbd88c574455 | 46 | buffer = (buffer << 3) | 0x0; // 3 bit padding |
foxdie | 0:cbd88c574455 | 47 | buffer = (buffer << 3) | priorityID; |
foxdie | 0:cbd88c574455 | 48 | buffer = (buffer << 13) | arbitrationID; |
foxdie | 0:cbd88c574455 | 49 | buffer = (buffer << 13) | senderID; |
foxdie | 0:cbd88c574455 | 50 | return buffer; |
foxdie | 0:cbd88c574455 | 51 | } |
foxdie | 0:cbd88c574455 | 52 | }; |
foxdie | 0:cbd88c574455 | 53 | } |
foxdie | 0:cbd88c574455 | 54 | |
foxdie | 0:cbd88c574455 | 55 | void clearNotify() |
foxdie | 0:cbd88c574455 | 56 | { |
foxdie | 0:cbd88c574455 | 57 | notify_led = 0; |
foxdie | 0:cbd88c574455 | 58 | } |
foxdie | 0:cbd88c574455 | 59 | void notifyUser() |
foxdie | 0:cbd88c574455 | 60 | { |
foxdie | 0:cbd88c574455 | 61 | // Turn on notify LED and set timer to turn it off again shortly |
foxdie | 0:cbd88c574455 | 62 | notify_led = 1; |
foxdie | 0:cbd88c574455 | 63 | notifyHandler.detach(); |
foxdie | 0:cbd88c574455 | 64 | notifyHandler.attach(&clearNotify, 0.1); |
foxdie | 0:cbd88c574455 | 65 | |
foxdie | 0:cbd88c574455 | 66 | // Set up notification header |
foxdie | 0:cbd88c574455 | 67 | CANHeader notifyHeader; |
foxdie | 0:cbd88c574455 | 68 | notifyHeader.priority(0x4); // Medium priority |
foxdie | 0:cbd88c574455 | 69 | notifyHeader.arbitration(0x186); // Text notification on top of cluster screen |
foxdie | 0:cbd88c574455 | 70 | notifyHeader.sender(0x81); // Sent from an audio source |
foxdie | 0:cbd88c574455 | 71 | |
foxdie | 0:cbd88c574455 | 72 | // Define notification message |
foxdie | 0:cbd88c574455 | 73 | string message = "GMLAN pwned!"; |
foxdie | 0:cbd88c574455 | 74 | int eachPacketLength = 8; |
foxdie | 0:cbd88c574455 | 75 | |
foxdie | 0:cbd88c574455 | 76 | // Initialise packet data |
foxdie | 0:cbd88c574455 | 77 | string packetData; |
foxdie | 0:cbd88c574455 | 78 | packetData.push_back(0x45); // Starting packet for message |
foxdie | 0:cbd88c574455 | 79 | packetData.push_back(0x1); // Multi-packet count, start at 1, we change it later |
foxdie | 0:cbd88c574455 | 80 | packetData.push_back(0x1); // Arrow to display 01 = both, 81 = up, 41 = down |
foxdie | 0:cbd88c574455 | 81 | |
foxdie | 0:cbd88c574455 | 82 | // Read notification message into packet data vector and add terminator at the end |
foxdie | 0:cbd88c574455 | 83 | for (int i = 0; i < message.size(); i++) |
foxdie | 0:cbd88c574455 | 84 | packetData.push_back(message[i]); |
foxdie | 0:cbd88c574455 | 85 | packetData.push_back(0x4); // Message terminator byte |
foxdie | 0:cbd88c574455 | 86 | |
foxdie | 0:cbd88c574455 | 87 | // Insert subsequent header every <eachPacketLength> bytes |
foxdie | 0:cbd88c574455 | 88 | int byteCount = 0; |
foxdie | 0:cbd88c574455 | 89 | int packetCount = 1; |
foxdie | 0:cbd88c574455 | 90 | div_t packetDivisor; |
foxdie | 0:cbd88c574455 | 91 | while (byteCount < packetData.size()) |
foxdie | 0:cbd88c574455 | 92 | { |
foxdie | 0:cbd88c574455 | 93 | packetDivisor = div(byteCount + 1, eachPacketLength); |
foxdie | 0:cbd88c574455 | 94 | if (packetDivisor.rem == 0 && byteCount + 1 < packetData.size()) |
foxdie | 0:cbd88c574455 | 95 | { |
foxdie | 0:cbd88c574455 | 96 | packetCount++; |
foxdie | 0:cbd88c574455 | 97 | string::iterator pdIndex = packetData.begin() + byteCount; |
foxdie | 0:cbd88c574455 | 98 | packetData.insert(pdIndex + 1, 1, 0x44); |
foxdie | 0:cbd88c574455 | 99 | packetData.insert(pdIndex + 2, 1, packetCount); |
foxdie | 0:cbd88c574455 | 100 | } |
foxdie | 0:cbd88c574455 | 101 | byteCount++;; |
foxdie | 0:cbd88c574455 | 102 | } |
foxdie | 0:cbd88c574455 | 103 | packetData[1] = packetCount; |
foxdie | 0:cbd88c574455 | 104 | |
foxdie | 0:cbd88c574455 | 105 | // Make sure we have null-padding bytes towards the end |
foxdie | 0:cbd88c574455 | 106 | packetDivisor = div(packetData.size(), eachPacketLength); |
foxdie | 0:cbd88c574455 | 107 | int padding = eachPacketLength - packetDivisor.rem; |
foxdie | 0:cbd88c574455 | 108 | while (padding > 0) |
foxdie | 0:cbd88c574455 | 109 | { |
foxdie | 0:cbd88c574455 | 110 | packetData.push_back(0x0); |
foxdie | 0:cbd88c574455 | 111 | padding--; |
foxdie | 0:cbd88c574455 | 112 | } |
foxdie | 0:cbd88c574455 | 113 | |
foxdie | 0:cbd88c574455 | 114 | // Split packet list into groups of 8 bytes |
foxdie | 0:cbd88c574455 | 115 | char buffer [eachPacketLength]; |
foxdie | 0:cbd88c574455 | 116 | for (int packetIndex = 0; packetIndex < packetCount; packetIndex++)// < packetData.size(); byte += eachPacketLength) |
foxdie | 0:cbd88c574455 | 117 | { |
foxdie | 0:cbd88c574455 | 118 | for (int i = 0; i < eachPacketLength; i++) |
foxdie | 0:cbd88c574455 | 119 | buffer[i] = packetData[(packetIndex*eachPacketLength)+i]; |
foxdie | 0:cbd88c574455 | 120 | |
foxdie | 0:cbd88c574455 | 121 | CANMessage data = CANMessage(notifyHeader.encode(), buffer, eachPacketLength); |
foxdie | 0:cbd88c574455 | 122 | |
foxdie | 0:cbd88c574455 | 123 | pc.printf("Attemping to send [0x%08X] ", data.id); |
foxdie | 0:cbd88c574455 | 124 | pc.printf("[%02X", data.data[0]); // Print first byte |
foxdie | 0:cbd88c574455 | 125 | for (int i = 1; i < data.len; i++) |
foxdie | 0:cbd88c574455 | 126 | pc.printf(" %02X", data.data[i]); // Print additional byte(s) with a preceeding space |
foxdie | 0:cbd88c574455 | 127 | pc.printf("]\r\n"); |
foxdie | 0:cbd88c574455 | 128 | |
foxdie | 0:cbd88c574455 | 129 | if (gmlan.write(data)) |
foxdie | 0:cbd88c574455 | 130 | pc.printf("Message sent!\r\n"); |
foxdie | 0:cbd88c574455 | 131 | pc.printf("Error count: TX=%u RX=%u\r\n", gmlan.tderror(), gmlan.rderror()); |
foxdie | 0:cbd88c574455 | 132 | } |
foxdie | 0:cbd88c574455 | 133 | } |
foxdie | 0:cbd88c574455 | 134 | |
foxdie | 0:cbd88c574455 | 135 | void processMessage() |
foxdie | 0:cbd88c574455 | 136 | { |
foxdie | 0:cbd88c574455 | 137 | // Turn on rx_led to indicate we are receiving data |
foxdie | 0:cbd88c574455 | 138 | rx_led = 1; |
foxdie | 0:cbd88c574455 | 139 | |
foxdie | 0:cbd88c574455 | 140 | // Create a CANMessage object and read the buffer into it |
foxdie | 0:cbd88c574455 | 141 | CANMessage rxmsg; |
foxdie | 0:cbd88c574455 | 142 | gmlan.read(rxmsg); |
foxdie | 0:cbd88c574455 | 143 | |
foxdie | 0:cbd88c574455 | 144 | // Check to see if our header is in the filtered packet list or we are filtering, if so we skip over |
foxdie | 0:cbd88c574455 | 145 | bool packetFound = false; |
foxdie | 0:cbd88c574455 | 146 | if (find(filteredPackets.begin(), filteredPackets.end(), rxmsg.id) != filteredPackets.end()) |
foxdie | 0:cbd88c574455 | 147 | packetFound = true; |
foxdie | 0:cbd88c574455 | 148 | |
foxdie | 0:cbd88c574455 | 149 | // If we are filtering packets and this one isn't found, add it to the list |
foxdie | 0:cbd88c574455 | 150 | if (filterPackets == true && packetFound == false) |
foxdie | 0:cbd88c574455 | 151 | { |
foxdie | 0:cbd88c574455 | 152 | filteredPackets.push_back(rxmsg.id); |
foxdie | 0:cbd88c574455 | 153 | pc.printf("Added 0x%08X to filter at position %u\r\n", rxmsg.id, filteredPackets.size()); |
foxdie | 0:cbd88c574455 | 154 | } |
foxdie | 0:cbd88c574455 | 155 | else if (filterPackets == false && packetFound == false) { |
foxdie | 0:cbd88c574455 | 156 | CANHeader rxHeader; |
foxdie | 0:cbd88c574455 | 157 | rxHeader.decode(rxmsg.id); |
foxdie | 0:cbd88c574455 | 158 | |
foxdie | 0:cbd88c574455 | 159 | // Print these results to the serial terminal using printf to format |
foxdie | 0:cbd88c574455 | 160 | // Ref: http://www.cplusplus.com/reference/cstdio/printf/ |
foxdie | 0:cbd88c574455 | 161 | pc.printf("[%10.2f] ", uptime.read()); // Seconds (to 2 decimal places) since start |
foxdie | 0:cbd88c574455 | 162 | pc.printf("[0x%1X] ", rxHeader.priority()); // Packet priority (0-7, 0 being highest) |
foxdie | 0:cbd88c574455 | 163 | pc.printf("[0x%4X] ", rxHeader.arbitration()); // Arbitration ID (0x0 to 0x1FFF) |
foxdie | 0:cbd88c574455 | 164 | pc.printf("[0x%4X] ", rxHeader.sender()); // ECU ID (0x0 to 0x1FFF) |
foxdie | 0:cbd88c574455 | 165 | pc.printf("[0x%08X] ", rxmsg.id); // Full header for legacy reasons (4 bytes) |
foxdie | 0:cbd88c574455 | 166 | pc.printf("[%d] ", rxmsg.len); // Length of message (0-8 bytes typically) |
foxdie | 0:cbd88c574455 | 167 | |
foxdie | 0:cbd88c574455 | 168 | // Process actual message here, only run this if we have a message |
foxdie | 0:cbd88c574455 | 169 | if (rxmsg.len > 0) |
foxdie | 0:cbd88c574455 | 170 | { |
foxdie | 0:cbd88c574455 | 171 | pc.printf("[%02X", rxmsg.data[0]); // Print first byte |
foxdie | 0:cbd88c574455 | 172 | for (int i = 1; i < rxmsg.len; i++) |
foxdie | 0:cbd88c574455 | 173 | pc.printf(" %02X", rxmsg.data[i]); // Print additional byte(s) with a preceeding space |
foxdie | 0:cbd88c574455 | 174 | pc.printf("]"); // Print closing bracket |
foxdie | 0:cbd88c574455 | 175 | } |
foxdie | 0:cbd88c574455 | 176 | pc.printf("\r\n"); // Print carriage return and newline |
foxdie | 0:cbd88c574455 | 177 | |
foxdie | 0:cbd88c574455 | 178 | // Try and do some clever stuff, see if we've got a particular packet and notify if we do |
foxdie | 0:cbd88c574455 | 179 | // In this example, we are testing to see if the steering wheel select button has been pressed |
foxdie | 0:cbd88c574455 | 180 | if (rxHeader.arbitration() == 0x68 && rxmsg.len > 0) |
foxdie | 0:cbd88c574455 | 181 | { |
foxdie | 0:cbd88c574455 | 182 | if (rxmsg.data[0] & 0x1 == 0x1) |
foxdie | 0:cbd88c574455 | 183 | { |
foxdie | 0:cbd88c574455 | 184 | notifyUser(); |
foxdie | 0:cbd88c574455 | 185 | } |
foxdie | 0:cbd88c574455 | 186 | } |
foxdie | 0:cbd88c574455 | 187 | } |
foxdie | 0:cbd88c574455 | 188 | |
foxdie | 0:cbd88c574455 | 189 | // Turn off our rx_led as we have finished reading data |
foxdie | 0:cbd88c574455 | 190 | rx_led = 0; |
foxdie | 0:cbd88c574455 | 191 | } |
foxdie | 0:cbd88c574455 | 192 | |
foxdie | 0:cbd88c574455 | 193 | void clearAndHome() |
foxdie | 0:cbd88c574455 | 194 | { |
foxdie | 0:cbd88c574455 | 195 | pc.printf("%c", 27); // ESC |
foxdie | 0:cbd88c574455 | 196 | pc.printf("[2J"); // clear screen |
foxdie | 0:cbd88c574455 | 197 | pc.printf("%c", 27); // ESC |
foxdie | 0:cbd88c574455 | 198 | pc.printf("[H"); // cursor to home |
foxdie | 0:cbd88c574455 | 199 | } |
foxdie | 0:cbd88c574455 | 200 | |
foxdie | 0:cbd88c574455 | 201 | int main() { |
foxdie | 0:cbd88c574455 | 202 | // Set serial baud rate to 115kbit |
foxdie | 0:cbd88c574455 | 203 | pc.baud(115200); |
foxdie | 0:cbd88c574455 | 204 | |
foxdie | 0:cbd88c574455 | 205 | // Set CANBUS to 33.3kbit and put into monitor mode (does not ACK packets, aka stealth mode) |
foxdie | 0:cbd88c574455 | 206 | int baudrate = 33333; |
foxdie | 0:cbd88c574455 | 207 | gmlan.frequency(baudrate); |
foxdie | 0:cbd88c574455 | 208 | gmlan.monitor(false); |
foxdie | 0:cbd88c574455 | 209 | |
foxdie | 0:cbd88c574455 | 210 | // Clear serial terminal |
foxdie | 0:cbd88c574455 | 211 | clearAndHome(); |
foxdie | 0:cbd88c574455 | 212 | pc.printf("Keys:\r\n"); |
foxdie | 0:cbd88c574455 | 213 | pc.printf("F = start/stop filter capture\r\n"); |
foxdie | 0:cbd88c574455 | 214 | pc.printf("D = display filtered headers\r\n"); |
foxdie | 0:cbd88c574455 | 215 | pc.printf("C = clear filter\r\n"); |
foxdie | 0:cbd88c574455 | 216 | pc.printf("R = restart mbed\r\n"); |
foxdie | 0:cbd88c574455 | 217 | pc.printf("T = send test packet\r\n"); |
foxdie | 0:cbd88c574455 | 218 | pc.printf("Starting packet capture at %i bps\r\n\r\n", baudrate); |
foxdie | 0:cbd88c574455 | 219 | |
foxdie | 0:cbd88c574455 | 220 | // Start capturing packets |
foxdie | 0:cbd88c574455 | 221 | uptime.start(); |
foxdie | 0:cbd88c574455 | 222 | gmlan.attach(&processMessage); |
foxdie | 0:cbd88c574455 | 223 | |
foxdie | 0:cbd88c574455 | 224 | while(1) { |
foxdie | 0:cbd88c574455 | 225 | // Poll serial for keypresses for certain tasks |
foxdie | 0:cbd88c574455 | 226 | if (pc.readable()) |
foxdie | 0:cbd88c574455 | 227 | { |
foxdie | 0:cbd88c574455 | 228 | switch (pc.getc()) |
foxdie | 0:cbd88c574455 | 229 | { |
foxdie | 0:cbd88c574455 | 230 | case 'f': |
foxdie | 0:cbd88c574455 | 231 | case 'F': |
foxdie | 0:cbd88c574455 | 232 | { |
foxdie | 0:cbd88c574455 | 233 | // Toggle filter |
foxdie | 0:cbd88c574455 | 234 | filterPackets = !filterPackets; |
foxdie | 0:cbd88c574455 | 235 | filter_led = (int)filterPackets; |
foxdie | 0:cbd88c574455 | 236 | break; |
foxdie | 0:cbd88c574455 | 237 | } |
foxdie | 0:cbd88c574455 | 238 | case 'c': |
foxdie | 0:cbd88c574455 | 239 | case 'C': |
foxdie | 0:cbd88c574455 | 240 | { |
foxdie | 0:cbd88c574455 | 241 | // Clear filter |
foxdie | 0:cbd88c574455 | 242 | filteredPackets.clear(); |
foxdie | 0:cbd88c574455 | 243 | pc.printf("Packet filter cleared\r\n"); |
foxdie | 0:cbd88c574455 | 244 | break; |
foxdie | 0:cbd88c574455 | 245 | } |
foxdie | 0:cbd88c574455 | 246 | case 'd': |
foxdie | 0:cbd88c574455 | 247 | case 'D': |
foxdie | 0:cbd88c574455 | 248 | { |
foxdie | 0:cbd88c574455 | 249 | // Show filter |
foxdie | 0:cbd88c574455 | 250 | clearAndHome(); |
foxdie | 0:cbd88c574455 | 251 | pc.printf("[%u] entries in filter:\r\n", filteredPackets.size()); |
foxdie | 0:cbd88c574455 | 252 | for (int i=0; i < filteredPackets.size(); i++) |
foxdie | 0:cbd88c574455 | 253 | pc.printf("0x%08X ", filteredPackets[i]); |
foxdie | 0:cbd88c574455 | 254 | pc.printf("\r\n"); |
foxdie | 0:cbd88c574455 | 255 | break; |
foxdie | 0:cbd88c574455 | 256 | } |
foxdie | 0:cbd88c574455 | 257 | case 'r': |
foxdie | 0:cbd88c574455 | 258 | case 'R': |
foxdie | 0:cbd88c574455 | 259 | { |
foxdie | 0:cbd88c574455 | 260 | // Restart mbed |
foxdie | 0:cbd88c574455 | 261 | pc.printf("Restarting mbed...\r\n"); |
foxdie | 0:cbd88c574455 | 262 | mbed_reset(); |
foxdie | 0:cbd88c574455 | 263 | break; |
foxdie | 0:cbd88c574455 | 264 | } |
foxdie | 0:cbd88c574455 | 265 | case 't': |
foxdie | 0:cbd88c574455 | 266 | case 'T': |
foxdie | 0:cbd88c574455 | 267 | { |
foxdie | 0:cbd88c574455 | 268 | // Trigger packet send |
foxdie | 0:cbd88c574455 | 269 | notifyUser(); |
foxdie | 0:cbd88c574455 | 270 | break; |
foxdie | 0:cbd88c574455 | 271 | } |
foxdie | 0:cbd88c574455 | 272 | default: |
foxdie | 0:cbd88c574455 | 273 | pc.printf("Unknown keypress!\r\n"); |
foxdie | 0:cbd88c574455 | 274 | break; |
foxdie | 0:cbd88c574455 | 275 | } |
foxdie | 0:cbd88c574455 | 276 | } |
foxdie | 0:cbd88c574455 | 277 | // Sleep for 20ms repeatedly, as all messages are handled by an interrupt, this prevents |
foxdie | 0:cbd88c574455 | 278 | // keeping the mbed at full load |
foxdie | 0:cbd88c574455 | 279 | wait(0.02); |
foxdie | 0:cbd88c574455 | 280 | } |
foxdie | 0:cbd88c574455 | 281 | } |