Racelogic / Mbed 2 deprecated VIPS_LTC_RAW_IMU

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@18:ad407a2ed4c9, 2021-05-17 (annotated)

Committer:
AndyA
Date:
Mon May 17 11:41:57 2021 +0000
Revision:
18:ad407a2ed4c9
Parent:
16:a8d3a0dbe4bf
Child:
22:0dd9c1b5664a
Fix digi-power FIZ not processing packet until after the next byte is received.

Who changed what in which revision?

UserRevisionLine numberNew 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 0:97661408d0f9 14
AndyA 1:dd1f7e162f91 15 VIPSSerial::VIPSSerial(const PinName Tx, const PinName Rx) : _port(Tx,Rx)
AndyA 1:dd1f7e162f91 16 {
AndyA 0:97661408d0f9 17 messagePrt = 0;
AndyA 0:97661408d0f9 18 messageLength = 0;
AndyA 1:dd1f7e162f91 19
AndyA 0:97661408d0f9 20 pointCount = 0;
AndyA 0:97661408d0f9 21 nextPosition = 0;
AndyA 1:dd1f7e162f91 22
AndyA 0:97661408d0f9 23 outputPtr = NULL;
AndyA 0:97661408d0f9 24 statusMessage = 0;
AndyA 1:dd1f7e162f91 25
AndyA 0:97661408d0f9 26 enableAllUpdates = false;
AndyA 0:97661408d0f9 27 newFormatMsg = false;
AndyA 18:ad407a2ed4c9 28
AndyA 18:ad407a2ed4c9 29 queueLen = 0;
AndyA 1:dd1f7e162f91 30
AndyA 16:a8d3a0dbe4bf 31 SmoothBy = 500;
AndyA 16:a8d3a0dbe4bf 32 XSmoothTotal = 0;
AndyA 16:a8d3a0dbe4bf 33 YSmoothTotal = 0;
AndyA 16:a8d3a0dbe4bf 34 ZSmoothTotal = 0;
AndyA 16:a8d3a0dbe4bf 35 SmoothRunning = false;
AndyA 16:a8d3a0dbe4bf 36
AndyA 16:a8d3a0dbe4bf 37 BypassMode = false;
AndyA 16:a8d3a0dbe4bf 38
AndyA 0:97661408d0f9 39 nextPosition= 0;
AndyA 3:14d241e29be3 40 _outputMask = 0x44;
AndyA 1:dd1f7e162f91 41 _port.baud(115200);
AndyA 1:dd1f7e162f91 42 }
AndyA 0:97661408d0f9 43
AndyA 1:dd1f7e162f91 44 void VIPSSerial::run(void)
AndyA 1:dd1f7e162f91 45 {
AndyA 1:dd1f7e162f91 46 _port.attach(callback(this, &VIPSSerial::onSerialRx));
AndyA 1:dd1f7e162f91 47 }
AndyA 0:97661408d0f9 48
AndyA 16:a8d3a0dbe4bf 49 void VIPSSerial::bypassTx(char byte)
AndyA 16:a8d3a0dbe4bf 50 {
AndyA 16:a8d3a0dbe4bf 51 _port.putc(byte);
AndyA 16:a8d3a0dbe4bf 52 }
AndyA 16:a8d3a0dbe4bf 53
AndyA 1:dd1f7e162f91 54 void VIPSSerial::onSerialRx(void)
AndyA 1:dd1f7e162f91 55 {
AndyA 0:97661408d0f9 56 while (_port.readable()) {
AndyA 16:a8d3a0dbe4bf 57 if (BypassMode) {
AndyA 16:a8d3a0dbe4bf 58 vipsBypassRx(_port.getc());
AndyA 16:a8d3a0dbe4bf 59 } else {
AndyA 16:a8d3a0dbe4bf 60 messageInBuffer[messagePrt] = _port.getc();
AndyA 16:a8d3a0dbe4bf 61 if (messagePrt==0) { // look for header
AndyA 16:a8d3a0dbe4bf 62 if ((messageInBuffer[0]==0x2A) || (messageInBuffer[0]==0x24)) {
AndyA 16:a8d3a0dbe4bf 63 messagePrt=1;
AndyA 16:a8d3a0dbe4bf 64 newFormatMsg=(messageInBuffer[0]==0x24);
AndyA 16:a8d3a0dbe4bf 65 }
AndyA 16:a8d3a0dbe4bf 66 } else if (newFormatMsg) {
AndyA 16:a8d3a0dbe4bf 67 if (messagePrt < 4) {
AndyA 16:a8d3a0dbe4bf 68 messagePrt++;
AndyA 16:a8d3a0dbe4bf 69 if (messagePrt==4) {
AndyA 16:a8d3a0dbe4bf 70 if (messageInBuffer[1]!=0xd9)
AndyA 16:a8d3a0dbe4bf 71 messagePrt=0;
AndyA 16:a8d3a0dbe4bf 72 messageLength = *(uint16_t*)(messageInBuffer+2);
AndyA 16:a8d3a0dbe4bf 73 if ((messageLength>115) || (messageLength<34))
AndyA 16:a8d3a0dbe4bf 74 messagePrt = 0;
AndyA 16:a8d3a0dbe4bf 75 }
AndyA 16:a8d3a0dbe4bf 76 } else {
AndyA 16:a8d3a0dbe4bf 77 messagePrt++;
AndyA 16:a8d3a0dbe4bf 78 if (messagePrt == messageLength) {
AndyA 16:a8d3a0dbe4bf 79 parsePostionInput_mocap();
AndyA 0:97661408d0f9 80 messagePrt=0;
AndyA 16:a8d3a0dbe4bf 81 }
AndyA 0:97661408d0f9 82 }
AndyA 0:97661408d0f9 83 } else {
AndyA 16:a8d3a0dbe4bf 84 if (messagePrt==1) {
AndyA 16:a8d3a0dbe4bf 85 if (messageInBuffer[1]<128) { // valid length
AndyA 16:a8d3a0dbe4bf 86 messageLength = messageInBuffer[1];
AndyA 16:a8d3a0dbe4bf 87 messagePrt = 2;
AndyA 16:a8d3a0dbe4bf 88 } else {
AndyA 16:a8d3a0dbe4bf 89 messagePrt=0;
AndyA 16:a8d3a0dbe4bf 90 }
AndyA 16:a8d3a0dbe4bf 91 } else { // in the middle of a message
AndyA 16:a8d3a0dbe4bf 92 messagePrt++;
AndyA 16:a8d3a0dbe4bf 93 if (messagePrt==messageLength) {
AndyA 16:a8d3a0dbe4bf 94 processRxMessage();
AndyA 16:a8d3a0dbe4bf 95 messagePrt=0;
AndyA 16:a8d3a0dbe4bf 96 }
AndyA 0:97661408d0f9 97 }
AndyA 0:97661408d0f9 98 }
AndyA 0:97661408d0f9 99 }
AndyA 0:97661408d0f9 100 }
AndyA 0:97661408d0f9 101 }
AndyA 0:97661408d0f9 102
AndyA 1:dd1f7e162f91 103 void VIPSSerial::processRxMessage()
AndyA 1:dd1f7e162f91 104 {
AndyA 0:97661408d0f9 105 if (!checkCRC(&messageInBuffer[0])) {
AndyA 14:76083dc18b0d 106 pc.puts("VIPS CRC error\r\n");
AndyA 0:97661408d0f9 107 return;
AndyA 0:97661408d0f9 108 }
AndyA 1:dd1f7e162f91 109
AndyA 0:97661408d0f9 110 switch (messageInBuffer[2]) {
AndyA 0:97661408d0f9 111 case _UWBFitted:
AndyA 0:97661408d0f9 112 sendAck(messageInBuffer[2]);
AndyA 0:97661408d0f9 113 break;
AndyA 0:97661408d0f9 114 case _EnableUWB:
AndyA 0:97661408d0f9 115 case _DisableUWB: // for all of these just send an ack.
AndyA 0:97661408d0f9 116 case _SetDeltaTime:
AndyA 0:97661408d0f9 117 sendAck(messageInBuffer[2]);
AndyA 0:97661408d0f9 118 break;
AndyA 0:97661408d0f9 119 case _GetPPSTime:
AndyA 0:97661408d0f9 120 sendVBOXTime();
AndyA 0:97661408d0f9 121 // send vbox tick counter
AndyA 0:97661408d0f9 122 break;
AndyA 0:97661408d0f9 123 case _VBOXReady:
AndyA 0:97661408d0f9 124 if (ppsActive) {
AndyA 0:97661408d0f9 125 sendAck(_VBOXReady);
AndyA 0:97661408d0f9 126 } else {
AndyA 0:97661408d0f9 127 sendNack(_VBOXReady);
AndyA 0:97661408d0f9 128 }
AndyA 0:97661408d0f9 129 break;
AndyA 0:97661408d0f9 130 case _UWBData_Legacy:
AndyA 0:97661408d0f9 131 parsePostionInput_legacy();
AndyA 1:dd1f7e162f91 132 break;
AndyA 0:97661408d0f9 133 default:
AndyA 1:dd1f7e162f91 134 break;
AndyA 1:dd1f7e162f91 135 }
AndyA 0:97661408d0f9 136 }
AndyA 0:97661408d0f9 137
AndyA 0:97661408d0f9 138
AndyA 1:dd1f7e162f91 139 void VIPSSerial::sendVBOXTime()
AndyA 1:dd1f7e162f91 140 {
AndyA 0:97661408d0f9 141 unsigned char timeValue[3];
AndyA 0:97661408d0f9 142 uint32_t timeToSend = VBOXTicks-1; // we track time at next PPS, message requires time at the last PPS.
AndyA 18:ad407a2ed4c9 143 timeValue[0]= timeToSend&0x00ff;
AndyA 0:97661408d0f9 144 timeValue[1]= (timeToSend>>8)&0x00ff;
AndyA 18:ad407a2ed4c9 145 timeValue[2]= (timeToSend>>16)&0x00ff;
AndyA 18:ad407a2ed4c9 146 queueResponse(_GetPPSTime,timeValue,3);
AndyA 7:87aea27cc68b 147 // pc.printf("Sending time = %d ",VBOXTicks);
AndyA 0:97661408d0f9 148 }
AndyA 0:97661408d0f9 149
AndyA 1:dd1f7e162f91 150 void VIPSSerial::sendAck(unsigned char function)
AndyA 1:dd1f7e162f91 151 {
AndyA 0:97661408d0f9 152 unsigned char ack=0x01;
AndyA 0:97661408d0f9 153 sendResponse(function,&ack,1);
AndyA 0:97661408d0f9 154 }
AndyA 0:97661408d0f9 155
AndyA 1:dd1f7e162f91 156 void VIPSSerial::sendNack(unsigned char function)
AndyA 1:dd1f7e162f91 157 {
AndyA 1:dd1f7e162f91 158 unsigned char nack=0x00;
AndyA 0:97661408d0f9 159 sendResponse(function,&nack,1);
AndyA 0:97661408d0f9 160 }
AndyA 0:97661408d0f9 161
AndyA 1:dd1f7e162f91 162 void VIPSSerial::sendResponse(unsigned char function, unsigned char* data, int dataLen)
AndyA 1:dd1f7e162f91 163 {
AndyA 1:dd1f7e162f91 164 messageOutBuffer[0]=0xff;
AndyA 1:dd1f7e162f91 165 messageOutBuffer[1]=dataLen+4;
AndyA 1:dd1f7e162f91 166 messageOutBuffer[2]=function;
AndyA 1:dd1f7e162f91 167 for (int i=0; i<dataLen; i++)
AndyA 1:dd1f7e162f91 168 messageOutBuffer[i+3] = data[i];
AndyA 1:dd1f7e162f91 169 VIPSSerial::getCRC(messageOutBuffer,dataLen+3,messageOutBuffer+dataLen+3);
AndyA 3:14d241e29be3 170 for (int i=0; i<dataLen+5; i++)
AndyA 0:97661408d0f9 171 _port.putc(messageOutBuffer[i]);
AndyA 0:97661408d0f9 172 }
AndyA 18:ad407a2ed4c9 173 void VIPSSerial::queueResponse(unsigned char function, unsigned char* data, int dataLen)
AndyA 18:ad407a2ed4c9 174 {
AndyA 18:ad407a2ed4c9 175 messageOutBuffer[0]=0xff;
AndyA 18:ad407a2ed4c9 176 messageOutBuffer[1]=dataLen+4;
AndyA 18:ad407a2ed4c9 177 messageOutBuffer[2]=function;
AndyA 18:ad407a2ed4c9 178 for (int i=0; i<dataLen; i++)
AndyA 18:ad407a2ed4c9 179 messageOutBuffer[i+3] = data[i];
AndyA 18:ad407a2ed4c9 180 VIPSSerial::getCRC(messageOutBuffer,dataLen+3,messageOutBuffer+dataLen+3);
AndyA 18:ad407a2ed4c9 181 queueLen = dataLen+5;
AndyA 18:ad407a2ed4c9 182 }
AndyA 18:ad407a2ed4c9 183
AndyA 18:ad407a2ed4c9 184 void VIPSSerial::sendQueued()
AndyA 18:ad407a2ed4c9 185 {
AndyA 18:ad407a2ed4c9 186 if (queueLen) {
AndyA 18:ad407a2ed4c9 187 for (int i=0; i<queueLen; i++)
AndyA 18:ad407a2ed4c9 188 _port.putc(messageOutBuffer[i]);
AndyA 18:ad407a2ed4c9 189 queueLen = 0;
AndyA 18:ad407a2ed4c9 190 }
AndyA 18:ad407a2ed4c9 191 }
AndyA 0:97661408d0f9 192
AndyA 1:dd1f7e162f91 193 void VIPSSerial::getCRC(void *data, int len, void *checksum)
AndyA 1:dd1f7e162f91 194 {
AndyA 1:dd1f7e162f91 195 uint8_t *dataPtr = (uint8_t *)data;
AndyA 3:14d241e29be3 196 uint8_t *crcPtr = (uint8_t *)checksum;
AndyA 0:97661408d0f9 197 uint16_t crc = 0x0;
AndyA 0:97661408d0f9 198 unsigned char x;
AndyA 0:97661408d0f9 199 int byteCount = 0;
AndyA 0:97661408d0f9 200
AndyA 0:97661408d0f9 201 while ((len--) > 0) {
AndyA 1:dd1f7e162f91 202 x = (unsigned char)(crc >> 8 ^ dataPtr[byteCount++]);
AndyA 0:97661408d0f9 203 x ^= (unsigned char)(x >> 4);
AndyA 0:97661408d0f9 204 crc = (uint16_t)((crc << 8) ^ (x << 12) ^ (x << 5) ^ x);
AndyA 0:97661408d0f9 205 }
AndyA 1:dd1f7e162f91 206 crcPtr[0] = crc >> 8;
AndyA 1:dd1f7e162f91 207 crcPtr[1] = crc &0x00ff;
AndyA 0:97661408d0f9 208 }
AndyA 0:97661408d0f9 209
AndyA 1:dd1f7e162f91 210 bool VIPSSerial::checkCRC(unsigned char* data)
AndyA 1:dd1f7e162f91 211 {
AndyA 0:97661408d0f9 212 unsigned char expectedCRC[2];
AndyA 0:97661408d0f9 213 int length = data[1];
AndyA 0:97661408d0f9 214 if (data[0] == 0xff) // for response length doesn't include the header
AndyA 0:97661408d0f9 215 length++;
AndyA 1:dd1f7e162f91 216 VIPSSerial::getCRC(data, length-2, expectedCRC);
AndyA 0:97661408d0f9 217 if ((data[length-2]==expectedCRC[0]) && (data[length-1]==expectedCRC[1]))
AndyA 0:97661408d0f9 218 return true;
AndyA 0:97661408d0f9 219 return false;
AndyA 0:97661408d0f9 220 }
AndyA 0:97661408d0f9 221
AndyA 0:97661408d0f9 222
AndyA 1:dd1f7e162f91 223 bool VIPSSerial::checkNewPacketRC(unsigned char* data)
AndyA 1:dd1f7e162f91 224 {
AndyA 0:97661408d0f9 225 unsigned char expectedCRC[2];
AndyA 0:97661408d0f9 226 int length = data[2] | (((int)data[3])<<8);
AndyA 1:dd1f7e162f91 227 VIPSSerial::getCRC(data, length-2, expectedCRC);
AndyA 0:97661408d0f9 228 if ((data[length-2]==expectedCRC[0]) && (data[length-1]==expectedCRC[1]))
AndyA 0:97661408d0f9 229 return true;
AndyA 0:97661408d0f9 230 return false;
AndyA 0:97661408d0f9 231 }
AndyA 0:97661408d0f9 232
AndyA 0:97661408d0f9 233
AndyA 1:dd1f7e162f91 234 void VIPSSerial::parsePostionInput_legacy()
AndyA 1:dd1f7e162f91 235 {
AndyA 0:97661408d0f9 236 printf("L");
AndyA 0:97661408d0f9 237 uint8_t tmpBuffer[8];
AndyA 0:97661408d0f9 238 int32_t tmpInt;
AndyA 0:97661408d0f9 239 memcpy((uint8_t *)(&tmpInt)+1, messageInBuffer+4, 3);
AndyA 0:97661408d0f9 240 tmpInt &= 0x00ffffff;
AndyA 0:97661408d0f9 241 lastPositions[nextPosition].pos.time = tmpInt*10;
AndyA 0:97661408d0f9 242 memcpy(tmpBuffer, messageInBuffer+7, 8);
AndyA 0:97661408d0f9 243 lastPositions[nextPosition].pos.X = *(double *)(tmpBuffer);
AndyA 0:97661408d0f9 244 memcpy(tmpBuffer, messageInBuffer+15, 8);
AndyA 0:97661408d0f9 245 lastPositions[nextPosition].pos.Y = *(double *)(tmpBuffer);
AndyA 0:97661408d0f9 246 memcpy((uint8_t *)(&tmpInt)+1, messageInBuffer+27, 3);
AndyA 0:97661408d0f9 247 if (tmpInt & 0x00800000)
AndyA 0:97661408d0f9 248 tmpInt |= 0xff000000;
AndyA 0:97661408d0f9 249 lastPositions[nextPosition].pos.Height = tmpInt/100.0f;
AndyA 0:97661408d0f9 250 lastPositions[nextPosition].pos.roll = 0;
AndyA 0:97661408d0f9 251 lastPositions[nextPosition].pos.pitch = 0;
AndyA 0:97661408d0f9 252 lastPositions[nextPosition].pos.yaw = 0;
AndyA 0:97661408d0f9 253
AndyA 0:97661408d0f9 254 if (enableAllUpdates) {
AndyA 1:dd1f7e162f91 255 printf("Add pos\r\n");
AndyA 1:dd1f7e162f91 256 outputPtr = &outputPosition;
AndyA 1:dd1f7e162f91 257 memcpy(outputPtr,&(lastPositions[nextPosition].pos),sizeof(position));
AndyA 0:97661408d0f9 258 }
AndyA 0:97661408d0f9 259
AndyA 0:97661408d0f9 260 nextPosition++;
AndyA 0:97661408d0f9 261 if (nextPosition == posHistoryLen) {
AndyA 0:97661408d0f9 262 nextPosition = 0;
AndyA 0:97661408d0f9 263 }
AndyA 0:97661408d0f9 264 pointCount++;
AndyA 0:97661408d0f9 265 }
AndyA 0:97661408d0f9 266
AndyA 1:dd1f7e162f91 267 void VIPSSerial::parsePostionInput_mocap()
AndyA 1:dd1f7e162f91 268 {
AndyA 0:97661408d0f9 269 if (!checkNewPacketRC(&messageInBuffer[0])) {
AndyA 0:97661408d0f9 270 pc.write("CRC error\r\n",11);
AndyA 0:97661408d0f9 271 return;
AndyA 0:97661408d0f9 272 }
AndyA 14:76083dc18b0d 273
AndyA 16:a8d3a0dbe4bf 274 // led1=!led1;
AndyA 14:76083dc18b0d 275
AndyA 0:97661408d0f9 276 lastPositions[nextPosition].time = TimeSinceLastFrame.read_us();
AndyA 0:97661408d0f9 277 uint32_t mask = *(uint32_t*)(messageInBuffer+4);
AndyA 0:97661408d0f9 278 int offset = 32; // end of position
AndyA 1:dd1f7e162f91 279
AndyA 0:97661408d0f9 280 lastPositions[nextPosition].pos.time = *(uint32_t*)(messageInBuffer+8);
AndyA 0:97661408d0f9 281 lastPositions[nextPosition].pos.X = *(double *)(messageInBuffer+12);
AndyA 0:97661408d0f9 282 lastPositions[nextPosition].pos.Y = *(double *)(messageInBuffer+20);
AndyA 0:97661408d0f9 283 lastPositions[nextPosition].pos.Height = *(float *)(messageInBuffer+28);
AndyA 10:053bac3e326b 284
AndyA 0:97661408d0f9 285 if (mask & 0x0002) { // parse status
AndyA 8:961bb15570a1 286 lastPositions[nextPosition].pos.beacons = messageInBuffer[offset++];
AndyA 8:961bb15570a1 287 lastPositions[nextPosition].pos.solutionType = messageInBuffer[offset++];
AndyA 8:961bb15570a1 288 lastPositions[nextPosition].pos.KFStatus = *(uint16_t*)(messageInBuffer + offset);
AndyA 8:961bb15570a1 289 offset +=2;
AndyA 0:97661408d0f9 290 }
AndyA 0:97661408d0f9 291
AndyA 0:97661408d0f9 292 if (mask & 0x0004) {
AndyA 0:97661408d0f9 293 lastPositions[nextPosition].pos.roll = *(float *)(messageInBuffer+offset);
AndyA 0:97661408d0f9 294 lastPositions[nextPosition].pos.pitch = *(float *)(messageInBuffer+offset+4);
AndyA 0:97661408d0f9 295 lastPositions[nextPosition].pos.yaw = *(float *)(messageInBuffer+offset+8);
AndyA 0:97661408d0f9 296 offset+=12;
AndyA 0:97661408d0f9 297 } else {
AndyA 0:97661408d0f9 298 lastPositions[nextPosition].pos.roll = 0;
AndyA 0:97661408d0f9 299 lastPositions[nextPosition].pos.pitch = 0;
AndyA 0:97661408d0f9 300 lastPositions[nextPosition].pos.yaw = 0;
AndyA 0:97661408d0f9 301 }
AndyA 0:97661408d0f9 302
AndyA 0:97661408d0f9 303 if (mask & 0x0008) { // velocity
AndyA 0:97661408d0f9 304
AndyA 0:97661408d0f9 305 offset+=8;
AndyA 1:dd1f7e162f91 306 }
AndyA 0:97661408d0f9 307 if (mask & 0x0010) {// vert velocity
AndyA 0:97661408d0f9 308
AndyA 1:dd1f7e162f91 309 offset+=4;
AndyA 0:97661408d0f9 310 }
AndyA 0:97661408d0f9 311 if (mask & 0x0020) { // pos uncertainty
AndyA 0:97661408d0f9 312
AndyA 1:dd1f7e162f91 313 offset+=12;
AndyA 0:97661408d0f9 314 if (mask & 0x0004) { // orientation uncertainty
AndyA 1:dd1f7e162f91 315 offset += 12;
AndyA 0:97661408d0f9 316 }
AndyA 0:97661408d0f9 317 if (mask & 0x0008) { // velocity uncertainty
AndyA 1:dd1f7e162f91 318 offset += 12;
AndyA 0:97661408d0f9 319 }
AndyA 0:97661408d0f9 320 }
AndyA 0:97661408d0f9 321 if (mask & 0x0040) { // accuracy
AndyA 2:a79201e302d7 322 lastPositions[nextPosition].pos.ID = *(messageInBuffer+offset+3);
AndyA 0:97661408d0f9 323 offset+=4;
AndyA 1:dd1f7e162f91 324 }
AndyA 1:dd1f7e162f91 325 if (mask & 0x0080) { // raw UWB
AndyA 0:97661408d0f9 326
AndyA 0:97661408d0f9 327 offset+=24;
AndyA 0:97661408d0f9 328 }
AndyA 1:dd1f7e162f91 329 if (mask & 0x0100) { // raw IMU
AndyA 0:97661408d0f9 330
AndyA 0:97661408d0f9 331 offset+=24;
AndyA 0:97661408d0f9 332 }
AndyA 0:97661408d0f9 333
AndyA 0:97661408d0f9 334 if (mask & 0x0200) {// rover info
AndyA 0:97661408d0f9 335
AndyA 1:dd1f7e162f91 336 offset+=4;
AndyA 0:97661408d0f9 337 }
AndyA 3:14d241e29be3 338 if (mask & 0x0400) {// FIZ data
AndyA 3:14d241e29be3 339
AndyA 3:14d241e29be3 340 offset+=4;
AndyA 3:14d241e29be3 341 }
AndyA 0:97661408d0f9 342
AndyA 16:a8d3a0dbe4bf 343 smoothOutputPacket(&(lastPositions[nextPosition].pos));
AndyA 16:a8d3a0dbe4bf 344
AndyA 0:97661408d0f9 345 if (enableAllUpdates) {
AndyA 1:dd1f7e162f91 346 // printf("Add pos\r\n");
AndyA 1:dd1f7e162f91 347 outputPtr = &outputPosition;
AndyA 1:dd1f7e162f91 348 memcpy(outputPtr,&(lastPositions[nextPosition].pos),sizeof(position));
AndyA 0:97661408d0f9 349 }
AndyA 0:97661408d0f9 350
AndyA 0:97661408d0f9 351 nextPosition++;
AndyA 0:97661408d0f9 352 if (nextPosition == posHistoryLen) {
AndyA 0:97661408d0f9 353 nextPosition = 0;
AndyA 0:97661408d0f9 354 }
AndyA 0:97661408d0f9 355 pointCount++;
AndyA 0:97661408d0f9 356 }
AndyA 0:97661408d0f9 357
AndyA 16:a8d3a0dbe4bf 358
AndyA 1:dd1f7e162f91 359 // send a position output for the requested time. Times are based on the global TimeSinceLastFrame timer.
AndyA 1:dd1f7e162f91 360 position* VIPSSerial::sendPositionForTime(uint32_t timeValue)
AndyA 1:dd1f7e162f91 361 {
AndyA 0:97661408d0f9 362 static uint32_t lastPoints = 0;
AndyA 0:97661408d0f9 363 if (pointCount < 2)
AndyA 0:97661408d0f9 364 return NULL;
AndyA 13:c2d9e975841b 365
AndyA 13:c2d9e975841b 366 __disable_irq(); // disable IRQ and make a copy of the data we're going to interpolate.
AndyA 1:dd1f7e162f91 367 int lastPoint = nextPosition - 1;
AndyA 1:dd1f7e162f91 368 int prevPoint = nextPosition - 2;
AndyA 1:dd1f7e162f91 369 if (lastPoint<0)
AndyA 1:dd1f7e162f91 370 lastPoint+=posHistoryLen;
AndyA 1:dd1f7e162f91 371 if (prevPoint<0)
AndyA 1:dd1f7e162f91 372 prevPoint+=posHistoryLen;
AndyA 0:97661408d0f9 373
AndyA 13:c2d9e975841b 374 memcpy(&lastPos,&lastPositions[lastPoint], sizeof(struct posAndTime_s));
AndyA 13:c2d9e975841b 375 memcpy(&prevPos,&lastPositions[prevPoint], sizeof(struct posAndTime_s));
AndyA 13:c2d9e975841b 376 __enable_irq();
AndyA 13:c2d9e975841b 377
AndyA 1:dd1f7e162f91 378 // calculate timestamps as a function of time since last frame
AndyA 8:961bb15570a1 379
AndyA 13:c2d9e975841b 380 uint64_t LastTimeMark = lastPos.time;
AndyA 13:c2d9e975841b 381 uint64_t PrevTimeMark = prevPos.time;
AndyA 10:053bac3e326b 382 uint64_t timeValueUnwrap = timeValue;
AndyA 10:053bac3e326b 383 if (PrevTimeMark > LastTimeMark)
AndyA 10:053bac3e326b 384 LastTimeMark += ((uint64_t)1)<<32;
AndyA 10:053bac3e326b 385 if (LastTimeMark > timeValueUnwrap)
AndyA 10:053bac3e326b 386 timeValueUnwrap += ((uint64_t)1)<<32;
AndyA 0:97661408d0f9 387
AndyA 13:c2d9e975841b 388 outputPosition.time = timeValueUnwrap-PrevTimeMark; // should be between 10,000 and 20,000
AndyA 10:053bac3e326b 389
AndyA 13:c2d9e975841b 390 // interpolate uses the position times. Replace them with the internal clock counts.
AndyA 13:c2d9e975841b 391 lastPos.pos.time = LastTimeMark-PrevTimeMark; // should be very close to 10,000
AndyA 13:c2d9e975841b 392 prevPos.pos.time = 0;
AndyA 0:97661408d0f9 393
AndyA 1:dd1f7e162f91 394 // interpolate position to requested time.
AndyA 13:c2d9e975841b 395 if (position::interp(&outputPosition, &(lastPos.pos), &(prevPos.pos))) {
AndyA 1:dd1f7e162f91 396 return &outputPosition;
AndyA 1:dd1f7e162f91 397 }
AndyA 14:76083dc18b0d 398
AndyA 14:76083dc18b0d 399 // interpolation failed. Return most recent location
AndyA 14:76083dc18b0d 400 return &lastPos.pos;
AndyA 0:97661408d0f9 401 }
AndyA 0:97661408d0f9 402
AndyA 16:a8d3a0dbe4bf 403 void VIPSSerial::smoothOutputPacket(position *posPtr)
AndyA 16:a8d3a0dbe4bf 404 {
AndyA 16:a8d3a0dbe4bf 405 if (hyperSmoothEnabled) {
AndyA 16:a8d3a0dbe4bf 406 if (!SmoothRunning) {
AndyA 16:a8d3a0dbe4bf 407 XSmoothTotal = posPtr->X * (SmoothBy - 1);
AndyA 16:a8d3a0dbe4bf 408 YSmoothTotal = posPtr->Y * (SmoothBy - 1);
AndyA 16:a8d3a0dbe4bf 409 YSmoothTotal = posPtr->Height * (SmoothBy - 1);
AndyA 16:a8d3a0dbe4bf 410 SmoothRunning = true;
AndyA 16:a8d3a0dbe4bf 411 }
AndyA 16:a8d3a0dbe4bf 412 //smooth the KF_X and KF_Y positions
AndyA 16:a8d3a0dbe4bf 413 XSmoothTotal += posPtr->X;
AndyA 16:a8d3a0dbe4bf 414 posPtr->X = XSmoothTotal / SmoothBy;
AndyA 16:a8d3a0dbe4bf 415 XSmoothTotal -= posPtr->X;
AndyA 16:a8d3a0dbe4bf 416
AndyA 16:a8d3a0dbe4bf 417 YSmoothTotal += posPtr->Y;
AndyA 16:a8d3a0dbe4bf 418 posPtr->Y = YSmoothTotal / SmoothBy;
AndyA 16:a8d3a0dbe4bf 419 YSmoothTotal -= posPtr->Y;
AndyA 16:a8d3a0dbe4bf 420
AndyA 16:a8d3a0dbe4bf 421 ZSmoothTotal += posPtr->Height;
AndyA 16:a8d3a0dbe4bf 422 posPtr->Height = ZSmoothTotal / SmoothBy;
AndyA 16:a8d3a0dbe4bf 423 ZSmoothTotal -= posPtr->Height;
AndyA 16:a8d3a0dbe4bf 424 } else {
AndyA 16:a8d3a0dbe4bf 425 SmoothRunning = false;
AndyA 16:a8d3a0dbe4bf 426 }
AndyA 16:a8d3a0dbe4bf 427 }