A class to receive data from the SparkFun 9DOF Razor IMU. It can be easily adapted to work with IMUs with different data formats.
imu.cpp
- Committer:
- avbotz
- Date:
- 2013-07-08
- Revision:
- 4:8f63393d49fb
- Parent:
- 3:f04d3d10d518
File content as of revision 4:8f63393d49fb:
#include "mbed.h" #include "imu.h" /* * IMU data averages, flat surface * 20.138444, -15.096182, 232.355290, -29.691927, -14.679685, 45.097056, -75.690469, 0.737977, -159.505392 */ IMU::IMU(int baud, PinName tx, PinName rx, Serial* pc) { p_pc = pc; p_pc->printf("Initializing IMU...\n"); p_device = new Serial(tx, rx); p_device->baud(baud); p_device->format(8, Serial::None, 1); readable = false; i_IMU = 0; accX = accY = accZ = gyrX = gyrY = gyrZ = magX = magY = magZ = 0; p_device->putc('4'); //Tell the IMU to start sending binary data, if it isn't already newData = false; } IMU::~IMU() { if (p_device != NULL){ delete p_device; //This prevents memory leaks and keeps the port accessible for future use. } } //Wrapper function to write a character to the IMU void IMU::putc(char c) { p_device->putc(c); } //Wrapper function to attach a callback function to the RX interrupt of the IMU Serial object void IMU::attach(void (*fptr)(void)) { p_pc->putc('a'); p_device->attach(fptr); } //Gets all data from the IMU read buffer. If we should parse, parse. void IMU::getData() { readable = false; //p_pc->printf("HELLO"); __enable_irq(); //NVIC_EnableIRQ(UART1_IRQn); while (p_device->readable()) { //p_pc->printf("readable"); // This snippet of code should be run whenever a character can be received from the IMU. char data = p_device->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; //p_pc->printf("new data\n\r"); } // Something went wrong. else if (i_IMU > 21) { for (int i = 0; i < 20; i++) { p_pc->printf("\n\r"); } //p_pc->printf("\t\t\t\t\t\t\t\t\t**\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*/) { parse(); } bufIMU[i_IMU] = data; i_IMU++; //parseNow = (buffer.at(buffer.length() - 1) == '#'); } } // 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 IMU::makeCorrect(short* i) { if ((*i)>>15) *i = 0-(~(*i)+1); else *i = 0+*i; } /* void IMU::attach(void) { p_device->attach(this->readIMU); }*/ void IMU::parse() { //p_pc->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. //Don't use this. It doesn't work. //Kevin said this didn't work because of endianness. The Cortex M3 has a macro to reverse. //See http://mbed.org/forum/helloworld/topic/1573/?page=1#comment-7818 /*accX = *((short*)(bufIMU + 2)); accY = *((short*)(bufIMU + 4)); accZ = *((short*)(bufIMU + 6)); gyrX = *((short*)(bufIMU + 8)); gyrY = *((short*)(bufIMU + 10)); gyrZ = *((short*)(bufIMU + 12)); magX = *((short*)(bufIMU + 14)); magY = *((short*)(bufIMU + 16)); magZ = *((short*)(bufIMU + 18));*/ //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); newData = true; //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 } //Callback called when there is a character to be read from the IMU void readIMU() { imu.readable = true; __disable_irq(); //NVIC_DisableIRQ(UART1_IRQn); } int main() { PC.format(8, Serial::None, 1); PC.baud(115200); for (int i = 0; i < 80; i++) { PC.putc('-'); } PC.printf("\n\r"); imu.attach(&readIMU); //imu.p_device->attach(&readIMU); imu.putc('6'); PC.printf("Waiting for data."); // Calibration variables long long sumAccX = 0, sumAccY = 0, sumAccZ = 0; long long sumGyrX = 0, sumGyrY = 0, sumGyrZ = 0; long long sumMagX = 0, sumMagY = 0, sumMagZ = 0; int num = 0; while (true){ if (imu.readable) { //PC.printf("fun"); imu.getData(); if (imu.newData) { PC.printf("Data: %d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", imu.accX, imu.accY, imu.accZ, imu.gyrX, imu.gyrY, imu.gyrZ, imu.magX, imu.magY, imu.magZ); sumAccX += imu.accX; sumAccY += imu.accY; sumAccZ += imu.accZ; sumGyrX += imu.gyrX; sumGyrY += imu.gyrY; sumGyrZ += imu.gyrZ; sumMagX += imu.magX; sumMagY += imu.magY; sumMagZ += imu.magZ; num++; PC.printf("Averages: %f, %f, %f, %f, %f, %f, %f, %f, %f\n", sumAccX/(double)num, sumAccY/(double)num, sumAccZ/(double)num, sumGyrX/(double)num, sumGyrY/(double)num, sumGyrZ/(double)num, sumMagX/(double)num, sumMagY/(double)num, sumMagZ/(double)num ); imu.newData = false; } } } }