MBED Example: How to use MS3DMGX2 3DM-GX2 Sensor libraries

.from: http://mbed.org/users/Blaze513/programs/3DM-GX2/lmj4el

XxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXx MS3DMGX2.cpp XxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXx

#include "MS3DMGX2.h"

MS3DMGX2::MS3DMGX2(PinName tx, PinName rx) : DataLines(tx, rx),
    CommandByte(0xCF), ResponseLength(6), PacketSize(31), Continuous(0)//,
   
    ///////////////////
    //PC(USBTX,USBRX)
    //////////////////
   
{
    DataLines.baud(115200);
    DataLines.format(8, Serial::None, 1);
    DataLines.attach(this, &MS3DMGX2::FillSerialBuffer, Serial::RxIrq);
       
        ////////////////////
    //PC.baud(9600);
    ////////////////////////
}

MS3DMGX2::~MS3DMGX2()
{
    delete this;
}

bool MS3DMGX2::Mode(unsigned char Selection)
{
    bool Result;
    switch (Selection & 0x03)
    {
        case 0x00:
            CommandByte = 0xCF;
            ResponseLength = 6;
            PacketSize = 31;
            break;
                //euler angles and angular rates
        case 0x01:
            CommandByte = 0xD2;
            ResponseLength = 9;
            PacketSize = 43;
            break;
                //gyro-stabilized acceleration, angular rate and magnetometer vector
        case 0x02:
            CommandByte = 0xC8;
            ResponseLength = 15;
            PacketSize = 67;
            break;
                //acceleration, angular rate and orientation matrix
        case 0x03:
            CommandByte = 0xCC;
            ResponseLength = 18;
            PacketSize = 79;
            break;
                //acceleration, angular rate, magnetometer vector, and orientation matrix
    }
        //record desired packet command and packet length as number of 16 bit fields
    if (Selection & 0x04)
    {
        unsigned char lbuff = PacketSize;
        PacketSize = 87;
       
        DataLines.putc(0xC4);
        DataLines.putc(0xC1);
        DataLines.putc(0x29);
        DataLines.putc(CommandByte);
            //send the desired continuous mode command
        Continuous = 1;
            //set synchronous mode to true
        while ((Buffer[BufferStart] != 0xC4) || (Buffer[BufferStart + 1] != CommandByte))
        {
            if (((BufferStart + 2) % PacketSize) != BufferEnd)
            {
                BufferStart++;
                BufferStart%=PacketSize;
            }
        }
            //find the response header
        while (((BufferStart + 8) % PacketSize) != BufferEnd);
        BufferEnd = 0;
        Result = Checksum(BufferStart, 8);
        BufferStart = 0;
        PacketSize = lbuff;////////////////NOTE: if there wasn't a packet waiting while the async cmd was made, there will be no "extra data" here
    }
    else
    {
        if (Continuous)
        {
            DataLines.putc(0xFA);
                //send stop continuous mode command
            Continuous = 0;
                //set synchronous mode to true
        }
        Result = 1;
    }
        //put the IMU into continuous mode if the correct flag is set
        //Computer.printf("stop command success %d \n", Workspace[0]);
        //Computer.printf("sync mode %d \n", SyncMode);
    /*if (Selection & 0x08)
    {
        DataLines.attach(this, &MS3DMGX2::Interrupt, Serial::RxIrq);
            //attach automatic buffer-writing function to serial interrupt
    }
    else
    {
        DataLines.attach(NULL, Serial::RxIrq);
            //attach a null to detach any previous interrupt
    }*/
        //attaches or detaches interrupt function depending on interrupt flag
    return Result;
        //return success or failure
}

bool MS3DMGX2::Readable()
{
    return BufferStart == BufferEnd;
}

/*void MS3DMGX2::AttachInterruptBuffer(float* Buffer)
{
    InterruptBuffer = Buffer;
}
    //store user's data pointer for use in interrupt modes

void MS3DMGX2::AttachInterruptFunction()
{
    DataLines.attach(this, &MS3DMGX2::Interrupt, Serial::RxIrq);
}
void MS3DMGX2::AttachInterruptFunction(void (*Function)())
{
    DataLines.attach(Function);
}
template<class Class> void AttachInterruptFunction(Class* Object, void (Class::*Function)())
{
    DataLines.attach(Object, Function, Serial::RxIrq);
}*/
    //overloads start interrupt modes, allowing user all possible options

void MS3DMGX2::RequestSyncRead()
{
    if (Continuous)
    {
        DataLines.putc(0xFA);
        Continuous = 0;
    }
    DataLines.putc(CommandByte);
}
    //lazy switches to synchronous mode and sends polled mode command byte

bool MS3DMGX2::Read(float* Data)
{
    bool Result;
    unsigned char t = 0;
   
    while ((Buffer[BufferStart] != CommandByte) && (t < PacketSize))
    {
        BufferStart++;
        BufferStart %= PacketSize;
        t++;
    }
        //find the header byte
    Result = Checksum(BufferStart, PacketSize);
        //compute checksum
    BufferStart++;
    BufferStart %= PacketSize;
        //move past header byte
    if (t < PacketSize)
    {
        for (unsigned int i = 0; i < ResponseLength; i++)
        {
            for(unsigned int j = 3; j < 4; j--)
            {
                ((unsigned char*)&Data[i])[j] = Buffer[BufferStart];
                BufferStart++;
                BufferStart %= PacketSize;
            }
        }
            //convert big endian bytes to little endian floats
        BufferStart += 6;
            //move index past timer and checksum bytes
        BufferStart %= PacketSize;
    }
        //if the header search did not timeout
   
    return Result;
}

/*MS3DMGX2::operator float*()
{
    Read((float*)&Workspace[8]);
    return (float*)&Workspace[8];
}
    //conversion function acts as shorthand for Read()

void MS3DMGX2::Interrupt()
{
    Read(InterruptBuffer);
}*/
    //this will be called when in an interrupt mode and a serial interrupt is generated

bool MS3DMGX2::Checksum(unsigned char Index, unsigned char Length)
{
    unsigned short Sum = 0;
    for (unsigned char i = 0; i < Length - 2; i++)
    {
        Sum += Buffer[Index];
        Index++;
        Index %= PacketSize;
    }
    return (((unsigned char*)&Sum)[0] == Buffer[Index+1])
        && (((unsigned char*)&Sum)[1] == Buffer[Index]);
}

void MS3DMGX2::FillSerialBuffer()//if the interrupt recurs faster than the time to complete its code, the rest of the system will be suspended indefinitely
{
    while (DataLines.readable())
    {
        Buffer[BufferEnd] = DataLines.getc();
        BufferEnd++;
      
        BufferEnd %= PacketSize;
    }
}
    //this automatically reads in new serial data as it is received to
    //make a software buffer that is big enough to handle an entire
    //packet; the processor is about 1000 times faster than the data lines

XxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXx MS3DMGX2.h XxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXx

#ifndef MS3DMGX2Library
#define MS3DMGX2Library

#include "stdint.h"
#include "mbed.h"

class MS3DMGX2
{
    private:
    /////////////////////
//Serial PC;
//////////////////////////

        Serial DataLines;
        volatile unsigned char Buffer[87];//volatile is for interrupted access
        volatile unsigned char BufferEnd;
        volatile unsigned char PacketSize;
        bool Checksum(unsigned char Index, unsigned char Length);
        void FillSerialBuffer();

    public:
        MS3DMGX2(PinName tx, PinName rx);
            //serial output, serial input
        ~MS3DMGX2();
            //release resources
        unsigned char BufferStart;
       
        unsigned char CommandByte;
        unsigned char ResponseLength;
        unsigned char Continuous; 
       
       
        bool Readable();
       
        bool Mode(unsigned char Selection);
            //argument sets operating mode;
            //set flag 0x08 to 0 for polled modes, or 1 for interrupt modes;
            //set flag 0x04 to 0 for synchronous modes, or 1 for asynchronous modes;
            //set flags 0x03 to 0 for pwm, 1 for analog, or 2 for serial;
            //asynchronous modes read user input command, measures and calculates
            //output, and writes the data packet to the serial buffer every 10 ms;
            //interrupt modes automatically read the packet into the buffer provided
            //by AttachInterruptBuffer with the selected input method every 10 ms
            //if in an asynchronous mode, or 10 ms after RequestSyncRead is called;
            //interrupt mode interrupts are generated if there are bytes on the
            //serial buffer and are only cleared if the serial buffer is emptied;
            //default is 0
        //void AttachInterruptBuffer(float* Buffer);
            //if interrupts are used, user must provide address to write result to
        //void AttachInterruptFunction();
            //this overload reattaches the native interrupt function
        //void AttachInterruptFunction(void (*Function)());
            //this overload attaches a function to the serial interrupt
        //template<class Class> void AttachInterruptFunction
        //    (Class* Object, void (Class::*Function)());
            //this overload attaches a member function to the serial interrupt;
            //to change the interrupt function, call one of the
            //"AttachInterruptFunction" overloads with pointers to the desired function;
            //changes polled mode to interrupt equivalent;
            //the interrupt will not be cleared until the serial buffer is emptied
        void RequestSyncRead();
            //this tells the device to prepare a synchronous reading;
            //must be called at least 10 ms before the reading is needed;
            //changes asynchronous mode to synchronous equivalent
        void DiscardSerialBuffer();
            //the serial port has a buffer and only the oldest data is read from it;
            //the buffer has limited space and, when full, the newest data is discarded;
            //this method allows the user to empty the buffer of old data so new data is used
        bool Read(float* Data);
            //get a reading from the device in the set mode;
            //RequestSyncRead() must be called at least 10 ms
            //before this method can be called in a synchronous mode;
            //may be called at any time during asynchronous mode;
            //it is assumed that the input buffer has enough
            //room to accomodate the desired data packet
        //operator float*();
            //shorthand for taking a reading;
            //ex: "float reading[packetlength]; reading = MB1210Object;"
};

#endif

XxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXx main.cpp XxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXx

#include "mbed.h"
#include "MS3DMGX2.h"

MS3DMGX2 IMU(p9, p10);
DigitalOut led(LED1);
DigitalOut led4(LED4);
Serial PC(USBTX,USBRX);
int main()
{
led=0;
led4=0;
PC.baud(9600);
    float data[9];
    bool isvalid;
    wait(1);
    isvalid = IMU.Mode(0x05);
    PC.printf("Data Valid: %d\r\n\r\n", isvalid);
    //IMU.RequestSyncRead();
    wait(1);
   
    while (1)
    {
        while(!IMU.Readable());
        isvalid = IMU.Read(data);
       
        PC.printf("IMU Accel x Reads %f\r\n",data[0]);//NOTE: the compiler will not even calculate unused variables
        PC.printf("IMU accel y Reads %f\r\n",data[1]);//If the data retrieved above was not used, it wouldn't even be retrieved and the validity check would not pass
        PC.printf("IMU accel z Reads %f\r\n",data[2]);//this is called "compiler optimization"
       
        PC.printf("IMU ang rate x Reads %f\r\n",data[3]);
        PC.printf("IMU ang rate y Reads %f\r\n",data[4]);
        PC.printf("IMU ang rate z Reads %f\r\n",data[5]);
       
        PC.printf("IMU mag x Reads %f\r\n",data[6]);
        PC.printf("IMU mag y Reads %f\r\n",data[7]);
        PC.printf("IMU mag z Reads %f\r\n",data[8]);
        PC.printf("Validity: %d\r\n\r\n", isvalid);
       
        wait_ms(1000);
    }
}

XxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXx EOF XxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXxXx