Racelogic / Mbed 2 deprecated VIPS_LTC_RAW_IMU

Dependencies:   BufferedSerial FatFileSystemCpp mbed

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?

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 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