Diff: imu.cpp

Revision:

2:d8b182fbe018

Child:

3:f04d3d10d518

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/imu.cpp	Fri Nov 04 22:24:02 2011 +0000
@@ -0,0 +1,174 @@
+#include "mbed.h"
+#include "imu.h"
+
+IMU::IMU(int baud, PinName tx, PinName rx, Serial* pc){
+    p_pc = pc;
+    p_pc->printf("Initializing IMU...\n");
+    p_device = new Serial(tx, rx);
+    p_device->baud(baud);
+    p_device->format(8, Serial::None, 1);
+    readable = false;
+    i_IMU = 0;
+    accX = accY = accZ = gyrX = gyrY = gyrZ = magX = magY = magZ = 0;
+    p_device->putc('6'); //Tell the IMU to start sending binary data, if it isn't already
+}
+
+IMU::~IMU() {
+    if (p_device != NULL){
+        delete p_device;    //This prevents memory leaks and keeps the port accessible for future use.
+    }
+}
+
+//Wrapper function to write a character to the IMU
+void IMU::putc(char c) {
+    p_device->putc(c);
+}
+
+
+//Wrapper function to attach a callback function to the RX interrupt of the IMU Serial object
+void IMU::attach(void (*fptr)(void)) {
+    p_pc->putc('a');
+    p_device->attach(fptr);
+}
+
+//Gets all data from the IMU read buffer. If we should parse, parse.
+void IMU::getData() {
+    readable = false;
+    //p_pc->printf("HELLO");
+    __enable_irq(); 
+    //NVIC_EnableIRQ(UART1_IRQn);
+    while (p_device->readable()) {
+        //p_pc->printf("readable");
+        // This snippet of code should be run whenever a character can be received from the IMU.
+
+        char data = p_device->getc();
+
+        // Start of a new set of data. Reset the counter to the first position in the buffer, and start throwing data in there.
+        if (data == '$') {
+            i_IMU = 0;
+            //p_pc->printf("new data\n\r");
+        }
+        // Something went wrong.
+        else if (i_IMU > 21) {
+            //p_pc->printf("\t\t\tIMU error.\n\r");
+            i_IMU = 21;
+        }
+
+        // End of the set of data. Parse the buffer
+        else if (i_IMU == 21 /*&& bufIMU[0] == '$' && bufIMU[20] == '\n' data == '\n' && i_IMU == 19*/) {
+            parse();
+        }
+
+
+        bufIMU[i_IMU] = data;
+        i_IMU++;
+        //parseNow = (buffer.at(buffer.length() - 1) == '#');
+    }
+}
+
+//So negative numbers that are transferred are in twos complement form
+// and the compiler seems to like to use things created by bitwise operators
+// in unsigned form so all of the bits are switched and we switch it back and center
+// it around 512 which we are using as out zero value
+inline void IMU::makeCorrect(short* i) {
+    if ((*i)>>15) *i = 512-(~(*i));
+    else *i = 512+*i;
+}
+
+/*
+void IMU::attach(void) {
+    p_device->attach(this->readIMU);
+}*/
+
+void IMU::parse() {
+    p_pc->printf("Parsing\n\r");
+
+    //This should be easier to understand than bitshifts. bufIMU is a pointer (arrays are pointers).
+    //We offset it to the start of the desired value. We then typecast bufIMU into a short (16 bit).
+    //This turns bufIMU into a pointer to the desired value. Now we dereference it.
+    //I'm not sure if we'll still need makeCorrect.
+    
+    //Don't use this. It doesn't work. 
+    //Kevin said this didn't work because of endianness. The Cortex M3 has a macro to reverse.
+    //See http://mbed.org/forum/helloworld/topic/1573/?page=1#comment-7818
+    /*accX = *((short*)(bufIMU + 2));
+    accY = *((short*)(bufIMU + 4));
+    accZ = *((short*)(bufIMU + 6));
+
+    gyrX = *((short*)(bufIMU + 8));
+    gyrY = *((short*)(bufIMU + 10));
+    gyrZ = *((short*)(bufIMU + 12));
+
+    magX = *((short*)(bufIMU + 14));
+    magY = *((short*)(bufIMU + 16));
+    magZ = *((short*)(bufIMU + 18));*/
+
+    //bufIMU contains binary data. Each variable sent by the IMU is a 16-bit integer
+    //broken down into two characters (in bufIMU[]). Here, we reconstitute the original integer by
+    //left-shifting the first character by 8 bits and ORing it with the second character.
+    
+    accX = (bufIMU[1]<<8 | bufIMU[2]);
+    accY = (bufIMU[3]<<8 | bufIMU[4]);
+    accZ = (bufIMU[5]<<8 | bufIMU[6]);
+
+    gyrX = (bufIMU[7]<<8 | bufIMU[8]);
+    gyrY = (bufIMU[9]<<8 | bufIMU[10]);
+    gyrZ = (bufIMU[11]<<8 | bufIMU[12]);
+
+    magX = (bufIMU[13]<<8 | bufIMU[14]);
+    magY = (bufIMU[15]<<8 | bufIMU[16]);
+    magZ = (bufIMU[17]<<8 | bufIMU[18]);
+    
+    makeCorrect(&accX);
+    makeCorrect(&accY);
+    makeCorrect(&accZ);
+
+    makeCorrect(&gyrX);
+    makeCorrect(&gyrY);
+    makeCorrect(&gyrZ);
+
+    makeCorrect(&magX);
+    makeCorrect(&magY);
+    makeCorrect(&magZ);
+
+
+    //PC.printf("Data: %d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ);
+
+    //accX = accY = accZ = gyrX = gyrY = gyrZ = magX = magY = magZ = 0;
+
+    /*
+    for (int i = 0; i < 21; i++) {
+        PC.printf("%d  ", bufIMU[i]);
+    }
+    PC.printf("\n\r"); 
+    */
+    //newIMUData = 1; // Update the flag
+}
+
+
+//Callback called when there is a character to be read from the IMU
+void readIMU() {
+    imu.readable = true;
+    __disable_irq();
+    //NVIC_DisableIRQ(UART1_IRQn);
+}
+
+int main() {
+    PC.format(8, Serial::None, 1);
+    PC.baud(115200);
+
+    for (int i = 0; i < 80; i++) {
+        PC.putc('-');
+    }
+    PC.printf("\n\r");
+    imu.attach(&readIMU);
+    //imu.p_device->attach(&readIMU);
+    imu.putc('6');
+    while (true){
+        if (imu.readable) {
+            //PC.printf("fun");
+            imu.getData();
+            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);
+        }
+    }
+}
\ No newline at end of file