File content as of revision 0:fd1fce2347d9:

#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"