Diff: 3DM-GX2.cpp

Revision:

0:fd1fce2347d9

diff -r 000000000000 -r fd1fce2347d9 3DM-GX2.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/3DM-GX2.cpp	Tue Aug 31 04:38:14 2010 +0000
@@ -0,0 +1,169 @@
+#include "3DM-GX2.h"
+
+MS3DMGX2::MS3DMGX2(PinName tx, PinName rx) : DataLines(tx, rx),
+    ModeCommand(0xCF), ModeLength(6), ModeSync(0), InterruptBuffer((float*)Workspace)
+{
+    DataLines.baud(115200);
+    DataLines.format(8, Serial::None, 1);
+    DataLines.attach(NULL, Serial::RxIrq);
+}
+
+unsigned char MS3DMGX2::Mode(unsigned char Selection)
+{
+    switch (Selection & 0x03)
+    {
+        case 0x00:
+            ModeCommand = 0xCF;
+            ModeLength = 6;
+            break;
+        case 0x01:
+            ModeCommand = 0xD2;
+            ModeLength = 9;
+            break;
+        case 0x02:
+            ModeCommand = 0xC8;
+            ModeLength = 15;
+            break;
+        case 0x03:
+            ModeCommand = 0xCC;
+            ModeLength = 18;
+            break;
+    }
+    if (Selection & 0x04)
+    {
+        DiscardSerialBuffer();
+        DataLines.putc(0xC4);
+        DataLines.putc(0xC1);
+        DataLines.putc(0x29);
+        DataLines.putc(ModeCommand);
+        ModeSync = 1;
+        wait_ms(10);
+        Workspace[0] = DataLines.getc();
+        Workspace[1] = DataLines.getc();
+        while (DataLines.readable() && ((Workspace[0] != 0xC4) || (Workspace[1] != ModeCommand)))
+        {
+            Workspace[0] = Workspace[1];
+            Workspace[1] = DataLines.getc();
+        }
+        unsigned char i = 7;
+        while (DataLines.readable() && (i > 1))
+        {
+            Workspace[i] = DataLines.getc();
+            i--;
+        }
+        CheckSum(0xC4, 1, &ModeCommand, &Workspace[4], Workspace);
+        Workspace[0] = (Workspace[0] != Workspace[2]) || (Workspace[1] != Workspace[3]);
+    }
+    else
+    {
+        DataLines.putc(0xFA);
+        ModeSync = 0;
+    }
+    if (Selection & 0x08)
+    {
+        DataLines.attach(this, &MS3DMGX2::Interrupt, Serial::RxIrq);
+    }
+    else
+    {
+        DataLines.attach(NULL, Serial::RxIrq);
+    }
+    if (Workspace[0])
+    {
+        return 0x01;
+    }
+    else
+    {
+        return 0x00;
+    }
+}
+
+void MS3DMGX2::AttachInterruptBuffer(float* Buffer)
+{
+    InterruptBuffer = Buffer;
+}
+    //store user's data pointer for use in interrupt modes
+
+void MS3DMGX2::RequestSyncRead()
+{
+    if (ModeSync)
+    {
+        DataLines.putc(0xFA);
+        ModeSync = 0x00;
+    }
+        //if operating in an asynchronous mode, switch to the synchronous equivalent
+    DataLines.putc(ModeCommand);
+        //send polled mode command byte
+}
+    //lazy switches to synchronous mode and sends polled mode command byte
+
+void MS3DMGX2::DiscardSerialBuffer()
+{
+    while (DataLines.readable())
+    {
+        DataLines.getc();
+    }
+}
+    //empty buffer of all data
+
+unsigned char MS3DMGX2::Read(float* Data)
+{
+    unsigned char j = 0;
+    do
+    {
+        j++;
+    } while (DataLines.readable() && (DataLines.getc() != ModeCommand) && (j < (ModeLength << 2)));
+        //find the header byte for synchronization
+    for (unsigned char i = 0; i < ModeLength; i++)
+    {
+        j = 3;
+        while (DataLines.readable() && (j < 4))
+        {
+            ((unsigned char*)&Data[i])[j] = DataLines.getc();
+            j--;
+        }
+    }
+        //change endianness and fill user buffer
+    j = 7;
+    while (DataLines.readable() && (j > 1))
+    {
+        Workspace[j] = DataLines.getc();
+        j--;
+    }
+        //change endianness and gather timer and checksum (bytes 2 and 3)
+    CheckSum(ModeCommand, ModeLength << 2, (unsigned char*)Data, &Workspace[4], Workspace);
+        //compute checksum and store in bytes 0 and 1
+    if ((Workspace[0] != Workspace[2]) || (Workspace[1] != Workspace[3]))
+    {
+        return 0x01;
+    }
+    else
+    {
+        return 0x00;
+    }
+        //verify checksum and return result
+}
+    //this method reads a measurement from the serial port, assuming one is waiting there
+
+void MS3DMGX2::Interrupt()
+{
+    Read(InterruptBuffer);
+    DiscardSerialBuffer();
+}
+    //this will be called when in an interrupt mode and a serial interrupt is generated
+
+void MS3DMGX2::CheckSum(unsigned char Header, unsigned char Length,
+    unsigned char* Data, unsigned char* Timer, unsigned char* Result)
+{
+    unsigned short Sum = Header;
+    for (unsigned char i = 0; i < Length; i++)
+    {
+        Sum += Data[i];
+    }
+    for (unsigned char i = 0; i < 4; i++)
+    {
+        Sum += Timer[i];
+    }
+    Result[0] = ((char*)&Sum)[0];
+    Result[1] = ((char*)&Sum)[1];
+}
+    //compute the checksum of a data packet and store the result in the first two bytes of "Result"
\ No newline at end of file