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@14:76083dc18b0d, 2021-02-25 (annotated)

Committer:
AndyA
Date:
Thu Feb 25 17:54:04 2021 +0000
Revision:
14:76083dc18b0d
Parent:
13:c2d9e975841b
Child:
16:a8d3a0dbe4bf
Disabled USB

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