VIPSSerialProtocol.cpp

00001 #include "LTCApp.h"
00002 #include <cstdint>
00003 #include <cstring>
00004 
00005 #define _DisableUWB 0x00
00006 #define _EnableUWB  0x01
00007 #define _GetPPSTime  0x02
00008 #define _UWBData_Legacy 0x03
00009 #define _UWBData_MOCAP 0x11
00010 #define _UWBFitted  0x04
00011 #define _VBOXReady  0x05
00012 #define _SetDeltaTime  0x07
00013 
00014 #define IdleTxBuffer (txBuf == TXBuffer1)?TXBuffer2:TXBuffer1
00015 #define ActiveTxBuffer (txBuf == TXBuffer1)?TXBuffer1:TXBuffer2
00016 
00017 
00018 VIPSSerial::VIPSSerial(const PinName Tx, const PinName Rx) : _port(Tx,Rx)
00019 {
00020     messagePrt = 0;
00021     messageLength = 0;
00022 
00023     pointCount = 0;
00024     nextPosition = 0;
00025 
00026     outputPtr = NULL;
00027     statusMessage = 0;
00028 
00029     enableAllUpdates = false;
00030     newFormatMsg = false;
00031 
00032     queueLen = 0;
00033 
00034     SmoothBy = 100; //was 500
00035     XSmoothTotal = 0;
00036     YSmoothTotal = 0;
00037     ZSmoothTotal = 0;
00038     SmoothRunning = false;
00039 
00040     BypassMode = false;
00041     directTx = false;
00042     txBuf = TXBuffer1;
00043     waitingBytes = 0;
00044     TransmitFinished=false;
00045     //self=this;
00046 
00047     nextPosition= 0;
00048     _outputMask = 0x44;
00049     _port.baud(115200);
00050 }
00051 
00052 void VIPSSerial::run(void)
00053 {
00054     _port.attach(callback(this, &VIPSSerial::onSerialRx));
00055 }
00056 
00057 void VIPSSerial::bypassTx(char byte)
00058 {
00059     _port.putc(byte);
00060 }
00061 
00062 void VIPSSerial::onTxTimeout()
00063 {
00064     TransmitFinished = true;
00065 }
00066 
00067 void VIPSSerial::onSerialRx(void)
00068 {
00069     while (_port.readable()) {
00070         if (BypassMode) {
00071             vipsBypassRx(_port.getc());
00072         } else if (directTx) {
00073             TxTimeout.attach_us(callback(this, &VIPSSerial::onTxTimeout), 200.0);
00074             if (waitingBytes < MaxBuffSize)
00075                 txBuf[waitingBytes++] = _port.getc();
00076         } else {
00077             messageInBuffer[messagePrt] = _port.getc();
00078             if (messagePrt==0) { // look for header
00079                 if ((messageInBuffer[0]==0x2A) || (messageInBuffer[0]==0x24)) {
00080                     messagePrt=1;
00081                     newFormatMsg=(messageInBuffer[0]==0x24);
00082                 }
00083             } else if (newFormatMsg) {
00084                 if (messagePrt < 4) {
00085                     messagePrt++;
00086                     if (messagePrt==4) {
00087                         if (messageInBuffer[1]!=0xd9)
00088                             messagePrt=0;
00089                         messageLength = *(uint16_t*)(messageInBuffer+2);
00090                         if ((messageLength>115) || (messageLength<34))
00091                             messagePrt = 0;
00092                     }
00093                 } else {
00094                     messagePrt++;
00095                     if (messagePrt == messageLength) {
00096                         parsePostionInput_mocap();
00097                         messagePrt=0;
00098                     }
00099                 }
00100             } else {
00101                 if (messagePrt==1) {
00102                     if (messageInBuffer[1]<128)            { // valid length
00103                         messageLength = messageInBuffer[1];
00104                         messagePrt = 2;
00105                     } else {
00106                         messagePrt=0;
00107                     }
00108                 } else { // in the middle of a message
00109                     messagePrt++;
00110                     if (messagePrt==messageLength) {
00111                         processRxMessage();
00112                         messagePrt=0;
00113                     }
00114                 }
00115             }
00116         }
00117     }
00118 }
00119 
00120 void VIPSSerial::processRxMessage()
00121 {
00122     if (!checkCRC(&messageInBuffer[0])) {
00123         pc.puts("VIPS CRC error\r\n");
00124         return;
00125     }
00126 
00127     switch (messageInBuffer[2]) {
00128         case _UWBFitted:
00129             sendAck(messageInBuffer[2]);
00130             break;
00131         case _EnableUWB:
00132         case _DisableUWB: // for all of these just send an ack.
00133         case _SetDeltaTime:
00134             sendAck(messageInBuffer[2]);
00135             break;
00136         case _GetPPSTime:
00137             sendVBOXTime();
00138             // send vbox tick counter
00139             break;
00140         case _VBOXReady:
00141             if (ppsActive) {
00142                 sendAck(_VBOXReady);
00143             } else {
00144                 sendNack(_VBOXReady);
00145             }
00146             break;
00147         case _UWBData_Legacy:
00148             parsePostionInput_legacy();
00149             break;
00150         default:
00151             break;
00152     }
00153 }
00154 
00155 
00156 void VIPSSerial::sendVBOXTime()
00157 {
00158     unsigned char timeValue[3];
00159     uint32_t timeToSend = VBOXTicks-1; // we track time at next PPS, message requires time at the last PPS.
00160     timeValue[0]= timeToSend&0x00ff;
00161     timeValue[1]= (timeToSend>>8)&0x00ff;
00162     timeValue[2]= (timeToSend>>16)&0x00ff;
00163     queueResponse(_GetPPSTime,timeValue,3);
00164 //    pc.printf("Sending time = %d ",VBOXTicks);
00165 }
00166 
00167 void VIPSSerial::sendAck(unsigned char function)
00168 {
00169     unsigned char ack=0x01;
00170     sendResponse(function,&ack,1);
00171 }
00172 
00173 void VIPSSerial::sendNack(unsigned char function)
00174 {
00175     unsigned char nack=0x00;
00176     sendResponse(function,&nack,1);
00177 }
00178 
00179 void VIPSSerial::sendResponse(unsigned char function, unsigned char* data, int dataLen)
00180 {
00181     messageOutBuffer[0]=0xff;
00182     messageOutBuffer[1]=dataLen+4;
00183     messageOutBuffer[2]=function;
00184     for (int i=0; i<dataLen; i++)
00185         messageOutBuffer[i+3] = data[i];
00186     VIPSSerial::getCRC(messageOutBuffer,dataLen+3,messageOutBuffer+dataLen+3);
00187     for (int i=0; i<dataLen+5; i++)
00188         _port.putc(messageOutBuffer[i]);
00189 }
00190 void VIPSSerial::queueResponse(unsigned char function, unsigned char* data, int dataLen)
00191 {
00192     messageOutBuffer[0]=0xff;
00193     messageOutBuffer[1]=dataLen+4;
00194     messageOutBuffer[2]=function;
00195     for (int i=0; i<dataLen; i++)
00196         messageOutBuffer[i+3] = data[i];
00197     VIPSSerial::getCRC(messageOutBuffer,dataLen+3,messageOutBuffer+dataLen+3);
00198     queueLen = dataLen+5;
00199 }
00200 
00201 void VIPSSerial::sendQueued()
00202 {
00203     if (queueLen) {
00204         for (int i=0; i<queueLen; i++)
00205             _port.putc(messageOutBuffer[i]);
00206         queueLen = 0;
00207     }
00208 }
00209 void VIPSSerial::sendDirectTX(unsigned char* data, int dataLen)
00210 {
00211     for (int i=0; i<dataLen; i++)
00212         _port.putc(data[i]);
00213 }
00214 
00215 void VIPSSerial::sendQuiet()
00216 {
00217     _port.putc(0x07);
00218     _port.putc(0x05);
00219     _port.putc(0x02);
00220     _port.putc(0x5A);
00221     _port.putc(0x27);
00222 }
00223 
00224 int VIPSSerial::getWaitingBuffer(unsigned char **TXBuffer, int *bytesToSend)
00225 {
00226     int bytes;
00227     if (TransmitFinished) {
00228         __disable_irq();
00229         bytes = waitingBytes;
00230         txBuf = IdleTxBuffer; //see Macro At Start
00231         waitingBytes = 0;
00232         __enable_irq();
00233         *bytesToSend = bytes;
00234         *TXBuffer = IdleTxBuffer; //See Macro at Start
00235         TransmitFinished = false;
00236     } else {
00237         bytes = 0;
00238     }
00239     return bytes;
00240 }
00241 
00242 void VIPSSerial::getCRC(void *data, int len, void *checksum)
00243 {
00244     uint8_t *dataPtr = (uint8_t *)data;
00245     uint8_t *crcPtr = (uint8_t *)checksum;
00246     uint16_t crc = 0x0;
00247     unsigned char x;
00248     int byteCount = 0;
00249 
00250     while ((len--) > 0) {
00251         x = (unsigned char)(crc >> 8 ^ dataPtr[byteCount++]);
00252         x ^= (unsigned char)(x >> 4);
00253         crc = (uint16_t)((crc << 8) ^ (x << 12) ^ (x << 5) ^ x);
00254     }
00255     crcPtr[0] = crc >> 8;
00256     crcPtr[1] = crc &0x00ff;
00257 }
00258 
00259 bool VIPSSerial::checkCRC(unsigned char* data)
00260 {
00261     unsigned char expectedCRC[2];
00262     int length = data[1];
00263     if (data[0] == 0xff) // for response length doesn't include the header
00264         length++;
00265     VIPSSerial::getCRC(data, length-2, expectedCRC);
00266     if ((data[length-2]==expectedCRC[0]) && (data[length-1]==expectedCRC[1]))
00267         return true;
00268     return false;
00269 }
00270 
00271 
00272 bool VIPSSerial::checkNewPacketRC(unsigned char* data)
00273 {
00274     unsigned char expectedCRC[2];
00275     int length = data[2] | (((int)data[3])<<8);
00276     VIPSSerial::getCRC(data, length-2, expectedCRC);
00277     if ((data[length-2]==expectedCRC[0]) && (data[length-1]==expectedCRC[1]))
00278         return true;
00279     return false;
00280 }
00281 
00282 
00283 void VIPSSerial::parsePostionInput_legacy()
00284 {
00285     printf("L");
00286     uint8_t tmpBuffer[8];
00287     int32_t tmpInt;
00288     memcpy((uint8_t *)(&tmpInt)+1, messageInBuffer+4, 3);
00289     tmpInt &= 0x00ffffff;
00290     lastPositions[nextPosition].pos.time = tmpInt*10;
00291     memcpy(tmpBuffer, messageInBuffer+7, 8);
00292     lastPositions[nextPosition].pos.X = *(double *)(tmpBuffer);
00293     memcpy(tmpBuffer, messageInBuffer+15, 8);
00294     lastPositions[nextPosition].pos.Y = *(double *)(tmpBuffer);
00295     memcpy((uint8_t *)(&tmpInt)+1, messageInBuffer+27, 3);
00296     if (tmpInt & 0x00800000)
00297         tmpInt |= 0xff000000;
00298     lastPositions[nextPosition].pos.Height = tmpInt/100.0f;
00299     lastPositions[nextPosition].pos.roll = 0;
00300     lastPositions[nextPosition].pos.pitch = 0;
00301     lastPositions[nextPosition].pos.yaw = 0;
00302 
00303     if (enableAllUpdates) {
00304         printf("Add pos\r\n");
00305         outputPtr = &outputPosition;
00306         memcpy(outputPtr,&(lastPositions[nextPosition].pos),sizeof(position));
00307     }
00308 
00309     nextPosition++;
00310     if (nextPosition == posHistoryLen) {
00311         nextPosition = 0;
00312     }
00313     pointCount++;
00314 }
00315 
00316 void VIPSSerial::parsePostionInput_mocap()
00317 {
00318     if (!checkNewPacketRC(&messageInBuffer[0])) {
00319         pc.write("CRC error\r\n",11);
00320         return;
00321     }
00322 
00323 //   led1=!led1;
00324 
00325     lastPositions[nextPosition].time = TimeSinceLastFrame.read_us();
00326     uint32_t mask = *(uint32_t*)(messageInBuffer+4);
00327     int offset = 32; // end of position
00328 
00329     lastPositions[nextPosition].pos.time = *(uint32_t*)(messageInBuffer+8);
00330     lastPositions[nextPosition].pos.X = *(double *)(messageInBuffer+12);
00331     lastPositions[nextPosition].pos.Y = *(double *)(messageInBuffer+20);
00332     lastPositions[nextPosition].pos.Height = *(float *)(messageInBuffer+28);
00333 
00334     if (mask & 0x0001)
00335         lastPositions[nextPosition].pos.LLAPosition = true;
00336 
00337     if (mask & 0x0002) { // parse status
00338         lastPositions[nextPosition].pos.beacons = messageInBuffer[offset++];
00339         lastPositions[nextPosition].pos.solutionType = messageInBuffer[offset++];
00340         lastPositions[nextPosition].pos.KFStatus = *(uint16_t*)(messageInBuffer + offset);
00341         offset +=2;
00342     }
00343 
00344     if (mask & 0x0004) {
00345         lastPositions[nextPosition].pos.roll = *(float *)(messageInBuffer+offset);
00346         lastPositions[nextPosition].pos.pitch = *(float *)(messageInBuffer+offset+4);
00347         lastPositions[nextPosition].pos.yaw = *(float *)(messageInBuffer+offset+8);
00348         offset+=12;
00349     } else {
00350         lastPositions[nextPosition].pos.roll = 0;
00351         lastPositions[nextPosition].pos.pitch = 0;
00352         lastPositions[nextPosition].pos.yaw = 0;
00353     }
00354 
00355     if (mask & 0x0008) { // velocity
00356 
00357         offset+=8;
00358     }
00359     if (mask & 0x0010) {// vert velocity
00360 
00361         offset+=4;
00362     }
00363     if (mask & 0x0020) { // pos uncertainty
00364 
00365         offset+=12;
00366         if (mask & 0x0004) { // orientation uncertainty
00367             offset += 12;
00368         }
00369         if (mask & 0x0008) { // velocity uncertainty
00370             offset += 12;
00371         }
00372     }
00373     if (mask & 0x0040) {  // accuracy
00374         lastPositions[nextPosition].pos.ID = *(messageInBuffer+offset+3);
00375         offset+=4;
00376     }
00377     if (mask & 0x0080) {  // raw UWB
00378         offset+=24;
00379     }
00380     if (mask & 0x0100) { // raw IMU
00381         lastPositions[nextPosition].pos.x_accel = *(float *)(messageInBuffer+offset);
00382         lastPositions[nextPosition].pos.y_accel  = *(float *)(messageInBuffer+offset+4);
00383         lastPositions[nextPosition].pos.z_accel  = *(float *)(messageInBuffer+offset+8);
00384         offset+=12;
00385         lastPositions[nextPosition].pos.x_gyro = *(float *)(messageInBuffer+offset);
00386         lastPositions[nextPosition].pos.y_gyro  = *(float *)(messageInBuffer+offset+4);
00387         lastPositions[nextPosition].pos.z_gyro  = *(float *)(messageInBuffer+offset+8);
00388         offset+=12;
00389         //offset+=24;
00390     } else {
00391         lastPositions[nextPosition].pos.x_accel = 0.0f;
00392         lastPositions[nextPosition].pos.y_accel  = 0.0f;
00393         lastPositions[nextPosition].pos.z_accel  = 0.0f;
00394         offset+=12;
00395         lastPositions[nextPosition].pos.x_gyro = 0.0f;
00396         lastPositions[nextPosition].pos.y_gyro  = 0.0f;
00397         lastPositions[nextPosition].pos.z_gyro  = 0.0f;
00398         offset+=12;
00399     }
00400 
00401     if (mask & 0x0200) {// rover info
00402         offset+=4;
00403     }
00404     if (mask & 0x0400) {// FIZ data
00405         offset+=4;
00406     }
00407     if (mask & 0x0800) {// Origin
00408         offset+=24;
00409     }
00410     if (mask & 0x1000) {// Beacon list
00411         memcpy(lastPositions[nextPosition].pos.UsedBeacons, messageInBuffer+offset, 12);
00412         lastPositions[nextPosition].pos.UsedBeaconsValid = true;
00413         offset+=12;
00414     } else
00415         lastPositions[nextPosition].pos.UsedBeaconsValid = false;
00416 
00417 
00418     if (UserSettings.AutoHyperSmooth) {
00419         int testValue = (lastPositions[nextPosition].pos.KFStatus & 0xE634);
00420 
00421         if ( ( ( testValue & 0x400) == 0x400) && (!hyperSmoothEnabled)) {
00422             EnableSmoothing(true);
00423             //pc.write("Auto HS On\r\n", 12);
00424         } else if ( ( ( testValue & 0x400) != 0x400) && (hyperSmoothEnabled) && (!forcedHyperSmooth)) {
00425             EnableSmoothing(false);
00426             //pc.write("Auto HS Off\r\n", 13);
00427         } //Auto Hypersmooth
00428     }
00429 
00430 
00431     smoothOutputPacket(&(lastPositions[nextPosition].pos));
00432 
00433     if (enableAllUpdates) {
00434 //        printf("Add pos\r\n");
00435         outputPtr = &outputPosition;
00436         memcpy(outputPtr,&(lastPositions[nextPosition].pos),sizeof(position));
00437     }
00438 
00439     nextPosition++;
00440     if (nextPosition == posHistoryLen) {
00441         nextPosition = 0;
00442     }
00443     pointCount++;
00444 }
00445 
00446 
00447 // send a position output for the requested time. Times are based on the global TimeSinceLastFrame timer.
00448 position* VIPSSerial::sendPositionForTime(uint32_t timeValue)
00449 {
00450 //    static uint32_t lastPoints = 0;
00451     if (pointCount < 2)
00452         return NULL;
00453 
00454     __disable_irq();   // disable IRQ and make a copy of the data we're going to interpolate.
00455     int lastPoint = nextPosition - 1;
00456     int prevPoint = nextPosition - 2;
00457     if (lastPoint<0)
00458         lastPoint+=posHistoryLen;
00459     if (prevPoint<0)
00460         prevPoint+=posHistoryLen;
00461 
00462     memcpy(&lastPos,&lastPositions[lastPoint], sizeof(struct posAndTime_s));
00463     memcpy(&prevPos,&lastPositions[prevPoint], sizeof(struct posAndTime_s));
00464     __enable_irq();
00465 
00466     // calculate timestamps as a function of time since last frame
00467 
00468     uint64_t LastTimeMark = lastPos.time;
00469     uint64_t PrevTimeMark = prevPos.time;
00470     uint64_t timeValueUnwrap = timeValue;
00471     if (PrevTimeMark > LastTimeMark)
00472         LastTimeMark += ((uint64_t)1)<<32;
00473     if (LastTimeMark > timeValueUnwrap)
00474         timeValueUnwrap += ((uint64_t)1)<<32;
00475 
00476     outputPosition.time = timeValueUnwrap-PrevTimeMark; // should be between 10,000 and 20,000
00477 
00478     // interpolate uses the position times. Replace them with the internal clock counts.
00479     lastPos.pos.time = LastTimeMark-PrevTimeMark; // should be very close to 10,000
00480     prevPos.pos.time = 0;
00481 
00482     // interpolate position to requested time.
00483     if (position::interp(&outputPosition, &(lastPos.pos), &(prevPos.pos))) {
00484         return &outputPosition;
00485     }
00486 
00487     // interpolation failed. Return most recent location
00488     return &lastPos.pos;
00489 }
00490 
00491 void VIPSSerial::smoothOutputPacket(position *posPtr)
00492 {
00493     xFilter.addPoint(posPtr->X);
00494     yFilter.addPoint(posPtr->Y);
00495     zFilter.addPoint(posPtr->Height);
00496 
00497     if (hyperSmoothEnabled) {
00498         if (!SmoothRunning) {
00499             XSmoothTotal = posPtr->X * (SmoothBy - 1);
00500             YSmoothTotal = posPtr->Y * (SmoothBy - 1);
00501             ZSmoothTotal = posPtr->Height * (SmoothBy - 1);
00502             SmoothRunning = true;
00503             //pc.write("Seeded Filter\r\n",11);
00504         }
00505         //smooth the KF_X and KF_Y positions
00506         XSmoothTotal += posPtr->X;
00507         posPtr->X = XSmoothTotal / SmoothBy;
00508         XSmoothTotal -= posPtr->X;
00509 
00510         YSmoothTotal += posPtr->Y;
00511         posPtr->Y = YSmoothTotal / SmoothBy;
00512         YSmoothTotal -= posPtr->Y;
00513 
00514         ZSmoothTotal += posPtr->Height;
00515         posPtr->Height = ZSmoothTotal / SmoothBy;
00516         ZSmoothTotal -= posPtr->Height;
00517     } else {
00518         SmoothRunning = false;
00519 //       pc.printf("filterX = %f\r\n",xFilter.lastValue());
00520         posPtr->X = xFilter.lastValue();
00521         posPtr->Y = yFilter.lastValue();
00522         posPtr->Height = zFilter.lastValue();
00523     }
00524     posPtr->roll = rollFilter.addPoint(posPtr->roll);
00525     posPtr->pitch = pitchFilter.addPoint(posPtr->pitch);
00526     posPtr->yaw = yawFilter.addPoint(posPtr->yaw);
00527 }
00528 
00529 bool VIPSSerial::setFilters(struct UserSettings_s *settings)
00530 {
00531     if (settings->FilterXY) {
00532         if (!xFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
00533             return false;
00534         if (!yFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
00535             return false;
00536     } else {
00537         xFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
00538         yFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
00539     }
00540 
00541     if (settings->FilterZ) {
00542         if (!zFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
00543             return false;
00544     } else {
00545         zFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
00546     }
00547 
00548     if (settings->FilterRoll) {
00549         if (!rollFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
00550             return false;
00551     } else {
00552         rollFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
00553     }
00554 
00555     if (settings->FilterPitch) {
00556         if (!pitchFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
00557             return false;
00558     } else {
00559         pitchFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
00560     }
00561 
00562     if (settings->FilterYaw) {
00563         if (!yawFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
00564             return false;
00565     } else {
00566         yawFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
00567     }
00568     return true;
00569 }