Jason Gaunt / Mbed 2 deprecated GMLAN-Node

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.

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }