Thomas Hamilton
interface class for an inertial measurement unit that uses a serial protocol.
3DM-GX2.cpp@0:fd1fce2347d9, 2010-08-31 (annotated)
- Committer:
- Blaze513
- Date:
- Tue Aug 31 04:38:14 2010 +0000
- Revision:
- 0:fd1fce2347d9
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| Blaze513 | 0:fd1fce2347d9 | 1 | #include "3DM-GX2.h" |
| Blaze513 | 0:fd1fce2347d9 | 2 | |
| Blaze513 | 0:fd1fce2347d9 | 3 | MS3DMGX2::MS3DMGX2(PinName tx, PinName rx) : DataLines(tx, rx), |
| Blaze513 | 0:fd1fce2347d9 | 4 | ModeCommand(0xCF), ModeLength(6), ModeSync(0), InterruptBuffer((float*)Workspace) |
| Blaze513 | 0:fd1fce2347d9 | 5 | { |
| Blaze513 | 0:fd1fce2347d9 | 6 | DataLines.baud(115200); |
| Blaze513 | 0:fd1fce2347d9 | 7 | DataLines.format(8, Serial::None, 1); |
| Blaze513 | 0:fd1fce2347d9 | 8 | DataLines.attach(NULL, Serial::RxIrq); |
| Blaze513 | 0:fd1fce2347d9 | 9 | } |
| Blaze513 | 0:fd1fce2347d9 | 10 | |
| Blaze513 | 0:fd1fce2347d9 | 11 | unsigned char MS3DMGX2::Mode(unsigned char Selection) |
| Blaze513 | 0:fd1fce2347d9 | 12 | { |
| Blaze513 | 0:fd1fce2347d9 | 13 | switch (Selection & 0x03) |
| Blaze513 | 0:fd1fce2347d9 | 14 | { |
| Blaze513 | 0:fd1fce2347d9 | 15 | case 0x00: |
| Blaze513 | 0:fd1fce2347d9 | 16 | ModeCommand = 0xCF; |
| Blaze513 | 0:fd1fce2347d9 | 17 | ModeLength = 6; |
| Blaze513 | 0:fd1fce2347d9 | 18 | break; |
| Blaze513 | 0:fd1fce2347d9 | 19 | case 0x01: |
| Blaze513 | 0:fd1fce2347d9 | 20 | ModeCommand = 0xD2; |
| Blaze513 | 0:fd1fce2347d9 | 21 | ModeLength = 9; |
| Blaze513 | 0:fd1fce2347d9 | 22 | break; |
| Blaze513 | 0:fd1fce2347d9 | 23 | case 0x02: |
| Blaze513 | 0:fd1fce2347d9 | 24 | ModeCommand = 0xC8; |
| Blaze513 | 0:fd1fce2347d9 | 25 | ModeLength = 15; |
| Blaze513 | 0:fd1fce2347d9 | 26 | break; |
| Blaze513 | 0:fd1fce2347d9 | 27 | case 0x03: |
| Blaze513 | 0:fd1fce2347d9 | 28 | ModeCommand = 0xCC; |
| Blaze513 | 0:fd1fce2347d9 | 29 | ModeLength = 18; |
| Blaze513 | 0:fd1fce2347d9 | 30 | break; |
| Blaze513 | 0:fd1fce2347d9 | 31 | } |
| Blaze513 | 0:fd1fce2347d9 | 32 | if (Selection & 0x04) |
| Blaze513 | 0:fd1fce2347d9 | 33 | { |
| Blaze513 | 0:fd1fce2347d9 | 34 | DiscardSerialBuffer(); |
| Blaze513 | 0:fd1fce2347d9 | 35 | DataLines.putc(0xC4); |
| Blaze513 | 0:fd1fce2347d9 | 36 | DataLines.putc(0xC1); |
| Blaze513 | 0:fd1fce2347d9 | 37 | DataLines.putc(0x29); |
| Blaze513 | 0:fd1fce2347d9 | 38 | DataLines.putc(ModeCommand); |
| Blaze513 | 0:fd1fce2347d9 | 39 | ModeSync = 1; |
| Blaze513 | 0:fd1fce2347d9 | 40 | wait_ms(10); |
| Blaze513 | 0:fd1fce2347d9 | 41 | Workspace[0] = DataLines.getc(); |
| Blaze513 | 0:fd1fce2347d9 | 42 | Workspace[1] = DataLines.getc(); |
| Blaze513 | 0:fd1fce2347d9 | 43 | while (DataLines.readable() && ((Workspace[0] != 0xC4) || (Workspace[1] != ModeCommand))) |
| Blaze513 | 0:fd1fce2347d9 | 44 | { |
| Blaze513 | 0:fd1fce2347d9 | 45 | Workspace[0] = Workspace[1]; |
| Blaze513 | 0:fd1fce2347d9 | 46 | Workspace[1] = DataLines.getc(); |
| Blaze513 | 0:fd1fce2347d9 | 47 | } |
| Blaze513 | 0:fd1fce2347d9 | 48 | unsigned char i = 7; |
| Blaze513 | 0:fd1fce2347d9 | 49 | while (DataLines.readable() && (i > 1)) |
| Blaze513 | 0:fd1fce2347d9 | 50 | { |
| Blaze513 | 0:fd1fce2347d9 | 51 | Workspace[i] = DataLines.getc(); |
| Blaze513 | 0:fd1fce2347d9 | 52 | i--; |
| Blaze513 | 0:fd1fce2347d9 | 53 | } |
| Blaze513 | 0:fd1fce2347d9 | 54 | CheckSum(0xC4, 1, &ModeCommand, &Workspace[4], Workspace); |
| Blaze513 | 0:fd1fce2347d9 | 55 | Workspace[0] = (Workspace[0] != Workspace[2]) || (Workspace[1] != Workspace[3]); |
| Blaze513 | 0:fd1fce2347d9 | 56 | } |
| Blaze513 | 0:fd1fce2347d9 | 57 | else |
| Blaze513 | 0:fd1fce2347d9 | 58 | { |
| Blaze513 | 0:fd1fce2347d9 | 59 | DataLines.putc(0xFA); |
| Blaze513 | 0:fd1fce2347d9 | 60 | ModeSync = 0; |
| Blaze513 | 0:fd1fce2347d9 | 61 | } |
| Blaze513 | 0:fd1fce2347d9 | 62 | if (Selection & 0x08) |
| Blaze513 | 0:fd1fce2347d9 | 63 | { |
| Blaze513 | 0:fd1fce2347d9 | 64 | DataLines.attach(this, &MS3DMGX2::Interrupt, Serial::RxIrq); |
| Blaze513 | 0:fd1fce2347d9 | 65 | } |
| Blaze513 | 0:fd1fce2347d9 | 66 | else |
| Blaze513 | 0:fd1fce2347d9 | 67 | { |
| Blaze513 | 0:fd1fce2347d9 | 68 | DataLines.attach(NULL, Serial::RxIrq); |
| Blaze513 | 0:fd1fce2347d9 | 69 | } |
| Blaze513 | 0:fd1fce2347d9 | 70 | if (Workspace[0]) |
| Blaze513 | 0:fd1fce2347d9 | 71 | { |
| Blaze513 | 0:fd1fce2347d9 | 72 | return 0x01; |
| Blaze513 | 0:fd1fce2347d9 | 73 | } |
| Blaze513 | 0:fd1fce2347d9 | 74 | else |
| Blaze513 | 0:fd1fce2347d9 | 75 | { |
| Blaze513 | 0:fd1fce2347d9 | 76 | return 0x00; |
| Blaze513 | 0:fd1fce2347d9 | 77 | } |
| Blaze513 | 0:fd1fce2347d9 | 78 | } |
| Blaze513 | 0:fd1fce2347d9 | 79 | |
| Blaze513 | 0:fd1fce2347d9 | 80 | void MS3DMGX2::AttachInterruptBuffer(float* Buffer) |
| Blaze513 | 0:fd1fce2347d9 | 81 | { |
| Blaze513 | 0:fd1fce2347d9 | 82 | InterruptBuffer = Buffer; |
| Blaze513 | 0:fd1fce2347d9 | 83 | } |
| Blaze513 | 0:fd1fce2347d9 | 84 | //store user's data pointer for use in interrupt modes |
| Blaze513 | 0:fd1fce2347d9 | 85 | |
| Blaze513 | 0:fd1fce2347d9 | 86 | void MS3DMGX2::RequestSyncRead() |
| Blaze513 | 0:fd1fce2347d9 | 87 | { |
| Blaze513 | 0:fd1fce2347d9 | 88 | if (ModeSync) |
| Blaze513 | 0:fd1fce2347d9 | 89 | { |
| Blaze513 | 0:fd1fce2347d9 | 90 | DataLines.putc(0xFA); |
| Blaze513 | 0:fd1fce2347d9 | 91 | ModeSync = 0x00; |
| Blaze513 | 0:fd1fce2347d9 | 92 | } |
| Blaze513 | 0:fd1fce2347d9 | 93 | //if operating in an asynchronous mode, switch to the synchronous equivalent |
| Blaze513 | 0:fd1fce2347d9 | 94 | DataLines.putc(ModeCommand); |
| Blaze513 | 0:fd1fce2347d9 | 95 | //send polled mode command byte |
| Blaze513 | 0:fd1fce2347d9 | 96 | } |
| Blaze513 | 0:fd1fce2347d9 | 97 | //lazy switches to synchronous mode and sends polled mode command byte |
| Blaze513 | 0:fd1fce2347d9 | 98 | |
| Blaze513 | 0:fd1fce2347d9 | 99 | void MS3DMGX2::DiscardSerialBuffer() |
| Blaze513 | 0:fd1fce2347d9 | 100 | { |
| Blaze513 | 0:fd1fce2347d9 | 101 | while (DataLines.readable()) |
| Blaze513 | 0:fd1fce2347d9 | 102 | { |
| Blaze513 | 0:fd1fce2347d9 | 103 | DataLines.getc(); |
| Blaze513 | 0:fd1fce2347d9 | 104 | } |
| Blaze513 | 0:fd1fce2347d9 | 105 | } |
| Blaze513 | 0:fd1fce2347d9 | 106 | //empty buffer of all data |
| Blaze513 | 0:fd1fce2347d9 | 107 | |
| Blaze513 | 0:fd1fce2347d9 | 108 | unsigned char MS3DMGX2::Read(float* Data) |
| Blaze513 | 0:fd1fce2347d9 | 109 | { |
| Blaze513 | 0:fd1fce2347d9 | 110 | unsigned char j = 0; |
| Blaze513 | 0:fd1fce2347d9 | 111 | do |
| Blaze513 | 0:fd1fce2347d9 | 112 | { |
| Blaze513 | 0:fd1fce2347d9 | 113 | j++; |
| Blaze513 | 0:fd1fce2347d9 | 114 | } while (DataLines.readable() && (DataLines.getc() != ModeCommand) && (j < (ModeLength << 2))); |
| Blaze513 | 0:fd1fce2347d9 | 115 | //find the header byte for synchronization |
| Blaze513 | 0:fd1fce2347d9 | 116 | for (unsigned char i = 0; i < ModeLength; i++) |
| Blaze513 | 0:fd1fce2347d9 | 117 | { |
| Blaze513 | 0:fd1fce2347d9 | 118 | j = 3; |
| Blaze513 | 0:fd1fce2347d9 | 119 | while (DataLines.readable() && (j < 4)) |
| Blaze513 | 0:fd1fce2347d9 | 120 | { |
| Blaze513 | 0:fd1fce2347d9 | 121 | ((unsigned char*)&Data[i])[j] = DataLines.getc(); |
| Blaze513 | 0:fd1fce2347d9 | 122 | j--; |
| Blaze513 | 0:fd1fce2347d9 | 123 | } |
| Blaze513 | 0:fd1fce2347d9 | 124 | } |
| Blaze513 | 0:fd1fce2347d9 | 125 | //change endianness and fill user buffer |
| Blaze513 | 0:fd1fce2347d9 | 126 | j = 7; |
| Blaze513 | 0:fd1fce2347d9 | 127 | while (DataLines.readable() && (j > 1)) |
| Blaze513 | 0:fd1fce2347d9 | 128 | { |
| Blaze513 | 0:fd1fce2347d9 | 129 | Workspace[j] = DataLines.getc(); |
| Blaze513 | 0:fd1fce2347d9 | 130 | j--; |
| Blaze513 | 0:fd1fce2347d9 | 131 | } |
| Blaze513 | 0:fd1fce2347d9 | 132 | //change endianness and gather timer and checksum (bytes 2 and 3) |
| Blaze513 | 0:fd1fce2347d9 | 133 | CheckSum(ModeCommand, ModeLength << 2, (unsigned char*)Data, &Workspace[4], Workspace); |
| Blaze513 | 0:fd1fce2347d9 | 134 | //compute checksum and store in bytes 0 and 1 |
| Blaze513 | 0:fd1fce2347d9 | 135 | if ((Workspace[0] != Workspace[2]) || (Workspace[1] != Workspace[3])) |
| Blaze513 | 0:fd1fce2347d9 | 136 | { |
| Blaze513 | 0:fd1fce2347d9 | 137 | return 0x01; |
| Blaze513 | 0:fd1fce2347d9 | 138 | } |
| Blaze513 | 0:fd1fce2347d9 | 139 | else |
| Blaze513 | 0:fd1fce2347d9 | 140 | { |
| Blaze513 | 0:fd1fce2347d9 | 141 | return 0x00; |
| Blaze513 | 0:fd1fce2347d9 | 142 | } |
| Blaze513 | 0:fd1fce2347d9 | 143 | //verify checksum and return result |
| Blaze513 | 0:fd1fce2347d9 | 144 | } |
| Blaze513 | 0:fd1fce2347d9 | 145 | //this method reads a measurement from the serial port, assuming one is waiting there |
| Blaze513 | 0:fd1fce2347d9 | 146 | |
| Blaze513 | 0:fd1fce2347d9 | 147 | void MS3DMGX2::Interrupt() |
| Blaze513 | 0:fd1fce2347d9 | 148 | { |
| Blaze513 | 0:fd1fce2347d9 | 149 | Read(InterruptBuffer); |
| Blaze513 | 0:fd1fce2347d9 | 150 | DiscardSerialBuffer(); |
| Blaze513 | 0:fd1fce2347d9 | 151 | } |
| Blaze513 | 0:fd1fce2347d9 | 152 | //this will be called when in an interrupt mode and a serial interrupt is generated |
| Blaze513 | 0:fd1fce2347d9 | 153 | |
| Blaze513 | 0:fd1fce2347d9 | 154 | void MS3DMGX2::CheckSum(unsigned char Header, unsigned char Length, |
| Blaze513 | 0:fd1fce2347d9 | 155 | unsigned char* Data, unsigned char* Timer, unsigned char* Result) |
| Blaze513 | 0:fd1fce2347d9 | 156 | { |
| Blaze513 | 0:fd1fce2347d9 | 157 | unsigned short Sum = Header; |
| Blaze513 | 0:fd1fce2347d9 | 158 | for (unsigned char i = 0; i < Length; i++) |
| Blaze513 | 0:fd1fce2347d9 | 159 | { |
| Blaze513 | 0:fd1fce2347d9 | 160 | Sum += Data[i]; |
| Blaze513 | 0:fd1fce2347d9 | 161 | } |
| Blaze513 | 0:fd1fce2347d9 | 162 | for (unsigned char i = 0; i < 4; i++) |
| Blaze513 | 0:fd1fce2347d9 | 163 | { |
| Blaze513 | 0:fd1fce2347d9 | 164 | Sum += Timer[i]; |
| Blaze513 | 0:fd1fce2347d9 | 165 | } |
| Blaze513 | 0:fd1fce2347d9 | 166 | Result[0] = ((char*)&Sum)[0]; |
| Blaze513 | 0:fd1fce2347d9 | 167 | Result[1] = ((char*)&Sum)[1]; |
| Blaze513 | 0:fd1fce2347d9 | 168 | } |
| Blaze513 | 0:fd1fce2347d9 | 169 | //compute the checksum of a data packet and store the result in the first two bytes of "Result" |