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@0:97661408d0f9, 2021-01-15 (annotated)

Committer:
AndyA
Date:
Fri Jan 15 11:49:01 2021 +0000
Revision:
0:97661408d0f9
Child:
1:dd1f7e162f91
first;

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