Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: BufferedSerial FatFileSystemCpp mbed
VIPSSerialProtocol.cpp@2:a79201e302d7, 2021-01-18 (annotated)
- Committer:
- AndyA
- Date:
- Mon Jan 18 09:15:04 2021 +0000
- Revision:
- 2:a79201e302d7
- Parent:
- 1:dd1f7e162f91
- Child:
- 3:14d241e29be3
Added ID
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
AndyA | 0:97661408d0f9 | 1 | #include "LTCApp.h" |
AndyA | 0:97661408d0f9 | 2 | #include <cstdint> |
AndyA | 0:97661408d0f9 | 3 | #include <cstring> |
AndyA | 0:97661408d0f9 | 4 | |
AndyA | 0:97661408d0f9 | 5 | #define _DisableUWB 0x00 |
AndyA | 0:97661408d0f9 | 6 | #define _EnableUWB 0x01 |
AndyA | 0:97661408d0f9 | 7 | #define _GetPPSTime 0x02 |
AndyA | 0:97661408d0f9 | 8 | #define _UWBData_Legacy 0x03 |
AndyA | 0:97661408d0f9 | 9 | #define _UWBData_MOCAP 0x11 |
AndyA | 0:97661408d0f9 | 10 | #define _UWBFitted 0x04 |
AndyA | 0:97661408d0f9 | 11 | #define _VBOXReady 0x05 |
AndyA | 0:97661408d0f9 | 12 | #define _SetDeltaTime 0x07 |
AndyA | 0:97661408d0f9 | 13 | |
AndyA | 1:dd1f7e162f91 | 14 | const char* VIPSStatusMessages[] = { "Got UWBFitted", "Got UWBEnabled", "Got GetTime", "Ack to VBOX ready", "Nak to VBOX ready", "CRC error", "VIPSAlive", NULL}; |
AndyA | 0:97661408d0f9 | 15 | |
AndyA | 0:97661408d0f9 | 16 | |
AndyA | 1:dd1f7e162f91 | 17 | VIPSSerial::VIPSSerial(const PinName Tx, const PinName Rx) : _port(Tx,Rx) |
AndyA | 1:dd1f7e162f91 | 18 | { |
AndyA | 0:97661408d0f9 | 19 | messagePrt = 0; |
AndyA | 0:97661408d0f9 | 20 | messageLength = 0; |
AndyA | 1:dd1f7e162f91 | 21 | |
AndyA | 0:97661408d0f9 | 22 | pointCount = 0; |
AndyA | 0:97661408d0f9 | 23 | nextPosition = 0; |
AndyA | 1:dd1f7e162f91 | 24 | |
AndyA | 0:97661408d0f9 | 25 | outputPtr = NULL; |
AndyA | 0:97661408d0f9 | 26 | statusMessage = 0; |
AndyA | 1:dd1f7e162f91 | 27 | |
AndyA | 0:97661408d0f9 | 28 | enableAllUpdates = false; |
AndyA | 0:97661408d0f9 | 29 | newFormatMsg = false; |
AndyA | 1:dd1f7e162f91 | 30 | |
AndyA | 0:97661408d0f9 | 31 | nextPosition= 0; |
AndyA | 1:dd1f7e162f91 | 32 | _port.baud(115200); |
AndyA | 1:dd1f7e162f91 | 33 | } |
AndyA | 0:97661408d0f9 | 34 | |
AndyA | 1:dd1f7e162f91 | 35 | void VIPSSerial::run(void) |
AndyA | 1:dd1f7e162f91 | 36 | { |
AndyA | 1:dd1f7e162f91 | 37 | _port.attach(callback(this, &VIPSSerial::onSerialRx)); |
AndyA | 1:dd1f7e162f91 | 38 | } |
AndyA | 0:97661408d0f9 | 39 | |
AndyA | 1:dd1f7e162f91 | 40 | void VIPSSerial::onSerialRx(void) |
AndyA | 1:dd1f7e162f91 | 41 | { |
AndyA | 0:97661408d0f9 | 42 | while (_port.readable()) { |
AndyA | 1:dd1f7e162f91 | 43 | messageInBuffer[messagePrt] = _port.getc(); |
AndyA | 0:97661408d0f9 | 44 | if (messagePrt==0) { // look for header |
AndyA | 1:dd1f7e162f91 | 45 | if ((messageInBuffer[0]==0x2A) || (messageInBuffer[0]==0x24)) { |
AndyA | 0:97661408d0f9 | 46 | messagePrt=1; |
AndyA | 0:97661408d0f9 | 47 | newFormatMsg=(messageInBuffer[0]==0x24); |
AndyA | 1:dd1f7e162f91 | 48 | } |
AndyA | 0:97661408d0f9 | 49 | } else if (newFormatMsg) { |
AndyA | 0:97661408d0f9 | 50 | if (messagePrt < 4) { |
AndyA | 0:97661408d0f9 | 51 | messagePrt++; |
AndyA | 1:dd1f7e162f91 | 52 | if (messagePrt==4) { |
AndyA | 0:97661408d0f9 | 53 | if (messageInBuffer[1]!=0xd9) |
AndyA | 0:97661408d0f9 | 54 | messagePrt=0; |
AndyA | 0:97661408d0f9 | 55 | messageLength = *(uint16_t*)(messageInBuffer+2); |
AndyA | 0:97661408d0f9 | 56 | if ((messageLength>115) || (messageLength<34)) |
AndyA | 0:97661408d0f9 | 57 | messagePrt = 0; |
AndyA | 0:97661408d0f9 | 58 | } |
AndyA | 0:97661408d0f9 | 59 | } else { |
AndyA | 0:97661408d0f9 | 60 | messagePrt++; |
AndyA | 0:97661408d0f9 | 61 | if (messagePrt == messageLength) { |
AndyA | 0:97661408d0f9 | 62 | parsePostionInput_mocap(); |
AndyA | 0:97661408d0f9 | 63 | messagePrt=0; |
AndyA | 0:97661408d0f9 | 64 | } |
AndyA | 0:97661408d0f9 | 65 | } |
AndyA | 0:97661408d0f9 | 66 | } else { |
AndyA | 0:97661408d0f9 | 67 | if (messagePrt==1) { |
AndyA | 0:97661408d0f9 | 68 | if (messageInBuffer[1]<128) { // valid length |
AndyA | 0:97661408d0f9 | 69 | messageLength = messageInBuffer[1]; |
AndyA | 0:97661408d0f9 | 70 | messagePrt = 2; |
AndyA | 0:97661408d0f9 | 71 | } else { |
AndyA | 0:97661408d0f9 | 72 | messagePrt=0; |
AndyA | 0:97661408d0f9 | 73 | } |
AndyA | 0:97661408d0f9 | 74 | } else { // in the middle of a message |
AndyA | 0:97661408d0f9 | 75 | messagePrt++; |
AndyA | 0:97661408d0f9 | 76 | if (messagePrt==messageLength) { |
AndyA | 0:97661408d0f9 | 77 | processRxMessage(); |
AndyA | 0:97661408d0f9 | 78 | messagePrt=0; |
AndyA | 0:97661408d0f9 | 79 | } |
AndyA | 0:97661408d0f9 | 80 | } |
AndyA | 0:97661408d0f9 | 81 | } |
AndyA | 0:97661408d0f9 | 82 | } |
AndyA | 0:97661408d0f9 | 83 | } |
AndyA | 0:97661408d0f9 | 84 | |
AndyA | 1:dd1f7e162f91 | 85 | void VIPSSerial::processRxMessage() |
AndyA | 1:dd1f7e162f91 | 86 | { |
AndyA | 0:97661408d0f9 | 87 | if (!checkCRC(&messageInBuffer[0])) { |
AndyA | 0:97661408d0f9 | 88 | // statusMessage = 6; |
AndyA | 0:97661408d0f9 | 89 | return; |
AndyA | 0:97661408d0f9 | 90 | } |
AndyA | 1:dd1f7e162f91 | 91 | |
AndyA | 0:97661408d0f9 | 92 | switch (messageInBuffer[2]) { |
AndyA | 0:97661408d0f9 | 93 | case _UWBFitted: |
AndyA | 1:dd1f7e162f91 | 94 | statusMessage = 1; |
AndyA | 0:97661408d0f9 | 95 | sendAck(messageInBuffer[2]); |
AndyA | 0:97661408d0f9 | 96 | break; |
AndyA | 0:97661408d0f9 | 97 | case _EnableUWB: |
AndyA | 1:dd1f7e162f91 | 98 | statusMessage=2; |
AndyA | 0:97661408d0f9 | 99 | case _DisableUWB: // for all of these just send an ack. |
AndyA | 0:97661408d0f9 | 100 | case _SetDeltaTime: |
AndyA | 0:97661408d0f9 | 101 | sendAck(messageInBuffer[2]); |
AndyA | 0:97661408d0f9 | 102 | break; |
AndyA | 0:97661408d0f9 | 103 | case _GetPPSTime: |
AndyA | 1:dd1f7e162f91 | 104 | statusMessage=3; |
AndyA | 0:97661408d0f9 | 105 | sendVBOXTime(); |
AndyA | 0:97661408d0f9 | 106 | // send vbox tick counter |
AndyA | 0:97661408d0f9 | 107 | break; |
AndyA | 0:97661408d0f9 | 108 | case _VBOXReady: |
AndyA | 0:97661408d0f9 | 109 | if (ppsActive) { |
AndyA | 1:dd1f7e162f91 | 110 | statusMessage=4; |
AndyA | 0:97661408d0f9 | 111 | sendAck(_VBOXReady); |
AndyA | 0:97661408d0f9 | 112 | } else { |
AndyA | 1:dd1f7e162f91 | 113 | statusMessage=5; |
AndyA | 0:97661408d0f9 | 114 | sendNack(_VBOXReady); |
AndyA | 0:97661408d0f9 | 115 | } |
AndyA | 0:97661408d0f9 | 116 | break; |
AndyA | 0:97661408d0f9 | 117 | case _UWBData_Legacy: |
AndyA | 0:97661408d0f9 | 118 | parsePostionInput_legacy(); |
AndyA | 1:dd1f7e162f91 | 119 | break; |
AndyA | 0:97661408d0f9 | 120 | default: |
AndyA | 1:dd1f7e162f91 | 121 | break; |
AndyA | 1:dd1f7e162f91 | 122 | } |
AndyA | 0:97661408d0f9 | 123 | } |
AndyA | 0:97661408d0f9 | 124 | |
AndyA | 0:97661408d0f9 | 125 | |
AndyA | 1:dd1f7e162f91 | 126 | void VIPSSerial::sendVBOXTime() |
AndyA | 1:dd1f7e162f91 | 127 | { |
AndyA | 0:97661408d0f9 | 128 | unsigned char timeValue[3]; |
AndyA | 0:97661408d0f9 | 129 | uint32_t timeToSend = VBOXTicks-1; // we track time at next PPS, message requires time at the last PPS. |
AndyA | 0:97661408d0f9 | 130 | timeValue[0]= (timeToSend>>16)&0x00ff; |
AndyA | 0:97661408d0f9 | 131 | timeValue[1]= (timeToSend>>8)&0x00ff; |
AndyA | 0:97661408d0f9 | 132 | timeValue[2]= timeToSend&0x00ff; |
AndyA | 0:97661408d0f9 | 133 | sendResponse(_GetPPSTime,timeValue,3); |
AndyA | 0:97661408d0f9 | 134 | } |
AndyA | 0:97661408d0f9 | 135 | |
AndyA | 1:dd1f7e162f91 | 136 | void VIPSSerial::sendAck(unsigned char function) |
AndyA | 1:dd1f7e162f91 | 137 | { |
AndyA | 0:97661408d0f9 | 138 | unsigned char ack=0x01; |
AndyA | 0:97661408d0f9 | 139 | sendResponse(function,&ack,1); |
AndyA | 0:97661408d0f9 | 140 | } |
AndyA | 0:97661408d0f9 | 141 | |
AndyA | 1:dd1f7e162f91 | 142 | void VIPSSerial::sendNack(unsigned char function) |
AndyA | 1:dd1f7e162f91 | 143 | { |
AndyA | 1:dd1f7e162f91 | 144 | unsigned char nack=0x00; |
AndyA | 0:97661408d0f9 | 145 | sendResponse(function,&nack,1); |
AndyA | 0:97661408d0f9 | 146 | } |
AndyA | 0:97661408d0f9 | 147 | |
AndyA | 1:dd1f7e162f91 | 148 | void VIPSSerial::sendResponse(unsigned char function, unsigned char* data, int dataLen) |
AndyA | 1:dd1f7e162f91 | 149 | { |
AndyA | 1:dd1f7e162f91 | 150 | messageOutBuffer[0]=0xff; |
AndyA | 1:dd1f7e162f91 | 151 | messageOutBuffer[1]=dataLen+4; |
AndyA | 1:dd1f7e162f91 | 152 | messageOutBuffer[2]=function; |
AndyA | 1:dd1f7e162f91 | 153 | for (int i=0; i<dataLen; i++) |
AndyA | 1:dd1f7e162f91 | 154 | messageOutBuffer[i+3] = data[i]; |
AndyA | 1:dd1f7e162f91 | 155 | VIPSSerial::getCRC(messageOutBuffer,dataLen+3,messageOutBuffer+dataLen+3); |
AndyA | 0:97661408d0f9 | 156 | for (int i=0;i<dataLen+5;i++) |
AndyA | 0:97661408d0f9 | 157 | _port.putc(messageOutBuffer[i]); |
AndyA | 0:97661408d0f9 | 158 | } |
AndyA | 0:97661408d0f9 | 159 | |
AndyA | 1:dd1f7e162f91 | 160 | void VIPSSerial::getCRC(void *data, int len, void *checksum) |
AndyA | 1:dd1f7e162f91 | 161 | { |
AndyA | 1:dd1f7e162f91 | 162 | uint8_t *dataPtr = (uint8_t *)data; |
AndyA | 1:dd1f7e162f91 | 163 | uint16_t *crcPtr = (uint16_t *)checksum; |
AndyA | 0:97661408d0f9 | 164 | uint16_t crc = 0x0; |
AndyA | 0:97661408d0f9 | 165 | unsigned char x; |
AndyA | 0:97661408d0f9 | 166 | int byteCount = 0; |
AndyA | 0:97661408d0f9 | 167 | |
AndyA | 0:97661408d0f9 | 168 | while ((len--) > 0) { |
AndyA | 1:dd1f7e162f91 | 169 | x = (unsigned char)(crc >> 8 ^ dataPtr[byteCount++]); |
AndyA | 0:97661408d0f9 | 170 | x ^= (unsigned char)(x >> 4); |
AndyA | 0:97661408d0f9 | 171 | crc = (uint16_t)((crc << 8) ^ (x << 12) ^ (x << 5) ^ x); |
AndyA | 0:97661408d0f9 | 172 | } |
AndyA | 1:dd1f7e162f91 | 173 | crcPtr[0] = crc >> 8; |
AndyA | 1:dd1f7e162f91 | 174 | crcPtr[1] = crc &0x00ff; |
AndyA | 0:97661408d0f9 | 175 | } |
AndyA | 0:97661408d0f9 | 176 | |
AndyA | 1:dd1f7e162f91 | 177 | bool VIPSSerial::checkCRC(unsigned char* data) |
AndyA | 1:dd1f7e162f91 | 178 | { |
AndyA | 0:97661408d0f9 | 179 | unsigned char expectedCRC[2]; |
AndyA | 0:97661408d0f9 | 180 | int length = data[1]; |
AndyA | 0:97661408d0f9 | 181 | if (data[0] == 0xff) // for response length doesn't include the header |
AndyA | 0:97661408d0f9 | 182 | length++; |
AndyA | 1:dd1f7e162f91 | 183 | VIPSSerial::getCRC(data, length-2, expectedCRC); |
AndyA | 0:97661408d0f9 | 184 | if ((data[length-2]==expectedCRC[0]) && (data[length-1]==expectedCRC[1])) |
AndyA | 0:97661408d0f9 | 185 | return true; |
AndyA | 0:97661408d0f9 | 186 | return false; |
AndyA | 0:97661408d0f9 | 187 | } |
AndyA | 0:97661408d0f9 | 188 | |
AndyA | 0:97661408d0f9 | 189 | |
AndyA | 1:dd1f7e162f91 | 190 | bool VIPSSerial::checkNewPacketRC(unsigned char* data) |
AndyA | 1:dd1f7e162f91 | 191 | { |
AndyA | 0:97661408d0f9 | 192 | unsigned char expectedCRC[2]; |
AndyA | 0:97661408d0f9 | 193 | int length = data[2] | (((int)data[3])<<8); |
AndyA | 1:dd1f7e162f91 | 194 | VIPSSerial::getCRC(data, length-2, expectedCRC); |
AndyA | 0:97661408d0f9 | 195 | if ((data[length-2]==expectedCRC[0]) && (data[length-1]==expectedCRC[1])) |
AndyA | 0:97661408d0f9 | 196 | return true; |
AndyA | 0:97661408d0f9 | 197 | return false; |
AndyA | 0:97661408d0f9 | 198 | } |
AndyA | 0:97661408d0f9 | 199 | |
AndyA | 0:97661408d0f9 | 200 | |
AndyA | 1:dd1f7e162f91 | 201 | void VIPSSerial::parsePostionInput_legacy() |
AndyA | 1:dd1f7e162f91 | 202 | { |
AndyA | 0:97661408d0f9 | 203 | printf("L"); |
AndyA | 0:97661408d0f9 | 204 | uint8_t tmpBuffer[8]; |
AndyA | 0:97661408d0f9 | 205 | int32_t tmpInt; |
AndyA | 0:97661408d0f9 | 206 | memcpy((uint8_t *)(&tmpInt)+1, messageInBuffer+4, 3); |
AndyA | 0:97661408d0f9 | 207 | tmpInt &= 0x00ffffff; |
AndyA | 0:97661408d0f9 | 208 | lastPositions[nextPosition].pos.time = tmpInt*10; |
AndyA | 0:97661408d0f9 | 209 | memcpy(tmpBuffer, messageInBuffer+7, 8); |
AndyA | 0:97661408d0f9 | 210 | lastPositions[nextPosition].pos.X = *(double *)(tmpBuffer); |
AndyA | 0:97661408d0f9 | 211 | memcpy(tmpBuffer, messageInBuffer+15, 8); |
AndyA | 0:97661408d0f9 | 212 | lastPositions[nextPosition].pos.Y = *(double *)(tmpBuffer); |
AndyA | 0:97661408d0f9 | 213 | memcpy((uint8_t *)(&tmpInt)+1, messageInBuffer+27, 3); |
AndyA | 0:97661408d0f9 | 214 | if (tmpInt & 0x00800000) |
AndyA | 0:97661408d0f9 | 215 | tmpInt |= 0xff000000; |
AndyA | 0:97661408d0f9 | 216 | lastPositions[nextPosition].pos.Height = tmpInt/100.0f; |
AndyA | 0:97661408d0f9 | 217 | lastPositions[nextPosition].pos.roll = 0; |
AndyA | 0:97661408d0f9 | 218 | lastPositions[nextPosition].pos.pitch = 0; |
AndyA | 0:97661408d0f9 | 219 | lastPositions[nextPosition].pos.yaw = 0; |
AndyA | 0:97661408d0f9 | 220 | |
AndyA | 0:97661408d0f9 | 221 | if (enableAllUpdates) { |
AndyA | 1:dd1f7e162f91 | 222 | printf("Add pos\r\n"); |
AndyA | 1:dd1f7e162f91 | 223 | outputPtr = &outputPosition; |
AndyA | 1:dd1f7e162f91 | 224 | memcpy(outputPtr,&(lastPositions[nextPosition].pos),sizeof(position)); |
AndyA | 0:97661408d0f9 | 225 | } |
AndyA | 0:97661408d0f9 | 226 | |
AndyA | 0:97661408d0f9 | 227 | nextPosition++; |
AndyA | 0:97661408d0f9 | 228 | if (nextPosition == posHistoryLen) { |
AndyA | 0:97661408d0f9 | 229 | nextPosition = 0; |
AndyA | 0:97661408d0f9 | 230 | } |
AndyA | 0:97661408d0f9 | 231 | pointCount++; |
AndyA | 0:97661408d0f9 | 232 | } |
AndyA | 0:97661408d0f9 | 233 | |
AndyA | 1:dd1f7e162f91 | 234 | void VIPSSerial::parsePostionInput_mocap() |
AndyA | 1:dd1f7e162f91 | 235 | { |
AndyA | 0:97661408d0f9 | 236 | if (!checkNewPacketRC(&messageInBuffer[0])) { |
AndyA | 0:97661408d0f9 | 237 | pc.write("CRC error\r\n",11); |
AndyA | 0:97661408d0f9 | 238 | return; |
AndyA | 0:97661408d0f9 | 239 | } |
AndyA | 0:97661408d0f9 | 240 | lastPositions[nextPosition].time = TimeSinceLastFrame.read_us(); |
AndyA | 0:97661408d0f9 | 241 | uint32_t mask = *(uint32_t*)(messageInBuffer+4); |
AndyA | 0:97661408d0f9 | 242 | int offset = 32; // end of position |
AndyA | 1:dd1f7e162f91 | 243 | |
AndyA | 0:97661408d0f9 | 244 | lastPositions[nextPosition].pos.time = *(uint32_t*)(messageInBuffer+8); |
AndyA | 0:97661408d0f9 | 245 | lastPositions[nextPosition].pos.X = *(double *)(messageInBuffer+12); |
AndyA | 0:97661408d0f9 | 246 | lastPositions[nextPosition].pos.Y = *(double *)(messageInBuffer+20); |
AndyA | 0:97661408d0f9 | 247 | lastPositions[nextPosition].pos.Height = *(float *)(messageInBuffer+28); |
AndyA | 0:97661408d0f9 | 248 | if (mask & 0x0002) { // parse status |
AndyA | 1:dd1f7e162f91 | 249 | |
AndyA | 0:97661408d0f9 | 250 | offset +=4; |
AndyA | 0:97661408d0f9 | 251 | } |
AndyA | 0:97661408d0f9 | 252 | |
AndyA | 0:97661408d0f9 | 253 | if (mask & 0x0004) { |
AndyA | 0:97661408d0f9 | 254 | lastPositions[nextPosition].pos.roll = *(float *)(messageInBuffer+offset); |
AndyA | 0:97661408d0f9 | 255 | lastPositions[nextPosition].pos.pitch = *(float *)(messageInBuffer+offset+4); |
AndyA | 0:97661408d0f9 | 256 | lastPositions[nextPosition].pos.yaw = *(float *)(messageInBuffer+offset+8); |
AndyA | 0:97661408d0f9 | 257 | offset+=12; |
AndyA | 0:97661408d0f9 | 258 | } else { |
AndyA | 0:97661408d0f9 | 259 | lastPositions[nextPosition].pos.roll = 0; |
AndyA | 0:97661408d0f9 | 260 | lastPositions[nextPosition].pos.pitch = 0; |
AndyA | 0:97661408d0f9 | 261 | lastPositions[nextPosition].pos.yaw = 0; |
AndyA | 0:97661408d0f9 | 262 | } |
AndyA | 0:97661408d0f9 | 263 | |
AndyA | 0:97661408d0f9 | 264 | if (mask & 0x0008) { // velocity |
AndyA | 0:97661408d0f9 | 265 | |
AndyA | 0:97661408d0f9 | 266 | offset+=8; |
AndyA | 1:dd1f7e162f91 | 267 | } |
AndyA | 0:97661408d0f9 | 268 | if (mask & 0x0010) {// vert velocity |
AndyA | 0:97661408d0f9 | 269 | |
AndyA | 1:dd1f7e162f91 | 270 | offset+=4; |
AndyA | 0:97661408d0f9 | 271 | } |
AndyA | 0:97661408d0f9 | 272 | if (mask & 0x0020) { // pos uncertainty |
AndyA | 0:97661408d0f9 | 273 | |
AndyA | 1:dd1f7e162f91 | 274 | offset+=12; |
AndyA | 0:97661408d0f9 | 275 | if (mask & 0x0004) { // orientation uncertainty |
AndyA | 1:dd1f7e162f91 | 276 | offset += 12; |
AndyA | 0:97661408d0f9 | 277 | } |
AndyA | 0:97661408d0f9 | 278 | if (mask & 0x0008) { // velocity uncertainty |
AndyA | 1:dd1f7e162f91 | 279 | offset += 12; |
AndyA | 0:97661408d0f9 | 280 | } |
AndyA | 0:97661408d0f9 | 281 | } |
AndyA | 0:97661408d0f9 | 282 | if (mask & 0x0040) { // accuracy |
AndyA | 2:a79201e302d7 | 283 | lastPositions[nextPosition].pos.ID = *(messageInBuffer+offset+3); |
AndyA | 0:97661408d0f9 | 284 | offset+=4; |
AndyA | 1:dd1f7e162f91 | 285 | } |
AndyA | 1:dd1f7e162f91 | 286 | if (mask & 0x0080) { // raw UWB |
AndyA | 0:97661408d0f9 | 287 | |
AndyA | 0:97661408d0f9 | 288 | offset+=24; |
AndyA | 0:97661408d0f9 | 289 | } |
AndyA | 1:dd1f7e162f91 | 290 | if (mask & 0x0100) { // raw IMU |
AndyA | 0:97661408d0f9 | 291 | |
AndyA | 0:97661408d0f9 | 292 | offset+=24; |
AndyA | 0:97661408d0f9 | 293 | } |
AndyA | 0:97661408d0f9 | 294 | |
AndyA | 0:97661408d0f9 | 295 | if (mask & 0x0200) {// rover info |
AndyA | 0:97661408d0f9 | 296 | |
AndyA | 1:dd1f7e162f91 | 297 | offset+=4; |
AndyA | 0:97661408d0f9 | 298 | } |
AndyA | 0:97661408d0f9 | 299 | |
AndyA | 0:97661408d0f9 | 300 | if (enableAllUpdates) { |
AndyA | 1:dd1f7e162f91 | 301 | // printf("Add pos\r\n"); |
AndyA | 1:dd1f7e162f91 | 302 | outputPtr = &outputPosition; |
AndyA | 1:dd1f7e162f91 | 303 | memcpy(outputPtr,&(lastPositions[nextPosition].pos),sizeof(position)); |
AndyA | 0:97661408d0f9 | 304 | } |
AndyA | 0:97661408d0f9 | 305 | |
AndyA | 0:97661408d0f9 | 306 | nextPosition++; |
AndyA | 0:97661408d0f9 | 307 | if (nextPosition == posHistoryLen) { |
AndyA | 0:97661408d0f9 | 308 | nextPosition = 0; |
AndyA | 0:97661408d0f9 | 309 | } |
AndyA | 0:97661408d0f9 | 310 | pointCount++; |
AndyA | 0:97661408d0f9 | 311 | } |
AndyA | 0:97661408d0f9 | 312 | |
AndyA | 1:dd1f7e162f91 | 313 | // send a position output for the requested time. Times are based on the global TimeSinceLastFrame timer. |
AndyA | 1:dd1f7e162f91 | 314 | position* VIPSSerial::sendPositionForTime(uint32_t timeValue) |
AndyA | 1:dd1f7e162f91 | 315 | { |
AndyA | 0:97661408d0f9 | 316 | static uint32_t lastPoints = 0; |
AndyA | 0:97661408d0f9 | 317 | if (pointCount < 2) |
AndyA | 0:97661408d0f9 | 318 | return NULL; |
AndyA | 0:97661408d0f9 | 319 | if (lastPoints == pointCount) |
AndyA | 0:97661408d0f9 | 320 | return NULL; |
AndyA | 0:97661408d0f9 | 321 | lastPoints = pointCount; |
AndyA | 1:dd1f7e162f91 | 322 | |
AndyA | 1:dd1f7e162f91 | 323 | int lastPoint = nextPosition - 1; |
AndyA | 1:dd1f7e162f91 | 324 | int prevPoint = nextPosition - 2; |
AndyA | 1:dd1f7e162f91 | 325 | if (lastPoint<0) |
AndyA | 1:dd1f7e162f91 | 326 | lastPoint+=posHistoryLen; |
AndyA | 1:dd1f7e162f91 | 327 | if (prevPoint<0) |
AndyA | 1:dd1f7e162f91 | 328 | prevPoint+=posHistoryLen; |
AndyA | 0:97661408d0f9 | 329 | |
AndyA | 1:dd1f7e162f91 | 330 | // calculate timestamps as a function of time since last frame |
AndyA | 1:dd1f7e162f91 | 331 | outputPosition.time = timeValue; |
AndyA | 1:dd1f7e162f91 | 332 | int32_t LastTimeMark = lastPositions[lastPoint].time; |
AndyA | 1:dd1f7e162f91 | 333 | int32_t PrevTimeMark = lastPositions[prevPoint].time; |
AndyA | 1:dd1f7e162f91 | 334 | if (PrevTimeMark > LastTimeMark) { |
AndyA | 1:dd1f7e162f91 | 335 | LastTimeMark += TimeSinceLastFrameWrap; |
AndyA | 1:dd1f7e162f91 | 336 | outputPosition.time += TimeSinceLastFrameWrap; |
AndyA | 1:dd1f7e162f91 | 337 | } |
AndyA | 1:dd1f7e162f91 | 338 | if (LastTimeMark > outputPosition.time) |
AndyA | 1:dd1f7e162f91 | 339 | outputPosition.time += TimeSinceLastFrameWrap; |
AndyA | 0:97661408d0f9 | 340 | |
AndyA | 1:dd1f7e162f91 | 341 | lastPositions[lastPoint].pos.time = LastTimeMark; |
AndyA | 1:dd1f7e162f91 | 342 | lastPositions[prevPoint].pos.time = PrevTimeMark; |
AndyA | 0:97661408d0f9 | 343 | |
AndyA | 1:dd1f7e162f91 | 344 | // interpolate position to requested time. |
AndyA | 1:dd1f7e162f91 | 345 | if (position::interp(&outputPosition, &(lastPositions[lastPoint].pos), &(lastPositions[prevPoint].pos))) { |
AndyA | 1:dd1f7e162f91 | 346 | printf("G\r\n"); |
AndyA | 1:dd1f7e162f91 | 347 | return &outputPosition; |
AndyA | 1:dd1f7e162f91 | 348 | } |
AndyA | 1:dd1f7e162f91 | 349 | printf(" B\r\n"); |
AndyA | 1:dd1f7e162f91 | 350 | |
AndyA | 1:dd1f7e162f91 | 351 | return NULL; |
AndyA | 0:97661408d0f9 | 352 | } |
AndyA | 0:97661408d0f9 | 353 |