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@69:47f800793d00, 2022-02-03 (annotated)

Committer:
JamieB
Date:
Thu Feb 03 11:50:12 2022 +0000
Revision:
69:47f800793d00
Parent:
66:066b16c6c34f
Child:
72:da2f1df6a50d
v24, Added TCP upload of VIPS configuration to Rover

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
JamieB 69:47f800793d00 14 #define IdleTxBuffer (txBuf == TXBuffer1)?TXBuffer2:TXBuffer1
JamieB 69:47f800793d00 15 #define ActiveTxBuffer (txBuf == TXBuffer1)?TXBuffer1:TXBuffer2
JamieB 69:47f800793d00 16
AndyA 0:97661408d0f9 17
AndyA 1:dd1f7e162f91 18 VIPSSerial::VIPSSerial(const PinName Tx, const PinName Rx) : _port(Tx,Rx)
AndyA 1:dd1f7e162f91 19 {
AndyA 0:97661408d0f9 20 messagePrt = 0;
AndyA 0:97661408d0f9 21 messageLength = 0;
AndyA 1:dd1f7e162f91 22
AndyA 0:97661408d0f9 23 pointCount = 0;
AndyA 0:97661408d0f9 24 nextPosition = 0;
AndyA 1:dd1f7e162f91 25
AndyA 0:97661408d0f9 26 outputPtr = NULL;
AndyA 0:97661408d0f9 27 statusMessage = 0;
AndyA 1:dd1f7e162f91 28
AndyA 0:97661408d0f9 29 enableAllUpdates = false;
AndyA 0:97661408d0f9 30 newFormatMsg = false;
JamieB 28:7600b18ccb7a 31
AndyA 18:ad407a2ed4c9 32 queueLen = 0;
AndyA 1:dd1f7e162f91 33
JamieB 66:066b16c6c34f 34 SmoothBy = 100; //was 500
AndyA 16:a8d3a0dbe4bf 35 XSmoothTotal = 0;
AndyA 16:a8d3a0dbe4bf 36 YSmoothTotal = 0;
AndyA 16:a8d3a0dbe4bf 37 ZSmoothTotal = 0;
AndyA 16:a8d3a0dbe4bf 38 SmoothRunning = false;
AndyA 16:a8d3a0dbe4bf 39
AndyA 16:a8d3a0dbe4bf 40 BypassMode = false;
JamieB 69:47f800793d00 41 directTx = false;
JamieB 69:47f800793d00 42 txBuf = TXBuffer1;
JamieB 69:47f800793d00 43 waitingBytes = 0;
AndyA 16:a8d3a0dbe4bf 44
AndyA 0:97661408d0f9 45 nextPosition= 0;
AndyA 3:14d241e29be3 46 _outputMask = 0x44;
AndyA 1:dd1f7e162f91 47 _port.baud(115200);
AndyA 1:dd1f7e162f91 48 }
AndyA 0:97661408d0f9 49
AndyA 1:dd1f7e162f91 50 void VIPSSerial::run(void)
AndyA 1:dd1f7e162f91 51 {
AndyA 1:dd1f7e162f91 52 _port.attach(callback(this, &VIPSSerial::onSerialRx));
AndyA 1:dd1f7e162f91 53 }
AndyA 0:97661408d0f9 54
AndyA 16:a8d3a0dbe4bf 55 void VIPSSerial::bypassTx(char byte)
AndyA 16:a8d3a0dbe4bf 56 {
AndyA 16:a8d3a0dbe4bf 57 _port.putc(byte);
AndyA 16:a8d3a0dbe4bf 58 }
AndyA 16:a8d3a0dbe4bf 59
AndyA 1:dd1f7e162f91 60 void VIPSSerial::onSerialRx(void)
AndyA 1:dd1f7e162f91 61 {
AndyA 0:97661408d0f9 62 while (_port.readable()) {
AndyA 16:a8d3a0dbe4bf 63 if (BypassMode) {
AndyA 16:a8d3a0dbe4bf 64 vipsBypassRx(_port.getc());
JamieB 69:47f800793d00 65 } else if (directTx) {
JamieB 69:47f800793d00 66 if (waitingBytes < MaxBuffSize)
JamieB 69:47f800793d00 67 txBuf[waitingBytes++] = _port.getc();
AndyA 16:a8d3a0dbe4bf 68 } else {
AndyA 16:a8d3a0dbe4bf 69 messageInBuffer[messagePrt] = _port.getc();
AndyA 16:a8d3a0dbe4bf 70 if (messagePrt==0) { // look for header
AndyA 16:a8d3a0dbe4bf 71 if ((messageInBuffer[0]==0x2A) || (messageInBuffer[0]==0x24)) {
AndyA 16:a8d3a0dbe4bf 72 messagePrt=1;
AndyA 16:a8d3a0dbe4bf 73 newFormatMsg=(messageInBuffer[0]==0x24);
AndyA 16:a8d3a0dbe4bf 74 }
AndyA 16:a8d3a0dbe4bf 75 } else if (newFormatMsg) {
AndyA 16:a8d3a0dbe4bf 76 if (messagePrt < 4) {
AndyA 16:a8d3a0dbe4bf 77 messagePrt++;
AndyA 16:a8d3a0dbe4bf 78 if (messagePrt==4) {
AndyA 16:a8d3a0dbe4bf 79 if (messageInBuffer[1]!=0xd9)
AndyA 16:a8d3a0dbe4bf 80 messagePrt=0;
AndyA 16:a8d3a0dbe4bf 81 messageLength = *(uint16_t*)(messageInBuffer+2);
AndyA 16:a8d3a0dbe4bf 82 if ((messageLength>115) || (messageLength<34))
AndyA 16:a8d3a0dbe4bf 83 messagePrt = 0;
AndyA 16:a8d3a0dbe4bf 84 }
AndyA 16:a8d3a0dbe4bf 85 } else {
AndyA 16:a8d3a0dbe4bf 86 messagePrt++;
AndyA 16:a8d3a0dbe4bf 87 if (messagePrt == messageLength) {
AndyA 16:a8d3a0dbe4bf 88 parsePostionInput_mocap();
AndyA 0:97661408d0f9 89 messagePrt=0;
AndyA 16:a8d3a0dbe4bf 90 }
AndyA 0:97661408d0f9 91 }
AndyA 0:97661408d0f9 92 } else {
AndyA 16:a8d3a0dbe4bf 93 if (messagePrt==1) {
AndyA 16:a8d3a0dbe4bf 94 if (messageInBuffer[1]<128) { // valid length
AndyA 16:a8d3a0dbe4bf 95 messageLength = messageInBuffer[1];
AndyA 16:a8d3a0dbe4bf 96 messagePrt = 2;
AndyA 16:a8d3a0dbe4bf 97 } else {
AndyA 16:a8d3a0dbe4bf 98 messagePrt=0;
AndyA 16:a8d3a0dbe4bf 99 }
AndyA 16:a8d3a0dbe4bf 100 } else { // in the middle of a message
AndyA 16:a8d3a0dbe4bf 101 messagePrt++;
AndyA 16:a8d3a0dbe4bf 102 if (messagePrt==messageLength) {
AndyA 16:a8d3a0dbe4bf 103 processRxMessage();
AndyA 16:a8d3a0dbe4bf 104 messagePrt=0;
AndyA 16:a8d3a0dbe4bf 105 }
AndyA 0:97661408d0f9 106 }
AndyA 0:97661408d0f9 107 }
AndyA 0:97661408d0f9 108 }
AndyA 0:97661408d0f9 109 }
AndyA 0:97661408d0f9 110 }
AndyA 0:97661408d0f9 111
AndyA 1:dd1f7e162f91 112 void VIPSSerial::processRxMessage()
AndyA 1:dd1f7e162f91 113 {
AndyA 0:97661408d0f9 114 if (!checkCRC(&messageInBuffer[0])) {
AndyA 14:76083dc18b0d 115 pc.puts("VIPS CRC error\r\n");
AndyA 0:97661408d0f9 116 return;
AndyA 0:97661408d0f9 117 }
AndyA 1:dd1f7e162f91 118
AndyA 0:97661408d0f9 119 switch (messageInBuffer[2]) {
AndyA 0:97661408d0f9 120 case _UWBFitted:
AndyA 0:97661408d0f9 121 sendAck(messageInBuffer[2]);
AndyA 0:97661408d0f9 122 break;
AndyA 0:97661408d0f9 123 case _EnableUWB:
AndyA 0:97661408d0f9 124 case _DisableUWB: // for all of these just send an ack.
AndyA 0:97661408d0f9 125 case _SetDeltaTime:
AndyA 0:97661408d0f9 126 sendAck(messageInBuffer[2]);
AndyA 0:97661408d0f9 127 break;
AndyA 0:97661408d0f9 128 case _GetPPSTime:
AndyA 0:97661408d0f9 129 sendVBOXTime();
AndyA 0:97661408d0f9 130 // send vbox tick counter
AndyA 0:97661408d0f9 131 break;
AndyA 0:97661408d0f9 132 case _VBOXReady:
AndyA 0:97661408d0f9 133 if (ppsActive) {
AndyA 0:97661408d0f9 134 sendAck(_VBOXReady);
AndyA 0:97661408d0f9 135 } else {
AndyA 0:97661408d0f9 136 sendNack(_VBOXReady);
AndyA 0:97661408d0f9 137 }
AndyA 0:97661408d0f9 138 break;
AndyA 0:97661408d0f9 139 case _UWBData_Legacy:
AndyA 0:97661408d0f9 140 parsePostionInput_legacy();
AndyA 1:dd1f7e162f91 141 break;
AndyA 0:97661408d0f9 142 default:
AndyA 1:dd1f7e162f91 143 break;
AndyA 1:dd1f7e162f91 144 }
AndyA 0:97661408d0f9 145 }
AndyA 0:97661408d0f9 146
AndyA 0:97661408d0f9 147
AndyA 1:dd1f7e162f91 148 void VIPSSerial::sendVBOXTime()
AndyA 1:dd1f7e162f91 149 {
AndyA 0:97661408d0f9 150 unsigned char timeValue[3];
AndyA 0:97661408d0f9 151 uint32_t timeToSend = VBOXTicks-1; // we track time at next PPS, message requires time at the last PPS.
AndyA 18:ad407a2ed4c9 152 timeValue[0]= timeToSend&0x00ff;
AndyA 0:97661408d0f9 153 timeValue[1]= (timeToSend>>8)&0x00ff;
AndyA 18:ad407a2ed4c9 154 timeValue[2]= (timeToSend>>16)&0x00ff;
AndyA 18:ad407a2ed4c9 155 queueResponse(_GetPPSTime,timeValue,3);
AndyA 7:87aea27cc68b 156 // pc.printf("Sending time = %d ",VBOXTicks);
AndyA 0:97661408d0f9 157 }
AndyA 0:97661408d0f9 158
AndyA 1:dd1f7e162f91 159 void VIPSSerial::sendAck(unsigned char function)
AndyA 1:dd1f7e162f91 160 {
AndyA 0:97661408d0f9 161 unsigned char ack=0x01;
AndyA 0:97661408d0f9 162 sendResponse(function,&ack,1);
AndyA 0:97661408d0f9 163 }
AndyA 0:97661408d0f9 164
AndyA 1:dd1f7e162f91 165 void VIPSSerial::sendNack(unsigned char function)
AndyA 1:dd1f7e162f91 166 {
AndyA 1:dd1f7e162f91 167 unsigned char nack=0x00;
AndyA 0:97661408d0f9 168 sendResponse(function,&nack,1);
AndyA 0:97661408d0f9 169 }
AndyA 0:97661408d0f9 170
AndyA 1:dd1f7e162f91 171 void VIPSSerial::sendResponse(unsigned char function, unsigned char* data, int dataLen)
AndyA 1:dd1f7e162f91 172 {
AndyA 1:dd1f7e162f91 173 messageOutBuffer[0]=0xff;
AndyA 1:dd1f7e162f91 174 messageOutBuffer[1]=dataLen+4;
AndyA 1:dd1f7e162f91 175 messageOutBuffer[2]=function;
AndyA 1:dd1f7e162f91 176 for (int i=0; i<dataLen; i++)
AndyA 1:dd1f7e162f91 177 messageOutBuffer[i+3] = data[i];
AndyA 1:dd1f7e162f91 178 VIPSSerial::getCRC(messageOutBuffer,dataLen+3,messageOutBuffer+dataLen+3);
AndyA 3:14d241e29be3 179 for (int i=0; i<dataLen+5; i++)
AndyA 0:97661408d0f9 180 _port.putc(messageOutBuffer[i]);
AndyA 0:97661408d0f9 181 }
AndyA 18:ad407a2ed4c9 182 void VIPSSerial::queueResponse(unsigned char function, unsigned char* data, int dataLen)
AndyA 18:ad407a2ed4c9 183 {
AndyA 18:ad407a2ed4c9 184 messageOutBuffer[0]=0xff;
AndyA 18:ad407a2ed4c9 185 messageOutBuffer[1]=dataLen+4;
AndyA 18:ad407a2ed4c9 186 messageOutBuffer[2]=function;
AndyA 18:ad407a2ed4c9 187 for (int i=0; i<dataLen; i++)
AndyA 18:ad407a2ed4c9 188 messageOutBuffer[i+3] = data[i];
AndyA 18:ad407a2ed4c9 189 VIPSSerial::getCRC(messageOutBuffer,dataLen+3,messageOutBuffer+dataLen+3);
AndyA 18:ad407a2ed4c9 190 queueLen = dataLen+5;
AndyA 18:ad407a2ed4c9 191 }
AndyA 18:ad407a2ed4c9 192
AndyA 18:ad407a2ed4c9 193 void VIPSSerial::sendQueued()
AndyA 18:ad407a2ed4c9 194 {
AndyA 18:ad407a2ed4c9 195 if (queueLen) {
AndyA 18:ad407a2ed4c9 196 for (int i=0; i<queueLen; i++)
AndyA 18:ad407a2ed4c9 197 _port.putc(messageOutBuffer[i]);
AndyA 18:ad407a2ed4c9 198 queueLen = 0;
AndyA 18:ad407a2ed4c9 199 }
AndyA 18:ad407a2ed4c9 200 }
JamieB 69:47f800793d00 201 void VIPSSerial::sendDirectTX(unsigned char* data, int dataLen)
JamieB 69:47f800793d00 202 {
JamieB 69:47f800793d00 203 for (int i=0; i<dataLen; i++)
JamieB 69:47f800793d00 204 _port.putc(data[i]);
JamieB 69:47f800793d00 205 }
JamieB 69:47f800793d00 206
JamieB 69:47f800793d00 207 void VIPSSerial::sendQuiet()
JamieB 69:47f800793d00 208 {
JamieB 69:47f800793d00 209 _port.putc(0x07);
JamieB 69:47f800793d00 210 _port.putc(0x05);
JamieB 69:47f800793d00 211 _port.putc(0x02);
JamieB 69:47f800793d00 212 _port.putc(0x5A);
JamieB 69:47f800793d00 213 _port.putc(0x27);
JamieB 69:47f800793d00 214 }
JamieB 69:47f800793d00 215
JamieB 69:47f800793d00 216 int VIPSSerial::getWaitingBuffer(unsigned char **TXBuffer, int *bytesToSend)
JamieB 69:47f800793d00 217 {
JamieB 69:47f800793d00 218 __disable_irq();
JamieB 69:47f800793d00 219 int bytes = waitingBytes;
JamieB 69:47f800793d00 220 txBuf = IdleTxBuffer;
JamieB 69:47f800793d00 221 waitingBytes = 0;
JamieB 69:47f800793d00 222 __enable_irq();
JamieB 69:47f800793d00 223 *bytesToSend = bytes;
JamieB 69:47f800793d00 224 *TXBuffer = IdleTxBuffer;
JamieB 69:47f800793d00 225 return bytes;
JamieB 69:47f800793d00 226 }
AndyA 0:97661408d0f9 227
AndyA 1:dd1f7e162f91 228 void VIPSSerial::getCRC(void *data, int len, void *checksum)
AndyA 1:dd1f7e162f91 229 {
AndyA 1:dd1f7e162f91 230 uint8_t *dataPtr = (uint8_t *)data;
AndyA 3:14d241e29be3 231 uint8_t *crcPtr = (uint8_t *)checksum;
AndyA 0:97661408d0f9 232 uint16_t crc = 0x0;
AndyA 0:97661408d0f9 233 unsigned char x;
AndyA 0:97661408d0f9 234 int byteCount = 0;
AndyA 0:97661408d0f9 235
AndyA 0:97661408d0f9 236 while ((len--) > 0) {
AndyA 1:dd1f7e162f91 237 x = (unsigned char)(crc >> 8 ^ dataPtr[byteCount++]);
AndyA 0:97661408d0f9 238 x ^= (unsigned char)(x >> 4);
AndyA 0:97661408d0f9 239 crc = (uint16_t)((crc << 8) ^ (x << 12) ^ (x << 5) ^ x);
AndyA 0:97661408d0f9 240 }
AndyA 1:dd1f7e162f91 241 crcPtr[0] = crc >> 8;
AndyA 1:dd1f7e162f91 242 crcPtr[1] = crc &0x00ff;
AndyA 0:97661408d0f9 243 }
AndyA 0:97661408d0f9 244
AndyA 1:dd1f7e162f91 245 bool VIPSSerial::checkCRC(unsigned char* data)
AndyA 1:dd1f7e162f91 246 {
AndyA 0:97661408d0f9 247 unsigned char expectedCRC[2];
AndyA 0:97661408d0f9 248 int length = data[1];
AndyA 0:97661408d0f9 249 if (data[0] == 0xff) // for response length doesn't include the header
AndyA 0:97661408d0f9 250 length++;
AndyA 1:dd1f7e162f91 251 VIPSSerial::getCRC(data, length-2, expectedCRC);
AndyA 0:97661408d0f9 252 if ((data[length-2]==expectedCRC[0]) && (data[length-1]==expectedCRC[1]))
AndyA 0:97661408d0f9 253 return true;
AndyA 0:97661408d0f9 254 return false;
AndyA 0:97661408d0f9 255 }
AndyA 0:97661408d0f9 256
AndyA 0:97661408d0f9 257
AndyA 1:dd1f7e162f91 258 bool VIPSSerial::checkNewPacketRC(unsigned char* data)
AndyA 1:dd1f7e162f91 259 {
AndyA 0:97661408d0f9 260 unsigned char expectedCRC[2];
AndyA 0:97661408d0f9 261 int length = data[2] | (((int)data[3])<<8);
AndyA 1:dd1f7e162f91 262 VIPSSerial::getCRC(data, length-2, expectedCRC);
AndyA 0:97661408d0f9 263 if ((data[length-2]==expectedCRC[0]) && (data[length-1]==expectedCRC[1]))
AndyA 0:97661408d0f9 264 return true;
AndyA 0:97661408d0f9 265 return false;
AndyA 0:97661408d0f9 266 }
AndyA 0:97661408d0f9 267
AndyA 0:97661408d0f9 268
AndyA 1:dd1f7e162f91 269 void VIPSSerial::parsePostionInput_legacy()
AndyA 1:dd1f7e162f91 270 {
AndyA 0:97661408d0f9 271 printf("L");
AndyA 0:97661408d0f9 272 uint8_t tmpBuffer[8];
AndyA 0:97661408d0f9 273 int32_t tmpInt;
AndyA 0:97661408d0f9 274 memcpy((uint8_t *)(&tmpInt)+1, messageInBuffer+4, 3);
AndyA 0:97661408d0f9 275 tmpInt &= 0x00ffffff;
AndyA 0:97661408d0f9 276 lastPositions[nextPosition].pos.time = tmpInt*10;
AndyA 0:97661408d0f9 277 memcpy(tmpBuffer, messageInBuffer+7, 8);
AndyA 0:97661408d0f9 278 lastPositions[nextPosition].pos.X = *(double *)(tmpBuffer);
AndyA 0:97661408d0f9 279 memcpy(tmpBuffer, messageInBuffer+15, 8);
AndyA 0:97661408d0f9 280 lastPositions[nextPosition].pos.Y = *(double *)(tmpBuffer);
AndyA 0:97661408d0f9 281 memcpy((uint8_t *)(&tmpInt)+1, messageInBuffer+27, 3);
AndyA 0:97661408d0f9 282 if (tmpInt & 0x00800000)
AndyA 0:97661408d0f9 283 tmpInt |= 0xff000000;
AndyA 0:97661408d0f9 284 lastPositions[nextPosition].pos.Height = tmpInt/100.0f;
AndyA 0:97661408d0f9 285 lastPositions[nextPosition].pos.roll = 0;
AndyA 0:97661408d0f9 286 lastPositions[nextPosition].pos.pitch = 0;
AndyA 0:97661408d0f9 287 lastPositions[nextPosition].pos.yaw = 0;
AndyA 0:97661408d0f9 288
AndyA 0:97661408d0f9 289 if (enableAllUpdates) {
AndyA 1:dd1f7e162f91 290 printf("Add pos\r\n");
AndyA 1:dd1f7e162f91 291 outputPtr = &outputPosition;
AndyA 1:dd1f7e162f91 292 memcpy(outputPtr,&(lastPositions[nextPosition].pos),sizeof(position));
AndyA 0:97661408d0f9 293 }
AndyA 0:97661408d0f9 294
AndyA 0:97661408d0f9 295 nextPosition++;
AndyA 0:97661408d0f9 296 if (nextPosition == posHistoryLen) {
AndyA 0:97661408d0f9 297 nextPosition = 0;
AndyA 0:97661408d0f9 298 }
AndyA 0:97661408d0f9 299 pointCount++;
AndyA 0:97661408d0f9 300 }
AndyA 0:97661408d0f9 301
AndyA 1:dd1f7e162f91 302 void VIPSSerial::parsePostionInput_mocap()
AndyA 1:dd1f7e162f91 303 {
AndyA 0:97661408d0f9 304 if (!checkNewPacketRC(&messageInBuffer[0])) {
AndyA 0:97661408d0f9 305 pc.write("CRC error\r\n",11);
AndyA 0:97661408d0f9 306 return;
AndyA 0:97661408d0f9 307 }
AndyA 14:76083dc18b0d 308
JamieB 28:7600b18ccb7a 309 // led1=!led1;
AndyA 14:76083dc18b0d 310
AndyA 0:97661408d0f9 311 lastPositions[nextPosition].time = TimeSinceLastFrame.read_us();
AndyA 0:97661408d0f9 312 uint32_t mask = *(uint32_t*)(messageInBuffer+4);
AndyA 0:97661408d0f9 313 int offset = 32; // end of position
AndyA 1:dd1f7e162f91 314
AndyA 0:97661408d0f9 315 lastPositions[nextPosition].pos.time = *(uint32_t*)(messageInBuffer+8);
AndyA 0:97661408d0f9 316 lastPositions[nextPosition].pos.X = *(double *)(messageInBuffer+12);
AndyA 0:97661408d0f9 317 lastPositions[nextPosition].pos.Y = *(double *)(messageInBuffer+20);
AndyA 0:97661408d0f9 318 lastPositions[nextPosition].pos.Height = *(float *)(messageInBuffer+28);
AndyA 10:053bac3e326b 319
AndyA 44:fe7fcf62c75d 320 if (mask & 0x0001)
AndyA 44:fe7fcf62c75d 321 lastPositions[nextPosition].pos.LLAPosition = true;
AndyA 44:fe7fcf62c75d 322
AndyA 0:97661408d0f9 323 if (mask & 0x0002) { // parse status
AndyA 8:961bb15570a1 324 lastPositions[nextPosition].pos.beacons = messageInBuffer[offset++];
AndyA 8:961bb15570a1 325 lastPositions[nextPosition].pos.solutionType = messageInBuffer[offset++];
AndyA 8:961bb15570a1 326 lastPositions[nextPosition].pos.KFStatus = *(uint16_t*)(messageInBuffer + offset);
AndyA 8:961bb15570a1 327 offset +=2;
AndyA 0:97661408d0f9 328 }
AndyA 0:97661408d0f9 329
AndyA 0:97661408d0f9 330 if (mask & 0x0004) {
AndyA 0:97661408d0f9 331 lastPositions[nextPosition].pos.roll = *(float *)(messageInBuffer+offset);
AndyA 0:97661408d0f9 332 lastPositions[nextPosition].pos.pitch = *(float *)(messageInBuffer+offset+4);
AndyA 0:97661408d0f9 333 lastPositions[nextPosition].pos.yaw = *(float *)(messageInBuffer+offset+8);
AndyA 0:97661408d0f9 334 offset+=12;
AndyA 0:97661408d0f9 335 } else {
AndyA 0:97661408d0f9 336 lastPositions[nextPosition].pos.roll = 0;
AndyA 0:97661408d0f9 337 lastPositions[nextPosition].pos.pitch = 0;
AndyA 0:97661408d0f9 338 lastPositions[nextPosition].pos.yaw = 0;
AndyA 0:97661408d0f9 339 }
AndyA 0:97661408d0f9 340
AndyA 0:97661408d0f9 341 if (mask & 0x0008) { // velocity
AndyA 0:97661408d0f9 342
AndyA 0:97661408d0f9 343 offset+=8;
AndyA 1:dd1f7e162f91 344 }
AndyA 0:97661408d0f9 345 if (mask & 0x0010) {// vert velocity
AndyA 0:97661408d0f9 346
AndyA 1:dd1f7e162f91 347 offset+=4;
AndyA 0:97661408d0f9 348 }
AndyA 0:97661408d0f9 349 if (mask & 0x0020) { // pos uncertainty
AndyA 0:97661408d0f9 350
AndyA 1:dd1f7e162f91 351 offset+=12;
AndyA 0:97661408d0f9 352 if (mask & 0x0004) { // orientation uncertainty
AndyA 1:dd1f7e162f91 353 offset += 12;
AndyA 0:97661408d0f9 354 }
AndyA 0:97661408d0f9 355 if (mask & 0x0008) { // velocity uncertainty
AndyA 1:dd1f7e162f91 356 offset += 12;
AndyA 0:97661408d0f9 357 }
AndyA 0:97661408d0f9 358 }
AndyA 0:97661408d0f9 359 if (mask & 0x0040) { // accuracy
AndyA 2:a79201e302d7 360 lastPositions[nextPosition].pos.ID = *(messageInBuffer+offset+3);
AndyA 0:97661408d0f9 361 offset+=4;
AndyA 1:dd1f7e162f91 362 }
AndyA 1:dd1f7e162f91 363 if (mask & 0x0080) { // raw UWB
AndyA 0:97661408d0f9 364 offset+=24;
AndyA 0:97661408d0f9 365 }
AndyA 1:dd1f7e162f91 366 if (mask & 0x0100) { // raw IMU
AndyA 0:97661408d0f9 367 offset+=24;
AndyA 0:97661408d0f9 368 }
AndyA 0:97661408d0f9 369
AndyA 0:97661408d0f9 370 if (mask & 0x0200) {// rover info
AndyA 1:dd1f7e162f91 371 offset+=4;
AndyA 0:97661408d0f9 372 }
AndyA 3:14d241e29be3 373 if (mask & 0x0400) {// FIZ data
AndyA 3:14d241e29be3 374 offset+=4;
AndyA 3:14d241e29be3 375 }
AndyA 44:fe7fcf62c75d 376 if (mask & 0x0800) {// Origin
AndyA 44:fe7fcf62c75d 377 offset+=24;
AndyA 44:fe7fcf62c75d 378 }
AndyA 44:fe7fcf62c75d 379 if (mask & 0x1000) {// Beacon list
AndyA 44:fe7fcf62c75d 380 memcpy(lastPositions[nextPosition].pos.UsedBeacons, messageInBuffer+offset, 12);
AndyA 44:fe7fcf62c75d 381 lastPositions[nextPosition].pos.UsedBeaconsValid = true;
AndyA 44:fe7fcf62c75d 382 offset+=12;
AndyA 44:fe7fcf62c75d 383 } else
AndyA 44:fe7fcf62c75d 384 lastPositions[nextPosition].pos.UsedBeaconsValid = false;
AndyA 44:fe7fcf62c75d 385
AndyA 0:97661408d0f9 386
JamieB 46:432d63e8fe64 387 if (UserSettings.AutoHyperSmooth) {
JamieB 46:432d63e8fe64 388 int testValue = (lastPositions[nextPosition].pos.KFStatus & 0xE634);
JamieB 46:432d63e8fe64 389
JamieB 46:432d63e8fe64 390 if ( ( ( testValue & 0x400) == 0x400) && (!hyperSmoothEnabled)) {
JamieB 46:432d63e8fe64 391 EnableSmoothing(true);
JamieB 57:ba6f516d93b9 392 //pc.write("Auto HS On\r\n", 12);
JamieB 66:066b16c6c34f 393 } else if ( ( ( testValue & 0x400) != 0x400) && (hyperSmoothEnabled) && (!forcedHyperSmooth)) {
JamieB 46:432d63e8fe64 394 EnableSmoothing(false);
JamieB 57:ba6f516d93b9 395 //pc.write("Auto HS Off\r\n", 13);
JamieB 46:432d63e8fe64 396 } //Auto Hypersmooth
JamieB 46:432d63e8fe64 397 }
JamieB 46:432d63e8fe64 398
JamieB 31:2eec69c777a7 399
AndyA 16:a8d3a0dbe4bf 400 smoothOutputPacket(&(lastPositions[nextPosition].pos));
AndyA 16:a8d3a0dbe4bf 401
AndyA 0:97661408d0f9 402 if (enableAllUpdates) {
AndyA 1:dd1f7e162f91 403 // printf("Add pos\r\n");
AndyA 1:dd1f7e162f91 404 outputPtr = &outputPosition;
AndyA 1:dd1f7e162f91 405 memcpy(outputPtr,&(lastPositions[nextPosition].pos),sizeof(position));
AndyA 0:97661408d0f9 406 }
AndyA 0:97661408d0f9 407
AndyA 0:97661408d0f9 408 nextPosition++;
AndyA 0:97661408d0f9 409 if (nextPosition == posHistoryLen) {
AndyA 0:97661408d0f9 410 nextPosition = 0;
AndyA 0:97661408d0f9 411 }
AndyA 0:97661408d0f9 412 pointCount++;
AndyA 0:97661408d0f9 413 }
AndyA 0:97661408d0f9 414
AndyA 16:a8d3a0dbe4bf 415
AndyA 1:dd1f7e162f91 416 // send a position output for the requested time. Times are based on the global TimeSinceLastFrame timer.
AndyA 1:dd1f7e162f91 417 position* VIPSSerial::sendPositionForTime(uint32_t timeValue)
AndyA 1:dd1f7e162f91 418 {
AndyA 22:0dd9c1b5664a 419 // static uint32_t lastPoints = 0;
AndyA 0:97661408d0f9 420 if (pointCount < 2)
AndyA 0:97661408d0f9 421 return NULL;
AndyA 13:c2d9e975841b 422
AndyA 13:c2d9e975841b 423 __disable_irq(); // disable IRQ and make a copy of the data we're going to interpolate.
AndyA 1:dd1f7e162f91 424 int lastPoint = nextPosition - 1;
AndyA 1:dd1f7e162f91 425 int prevPoint = nextPosition - 2;
AndyA 1:dd1f7e162f91 426 if (lastPoint<0)
AndyA 1:dd1f7e162f91 427 lastPoint+=posHistoryLen;
AndyA 1:dd1f7e162f91 428 if (prevPoint<0)
AndyA 1:dd1f7e162f91 429 prevPoint+=posHistoryLen;
AndyA 0:97661408d0f9 430
AndyA 13:c2d9e975841b 431 memcpy(&lastPos,&lastPositions[lastPoint], sizeof(struct posAndTime_s));
AndyA 13:c2d9e975841b 432 memcpy(&prevPos,&lastPositions[prevPoint], sizeof(struct posAndTime_s));
AndyA 13:c2d9e975841b 433 __enable_irq();
AndyA 13:c2d9e975841b 434
AndyA 1:dd1f7e162f91 435 // calculate timestamps as a function of time since last frame
AndyA 8:961bb15570a1 436
AndyA 13:c2d9e975841b 437 uint64_t LastTimeMark = lastPos.time;
AndyA 13:c2d9e975841b 438 uint64_t PrevTimeMark = prevPos.time;
AndyA 10:053bac3e326b 439 uint64_t timeValueUnwrap = timeValue;
AndyA 10:053bac3e326b 440 if (PrevTimeMark > LastTimeMark)
AndyA 10:053bac3e326b 441 LastTimeMark += ((uint64_t)1)<<32;
AndyA 10:053bac3e326b 442 if (LastTimeMark > timeValueUnwrap)
AndyA 10:053bac3e326b 443 timeValueUnwrap += ((uint64_t)1)<<32;
AndyA 0:97661408d0f9 444
AndyA 13:c2d9e975841b 445 outputPosition.time = timeValueUnwrap-PrevTimeMark; // should be between 10,000 and 20,000
AndyA 10:053bac3e326b 446
AndyA 13:c2d9e975841b 447 // interpolate uses the position times. Replace them with the internal clock counts.
AndyA 13:c2d9e975841b 448 lastPos.pos.time = LastTimeMark-PrevTimeMark; // should be very close to 10,000
AndyA 13:c2d9e975841b 449 prevPos.pos.time = 0;
AndyA 0:97661408d0f9 450
AndyA 1:dd1f7e162f91 451 // interpolate position to requested time.
AndyA 13:c2d9e975841b 452 if (position::interp(&outputPosition, &(lastPos.pos), &(prevPos.pos))) {
AndyA 1:dd1f7e162f91 453 return &outputPosition;
AndyA 1:dd1f7e162f91 454 }
AndyA 14:76083dc18b0d 455
AndyA 14:76083dc18b0d 456 // interpolation failed. Return most recent location
AndyA 14:76083dc18b0d 457 return &lastPos.pos;
AndyA 0:97661408d0f9 458 }
AndyA 0:97661408d0f9 459
AndyA 16:a8d3a0dbe4bf 460 void VIPSSerial::smoothOutputPacket(position *posPtr)
AndyA 16:a8d3a0dbe4bf 461 {
AndyA 35:7ecf25d9c414 462 xFilter.addPoint(posPtr->X);
AndyA 35:7ecf25d9c414 463 yFilter.addPoint(posPtr->Y);
AndyA 35:7ecf25d9c414 464 zFilter.addPoint(posPtr->Height);
AndyA 35:7ecf25d9c414 465
AndyA 16:a8d3a0dbe4bf 466 if (hyperSmoothEnabled) {
AndyA 16:a8d3a0dbe4bf 467 if (!SmoothRunning) {
AndyA 16:a8d3a0dbe4bf 468 XSmoothTotal = posPtr->X * (SmoothBy - 1);
AndyA 16:a8d3a0dbe4bf 469 YSmoothTotal = posPtr->Y * (SmoothBy - 1);
JamieB 28:7600b18ccb7a 470 ZSmoothTotal = posPtr->Height * (SmoothBy - 1);
AndyA 16:a8d3a0dbe4bf 471 SmoothRunning = true;
JamieB 46:432d63e8fe64 472 //pc.write("Seeded Filter\r\n",11);
AndyA 16:a8d3a0dbe4bf 473 }
AndyA 16:a8d3a0dbe4bf 474 //smooth the KF_X and KF_Y positions
AndyA 16:a8d3a0dbe4bf 475 XSmoothTotal += posPtr->X;
AndyA 16:a8d3a0dbe4bf 476 posPtr->X = XSmoothTotal / SmoothBy;
AndyA 16:a8d3a0dbe4bf 477 XSmoothTotal -= posPtr->X;
AndyA 16:a8d3a0dbe4bf 478
AndyA 16:a8d3a0dbe4bf 479 YSmoothTotal += posPtr->Y;
AndyA 16:a8d3a0dbe4bf 480 posPtr->Y = YSmoothTotal / SmoothBy;
AndyA 16:a8d3a0dbe4bf 481 YSmoothTotal -= posPtr->Y;
AndyA 16:a8d3a0dbe4bf 482
AndyA 16:a8d3a0dbe4bf 483 ZSmoothTotal += posPtr->Height;
AndyA 16:a8d3a0dbe4bf 484 posPtr->Height = ZSmoothTotal / SmoothBy;
AndyA 16:a8d3a0dbe4bf 485 ZSmoothTotal -= posPtr->Height;
AndyA 16:a8d3a0dbe4bf 486 } else {
AndyA 16:a8d3a0dbe4bf 487 SmoothRunning = false;
AndyA 44:fe7fcf62c75d 488 // pc.printf("filterX = %f\r\n",xFilter.lastValue());
AndyA 35:7ecf25d9c414 489 posPtr->X = xFilter.lastValue();
AndyA 35:7ecf25d9c414 490 posPtr->Y = yFilter.lastValue();
AndyA 35:7ecf25d9c414 491 posPtr->Height = zFilter.lastValue();
AndyA 16:a8d3a0dbe4bf 492 }
AndyA 35:7ecf25d9c414 493 posPtr->roll = rollFilter.addPoint(posPtr->roll);
AndyA 35:7ecf25d9c414 494 posPtr->pitch = pitchFilter.addPoint(posPtr->pitch);
AndyA 35:7ecf25d9c414 495 posPtr->yaw = yawFilter.addPoint(posPtr->yaw);
AndyA 16:a8d3a0dbe4bf 496 }
AndyA 35:7ecf25d9c414 497
AndyA 35:7ecf25d9c414 498 bool VIPSSerial::setFilters(struct UserSettings_s *settings)
AndyA 35:7ecf25d9c414 499 {
AndyA 35:7ecf25d9c414 500 if (settings->FilterXY) {
AndyA 35:7ecf25d9c414 501 if (!xFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
AndyA 35:7ecf25d9c414 502 return false;
AndyA 35:7ecf25d9c414 503 if (!yFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
AndyA 35:7ecf25d9c414 504 return false;
AndyA 35:7ecf25d9c414 505 } else {
AndyA 35:7ecf25d9c414 506 xFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
AndyA 35:7ecf25d9c414 507 yFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
AndyA 35:7ecf25d9c414 508 }
AndyA 35:7ecf25d9c414 509
AndyA 35:7ecf25d9c414 510 if (settings->FilterZ) {
AndyA 35:7ecf25d9c414 511 if (!zFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
AndyA 35:7ecf25d9c414 512 return false;
AndyA 35:7ecf25d9c414 513 } else {
AndyA 35:7ecf25d9c414 514 zFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
AndyA 35:7ecf25d9c414 515 }
AndyA 35:7ecf25d9c414 516
AndyA 35:7ecf25d9c414 517 if (settings->FilterRoll) {
AndyA 35:7ecf25d9c414 518 if (!rollFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
AndyA 35:7ecf25d9c414 519 return false;
AndyA 35:7ecf25d9c414 520 } else {
AndyA 35:7ecf25d9c414 521 rollFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
AndyA 35:7ecf25d9c414 522 }
AndyA 35:7ecf25d9c414 523
AndyA 35:7ecf25d9c414 524 if (settings->FilterPitch) {
AndyA 35:7ecf25d9c414 525 if (!pitchFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
AndyA 35:7ecf25d9c414 526 return false;
AndyA 35:7ecf25d9c414 527 } else {
AndyA 35:7ecf25d9c414 528 pitchFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
AndyA 35:7ecf25d9c414 529 }
AndyA 35:7ecf25d9c414 530
AndyA 35:7ecf25d9c414 531 if (settings->FilterYaw) {
AndyA 35:7ecf25d9c414 532 if (!yawFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
AndyA 35:7ecf25d9c414 533 return false;
AndyA 35:7ecf25d9c414 534 } else {
AndyA 35:7ecf25d9c414 535 yawFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
AndyA 35:7ecf25d9c414 536 }
AndyA 35:7ecf25d9c414 537 return true;
AndyA 35:7ecf25d9c414 538 }