MS3DMGX2.cpp

00001 #include "MS3DMGX2.h"
00002 
00003 MS3DMGX2::MS3DMGX2(PinName tx, PinName rx) : DataLines(tx, rx),
00004     CommandByte(0xCF), ResponseLength(6), PacketSize(31), Continuous(0)//,
00005     
00006     ///////////////////
00007     //PC(USBTX,USBRX)
00008     //////////////////
00009     
00010 {
00011     DataLines.baud(115200);
00012     DataLines.format(8, Serial::None, 1);
00013     DataLines.attach(this, &MS3DMGX2::FillSerialBuffer, Serial::RxIrq);
00014         
00015         ////////////////////
00016     //PC.baud(9600);
00017     ////////////////////////
00018 }
00019 
00020 MS3DMGX2::~MS3DMGX2()
00021 {
00022     delete this;
00023 }
00024 
00025 bool MS3DMGX2::Mode(unsigned char Selection)
00026 {
00027     bool Result;
00028     switch (Selection & 0x03)
00029     {
00030         case 0x00:
00031             CommandByte = 0xCF;
00032             ResponseLength = 6;
00033             PacketSize = 31;
00034             break;
00035                 //euler angles and angular rates
00036         case 0x01:
00037             CommandByte = 0xD2;
00038             ResponseLength = 9;
00039             PacketSize = 43;
00040             break;
00041                 //gyro-stabilized acceleration, angular rate and magnetometer vector
00042         case 0x02:
00043             CommandByte = 0xC8;
00044             ResponseLength = 15;
00045             PacketSize = 67;
00046             break;
00047                 //acceleration, angular rate and orientation matrix
00048         case 0x03:
00049             CommandByte = 0xCC;
00050             ResponseLength = 18;
00051             PacketSize = 79;
00052             break;
00053                 //acceleration, angular rate, magnetometer vector, and orientation matrix
00054     }
00055         //record desired packet command and packet length as number of 16 bit fields
00056     if (Selection & 0x04)
00057     {
00058         unsigned char lbuff = PacketSize;
00059         PacketSize = 87;
00060         
00061         DataLines.putc(0xC4);
00062         DataLines.putc(0xC1);
00063         DataLines.putc(0x29);
00064         DataLines.putc(CommandByte);
00065             //send the desired continuous mode command
00066         Continuous = 1;
00067             //set synchronous mode to true
00068         while ((Buffer[BufferStart] != 0xC4) || (Buffer[BufferStart + 1] != CommandByte))
00069         {
00070             if (((BufferStart + 2) % PacketSize) != BufferEnd)
00071             {
00072                 BufferStart++;
00073                 BufferStart%=PacketSize;
00074             }
00075         }
00076             //find the response header
00077         while (((BufferStart + 8) % PacketSize) != BufferEnd);
00078         BufferEnd = 0;
00079         Result = Checksum(BufferStart, 8);
00080         BufferStart = 0;
00081         PacketSize = lbuff;////////////////NOTE: if there wasn't a packet waiting while the async cmd was made, there will be no "extra data" here
00082     }
00083     else
00084     {
00085         if (Continuous)
00086         {
00087             DataLines.putc(0xFA);
00088                 //send stop continuous mode command
00089             Continuous = 0;
00090                 //set synchronous mode to true
00091         }
00092         Result = 1;
00093     }
00094         //put the IMU into continuous mode if the correct flag is set
00095         //Computer.printf("stop command success %d \n", Workspace[0]);
00096         //Computer.printf("sync mode %d \n", SyncMode);
00097     /*if (Selection & 0x08)
00098     {
00099         DataLines.attach(this, &MS3DMGX2::Interrupt, Serial::RxIrq);
00100             //attach automatic buffer-writing function to serial interrupt
00101     }
00102     else
00103     {
00104         DataLines.attach(NULL, Serial::RxIrq);
00105             //attach a null to detach any previous interrupt
00106     }*/
00107         //attaches or detaches interrupt function depending on interrupt flag
00108     return Result;
00109         //return success or failure
00110 }
00111 
00112 bool MS3DMGX2::Readable()
00113 {
00114     return BufferStart == BufferEnd;
00115 }
00116 
00117 /*void MS3DMGX2::AttachInterruptBuffer(float* Buffer)
00118 {
00119     InterruptBuffer = Buffer;
00120 }
00121     //store user's data pointer for use in interrupt modes
00122 
00123 void MS3DMGX2::AttachInterruptFunction()
00124 {
00125     DataLines.attach(this, &MS3DMGX2::Interrupt, Serial::RxIrq);
00126 }
00127 void MS3DMGX2::AttachInterruptFunction(void (*Function)())
00128 {
00129     DataLines.attach(Function);
00130 }
00131 template<class Class> void AttachInterruptFunction(Class* Object, void (Class::*Function)())
00132 {
00133     DataLines.attach(Object, Function, Serial::RxIrq);
00134 }*/
00135     //overloads start interrupt modes, allowing user all possible options
00136 
00137 void MS3DMGX2::RequestSyncRead()
00138 {
00139     if (Continuous)
00140     {
00141         DataLines.putc(0xFA);
00142         Continuous = 0;
00143     }
00144     DataLines.putc(CommandByte);
00145 }
00146     //lazy switches to synchronous mode and sends polled mode command byte
00147 
00148 bool MS3DMGX2::Read(float* Data)
00149 {
00150     bool Result;
00151     unsigned char t = 0;
00152     
00153     while ((Buffer[BufferStart] != CommandByte) && (t < PacketSize))
00154     {
00155         BufferStart++;
00156         BufferStart %= PacketSize;
00157         t++;
00158     }
00159         //find the header byte
00160     Result = Checksum(BufferStart, PacketSize);
00161         //compute checksum
00162     BufferStart++;
00163     BufferStart %= PacketSize;
00164         //move past header byte
00165     if (t < PacketSize)
00166     {
00167         for (unsigned int i = 0; i < ResponseLength; i++)
00168         {
00169             for(unsigned int j = 3; j < 4; j--)
00170             {
00171                 ((unsigned char*)&Data[i])[j] = Buffer[BufferStart];
00172                 BufferStart++;
00173                 BufferStart %= PacketSize;
00174             }
00175         }
00176             //convert big endian bytes to little endian floats
00177         BufferStart += 6;
00178             //move index past timer and checksum bytes
00179         BufferStart %= PacketSize;
00180     }
00181         //if the header search did not timeout
00182     
00183     return Result;
00184 }
00185 
00186 /*MS3DMGX2::operator float*()
00187 {
00188     Read((float*)&Workspace[8]);
00189     return (float*)&Workspace[8];
00190 }
00191     //conversion function acts as shorthand for Read()
00192 
00193 void MS3DMGX2::Interrupt()
00194 {
00195     Read(InterruptBuffer);
00196 }*/
00197     //this will be called when in an interrupt mode and a serial interrupt is generated
00198 
00199 bool MS3DMGX2::Checksum(unsigned char Index, unsigned char Length)
00200 {
00201     unsigned short Sum = 0;
00202     for (unsigned char i = 0; i < Length - 2; i++)
00203     {
00204         Sum += Buffer[Index];
00205         Index++;
00206         Index %= PacketSize;
00207     }
00208     return (((unsigned char*)&Sum)[0] == Buffer[Index+1])
00209         && (((unsigned char*)&Sum)[1] == Buffer[Index]);
00210 }
00211 
00212 void MS3DMGX2::FillSerialBuffer()//if the interrupt recurs faster than the time to complete its code, the rest of the system will be suspended indefinitely
00213 {
00214     while (DataLines.readable())
00215     {
00216         Buffer[BufferEnd] = DataLines.getc();
00217         BufferEnd++;
00218        
00219         BufferEnd %= PacketSize;
00220     }
00221 }
00222     //this automatically reads in new serial data as it is received to
00223     //make a software buffer that is big enough to handle an entire
00224     //packet; the processor is about 1000 times faster than the data lines