Amador Valley High School Robotics Team / Mbed 2 deprecated SparkFun_razor_imu

A class to receive data from the SparkFun 9DOF Razor IMU. It can be easily adapted to work with IMUs with different data formats.

Dependencies:   mbed

imu.cpp@4:8f63393d49fb, 2013-07-08 (annotated)

Committer:
avbotz
Date:
Mon Jul 08 04:04:21 2013 +0000
Revision:
4:8f63393d49fb
Parent:
3:f04d3d10d518 
Initial commit.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
avbotz 2:d8b182fbe018 1 #include "mbed.h"
avbotz 2:d8b182fbe018 2 #include "imu.h"
avbotz 2:d8b182fbe018 3
avbotz 3:f04d3d10d518 4 /*
avbotz 3:f04d3d10d518 5 * IMU data averages, flat surface
avbotz 3:f04d3d10d518 6 * 20.138444, -15.096182, 232.355290, -29.691927, -14.679685, 45.097056, -75.690469, 0.737977, -159.505392
avbotz 3:f04d3d10d518 7 */
avbotz 3:f04d3d10d518 8
avbotz 4:8f63393d49fb 9 IMU::IMU(int baud, PinName tx, PinName rx, Serial* pc) {
avbotz 2:d8b182fbe018 10 p_pc = pc;
avbotz 2:d8b182fbe018 11 p_pc->printf("Initializing IMU...\n");
avbotz 2:d8b182fbe018 12 p_device = new Serial(tx, rx);
avbotz 2:d8b182fbe018 13 p_device->baud(baud);
avbotz 2:d8b182fbe018 14 p_device->format(8, Serial::None, 1);
avbotz 2:d8b182fbe018 15 readable = false;
avbotz 2:d8b182fbe018 16 i_IMU = 0;
avbotz 2:d8b182fbe018 17 accX = accY = accZ = gyrX = gyrY = gyrZ = magX = magY = magZ = 0;
avbotz 4:8f63393d49fb 18 p_device->putc('4'); //Tell the IMU to start sending binary data, if it isn't already
avbotz 3:f04d3d10d518 19 newData = false;
avbotz 2:d8b182fbe018 20 }
avbotz 2:d8b182fbe018 21
avbotz 2:d8b182fbe018 22 IMU::~IMU() {
avbotz 2:d8b182fbe018 23 if (p_device != NULL){
avbotz 2:d8b182fbe018 24 delete p_device; //This prevents memory leaks and keeps the port accessible for future use.
avbotz 2:d8b182fbe018 25 }
avbotz 2:d8b182fbe018 26 }
avbotz 2:d8b182fbe018 27
avbotz 2:d8b182fbe018 28 //Wrapper function to write a character to the IMU
avbotz 2:d8b182fbe018 29 void IMU::putc(char c) {
avbotz 2:d8b182fbe018 30 p_device->putc(c);
avbotz 2:d8b182fbe018 31 }
avbotz 2:d8b182fbe018 32
avbotz 2:d8b182fbe018 33
avbotz 2:d8b182fbe018 34 //Wrapper function to attach a callback function to the RX interrupt of the IMU Serial object
avbotz 2:d8b182fbe018 35 void IMU::attach(void (*fptr)(void)) {
avbotz 2:d8b182fbe018 36 p_pc->putc('a');
avbotz 2:d8b182fbe018 37 p_device->attach(fptr);
avbotz 2:d8b182fbe018 38 }
avbotz 2:d8b182fbe018 39
avbotz 2:d8b182fbe018 40 //Gets all data from the IMU read buffer. If we should parse, parse.
avbotz 2:d8b182fbe018 41 void IMU::getData() {
avbotz 2:d8b182fbe018 42 readable = false;
avbotz 2:d8b182fbe018 43 //p_pc->printf("HELLO");
avbotz 2:d8b182fbe018 44 __enable_irq();
avbotz 2:d8b182fbe018 45 //NVIC_EnableIRQ(UART1_IRQn);
avbotz 2:d8b182fbe018 46 while (p_device->readable()) {
avbotz 2:d8b182fbe018 47 //p_pc->printf("readable");
avbotz 2:d8b182fbe018 48 // This snippet of code should be run whenever a character can be received from the IMU.
avbotz 2:d8b182fbe018 49
avbotz 2:d8b182fbe018 50 char data = p_device->getc();
avbotz 2:d8b182fbe018 51
avbotz 2:d8b182fbe018 52 // Start of a new set of data. Reset the counter to the first position in the buffer, and start throwing data in there.
avbotz 2:d8b182fbe018 53 if (data == '$') {
avbotz 2:d8b182fbe018 54 i_IMU = 0;
avbotz 2:d8b182fbe018 55 //p_pc->printf("new data\n\r");
avbotz 2:d8b182fbe018 56 }
avbotz 2:d8b182fbe018 57 // Something went wrong.
avbotz 2:d8b182fbe018 58 else if (i_IMU > 21) {
avbotz 3:f04d3d10d518 59 for (int i = 0; i < 20; i++) {
avbotz 3:f04d3d10d518 60 p_pc->printf("\n\r");
avbotz 3:f04d3d10d518 61 }
avbotz 3:f04d3d10d518 62 //p_pc->printf("\t\t\t\t\t\t\t\t\t**\n\r");
avbotz 2:d8b182fbe018 63 i_IMU = 21;
avbotz 2:d8b182fbe018 64 }
avbotz 2:d8b182fbe018 65
avbotz 2:d8b182fbe018 66 // End of the set of data. Parse the buffer
avbotz 3:f04d3d10d518 67 else if (i_IMU == 21 && bufIMU[0] == '$' /*&& bufIMU[20] == '\n' data == '\n' && i_IMU == 19*/) {
avbotz 2:d8b182fbe018 68 parse();
avbotz 2:d8b182fbe018 69 }
avbotz 2:d8b182fbe018 70
avbotz 2:d8b182fbe018 71
avbotz 2:d8b182fbe018 72 bufIMU[i_IMU] = data;
avbotz 2:d8b182fbe018 73 i_IMU++;
avbotz 2:d8b182fbe018 74 //parseNow = (buffer.at(buffer.length() - 1) == '#');
avbotz 2:d8b182fbe018 75 }
avbotz 2:d8b182fbe018 76 }
avbotz 2:d8b182fbe018 77
avbotz 3:f04d3d10d518 78 // So negative numbers that are transferred are in twos complement form
avbotz 2:d8b182fbe018 79 // and the compiler seems to like to use things created by bitwise operators
avbotz 2:d8b182fbe018 80 // in unsigned form so all of the bits are switched and we switch it back and center
avbotz 2:d8b182fbe018 81 // it around 512 which we are using as out zero value
avbotz 2:d8b182fbe018 82 inline void IMU::makeCorrect(short* i) {
avbotz 4:8f63393d49fb 83 if ((*i)>>15) *i = 0-(~(*i)+1);
avbotz 3:f04d3d10d518 84 else *i = 0+*i;
avbotz 2:d8b182fbe018 85 }
avbotz 2:d8b182fbe018 86
avbotz 2:d8b182fbe018 87 /*
avbotz 2:d8b182fbe018 88 void IMU::attach(void) {
avbotz 2:d8b182fbe018 89 p_device->attach(this->readIMU);
avbotz 2:d8b182fbe018 90 }*/
avbotz 2:d8b182fbe018 91
avbotz 2:d8b182fbe018 92 void IMU::parse() {
avbotz 3:f04d3d10d518 93 //p_pc->printf("Parsing\n\r");
avbotz 2:d8b182fbe018 94
avbotz 2:d8b182fbe018 95 //This should be easier to understand than bitshifts. bufIMU is a pointer (arrays are pointers).
avbotz 2:d8b182fbe018 96 //We offset it to the start of the desired value. We then typecast bufIMU into a short (16 bit).
avbotz 2:d8b182fbe018 97 //This turns bufIMU into a pointer to the desired value. Now we dereference it.
avbotz 2:d8b182fbe018 98 //I'm not sure if we'll still need makeCorrect.
avbotz 2:d8b182fbe018 99
avbotz 2:d8b182fbe018 100 //Don't use this. It doesn't work.
avbotz 2:d8b182fbe018 101 //Kevin said this didn't work because of endianness. The Cortex M3 has a macro to reverse.
avbotz 2:d8b182fbe018 102 //See http://mbed.org/forum/helloworld/topic/1573/?page=1#comment-7818
avbotz 2:d8b182fbe018 103 /*accX = *((short*)(bufIMU + 2));
avbotz 2:d8b182fbe018 104 accY = *((short*)(bufIMU + 4));
avbotz 2:d8b182fbe018 105 accZ = *((short*)(bufIMU + 6));
avbotz 2:d8b182fbe018 106
avbotz 2:d8b182fbe018 107 gyrX = *((short*)(bufIMU + 8));
avbotz 2:d8b182fbe018 108 gyrY = *((short*)(bufIMU + 10));
avbotz 2:d8b182fbe018 109 gyrZ = *((short*)(bufIMU + 12));
avbotz 2:d8b182fbe018 110
avbotz 2:d8b182fbe018 111 magX = *((short*)(bufIMU + 14));
avbotz 2:d8b182fbe018 112 magY = *((short*)(bufIMU + 16));
avbotz 2:d8b182fbe018 113 magZ = *((short*)(bufIMU + 18));*/
avbotz 2:d8b182fbe018 114
avbotz 2:d8b182fbe018 115 //bufIMU contains binary data. Each variable sent by the IMU is a 16-bit integer
avbotz 2:d8b182fbe018 116 //broken down into two characters (in bufIMU[]). Here, we reconstitute the original integer by
avbotz 2:d8b182fbe018 117 //left-shifting the first character by 8 bits and ORing it with the second character.
avbotz 2:d8b182fbe018 118
avbotz 2:d8b182fbe018 119 accX = (bufIMU[1]<<8 | bufIMU[2]);
avbotz 2:d8b182fbe018 120 accY = (bufIMU[3]<<8 | bufIMU[4]);
avbotz 2:d8b182fbe018 121 accZ = (bufIMU[5]<<8 | bufIMU[6]);
avbotz 2:d8b182fbe018 122
avbotz 2:d8b182fbe018 123 gyrX = (bufIMU[7]<<8 | bufIMU[8]);
avbotz 2:d8b182fbe018 124 gyrY = (bufIMU[9]<<8 | bufIMU[10]);
avbotz 2:d8b182fbe018 125 gyrZ = (bufIMU[11]<<8 | bufIMU[12]);
avbotz 2:d8b182fbe018 126
avbotz 2:d8b182fbe018 127 magX = (bufIMU[13]<<8 | bufIMU[14]);
avbotz 2:d8b182fbe018 128 magY = (bufIMU[15]<<8 | bufIMU[16]);
avbotz 2:d8b182fbe018 129 magZ = (bufIMU[17]<<8 | bufIMU[18]);
avbotz 2:d8b182fbe018 130
avbotz 2:d8b182fbe018 131 makeCorrect(&accX);
avbotz 2:d8b182fbe018 132 makeCorrect(&accY);
avbotz 2:d8b182fbe018 133 makeCorrect(&accZ);
avbotz 2:d8b182fbe018 134
avbotz 2:d8b182fbe018 135 makeCorrect(&gyrX);
avbotz 2:d8b182fbe018 136 makeCorrect(&gyrY);
avbotz 2:d8b182fbe018 137 makeCorrect(&gyrZ);
avbotz 2:d8b182fbe018 138
avbotz 2:d8b182fbe018 139 makeCorrect(&magX);
avbotz 2:d8b182fbe018 140 makeCorrect(&magY);
avbotz 2:d8b182fbe018 141 makeCorrect(&magZ);
avbotz 3:f04d3d10d518 142
avbotz 3:f04d3d10d518 143 newData = true;
avbotz 3:f04d3d10d518 144
avbotz 2:d8b182fbe018 145 //PC.printf("Data: %d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ);
avbotz 2:d8b182fbe018 146
avbotz 2:d8b182fbe018 147 //accX = accY = accZ = gyrX = gyrY = gyrZ = magX = magY = magZ = 0;
avbotz 2:d8b182fbe018 148
avbotz 2:d8b182fbe018 149 /*
avbotz 2:d8b182fbe018 150 for (int i = 0; i < 21; i++) {
avbotz 2:d8b182fbe018 151 PC.printf("%d ", bufIMU[i]);
avbotz 2:d8b182fbe018 152 }
avbotz 2:d8b182fbe018 153 PC.printf("\n\r");
avbotz 2:d8b182fbe018 154 */
avbotz 2:d8b182fbe018 155 //newIMUData = 1; // Update the flag
avbotz 2:d8b182fbe018 156 }
avbotz 2:d8b182fbe018 157
avbotz 2:d8b182fbe018 158
avbotz 2:d8b182fbe018 159 //Callback called when there is a character to be read from the IMU
avbotz 2:d8b182fbe018 160 void readIMU() {
avbotz 2:d8b182fbe018 161 imu.readable = true;
avbotz 2:d8b182fbe018 162 __disable_irq();
avbotz 2:d8b182fbe018 163 //NVIC_DisableIRQ(UART1_IRQn);
avbotz 2:d8b182fbe018 164 }
avbotz 2:d8b182fbe018 165
avbotz 2:d8b182fbe018 166 int main() {
avbotz 2:d8b182fbe018 167 PC.format(8, Serial::None, 1);
avbotz 2:d8b182fbe018 168 PC.baud(115200);
avbotz 2:d8b182fbe018 169
avbotz 2:d8b182fbe018 170 for (int i = 0; i < 80; i++) {
avbotz 2:d8b182fbe018 171 PC.putc('-');
avbotz 2:d8b182fbe018 172 }
avbotz 2:d8b182fbe018 173 PC.printf("\n\r");
avbotz 2:d8b182fbe018 174 imu.attach(&readIMU);
avbotz 2:d8b182fbe018 175 //imu.p_device->attach(&readIMU);
avbotz 2:d8b182fbe018 176 imu.putc('6');
avbotz 3:f04d3d10d518 177
avbotz 3:f04d3d10d518 178 PC.printf("Waiting for data.");
avbotz 3:f04d3d10d518 179
avbotz 3:f04d3d10d518 180 // Calibration variables
avbotz 3:f04d3d10d518 181 long long sumAccX = 0, sumAccY = 0, sumAccZ = 0;
avbotz 3:f04d3d10d518 182 long long sumGyrX = 0, sumGyrY = 0, sumGyrZ = 0;
avbotz 3:f04d3d10d518 183 long long sumMagX = 0, sumMagY = 0, sumMagZ = 0;
avbotz 3:f04d3d10d518 184 int num = 0;
avbotz 3:f04d3d10d518 185
avbotz 2:d8b182fbe018 186 while (true){
avbotz 2:d8b182fbe018 187 if (imu.readable) {
avbotz 2:d8b182fbe018 188 //PC.printf("fun");
avbotz 2:d8b182fbe018 189 imu.getData();
avbotz 3:f04d3d10d518 190 if (imu.newData) {
avbotz 3:f04d3d10d518 191 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);
avbotz 3:f04d3d10d518 192
avbotz 3:f04d3d10d518 193 sumAccX += imu.accX;
avbotz 3:f04d3d10d518 194 sumAccY += imu.accY;
avbotz 3:f04d3d10d518 195 sumAccZ += imu.accZ;
avbotz 3:f04d3d10d518 196
avbotz 3:f04d3d10d518 197 sumGyrX += imu.gyrX;
avbotz 3:f04d3d10d518 198 sumGyrY += imu.gyrY;
avbotz 3:f04d3d10d518 199 sumGyrZ += imu.gyrZ;
avbotz 3:f04d3d10d518 200
avbotz 3:f04d3d10d518 201 sumMagX += imu.magX;
avbotz 3:f04d3d10d518 202 sumMagY += imu.magY;
avbotz 3:f04d3d10d518 203 sumMagZ += imu.magZ;
avbotz 3:f04d3d10d518 204
avbotz 3:f04d3d10d518 205 num++;
avbotz 3:f04d3d10d518 206
avbotz 3:f04d3d10d518 207 PC.printf("Averages: %f, %f, %f, %f, %f, %f, %f, %f, %f\n",
avbotz 3:f04d3d10d518 208 sumAccX/(double)num, sumAccY/(double)num, sumAccZ/(double)num,
avbotz 3:f04d3d10d518 209 sumGyrX/(double)num, sumGyrY/(double)num, sumGyrZ/(double)num,
avbotz 3:f04d3d10d518 210 sumMagX/(double)num, sumMagY/(double)num, sumMagZ/(double)num
avbotz 3:f04d3d10d518 211 );
avbotz 3:f04d3d10d518 212
avbotz 3:f04d3d10d518 213 imu.newData = false;
avbotz 3:f04d3d10d518 214 }
avbotz 2:d8b182fbe018 215 }
avbotz 2:d8b182fbe018 216 }
avbotz 2:d8b182fbe018 217 }