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@3:f04d3d10d518, 2011-11-05 (annotated)

Committer:
avbotz
Date:
Sat Nov 05 01:02:59 2011 +0000
Revision:
3:f04d3d10d518
Parent:
2:d8b182fbe018
Child:
4:8f63393d49fb
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.

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 2:d8b182fbe018 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 2:d8b182fbe018 18 p_device->putc('6'); //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 3:f04d3d10d518 83 if ((*i)>>15) *i = 0-(~(*i));
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 }