A class to receive data from the SparkFun 9DOF Razor IMU. It can be easily adapted to work with IMUs with different data formats.
main.cpp@0:a260d84e07fc, 2011-10-16 (annotated)
- Committer:
- avbotz
- Date:
- Sun Oct 16 05:37:27 2011 +0000
- Revision:
- 0:a260d84e07fc
- Child:
- 1:fdfa313b9cc3
Sort of working IMU code. Relies on the IMU being in ascii mode.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
avbotz |
0:a260d84e07fc | 1 | /* |
avbotz |
0:a260d84e07fc | 2 | * Demo to relay I/O between a computer and the IMU. Make sure to connect the GND wire on the IMU to pin 1 (GND) on the mbed so there's a return current |
avbotz |
0:a260d84e07fc | 3 | * Updated to use interrupts - this will help when we intergrate this code into AVNavControl |
avbotz |
0:a260d84e07fc | 4 | * 9dof razor from sparkfun |
avbotz |
0:a260d84e07fc | 5 | */ |
avbotz |
0:a260d84e07fc | 6 | |
avbotz |
0:a260d84e07fc | 7 | #define GYRO_SCALE 14.375 // ticks per degree, http://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf |
avbotz |
0:a260d84e07fc | 8 | |
avbotz |
0:a260d84e07fc | 9 | #include "mbed.h" |
avbotz |
0:a260d84e07fc | 10 | #include <string> |
avbotz |
0:a260d84e07fc | 11 | #include <sstream> |
avbotz |
0:a260d84e07fc | 12 | |
avbotz |
0:a260d84e07fc | 13 | Serial IMU(p9, p10); // tx, rx |
avbotz |
0:a260d84e07fc | 14 | Serial PC(USBTX, USBRX); |
avbotz |
0:a260d84e07fc | 15 | |
avbotz |
0:a260d84e07fc | 16 | DigitalOut myled1(LED1); |
avbotz |
0:a260d84e07fc | 17 | DigitalOut myled2(LED2); |
avbotz |
0:a260d84e07fc | 18 | DigitalOut myled3(LED3); |
avbotz |
0:a260d84e07fc | 19 | DigitalOut myled4(LED4); |
avbotz |
0:a260d84e07fc | 20 | |
avbotz |
0:a260d84e07fc | 21 | bool IMUreadable = false; |
avbotz |
0:a260d84e07fc | 22 | bool PCreadable = false; |
avbotz |
0:a260d84e07fc | 23 | bool parseNow = false; |
avbotz |
0:a260d84e07fc | 24 | |
avbotz |
0:a260d84e07fc | 25 | bool debugMode = false; |
avbotz |
0:a260d84e07fc | 26 | |
avbotz |
0:a260d84e07fc | 27 | string buffer; |
avbotz |
0:a260d84e07fc | 28 | |
avbotz |
0:a260d84e07fc | 29 | void readIMU(); |
avbotz |
0:a260d84e07fc | 30 | void readPC(); |
avbotz |
0:a260d84e07fc | 31 | |
avbotz |
0:a260d84e07fc | 32 | void parse(string); |
avbotz |
0:a260d84e07fc | 33 | |
avbotz |
0:a260d84e07fc | 34 | int accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ; |
avbotz |
0:a260d84e07fc | 35 | |
avbotz |
0:a260d84e07fc | 36 | int main() { |
avbotz |
0:a260d84e07fc | 37 | IMU.format(8, Serial::None, 1); |
avbotz |
0:a260d84e07fc | 38 | PC.format(8, Serial::None, 1); |
avbotz |
0:a260d84e07fc | 39 | IMU.baud(57600); |
avbotz |
0:a260d84e07fc | 40 | PC.baud(115200); |
avbotz |
0:a260d84e07fc | 41 | |
avbotz |
0:a260d84e07fc | 42 | PC.printf("----------------------------\n\r"); |
avbotz |
0:a260d84e07fc | 43 | PC.printf("debug: device reset\n\r"); |
avbotz |
0:a260d84e07fc | 44 | PC.printf("debug: gotta have my bowl; gotta have serial. serial is up.\n\r"); |
avbotz |
0:a260d84e07fc | 45 | |
avbotz |
0:a260d84e07fc | 46 | PC.attach(&readPC); |
avbotz |
0:a260d84e07fc | 47 | IMU.attach(&readIMU); |
avbotz |
0:a260d84e07fc | 48 | |
avbotz |
0:a260d84e07fc | 49 | //__disable_irq(); |
avbotz |
0:a260d84e07fc | 50 | PC.printf("debug: attached interrupts.\n\r"); |
avbotz |
0:a260d84e07fc | 51 | |
avbotz |
0:a260d84e07fc | 52 | if (!debugMode) IMU.putc('4'); //tell the imu to start sending data, if it's not sending data already |
avbotz |
0:a260d84e07fc | 53 | PC.printf("debug: told the IMU to start sending data\n\r"); |
avbotz |
0:a260d84e07fc | 54 | |
avbotz |
0:a260d84e07fc | 55 | //The main loop |
avbotz |
0:a260d84e07fc | 56 | while (1) { |
avbotz |
0:a260d84e07fc | 57 | __enable_irq(); |
avbotz |
0:a260d84e07fc | 58 | if (IMUreadable) { |
avbotz |
0:a260d84e07fc | 59 | myled2 = 1; |
avbotz |
0:a260d84e07fc | 60 | if (debugMode){ |
avbotz |
0:a260d84e07fc | 61 | while (IMU.readable()) PC.putc(IMU.getc()); |
avbotz |
0:a260d84e07fc | 62 | } |
avbotz |
0:a260d84e07fc | 63 | else { |
avbotz |
0:a260d84e07fc | 64 | while (IMU.readable()){ |
avbotz |
0:a260d84e07fc | 65 | buffer.push_back(IMU.getc()); |
avbotz |
0:a260d84e07fc | 66 | parseNow = (buffer.at(buffer.length() - 1) == '#'); |
avbotz |
0:a260d84e07fc | 67 | } |
avbotz |
0:a260d84e07fc | 68 | } |
avbotz |
0:a260d84e07fc | 69 | |
avbotz |
0:a260d84e07fc | 70 | IMUreadable = false; |
avbotz |
0:a260d84e07fc | 71 | myled2 = 0; |
avbotz |
0:a260d84e07fc | 72 | } |
avbotz |
0:a260d84e07fc | 73 | if (PCreadable) { |
avbotz |
0:a260d84e07fc | 74 | myled1 = 1; |
avbotz |
0:a260d84e07fc | 75 | while (PC.readable()) IMU.putc(PC.getc()); |
avbotz |
0:a260d84e07fc | 76 | PCreadable = false; |
avbotz |
0:a260d84e07fc | 77 | myled1 = 0; |
avbotz |
0:a260d84e07fc | 78 | } |
avbotz |
0:a260d84e07fc | 79 | if (parseNow) { |
avbotz |
0:a260d84e07fc | 80 | parse(buffer); |
avbotz |
0:a260d84e07fc | 81 | buffer.clear(); |
avbotz |
0:a260d84e07fc | 82 | parseNow = false; |
avbotz |
0:a260d84e07fc | 83 | } |
avbotz |
0:a260d84e07fc | 84 | } |
avbotz |
0:a260d84e07fc | 85 | } |
avbotz |
0:a260d84e07fc | 86 | |
avbotz |
0:a260d84e07fc | 87 | //Interrupt called when there is a character to be read from the PC |
avbotz |
0:a260d84e07fc | 88 | //To avoid a livelock, we disable interrupts at the end of the interrupt. |
avbotz |
0:a260d84e07fc | 89 | //Then, in the main loop, we read everything from the buffer |
avbotz |
0:a260d84e07fc | 90 | void readPC(){ |
avbotz |
0:a260d84e07fc | 91 | PCreadable = true; |
avbotz |
0:a260d84e07fc | 92 | __disable_irq(); |
avbotz |
0:a260d84e07fc | 93 | } |
avbotz |
0:a260d84e07fc | 94 | |
avbotz |
0:a260d84e07fc | 95 | //Interrupt called when there is a character to be read from the IMU |
avbotz |
0:a260d84e07fc | 96 | void readIMU(){ |
avbotz |
0:a260d84e07fc | 97 | IMUreadable = true; |
avbotz |
0:a260d84e07fc | 98 | __disable_irq(); |
avbotz |
0:a260d84e07fc | 99 | } |
avbotz |
0:a260d84e07fc | 100 | |
avbotz |
0:a260d84e07fc | 101 | //Checks data integrity, then stores the IMU values into variables |
avbotz |
0:a260d84e07fc | 102 | void parse(string buf){ |
avbotz |
0:a260d84e07fc | 103 | if (buf.find('$') != buf.npos && buf.find('#') != buf.npos){ |
avbotz |
0:a260d84e07fc | 104 | for (int i = 0; i < buf.length(); i++) if (buf[i] == '$' || buf[i] == ',' || buf[i] == '#') buf[i] = ' '; |
avbotz |
0:a260d84e07fc | 105 | stringstream ss(buf); |
avbotz |
0:a260d84e07fc | 106 | ss >> accX >> accY >> accZ >> gyrX >> gyrY >> gyrZ >> magX >> magY >> magZ; |
avbotz |
0:a260d84e07fc | 107 | |
avbotz |
0:a260d84e07fc | 108 | PC.printf("%d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ); |
avbotz |
0:a260d84e07fc | 109 | } |
avbotz |
0:a260d84e07fc | 110 | else PC.printf("Bad parse!\n\r"); |
avbotz |
0:a260d84e07fc | 111 | } |