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@16:a8d3a0dbe4bf, 2021-04-30 (annotated)

Committer:
AndyA
Date:
Fri Apr 30 11:26:34 2021 +0000
Revision:
16:a8d3a0dbe4bf
Parent:
14:76083dc18b0d
Child:
18:ad407a2ed4c9
Add support for multiple types of FIZ reader.; Added support for digipower FIZ as used by skycam

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