Diff: Arduimu.cpp

Revision:

0:d0d40a6fe11c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Arduimu.cpp	Mon Mar 24 20:26:15 2014 +0000
@@ -0,0 +1,85 @@
+#include "Arduimu.h"
+#include <cstring>
+
+#define preamble  "DIYd"
+
+typedef union {
+    int16_t integerVal;
+    char bytes[2];
+} value_t;
+
+
+Arduimu::Arduimu(PinName tx, PinName rx):imu(tx,rx)
+{
+    imu.baud(38400);
+    imu.attach(this,&Arduimu::receive);
+}
+
+float Arduimu::getRoll()
+{
+    return roll;
+}
+float Arduimu::getPitch()
+{
+    return pitch;
+}
+float Arduimu::getYaw()
+{
+    return yaw;
+}
+void Arduimu::getOrientation(float& roll, float& pitch, float& yaw)
+{
+    roll = this->roll;
+    pitch = this->pitch;
+    yaw = this->yaw;
+}
+
+void Arduimu::receive()
+{
+    char chk_a = 0;
+    char chk_b = 0;
+    value_t temp;
+    temp.integerVal = 0;
+    //The message is 14 bytes with a preamble of 'DIYd' + 0x06 + 0x02
+    char msgBuf[14];
+    //Wait for next message
+    while(imu.getc() != 'D');
+    __disable_irq();    // Disable Interrupts so we dont loose data
+    msgBuf[0] = 'D';
+    msgBuf[1] = imu.getc(); //I
+    msgBuf[2] = imu.getc(); //Y
+    msgBuf[3] = imu.getc(); //d
+    msgBuf[4] = imu.getc(); //0x06
+    msgBuf[5] = imu.getc(); //0x02
+
+    if(strcmp(msgBuf,preamble) && msgBuf[4] == 0x06 && msgBuf[5] == 0x02) {
+        //we have a valid message
+        //Fill in the rest of the values
+        for(int i = 6; i < 14; i++) {
+            msgBuf[i] = imu.getc();
+        }
+        __enable_irq();     // Enable Interrupts
+        for(int i = 6; i < 12; i++) {
+            chk_a+=msgBuf[i];
+            chk_b+=chk_a;
+        }
+
+        temp.bytes[0] = msgBuf[6];
+        temp.bytes[1] = msgBuf[7];
+        roll = (float) temp.integerVal / 100.0;
+
+        temp.bytes[0] = msgBuf[8];
+        temp.bytes[1] = msgBuf[9];
+        pitch = (float) temp.integerVal / 100.0;
+
+        temp.bytes[0] = msgBuf[10];
+        temp.bytes[1] = msgBuf[11];
+        yaw = (float) temp.integerVal / 100.0;
+    } else
+        __enable_irq();     // Enable Interrupts
+}
+
+void Arduimu::putc(char c)
+{
+    imu.putc(c);
+}