Thomas Hamilton / Mbed 2 deprecated 3DM-GX2

interface class for an inertial measurement unit that uses a serial protocol.

Dependencies:   mbed

MS3DMGX2.cpp@1:555c2c2bf9d3, 2011-02-13 (annotated)

Committer:
Blaze513
Date:
Sun Feb 13 10:49:06 2011 +0000
Revision:
1:555c2c2bf9d3
interrupt still under construction

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Blaze513 1:555c2c2bf9d3 1 #include "MS3DMGX2.h"
Blaze513 1:555c2c2bf9d3 2
Blaze513 1:555c2c2bf9d3 3 MS3DMGX2::MS3DMGX2(PinName tx, PinName rx) : DataLines(tx, rx),
Blaze513 1:555c2c2bf9d3 4 CommandByte(0xCF), ResponseLength(6), PacketSize(31), Continuous(0)//,
Blaze513 1:555c2c2bf9d3 5
Blaze513 1:555c2c2bf9d3 6 ///////////////////
Blaze513 1:555c2c2bf9d3 7 //PC(USBTX,USBRX)
Blaze513 1:555c2c2bf9d3 8 //////////////////
Blaze513 1:555c2c2bf9d3 9
Blaze513 1:555c2c2bf9d3 10 {
Blaze513 1:555c2c2bf9d3 11 DataLines.baud(115200);
Blaze513 1:555c2c2bf9d3 12 DataLines.format(8, Serial::None, 1);
Blaze513 1:555c2c2bf9d3 13 DataLines.attach(this, &MS3DMGX2::FillSerialBuffer, Serial::RxIrq);
Blaze513 1:555c2c2bf9d3 14
Blaze513 1:555c2c2bf9d3 15 ////////////////////
Blaze513 1:555c2c2bf9d3 16 //PC.baud(9600);
Blaze513 1:555c2c2bf9d3 17 ////////////////////////
Blaze513 1:555c2c2bf9d3 18 }
Blaze513 1:555c2c2bf9d3 19
Blaze513 1:555c2c2bf9d3 20 MS3DMGX2::~MS3DMGX2()
Blaze513 1:555c2c2bf9d3 21 {
Blaze513 1:555c2c2bf9d3 22 delete this;
Blaze513 1:555c2c2bf9d3 23 }
Blaze513 1:555c2c2bf9d3 24
Blaze513 1:555c2c2bf9d3 25 bool MS3DMGX2::Mode(unsigned char Selection)
Blaze513 1:555c2c2bf9d3 26 {
Blaze513 1:555c2c2bf9d3 27 bool Result;
Blaze513 1:555c2c2bf9d3 28 switch (Selection & 0x03)
Blaze513 1:555c2c2bf9d3 29 {
Blaze513 1:555c2c2bf9d3 30 case 0x00:
Blaze513 1:555c2c2bf9d3 31 CommandByte = 0xCF;
Blaze513 1:555c2c2bf9d3 32 ResponseLength = 6;
Blaze513 1:555c2c2bf9d3 33 PacketSize = 31;
Blaze513 1:555c2c2bf9d3 34 break;
Blaze513 1:555c2c2bf9d3 35 //euler angles and angular rates
Blaze513 1:555c2c2bf9d3 36 case 0x01:
Blaze513 1:555c2c2bf9d3 37 CommandByte = 0xD2;
Blaze513 1:555c2c2bf9d3 38 ResponseLength = 9;
Blaze513 1:555c2c2bf9d3 39 PacketSize = 43;
Blaze513 1:555c2c2bf9d3 40 break;
Blaze513 1:555c2c2bf9d3 41 //gyro-stabilized acceleration, angular rate and magnetometer vector
Blaze513 1:555c2c2bf9d3 42 case 0x02:
Blaze513 1:555c2c2bf9d3 43 CommandByte = 0xC8;
Blaze513 1:555c2c2bf9d3 44 ResponseLength = 15;
Blaze513 1:555c2c2bf9d3 45 PacketSize = 67;
Blaze513 1:555c2c2bf9d3 46 break;
Blaze513 1:555c2c2bf9d3 47 //acceleration, angular rate and orientation matrix
Blaze513 1:555c2c2bf9d3 48 case 0x03:
Blaze513 1:555c2c2bf9d3 49 CommandByte = 0xCC;
Blaze513 1:555c2c2bf9d3 50 ResponseLength = 18;
Blaze513 1:555c2c2bf9d3 51 PacketSize = 79;
Blaze513 1:555c2c2bf9d3 52 break;
Blaze513 1:555c2c2bf9d3 53 //acceleration, angular rate, magnetometer vector, and orientation matrix
Blaze513 1:555c2c2bf9d3 54 }
Blaze513 1:555c2c2bf9d3 55 //record desired packet command and packet length as number of 16 bit fields
Blaze513 1:555c2c2bf9d3 56 if (Selection & 0x04)
Blaze513 1:555c2c2bf9d3 57 {
Blaze513 1:555c2c2bf9d3 58 unsigned char lbuff = PacketSize;
Blaze513 1:555c2c2bf9d3 59 PacketSize = 87;
Blaze513 1:555c2c2bf9d3 60
Blaze513 1:555c2c2bf9d3 61 DataLines.putc(0xC4);
Blaze513 1:555c2c2bf9d3 62 DataLines.putc(0xC1);
Blaze513 1:555c2c2bf9d3 63 DataLines.putc(0x29);
Blaze513 1:555c2c2bf9d3 64 DataLines.putc(CommandByte);
Blaze513 1:555c2c2bf9d3 65 //send the desired continuous mode command
Blaze513 1:555c2c2bf9d3 66 Continuous = 1;
Blaze513 1:555c2c2bf9d3 67 //set synchronous mode to true
Blaze513 1:555c2c2bf9d3 68 while ((Buffer[BufferStart] != 0xC4) || (Buffer[BufferStart + 1] != CommandByte))
Blaze513 1:555c2c2bf9d3 69 {
Blaze513 1:555c2c2bf9d3 70 if (((BufferStart + 2) % PacketSize) != BufferEnd)
Blaze513 1:555c2c2bf9d3 71 {
Blaze513 1:555c2c2bf9d3 72 BufferStart++;
Blaze513 1:555c2c2bf9d3 73 BufferStart%=PacketSize;
Blaze513 1:555c2c2bf9d3 74 }
Blaze513 1:555c2c2bf9d3 75 }
Blaze513 1:555c2c2bf9d3 76 //find the response header
Blaze513 1:555c2c2bf9d3 77 while (((BufferStart + 8) % PacketSize) != BufferEnd);
Blaze513 1:555c2c2bf9d3 78 BufferEnd = 0;
Blaze513 1:555c2c2bf9d3 79 Result = Checksum(BufferStart, 8);
Blaze513 1:555c2c2bf9d3 80 BufferStart = 0;
Blaze513 1:555c2c2bf9d3 81 PacketSize = lbuff;////////////////NOTE: if there wasn't a packet waiting while the async cmd was made, there will be no "extra data" here
Blaze513 1:555c2c2bf9d3 82 }
Blaze513 1:555c2c2bf9d3 83 else
Blaze513 1:555c2c2bf9d3 84 {
Blaze513 1:555c2c2bf9d3 85 if (Continuous)
Blaze513 1:555c2c2bf9d3 86 {
Blaze513 1:555c2c2bf9d3 87 DataLines.putc(0xFA);
Blaze513 1:555c2c2bf9d3 88 //send stop continuous mode command
Blaze513 1:555c2c2bf9d3 89 Continuous = 0;
Blaze513 1:555c2c2bf9d3 90 //set synchronous mode to true
Blaze513 1:555c2c2bf9d3 91 }
Blaze513 1:555c2c2bf9d3 92 Result = 1;
Blaze513 1:555c2c2bf9d3 93 }
Blaze513 1:555c2c2bf9d3 94 //put the IMU into continuous mode if the correct flag is set
Blaze513 1:555c2c2bf9d3 95 //Computer.printf("stop command success %d \n", Workspace[0]);
Blaze513 1:555c2c2bf9d3 96 //Computer.printf("sync mode %d \n", SyncMode);
Blaze513 1:555c2c2bf9d3 97 /*if (Selection & 0x08)
Blaze513 1:555c2c2bf9d3 98 {
Blaze513 1:555c2c2bf9d3 99 DataLines.attach(this, &MS3DMGX2::Interrupt, Serial::RxIrq);
Blaze513 1:555c2c2bf9d3 100 //attach automatic buffer-writing function to serial interrupt
Blaze513 1:555c2c2bf9d3 101 }
Blaze513 1:555c2c2bf9d3 102 else
Blaze513 1:555c2c2bf9d3 103 {
Blaze513 1:555c2c2bf9d3 104 DataLines.attach(NULL, Serial::RxIrq);
Blaze513 1:555c2c2bf9d3 105 //attach a null to detach any previous interrupt
Blaze513 1:555c2c2bf9d3 106 }*/
Blaze513 1:555c2c2bf9d3 107 //attaches or detaches interrupt function depending on interrupt flag
Blaze513 1:555c2c2bf9d3 108 return Result;
Blaze513 1:555c2c2bf9d3 109 //return success or failure
Blaze513 1:555c2c2bf9d3 110 }
Blaze513 1:555c2c2bf9d3 111
Blaze513 1:555c2c2bf9d3 112 bool MS3DMGX2::Readable()
Blaze513 1:555c2c2bf9d3 113 {
Blaze513 1:555c2c2bf9d3 114 return BufferStart == BufferEnd;
Blaze513 1:555c2c2bf9d3 115 }
Blaze513 1:555c2c2bf9d3 116
Blaze513 1:555c2c2bf9d3 117 /*void MS3DMGX2::AttachInterruptBuffer(float* Buffer)
Blaze513 1:555c2c2bf9d3 118 {
Blaze513 1:555c2c2bf9d3 119 InterruptBuffer = Buffer;
Blaze513 1:555c2c2bf9d3 120 }
Blaze513 1:555c2c2bf9d3 121 //store user's data pointer for use in interrupt modes
Blaze513 1:555c2c2bf9d3 122
Blaze513 1:555c2c2bf9d3 123 void MS3DMGX2::AttachInterruptFunction()
Blaze513 1:555c2c2bf9d3 124 {
Blaze513 1:555c2c2bf9d3 125 DataLines.attach(this, &MS3DMGX2::Interrupt, Serial::RxIrq);
Blaze513 1:555c2c2bf9d3 126 }
Blaze513 1:555c2c2bf9d3 127 void MS3DMGX2::AttachInterruptFunction(void (*Function)())
Blaze513 1:555c2c2bf9d3 128 {
Blaze513 1:555c2c2bf9d3 129 DataLines.attach(Function);
Blaze513 1:555c2c2bf9d3 130 }
Blaze513 1:555c2c2bf9d3 131 template<class Class> void AttachInterruptFunction(Class* Object, void (Class::*Function)())
Blaze513 1:555c2c2bf9d3 132 {
Blaze513 1:555c2c2bf9d3 133 DataLines.attach(Object, Function, Serial::RxIrq);
Blaze513 1:555c2c2bf9d3 134 }*/
Blaze513 1:555c2c2bf9d3 135 //overloads start interrupt modes, allowing user all possible options
Blaze513 1:555c2c2bf9d3 136
Blaze513 1:555c2c2bf9d3 137 void MS3DMGX2::RequestSyncRead()
Blaze513 1:555c2c2bf9d3 138 {
Blaze513 1:555c2c2bf9d3 139 if (Continuous)
Blaze513 1:555c2c2bf9d3 140 {
Blaze513 1:555c2c2bf9d3 141 DataLines.putc(0xFA);
Blaze513 1:555c2c2bf9d3 142 Continuous = 0;
Blaze513 1:555c2c2bf9d3 143 }
Blaze513 1:555c2c2bf9d3 144 DataLines.putc(CommandByte);
Blaze513 1:555c2c2bf9d3 145 }
Blaze513 1:555c2c2bf9d3 146 //lazy switches to synchronous mode and sends polled mode command byte
Blaze513 1:555c2c2bf9d3 147
Blaze513 1:555c2c2bf9d3 148 bool MS3DMGX2::Read(float* Data)
Blaze513 1:555c2c2bf9d3 149 {
Blaze513 1:555c2c2bf9d3 150 bool Result;
Blaze513 1:555c2c2bf9d3 151 unsigned char t = 0;
Blaze513 1:555c2c2bf9d3 152
Blaze513 1:555c2c2bf9d3 153 while ((Buffer[BufferStart] != CommandByte) && (t < PacketSize))
Blaze513 1:555c2c2bf9d3 154 {
Blaze513 1:555c2c2bf9d3 155 BufferStart++;
Blaze513 1:555c2c2bf9d3 156 BufferStart %= PacketSize;
Blaze513 1:555c2c2bf9d3 157 t++;
Blaze513 1:555c2c2bf9d3 158 }
Blaze513 1:555c2c2bf9d3 159 //find the header byte
Blaze513 1:555c2c2bf9d3 160 Result = Checksum(BufferStart, PacketSize);
Blaze513 1:555c2c2bf9d3 161 //compute checksum
Blaze513 1:555c2c2bf9d3 162 BufferStart++;
Blaze513 1:555c2c2bf9d3 163 BufferStart %= PacketSize;
Blaze513 1:555c2c2bf9d3 164 //move past header byte
Blaze513 1:555c2c2bf9d3 165 if (t < PacketSize)
Blaze513 1:555c2c2bf9d3 166 {
Blaze513 1:555c2c2bf9d3 167 for (unsigned int i = 0; i < ResponseLength; i++)
Blaze513 1:555c2c2bf9d3 168 {
Blaze513 1:555c2c2bf9d3 169 for(unsigned int j = 3; j < 4; j--)
Blaze513 1:555c2c2bf9d3 170 {
Blaze513 1:555c2c2bf9d3 171 ((unsigned char*)&Data[i])[j] = Buffer[BufferStart];
Blaze513 1:555c2c2bf9d3 172 BufferStart++;
Blaze513 1:555c2c2bf9d3 173 BufferStart %= PacketSize;
Blaze513 1:555c2c2bf9d3 174 }
Blaze513 1:555c2c2bf9d3 175 }
Blaze513 1:555c2c2bf9d3 176 //convert big endian bytes to little endian floats
Blaze513 1:555c2c2bf9d3 177 BufferStart += 6;
Blaze513 1:555c2c2bf9d3 178 //move index past timer and checksum bytes
Blaze513 1:555c2c2bf9d3 179 BufferStart %= PacketSize;
Blaze513 1:555c2c2bf9d3 180 }
Blaze513 1:555c2c2bf9d3 181 //if the header search did not timeout
Blaze513 1:555c2c2bf9d3 182
Blaze513 1:555c2c2bf9d3 183 return Result;
Blaze513 1:555c2c2bf9d3 184 }
Blaze513 1:555c2c2bf9d3 185
Blaze513 1:555c2c2bf9d3 186 /*MS3DMGX2::operator float*()
Blaze513 1:555c2c2bf9d3 187 {
Blaze513 1:555c2c2bf9d3 188 Read((float*)&Workspace[8]);
Blaze513 1:555c2c2bf9d3 189 return (float*)&Workspace[8];
Blaze513 1:555c2c2bf9d3 190 }
Blaze513 1:555c2c2bf9d3 191 //conversion function acts as shorthand for Read()
Blaze513 1:555c2c2bf9d3 192
Blaze513 1:555c2c2bf9d3 193 void MS3DMGX2::Interrupt()
Blaze513 1:555c2c2bf9d3 194 {
Blaze513 1:555c2c2bf9d3 195 Read(InterruptBuffer);
Blaze513 1:555c2c2bf9d3 196 }*/
Blaze513 1:555c2c2bf9d3 197 //this will be called when in an interrupt mode and a serial interrupt is generated
Blaze513 1:555c2c2bf9d3 198
Blaze513 1:555c2c2bf9d3 199 bool MS3DMGX2::Checksum(unsigned char Index, unsigned char Length)
Blaze513 1:555c2c2bf9d3 200 {
Blaze513 1:555c2c2bf9d3 201 unsigned short Sum = 0;
Blaze513 1:555c2c2bf9d3 202 for (unsigned char i = 0; i < Length - 2; i++)
Blaze513 1:555c2c2bf9d3 203 {
Blaze513 1:555c2c2bf9d3 204 Sum += Buffer[Index];
Blaze513 1:555c2c2bf9d3 205 Index++;
Blaze513 1:555c2c2bf9d3 206 Index %= PacketSize;
Blaze513 1:555c2c2bf9d3 207 }
Blaze513 1:555c2c2bf9d3 208 return (((unsigned char*)&Sum)[0] == Buffer[Index+1])
Blaze513 1:555c2c2bf9d3 209 && (((unsigned char*)&Sum)[1] == Buffer[Index]);
Blaze513 1:555c2c2bf9d3 210 }
Blaze513 1:555c2c2bf9d3 211
Blaze513 1:555c2c2bf9d3 212 void MS3DMGX2::FillSerialBuffer()//if the interrupt recurs faster than the time to complete its code, the rest of the system will be suspended indefinitely
Blaze513 1:555c2c2bf9d3 213 {
Blaze513 1:555c2c2bf9d3 214 while (DataLines.readable())
Blaze513 1:555c2c2bf9d3 215 {
Blaze513 1:555c2c2bf9d3 216 Buffer[BufferEnd] = DataLines.getc();
Blaze513 1:555c2c2bf9d3 217 BufferEnd++;
Blaze513 1:555c2c2bf9d3 218
Blaze513 1:555c2c2bf9d3 219 BufferEnd %= PacketSize;
Blaze513 1:555c2c2bf9d3 220 }
Blaze513 1:555c2c2bf9d3 221 }
Blaze513 1:555c2c2bf9d3 222 //this automatically reads in new serial data as it is received to
Blaze513 1:555c2c2bf9d3 223 //make a software buffer that is big enough to handle an entire
Blaze513 1:555c2c2bf9d3 224 //packet; the processor is about 1000 times faster than the data lines