Revision 2:d8b182fbe018, committed 2011-11-04

Comitter:

avbotz

Date:

Fri Nov 04 22:24:02 2011 +0000

Parent:

1:fdfa313b9cc3

Child:

3:f04d3d10d518

Commit message:

Appears to parse data correctly (centered around 512), but sometimes we get bad data.

Changed in this revision

--- /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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/imu.h	Fri Nov 04 22:24:02 2011 +0000
@@ -0,0 +1,34 @@
+#pragma once
+
+#include "mbed.h"
+
+Serial PC(USBTX, USBRX);
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+
+class IMU{
+    public:
+        IMU(int baud, PinName tx, PinName rx, Serial* pc);
+        ~IMU();
+        
+        void getData();
+        void attach(void (*fptr)(void));
+        void putc(char c);
+        //void readIMU();
+        short accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ;
+        bool readable;
+        
+    private:
+        Serial* p_device;
+        Serial* p_pc;
+        inline void makeCorrect(short* i);
+        void parse();
+        
+        int i_IMU;
+        char bufIMU[21];
+} imu(57600, p9, p10, &PC);
+
+void readIMU();

--- a/main.cpp	Mon Oct 17 15:19:01 2011 +0000
+++ b/main.cpp	Fri Nov 04 22:24:02 2011 +0000
@@ -1,9 +1,9 @@
 /*
  * 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
  * Updated to use interrupts - this will help when we intergrate this code into AVNavControl
- * 9dof razor from sparkfun
+ * 9dof razor from sparkfun, http://www.sparkfun.com/products/10736
  */
-
+/*
 #define GYRO_SCALE 14.375  // ticks per degree, http://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf 
 
 #include "mbed.h"
@@ -29,7 +29,10 @@
 int i_IMU;
 char bufIMU[21];
 
+AnalogOut aout(p18);
+
 int main() {
+    aout = 1.0;
     // Set up the connection. Read up about parity and stop bits if this is confusing.
     IMU.format(8, Serial::None, 1);
     PC.format(8, Serial::None, 1);
@@ -69,23 +72,41 @@
                 }
 
                 // End of the set of data. Parse the buffer
-                else if (i_IMU == 21 /*&& bufIMU[0] == '$' && bufIMU[20] == '\n' data == '\n' && i_IMU == 19*/) {
+                else if (i_IMU == 21) { && bufIMU[0] == '$' && bufIMU[20] == '\n' data == '\n' && i_IMU == 19) {
                     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. Adit, check plz <3
+                    accX = *((short*)(bufIMU + 1));
+                    accY = *((short*)(bufIMU + 3));
+                    accZ = *((short*)(bufIMU + 5));
+                    
+                    gyrX = *((short*)(bufIMU + 7));
+                    gyrY = *((short*)(bufIMU + 9));
+                    gyrZ = *((short*)(bufIMU + 11));
+                    
+                    magX = *((short*)(bufIMU + 13));
+                    magY = *((short*)(bufIMU + 15));
+                    magZ = *((short*)(bufIMU + 17));
+                    
                     //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]);
+                    
+                    
+                    //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]);
+                    //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]);
+                    //magX = (bufIMU[13]<<8 | bufIMU[14]);
+                    //magY = (bufIMU[15]<<8 | bufIMU[16]);
+                    //magZ = (bufIMU[17]<<8 | bufIMU[18]);
                     
                     makeCorrect(&accX);
                     makeCorrect(&accY);
@@ -104,12 +125,12 @@
 
                     //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");
-                    */
+                    
+                    //for (int i = 0; i < 21; i++) {
+                    //    PC.printf("%d  ", bufIMU[i]);
+                    //}
+                    //PC.printf("\n\r");
+                    
                     //newIMUData = 1; // Update the flag
                 }
 
@@ -127,11 +148,11 @@
             PCreadable = false;
             myled1 = 0;
         }
-        /*if (parseNow) {
-            parse(buffer);
-            buffer.clear();
-            parseNow = false;
-        }*/
+        //if (parseNow) {
+        //    parse(buffer);
+        //    buffer.clear();
+        //    parseNow = false;
+        //}
     }
 }
 
@@ -150,10 +171,12 @@
 }
 
 
-//The bitshift performed always creates a positive integer. Sometimes, the IMU
-//values are negative. This fixes that - it centers the value around 512. That is,
-//512 is 0 for the IMU variables. Values above it are positive, and values below it are negative.
+//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 makeCorrect (short* i) {
     if ((*i)>>15) *i = 512 - (~(*i));
     else *i = 512 + *i;
-}
\ No newline at end of file
+}
+*/
\ No newline at end of file