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@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?
User | Revision | Line number | New 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 | } |