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@13:c2d9e975841b, 2021-02-22 (annotated)

Committer:
AndyA
Date:
Mon Feb 22 10:44:27 2021 +0000
Revision:
13:c2d9e975841b
Parent:
10:053bac3e326b
Child:
14:76083dc18b0d
Copy vips data before interpolating to avoid it changing.

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