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@2:d8b182fbe018, 2011-11-04 (annotated)

Committer:
avbotz
Date:
Fri Nov 04 22:24:02 2011 +0000
Revision:
2:d8b182fbe018
Child:
3:f04d3d10d518
Appears to parse data correctly (centered around 512), but sometimes we get bad data.

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