Diff: main.cpp

Revision:

0:090f184c5b52

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Sep 08 08:43:02 2014 +0000
@@ -0,0 +1,80 @@
+#include "mbed.h"
+
+DigitalOut myled(LED1);
+SPI spi(p5, p6, p7); // mosi, miso, sclk
+DigitalOut cs(p8); // chip sellect
+Serial pc(USBTX, USBRX);  // tx, rx
+
+float floatingPointDecoder(uint32_t VALUE) {
+    uint32_t value_Temp = 0;
+    float val1 = 0.0f;
+    float val2 = 0.0f;
+    float val3 = 0.0f;
+    ( VALUE & (1 << 31) ) ? val1 = -1.0f : val1 = 1.0f;
+    value_Temp = VALUE << 1;
+    value_Temp = value_Temp >> 24;
+    val2 = (float) value_Temp - 127.0f;
+    value_Temp = VALUE << 8;
+    value_Temp = value_Temp >> 8;
+    for(int i=24-1, j=0; i>=0; i--, j++) {
+        ( value_Temp & (1 << i) ) ? val3 += 1.0f / ( pow(2.0,j) ) : val3 += 0.0f;
+    }
+    return val1 * pow(2.0f,val2) * val3;
+}
+
+int main() {
+    int i = 0;
+    uint8_t value[13] = {0};
+    uint32_t pyr[3] = {0};
+    float yaw = 0.0f, pitch = 0.0f, roll = 0.0f;
+
+    pc.baud(9600);while(1){
+
+    // Chip must be deselected
+    cs = 1;
+
+    // Setup the spi for 8 bit data, high steady state clock,
+    // second edge capture, with a 1MHz clock rate
+    spi.format(8,0);
+    spi.frequency(1000000);
+    // Select the device by seting chip select low
+    cs = 0;
+
+    // Send 0xF6, to start communication on SPI
+    value[0] = spi.write(0xF6);
+    //printf("F6 is sent and 00 is waiting. The return result is:  0x%X\n", value[0]);
+
+    // Send 0x07, to request untared euler angles
+    value[0] = spi.write(0x07);
+    //printf("07 is sent and 04 is waiting. The return result is:  0x%X\n", value[0]);
+
+    for(i=0; i<=12; i++) {
+        value[i] = spi.write(0xFF);
+        if(value[i] == 1) {
+            i = 0;
+        }
+        wait(0.01);
+    }
+
+    pyr[0] = value[1];
+    pyr[0] = (pyr[0] << 8) + value[2];
+    pyr[0] = (pyr[0] << 8) + value[3];
+    pyr[0] = (pyr[0] << 8) + value[4];
+    pitch = floatingPointDecoder(pyr[0]) * 180.0f / 3.1415926535897932384626433832795;
+    pyr[1] = value[5];
+    pyr[1] = (pyr[1] << 8) + value[6];
+    pyr[1] = (pyr[1] << 8) + value[7];
+    pyr[1] = (pyr[1] << 8) + value[8];
+    yaw = floatingPointDecoder(pyr[1]) * 180.0f / 3.1415926535897932384626433832795;
+    pyr[2] = value[9];
+    pyr[2] = (pyr[2] << 8) + value[10];
+    pyr[2] = (pyr[2] << 8) + value[11];
+    pyr[2] = (pyr[2] << 8) + value[12];
+    roll = floatingPointDecoder(pyr[2]) * 180.0f / 3.1415926535897932384626433832795;
+
+    printf("Pitch: %f     Yaw: %f     Roll: %f\n", pitch, yaw, roll);
+
+    // Deselect the device
+    cs = 1;
+    wait(1);}
+}
\ No newline at end of file