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
- Committer:
- avbotz
- Date:
- 2013-07-08
- Revision:
- 4:8f63393d49fb
- Parent:
- 2:d8b182fbe018
File content as of revision 4:8f63393d49fb:
/* * 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, http://www.sparkfun.com/products/10736 */ #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" Serial IMU(p9, p10); // tx, rx Serial PC(USBTX, USBRX); DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); DigitalOut myled4(LED4); bool IMUreadable = false; bool PCreadable = false; void readIMU(); void readPC(); inline void makeCorrect(short* i); short 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]; AnalogOut aout(p18); int main() { aout = 1.0; // 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); for (int i = 0; i < 80; i++) { PC.putc('-'); } PC.printf("\n\r"); PC.attach(&readPC); IMU.attach(&readIMU); 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; 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"); //This should be easier to understand than bitshifts. bufIMU is a pointer (arrays are pointers). //We offset it to the start of the desired value. We then typecast bufIMU into a short (16 bit). //This turns bufIMU into a pointer to the desired value. Now we dereference it. //I'm not sure if we'll still need makeCorrect. Adit, check plz <3 accX = *((short*)(bufIMU + 1)); accY = *((short*)(bufIMU + 3)); accZ = *((short*)(bufIMU + 5)); gyrX = *((short*)(bufIMU + 7)); gyrY = *((short*)(bufIMU + 9)); gyrZ = *((short*)(bufIMU + 11)); magX = *((short*)(bufIMU + 13)); magY = *((short*)(bufIMU + 15)); magZ = *((short*)(bufIMU + 17)); //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) == '#'); } IMUreadable = false; myled2 = 0; } if (PCreadable) { myled1 = 1; while (PC.readable()) IMU.putc(PC.getc()); PCreadable = false; myled1 = 0; } //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() { PCreadable = true; __disable_irq(); } //Interrupt called when there is a character to be read from the IMU void readIMU() { IMUreadable = true; __disable_irq(); } //So negative numbers that are transferred are in twos complement form // and the compiler seems to like to use things created by bitwise operators // in unsigned form so all of the bits are switched and we switch it back and center // it around 512 which we are using as out zero value inline void makeCorrect (short* i) { if ((*i)>>15) *i = 512 - (~(*i)); else *i = 512 + *i; }