A class to receive data from the SparkFun 9DOF Razor IMU. It can be easily adapted to work with IMUs with different data formats.
main.cpp@2:d8b182fbe018, 2011-11-04 (annotated)
- Committer:
- avbotz
- Date:
- Fri Nov 04 22:24:02 2011 +0000
- Revision:
- 2:d8b182fbe018
- Parent:
- 1:fdfa313b9cc3
- Child:
- 4:8f63393d49fb
Appears to parse data correctly (centered around 512), but sometimes we get bad data.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
avbotz |
1:fdfa313b9cc3 | 1 | /* |
avbotz |
0:a260d84e07fc | 2 | * Demo to relay I/O between a computer and the IMU. Make sure to connect the GND wire on the IMU to pin 1 (GND) on the mbed so there's a return current |
avbotz |
0:a260d84e07fc | 3 | * Updated to use interrupts - this will help when we intergrate this code into AVNavControl |
avbotz |
2:d8b182fbe018 | 4 | * 9dof razor from sparkfun, http://www.sparkfun.com/products/10736 |
avbotz |
0:a260d84e07fc | 5 | */ |
avbotz |
2:d8b182fbe018 | 6 | /* |
avbotz |
0:a260d84e07fc | 7 | #define GYRO_SCALE 14.375 // ticks per degree, http://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf |
avbotz |
0:a260d84e07fc | 8 | |
avbotz |
0:a260d84e07fc | 9 | #include "mbed.h" |
avbotz |
0:a260d84e07fc | 10 | |
avbotz |
0:a260d84e07fc | 11 | Serial IMU(p9, p10); // tx, rx |
avbotz |
0:a260d84e07fc | 12 | Serial PC(USBTX, USBRX); |
avbotz |
0:a260d84e07fc | 13 | |
avbotz |
0:a260d84e07fc | 14 | DigitalOut myled1(LED1); |
avbotz |
0:a260d84e07fc | 15 | DigitalOut myled2(LED2); |
avbotz |
0:a260d84e07fc | 16 | DigitalOut myled3(LED3); |
avbotz |
0:a260d84e07fc | 17 | DigitalOut myled4(LED4); |
avbotz |
0:a260d84e07fc | 18 | |
avbotz |
0:a260d84e07fc | 19 | bool IMUreadable = false; |
avbotz |
0:a260d84e07fc | 20 | bool PCreadable = false; |
avbotz |
0:a260d84e07fc | 21 | |
avbotz |
0:a260d84e07fc | 22 | void readIMU(); |
avbotz |
0:a260d84e07fc | 23 | void readPC(); |
avbotz |
1:fdfa313b9cc3 | 24 | inline void makeCorrect(short* i); |
avbotz |
0:a260d84e07fc | 25 | |
avbotz |
1:fdfa313b9cc3 | 26 | short accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ; |
avbotz |
0:a260d84e07fc | 27 | |
avbotz |
1:fdfa313b9cc3 | 28 | // Expected format: {$, accX, accX, accY, accY, accZ, accZ, gyrX, gyrX, gyrY, gyrY, gyrZ, gyrZ, magX, magX, magY, magY, magZ, magZ, #, \r} |
avbotz |
1:fdfa313b9cc3 | 29 | int i_IMU; |
avbotz |
1:fdfa313b9cc3 | 30 | char bufIMU[21]; |
avbotz |
0:a260d84e07fc | 31 | |
avbotz |
2:d8b182fbe018 | 32 | AnalogOut aout(p18); |
avbotz |
2:d8b182fbe018 | 33 | |
avbotz |
0:a260d84e07fc | 34 | int main() { |
avbotz |
2:d8b182fbe018 | 35 | aout = 1.0; |
avbotz |
1:fdfa313b9cc3 | 36 | // Set up the connection. Read up about parity and stop bits if this is confusing. |
avbotz |
0:a260d84e07fc | 37 | IMU.format(8, Serial::None, 1); |
avbotz |
0:a260d84e07fc | 38 | PC.format(8, Serial::None, 1); |
avbotz |
0:a260d84e07fc | 39 | IMU.baud(57600); |
avbotz |
0:a260d84e07fc | 40 | PC.baud(115200); |
avbotz |
1:fdfa313b9cc3 | 41 | |
avbotz |
1:fdfa313b9cc3 | 42 | for (int i = 0; i < 80; i++) { |
avbotz |
1:fdfa313b9cc3 | 43 | PC.putc('-'); |
avbotz |
1:fdfa313b9cc3 | 44 | } |
avbotz |
1:fdfa313b9cc3 | 45 | PC.printf("\n\r"); |
avbotz |
1:fdfa313b9cc3 | 46 | |
avbotz |
0:a260d84e07fc | 47 | PC.attach(&readPC); |
avbotz |
0:a260d84e07fc | 48 | IMU.attach(&readIMU); |
avbotz |
1:fdfa313b9cc3 | 49 | |
avbotz |
1:fdfa313b9cc3 | 50 | IMU.putc('6'); //tell the IMU to start sending data in binary mode, if it's not sending data already |
avbotz |
1:fdfa313b9cc3 | 51 | |
avbotz |
1:fdfa313b9cc3 | 52 | //The main loop |
avbotz |
1:fdfa313b9cc3 | 53 | while (true) { |
avbotz |
0:a260d84e07fc | 54 | __enable_irq(); |
avbotz |
0:a260d84e07fc | 55 | if (IMUreadable) { |
avbotz |
0:a260d84e07fc | 56 | myled2 = 1; |
avbotz |
1:fdfa313b9cc3 | 57 | |
avbotz |
1:fdfa313b9cc3 | 58 | while (IMU.readable()) { |
avbotz |
1:fdfa313b9cc3 | 59 | // This snippet of code should be run whenever a character can be received from the IMU. |
avbotz |
1:fdfa313b9cc3 | 60 | |
avbotz |
1:fdfa313b9cc3 | 61 | char data = IMU.getc(); |
avbotz |
1:fdfa313b9cc3 | 62 | |
avbotz |
1:fdfa313b9cc3 | 63 | // Start of a new set of data. Reset the counter to the first position in the buffer, and start throwing data in there. |
avbotz |
1:fdfa313b9cc3 | 64 | if (data == '$') { |
avbotz |
1:fdfa313b9cc3 | 65 | i_IMU = 0; |
avbotz |
1:fdfa313b9cc3 | 66 | printf("new data\n\r"); |
avbotz |
1:fdfa313b9cc3 | 67 | } |
avbotz |
1:fdfa313b9cc3 | 68 | // Something went wrong. |
avbotz |
1:fdfa313b9cc3 | 69 | else if (i_IMU > 21) { |
avbotz |
1:fdfa313b9cc3 | 70 | printf("\t\t\tIMU error.\n\r"); |
avbotz |
1:fdfa313b9cc3 | 71 | i_IMU = 21; |
avbotz |
1:fdfa313b9cc3 | 72 | } |
avbotz |
1:fdfa313b9cc3 | 73 | |
avbotz |
1:fdfa313b9cc3 | 74 | // End of the set of data. Parse the buffer |
avbotz |
2:d8b182fbe018 | 75 | else if (i_IMU == 21) { && bufIMU[0] == '$' && bufIMU[20] == '\n' data == '\n' && i_IMU == 19) { |
avbotz |
1:fdfa313b9cc3 | 76 | printf("Parsing\n\r"); |
avbotz |
1:fdfa313b9cc3 | 77 | |
avbotz |
2:d8b182fbe018 | 78 | //This should be easier to understand than bitshifts. bufIMU is a pointer (arrays are pointers). |
avbotz |
2:d8b182fbe018 | 79 | //We offset it to the start of the desired value. We then typecast bufIMU into a short (16 bit). |
avbotz |
2:d8b182fbe018 | 80 | //This turns bufIMU into a pointer to the desired value. Now we dereference it. |
avbotz |
2:d8b182fbe018 | 81 | //I'm not sure if we'll still need makeCorrect. Adit, check plz <3 |
avbotz |
2:d8b182fbe018 | 82 | accX = *((short*)(bufIMU + 1)); |
avbotz |
2:d8b182fbe018 | 83 | accY = *((short*)(bufIMU + 3)); |
avbotz |
2:d8b182fbe018 | 84 | accZ = *((short*)(bufIMU + 5)); |
avbotz |
2:d8b182fbe018 | 85 | |
avbotz |
2:d8b182fbe018 | 86 | gyrX = *((short*)(bufIMU + 7)); |
avbotz |
2:d8b182fbe018 | 87 | gyrY = *((short*)(bufIMU + 9)); |
avbotz |
2:d8b182fbe018 | 88 | gyrZ = *((short*)(bufIMU + 11)); |
avbotz |
2:d8b182fbe018 | 89 | |
avbotz |
2:d8b182fbe018 | 90 | magX = *((short*)(bufIMU + 13)); |
avbotz |
2:d8b182fbe018 | 91 | magY = *((short*)(bufIMU + 15)); |
avbotz |
2:d8b182fbe018 | 92 | magZ = *((short*)(bufIMU + 17)); |
avbotz |
2:d8b182fbe018 | 93 | |
avbotz |
1:fdfa313b9cc3 | 94 | //bufIMU contains binary data. Each variable sent by the IMU is a 16-bit integer |
avbotz |
1:fdfa313b9cc3 | 95 | //broken down into two characters (in bufIMU[]). Here, we reconstitute the original integer by |
avbotz |
1:fdfa313b9cc3 | 96 | //left-shifting the first character by 8 bits and ORing it with the second character. |
avbotz |
2:d8b182fbe018 | 97 | |
avbotz |
2:d8b182fbe018 | 98 | |
avbotz |
2:d8b182fbe018 | 99 | //accX = (bufIMU[1]<<8 | bufIMU[2]); |
avbotz |
2:d8b182fbe018 | 100 | //accY = (bufIMU[3]<<8 | bufIMU[4]); |
avbotz |
2:d8b182fbe018 | 101 | //accZ = (bufIMU[5]<<8 | bufIMU[6]); |
avbotz |
1:fdfa313b9cc3 | 102 | |
avbotz |
2:d8b182fbe018 | 103 | //gyrX = (bufIMU[7]<<8 | bufIMU[8]); |
avbotz |
2:d8b182fbe018 | 104 | //gyrY = (bufIMU[9]<<8 | bufIMU[10]); |
avbotz |
2:d8b182fbe018 | 105 | //gyrZ = (bufIMU[11]<<8 | bufIMU[12]); |
avbotz |
1:fdfa313b9cc3 | 106 | |
avbotz |
2:d8b182fbe018 | 107 | //magX = (bufIMU[13]<<8 | bufIMU[14]); |
avbotz |
2:d8b182fbe018 | 108 | //magY = (bufIMU[15]<<8 | bufIMU[16]); |
avbotz |
2:d8b182fbe018 | 109 | //magZ = (bufIMU[17]<<8 | bufIMU[18]); |
avbotz |
1:fdfa313b9cc3 | 110 | |
avbotz |
1:fdfa313b9cc3 | 111 | makeCorrect(&accX); |
avbotz |
1:fdfa313b9cc3 | 112 | makeCorrect(&accY); |
avbotz |
1:fdfa313b9cc3 | 113 | makeCorrect(&accZ); |
avbotz |
1:fdfa313b9cc3 | 114 | |
avbotz |
1:fdfa313b9cc3 | 115 | makeCorrect(&gyrX); |
avbotz |
1:fdfa313b9cc3 | 116 | makeCorrect(&gyrY); |
avbotz |
1:fdfa313b9cc3 | 117 | makeCorrect(&gyrZ); |
avbotz |
1:fdfa313b9cc3 | 118 | |
avbotz |
1:fdfa313b9cc3 | 119 | makeCorrect(&magX); |
avbotz |
1:fdfa313b9cc3 | 120 | makeCorrect(&magY); |
avbotz |
1:fdfa313b9cc3 | 121 | makeCorrect(&magZ); |
avbotz |
1:fdfa313b9cc3 | 122 | |
avbotz |
1:fdfa313b9cc3 | 123 | |
avbotz |
1:fdfa313b9cc3 | 124 | PC.printf("Data: %d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ); |
avbotz |
1:fdfa313b9cc3 | 125 | |
avbotz |
1:fdfa313b9cc3 | 126 | //accX = accY = accZ = gyrX = gyrY = gyrZ = magX = magY = magZ = 0; |
avbotz |
1:fdfa313b9cc3 | 127 | |
avbotz |
2:d8b182fbe018 | 128 | |
avbotz |
2:d8b182fbe018 | 129 | //for (int i = 0; i < 21; i++) { |
avbotz |
2:d8b182fbe018 | 130 | // PC.printf("%d ", bufIMU[i]); |
avbotz |
2:d8b182fbe018 | 131 | //} |
avbotz |
2:d8b182fbe018 | 132 | //PC.printf("\n\r"); |
avbotz |
2:d8b182fbe018 | 133 | |
avbotz |
1:fdfa313b9cc3 | 134 | //newIMUData = 1; // Update the flag |
avbotz |
1:fdfa313b9cc3 | 135 | } |
avbotz |
1:fdfa313b9cc3 | 136 | |
avbotz |
1:fdfa313b9cc3 | 137 | |
avbotz |
1:fdfa313b9cc3 | 138 | bufIMU[i_IMU] = data; |
avbotz |
1:fdfa313b9cc3 | 139 | i_IMU++; |
avbotz |
1:fdfa313b9cc3 | 140 | //parseNow = (buffer.at(buffer.length() - 1) == '#'); |
avbotz |
0:a260d84e07fc | 141 | } |
avbotz |
0:a260d84e07fc | 142 | IMUreadable = false; |
avbotz |
0:a260d84e07fc | 143 | myled2 = 0; |
avbotz |
0:a260d84e07fc | 144 | } |
avbotz |
0:a260d84e07fc | 145 | if (PCreadable) { |
avbotz |
0:a260d84e07fc | 146 | myled1 = 1; |
avbotz |
0:a260d84e07fc | 147 | while (PC.readable()) IMU.putc(PC.getc()); |
avbotz |
0:a260d84e07fc | 148 | PCreadable = false; |
avbotz |
0:a260d84e07fc | 149 | myled1 = 0; |
avbotz |
0:a260d84e07fc | 150 | } |
avbotz |
2:d8b182fbe018 | 151 | //if (parseNow) { |
avbotz |
2:d8b182fbe018 | 152 | // parse(buffer); |
avbotz |
2:d8b182fbe018 | 153 | // buffer.clear(); |
avbotz |
2:d8b182fbe018 | 154 | // parseNow = false; |
avbotz |
2:d8b182fbe018 | 155 | //} |
avbotz |
0:a260d84e07fc | 156 | } |
avbotz |
0:a260d84e07fc | 157 | } |
avbotz |
0:a260d84e07fc | 158 | |
avbotz |
0:a260d84e07fc | 159 | //Interrupt called when there is a character to be read from the PC |
avbotz |
0:a260d84e07fc | 160 | //To avoid a livelock, we disable interrupts at the end of the interrupt. |
avbotz |
0:a260d84e07fc | 161 | //Then, in the main loop, we read everything from the buffer |
avbotz |
1:fdfa313b9cc3 | 162 | void readPC() { |
avbotz |
0:a260d84e07fc | 163 | PCreadable = true; |
avbotz |
0:a260d84e07fc | 164 | __disable_irq(); |
avbotz |
0:a260d84e07fc | 165 | } |
avbotz |
0:a260d84e07fc | 166 | |
avbotz |
0:a260d84e07fc | 167 | //Interrupt called when there is a character to be read from the IMU |
avbotz |
1:fdfa313b9cc3 | 168 | void readIMU() { |
avbotz |
0:a260d84e07fc | 169 | IMUreadable = true; |
avbotz |
0:a260d84e07fc | 170 | __disable_irq(); |
avbotz |
0:a260d84e07fc | 171 | } |
avbotz |
0:a260d84e07fc | 172 | |
avbotz |
0:a260d84e07fc | 173 | |
avbotz |
2:d8b182fbe018 | 174 | //So negative numbers that are transferred are in twos complement form |
avbotz |
2:d8b182fbe018 | 175 | // and the compiler seems to like to use things created by bitwise operators |
avbotz |
2:d8b182fbe018 | 176 | // in unsigned form so all of the bits are switched and we switch it back and center |
avbotz |
2:d8b182fbe018 | 177 | // it around 512 which we are using as out zero value |
avbotz |
1:fdfa313b9cc3 | 178 | inline void makeCorrect (short* i) { |
avbotz |
1:fdfa313b9cc3 | 179 | if ((*i)>>15) *i = 512 - (~(*i)); |
avbotz |
1:fdfa313b9cc3 | 180 | else *i = 512 + *i; |
avbotz |
2:d8b182fbe018 | 181 | } |
avbotz |
2:d8b182fbe018 | 182 | */ |