A class to receive data from the SparkFun 9DOF Razor IMU. It can be easily adapted to work with IMUs with different data formats.
Revision 1:fdfa313b9cc3, committed 2011-10-17
- Comitter:
- avbotz
- Date:
- Mon Oct 17 15:19:01 2011 +0000
- Parent:
- 0:a260d84e07fc
- Child:
- 2:d8b182fbe018
- Commit message:
- IMU parsing code that utilizes the IMU\'s binary mode
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sun Oct 16 05:37:27 2011 +0000 +++ b/main.cpp Mon Oct 17 15:19:01 2011 +0000 @@ -1,14 +1,12 @@ -/* +/* * 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 * Updated to use interrupts - this will help when we intergrate this code into AVNavControl * 9dof razor from sparkfun */ - + #define GYRO_SCALE 14.375 // ticks per degree, http://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf #include "mbed.h" -#include <string> -#include <sstream> Serial IMU(p9, p10); // tx, rx Serial PC(USBTX, USBRX); @@ -20,53 +18,106 @@ bool IMUreadable = false; bool PCreadable = false; -bool parseNow = false; - -bool debugMode = false; - -string buffer; void readIMU(); void readPC(); +inline void makeCorrect(short* i); -void parse(string); +short accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ; -int accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ; +// Expected format: {$, accX, accX, accY, accY, accZ, accZ, gyrX, gyrX, gyrY, gyrY, gyrZ, gyrZ, magX, magX, magY, magY, magZ, magZ, #, \r} +int i_IMU; +char bufIMU[21]; int main() { + // Set up the connection. Read up about parity and stop bits if this is confusing. IMU.format(8, Serial::None, 1); PC.format(8, Serial::None, 1); IMU.baud(57600); PC.baud(115200); - - PC.printf("----------------------------\n\r"); - PC.printf("debug: device reset\n\r"); - PC.printf("debug: gotta have my bowl; gotta have serial. serial is up.\n\r"); - + + for (int i = 0; i < 80; i++) { + PC.putc('-'); + } + PC.printf("\n\r"); + PC.attach(&readPC); IMU.attach(&readIMU); - - //__disable_irq(); - PC.printf("debug: attached interrupts.\n\r"); - - if (!debugMode) IMU.putc('4'); //tell the imu to start sending data, if it's not sending data already - PC.printf("debug: told the IMU to start sending data\n\r"); - - //The main loop - while (1) { + + IMU.putc('6'); //tell the IMU to start sending data in binary mode, if it's not sending data already + + //The main loop + while (true) { __enable_irq(); if (IMUreadable) { myled2 = 1; - if (debugMode){ - while (IMU.readable()) PC.putc(IMU.getc()); + + while (IMU.readable()) { + // This snippet of code should be run whenever a character can be received from the IMU. + + char data = IMU.getc(); + + // Start of a new set of data. Reset the counter to the first position in the buffer, and start throwing data in there. + if (data == '$') { + i_IMU = 0; + printf("new data\n\r"); + } + // Something went wrong. + else if (i_IMU > 21) { + printf("\t\t\tIMU error.\n\r"); + i_IMU = 21; + } + + // End of the set of data. Parse the buffer + else if (i_IMU == 21 /*&& bufIMU[0] == '$' && bufIMU[20] == '\n' data == '\n' && i_IMU == 19*/) { + printf("Parsing\n\r"); + + //bufIMU contains binary data. Each variable sent by the IMU is a 16-bit integer + //broken down into two characters (in bufIMU[]). Here, we reconstitute the original integer by + //left-shifting the first character by 8 bits and ORing it with the second character. + accX = (bufIMU[1]<<8 | bufIMU[2]); + accY = (bufIMU[3]<<8 | bufIMU[4]); + accZ = (bufIMU[5]<<8 | bufIMU[6]); + + gyrX = (bufIMU[7]<<8 | bufIMU[8]); + gyrY = (bufIMU[9]<<8 | bufIMU[10]); + gyrZ = (bufIMU[11]<<8 | bufIMU[12]); + + magX = (bufIMU[13]<<8 | bufIMU[14]); + magY = (bufIMU[15]<<8 | bufIMU[16]); + magZ = (bufIMU[17]<<8 | bufIMU[18]); + + makeCorrect(&accX); + makeCorrect(&accY); + makeCorrect(&accZ); + + makeCorrect(&gyrX); + makeCorrect(&gyrY); + makeCorrect(&gyrZ); + + makeCorrect(&magX); + makeCorrect(&magY); + makeCorrect(&magZ); + + + PC.printf("Data: %d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ); + + //accX = accY = accZ = gyrX = gyrY = gyrZ = magX = magY = magZ = 0; + + /* + for (int i = 0; i < 21; i++) { + PC.printf("%d ", bufIMU[i]); + } + PC.printf("\n\r"); + */ + //newIMUData = 1; // Update the flag + } + + + bufIMU[i_IMU] = data; + i_IMU++; + //parseNow = (buffer.at(buffer.length() - 1) == '#'); } - else { - while (IMU.readable()){ - buffer.push_back(IMU.getc()); - parseNow = (buffer.at(buffer.length() - 1) == '#'); - } - } - IMUreadable = false; myled2 = 0; } @@ -76,36 +127,33 @@ PCreadable = false; myled1 = 0; } - if (parseNow) { + /*if (parseNow) { parse(buffer); buffer.clear(); parseNow = false; - } + }*/ } } //Interrupt called when there is a character to be read from the PC //To avoid a livelock, we disable interrupts at the end of the interrupt. //Then, in the main loop, we read everything from the buffer -void readPC(){ +void readPC() { PCreadable = true; __disable_irq(); } //Interrupt called when there is a character to be read from the IMU -void readIMU(){ +void readIMU() { IMUreadable = true; __disable_irq(); } -//Checks data integrity, then stores the IMU values into variables -void parse(string buf){ - if (buf.find('$') != buf.npos && buf.find('#') != buf.npos){ - for (int i = 0; i < buf.length(); i++) if (buf[i] == '$' || buf[i] == ',' || buf[i] == '#') buf[i] = ' '; - stringstream ss(buf); - ss >> accX >> accY >> accZ >> gyrX >> gyrY >> gyrZ >> magX >> magY >> magZ; - PC.printf("%d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ); - } - else PC.printf("Bad parse!\n\r"); -} +//The bitshift performed always creates a positive integer. Sometimes, the IMU +//values are negative. This fixes that - it centers the value around 512. That is, +//512 is 0 for the IMU variables. Values above it are positive, and values below it are negative. +inline void makeCorrect (short* i) { + if ((*i)>>15) *i = 512 - (~(*i)); + else *i = 512 + *i; +} \ No newline at end of file