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 3:f04d3d10d518, committed 2011-11-05
- Comitter:
- avbotz
- Date:
- Sat Nov 05 01:02:59 2011 +0000
- Parent:
- 2:d8b182fbe018
- Child:
- 4:8f63393d49fb
- Commit message:
- Added code for finding the average sensor values (for calibration). Problems: Average includes garbage data. We know when theres an IMU error, but Im not sure the best way to prevent parsing.
Changed in this revision
imu.cpp | Show annotated file Show diff for this revision Revisions of this file |
imu.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/imu.cpp Fri Nov 04 22:24:02 2011 +0000 +++ b/imu.cpp Sat Nov 05 01:02:59 2011 +0000 @@ -1,6 +1,11 @@ #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"); @@ -11,6 +16,7 @@ i_IMU = 0; accX = accY = accZ = gyrX = gyrY = gyrZ = magX = magY = magZ = 0; p_device->putc('6'); //Tell the IMU to start sending binary data, if it isn't already + newData = false; } IMU::~IMU() { @@ -50,12 +56,15 @@ } // Something went wrong. else if (i_IMU > 21) { - //p_pc->printf("\t\t\tIMU error.\n\r"); + 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*/) { + else if (i_IMU == 21 && bufIMU[0] == '$' /*&& bufIMU[20] == '\n' data == '\n' && i_IMU == 19*/) { parse(); } @@ -66,13 +75,13 @@ } } -//So negative numbers that are transferred are in twos complement form +// 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 = 512-(~(*i)); - else *i = 512+*i; + if ((*i)>>15) *i = 0-(~(*i)); + else *i = 0+*i; } /* @@ -81,7 +90,7 @@ }*/ void IMU::parse() { - p_pc->printf("Parsing\n\r"); + //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). @@ -130,8 +139,9 @@ 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; @@ -164,11 +174,44 @@ 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(); - 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); + 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; + } } } } \ No newline at end of file
--- a/imu.h Fri Nov 04 22:24:02 2011 +0000 +++ b/imu.h Sat Nov 05 01:02:59 2011 +0000 @@ -20,6 +20,7 @@ //void readIMU(); short accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ; bool readable; + bool newData; private: Serial* p_device;