I messed up the merge, so pushing it over to another repo so I don't lose it. Will tidy up and remove later
Dependencies: BufferedSerial FatFileSystemCpp mbed
VIPSSerialProtocol.cpp
- Committer:
- JamieB
- Date:
- 21 months ago
- Revision:
- 85:0cc5931bb9ef
- Parent:
- 79:1910ae03cb2e
File content as of revision 85:0cc5931bb9ef:
#include "LTCApp.h" #include <cstdint> #include <cstring> #define _DisableUWB 0x00 #define _EnableUWB 0x01 #define _GetPPSTime 0x02 #define _UWBData_Legacy 0x03 #define _UWBData_MOCAP 0x11 #define _UWBFitted 0x04 #define _VBOXReady 0x05 #define _SetDeltaTime 0x07 #define IdleTxBuffer (txBuf == TXBuffer1)?TXBuffer2:TXBuffer1 #define ActiveTxBuffer (txBuf == TXBuffer1)?TXBuffer1:TXBuffer2 VIPSSerial::VIPSSerial(const PinName Tx, const PinName Rx) : _port(Tx,Rx) { messagePrt = 0; messageLength = 0; pointCount = 0; nextPosition = 0; outputPtr = NULL; statusMessage = 0; enableAllUpdates = false; newFormatMsg = false; queueLen = 0; SmoothBy = 100; //was 500 XSmoothTotal = 0; YSmoothTotal = 0; ZSmoothTotal = 0; SmoothRunning = false; BypassMode = false; directTx = false; txBuf = TXBuffer1; waitingBytes = 0; TransmitFinished=false; //self=this; nextPosition= 0; _outputMask = 0x44; _port.baud(115200); } void VIPSSerial::run(void) { _port.attach(callback(this, &VIPSSerial::onSerialRx)); } void VIPSSerial::bypassTx(char byte) { _port.putc(byte); } void VIPSSerial::onTxTimeout() { TransmitFinished = true; } void VIPSSerial::onSerialRx(void) { while (_port.readable()) { if (BypassMode) { vipsBypassRx(_port.getc()); } else if (directTx) { TxTimeout.attach_us(callback(this, &VIPSSerial::onTxTimeout), 200.0); if (waitingBytes < MaxBuffSize) txBuf[waitingBytes++] = _port.getc(); } else { messageInBuffer[messagePrt] = _port.getc(); if (messagePrt==0) { // look for header if ((messageInBuffer[0]==0x2A) || (messageInBuffer[0]==0x24)) { messagePrt=1; newFormatMsg=(messageInBuffer[0]==0x24); } } else if (newFormatMsg) { if (messagePrt < 4) { messagePrt++; if (messagePrt==4) { if (messageInBuffer[1]!=0xd9) messagePrt=0; messageLength = *(uint16_t*)(messageInBuffer+2); if ((messageLength>115) || (messageLength<34)) messagePrt = 0; } } else { messagePrt++; if (messagePrt == messageLength) { parsePostionInput_mocap(); messagePrt=0; } } } else { if (messagePrt==1) { if (messageInBuffer[1]<128) { // valid length messageLength = messageInBuffer[1]; messagePrt = 2; } else { messagePrt=0; } } else { // in the middle of a message messagePrt++; if (messagePrt==messageLength) { processRxMessage(); messagePrt=0; } } } } } } void VIPSSerial::processRxMessage() { if (!checkCRC(&messageInBuffer[0])) { pc.puts("VIPS CRC error\r\n"); return; } switch (messageInBuffer[2]) { case _UWBFitted: sendAck(messageInBuffer[2]); break; case _EnableUWB: case _DisableUWB: // for all of these just send an ack. case _SetDeltaTime: sendAck(messageInBuffer[2]); break; case _GetPPSTime: sendVBOXTime(); // send vbox tick counter break; case _VBOXReady: if (ppsActive) { sendAck(_VBOXReady); } else { sendNack(_VBOXReady); } break; case _UWBData_Legacy: parsePostionInput_legacy(); break; default: break; } } void VIPSSerial::sendVBOXTime() { unsigned char timeValue[3]; uint32_t timeToSend = VBOXTicks-1; // we track time at next PPS, message requires time at the last PPS. timeValue[0]= timeToSend&0x00ff; timeValue[1]= (timeToSend>>8)&0x00ff; timeValue[2]= (timeToSend>>16)&0x00ff; queueResponse(_GetPPSTime,timeValue,3); // pc.printf("Sending time = %d ",VBOXTicks); } void VIPSSerial::sendAck(unsigned char function) { unsigned char ack=0x01; sendResponse(function,&ack,1); } void VIPSSerial::sendNack(unsigned char function) { unsigned char nack=0x00; sendResponse(function,&nack,1); } void VIPSSerial::sendResponse(unsigned char function, unsigned char* data, int dataLen) { messageOutBuffer[0]=0xff; messageOutBuffer[1]=dataLen+4; messageOutBuffer[2]=function; for (int i=0; i<dataLen; i++) messageOutBuffer[i+3] = data[i]; VIPSSerial::getCRC(messageOutBuffer,dataLen+3,messageOutBuffer+dataLen+3); for (int i=0; i<dataLen+5; i++) _port.putc(messageOutBuffer[i]); } void VIPSSerial::queueResponse(unsigned char function, unsigned char* data, int dataLen) { messageOutBuffer[0]=0xff; messageOutBuffer[1]=dataLen+4; messageOutBuffer[2]=function; for (int i=0; i<dataLen; i++) messageOutBuffer[i+3] = data[i]; VIPSSerial::getCRC(messageOutBuffer,dataLen+3,messageOutBuffer+dataLen+3); queueLen = dataLen+5; } void VIPSSerial::sendQueued() { if (queueLen) { for (int i=0; i<queueLen; i++) _port.putc(messageOutBuffer[i]); queueLen = 0; } } void VIPSSerial::sendDirectTX(unsigned char* data, int dataLen) { for (int i=0; i<dataLen; i++) _port.putc(data[i]); } void VIPSSerial::sendQuiet() { _port.putc(0x07); _port.putc(0x05); _port.putc(0x02); _port.putc(0x5A); _port.putc(0x27); } int VIPSSerial::getWaitingBuffer(unsigned char **TXBuffer, int *bytesToSend) { int bytes; if (TransmitFinished) { __disable_irq(); bytes = waitingBytes; txBuf = IdleTxBuffer; //see Macro At Start waitingBytes = 0; __enable_irq(); *bytesToSend = bytes; *TXBuffer = IdleTxBuffer; //See Macro at Start TransmitFinished = false; } else { bytes = 0; } return bytes; } void VIPSSerial::getCRC(void *data, int len, void *checksum) { uint8_t *dataPtr = (uint8_t *)data; uint8_t *crcPtr = (uint8_t *)checksum; uint16_t crc = 0x0; unsigned char x; int byteCount = 0; while ((len--) > 0) { x = (unsigned char)(crc >> 8 ^ dataPtr[byteCount++]); x ^= (unsigned char)(x >> 4); crc = (uint16_t)((crc << 8) ^ (x << 12) ^ (x << 5) ^ x); } crcPtr[0] = crc >> 8; crcPtr[1] = crc &0x00ff; } bool VIPSSerial::checkCRC(unsigned char* data) { unsigned char expectedCRC[2]; int length = data[1]; if (data[0] == 0xff) // for response length doesn't include the header length++; VIPSSerial::getCRC(data, length-2, expectedCRC); if ((data[length-2]==expectedCRC[0]) && (data[length-1]==expectedCRC[1])) return true; return false; } bool VIPSSerial::checkNewPacketRC(unsigned char* data) { unsigned char expectedCRC[2]; int length = data[2] | (((int)data[3])<<8); VIPSSerial::getCRC(data, length-2, expectedCRC); if ((data[length-2]==expectedCRC[0]) && (data[length-1]==expectedCRC[1])) return true; return false; } void VIPSSerial::parsePostionInput_legacy() { printf("L"); uint8_t tmpBuffer[8]; int32_t tmpInt; memcpy((uint8_t *)(&tmpInt)+1, messageInBuffer+4, 3); tmpInt &= 0x00ffffff; lastPositions[nextPosition].pos.time = tmpInt*10; memcpy(tmpBuffer, messageInBuffer+7, 8); lastPositions[nextPosition].pos.X = *(double *)(tmpBuffer); memcpy(tmpBuffer, messageInBuffer+15, 8); lastPositions[nextPosition].pos.Y = *(double *)(tmpBuffer); memcpy((uint8_t *)(&tmpInt)+1, messageInBuffer+27, 3); if (tmpInt & 0x00800000) tmpInt |= 0xff000000; lastPositions[nextPosition].pos.Height = tmpInt/100.0f; lastPositions[nextPosition].pos.roll = 0; lastPositions[nextPosition].pos.pitch = 0; lastPositions[nextPosition].pos.yaw = 0; if (enableAllUpdates) { printf("Add pos\r\n"); outputPtr = &outputPosition; memcpy(outputPtr,&(lastPositions[nextPosition].pos),sizeof(position)); } nextPosition++; if (nextPosition == posHistoryLen) { nextPosition = 0; } pointCount++; } void VIPSSerial::parsePostionInput_mocap() { if (!checkNewPacketRC(&messageInBuffer[0])) { pc.write("CRC error\r\n",11); return; } // led1=!led1; lastPositions[nextPosition].time = TimeSinceLastFrame.read_us(); uint32_t mask = *(uint32_t*)(messageInBuffer+4); int offset = 32; // end of position lastPositions[nextPosition].pos.time = *(uint32_t*)(messageInBuffer+8); lastPositions[nextPosition].pos.X = *(double *)(messageInBuffer+12); lastPositions[nextPosition].pos.Y = *(double *)(messageInBuffer+20); lastPositions[nextPosition].pos.Height = *(float *)(messageInBuffer+28); if (mask & 0x0001) lastPositions[nextPosition].pos.LLAPosition = true; if (mask & 0x0002) { // parse status lastPositions[nextPosition].pos.beacons = messageInBuffer[offset++]; lastPositions[nextPosition].pos.solutionType = messageInBuffer[offset++]; lastPositions[nextPosition].pos.KFStatus = *(uint16_t*)(messageInBuffer + offset); offset +=2; } if (mask & 0x0004) { lastPositions[nextPosition].pos.roll = *(float *)(messageInBuffer+offset); lastPositions[nextPosition].pos.pitch = *(float *)(messageInBuffer+offset+4); lastPositions[nextPosition].pos.yaw = *(float *)(messageInBuffer+offset+8); offset+=12; } else { lastPositions[nextPosition].pos.roll = 0; lastPositions[nextPosition].pos.pitch = 0; lastPositions[nextPosition].pos.yaw = 0; } if (mask & 0x0008) { // velocity offset+=8; } if (mask & 0x0010) {// vert velocity offset+=4; } if (mask & 0x0020) { // pos uncertainty offset+=12; if (mask & 0x0004) { // orientation uncertainty offset += 12; } if (mask & 0x0008) { // velocity uncertainty offset += 12; } } if (mask & 0x0040) { // accuracy lastPositions[nextPosition].pos.ID = *(messageInBuffer+offset+3); offset+=4; } if (mask & 0x0080) { // raw UWB offset+=24; } if (mask & 0x0100) { // raw IMU lastPositions[nextPosition].pos.x_accel = *(float *)(messageInBuffer+offset); lastPositions[nextPosition].pos.y_accel = *(float *)(messageInBuffer+offset+4); lastPositions[nextPosition].pos.z_accel = *(float *)(messageInBuffer+offset+8); offset+=12; lastPositions[nextPosition].pos.x_gyro = *(float *)(messageInBuffer+offset); lastPositions[nextPosition].pos.y_gyro = *(float *)(messageInBuffer+offset+4); lastPositions[nextPosition].pos.z_gyro = *(float *)(messageInBuffer+offset+8); offset+=12; //offset+=24; } else { lastPositions[nextPosition].pos.x_accel = 0.0f; lastPositions[nextPosition].pos.y_accel = 0.0f; lastPositions[nextPosition].pos.z_accel = 0.0f; offset+=12; lastPositions[nextPosition].pos.x_gyro = 0.0f; lastPositions[nextPosition].pos.y_gyro = 0.0f; lastPositions[nextPosition].pos.z_gyro = 0.0f; offset+=12; } if (mask & 0x0200) {// rover info offset+=4; } if (mask & 0x0400) {// FIZ data offset+=4; } if (mask & 0x0800) {// Origin offset+=24; } if (mask & 0x1000) {// Beacon list memcpy(lastPositions[nextPosition].pos.UsedBeacons, messageInBuffer+offset, 12); lastPositions[nextPosition].pos.UsedBeaconsValid = true; offset+=12; } else lastPositions[nextPosition].pos.UsedBeaconsValid = false; if (UserSettings.AutoHyperSmooth) { int testValue = (lastPositions[nextPosition].pos.KFStatus & 0xE634); if ( ( ( testValue & 0x400) == 0x400) && (!hyperSmoothEnabled)) { EnableSmoothing(true); //pc.write("Auto HS On\r\n", 12); } else if ( ( ( testValue & 0x400) != 0x400) && (hyperSmoothEnabled) && (!forcedHyperSmooth)) { EnableSmoothing(false); //pc.write("Auto HS Off\r\n", 13); } //Auto Hypersmooth } smoothOutputPacket(&(lastPositions[nextPosition].pos)); if (enableAllUpdates) { // printf("Add pos\r\n"); outputPtr = &outputPosition; memcpy(outputPtr,&(lastPositions[nextPosition].pos),sizeof(position)); } nextPosition++; if (nextPosition == posHistoryLen) { nextPosition = 0; } pointCount++; } // send a position output for the requested time. Times are based on the global TimeSinceLastFrame timer. position* VIPSSerial::sendPositionForTime(uint32_t timeValue) { // static uint32_t lastPoints = 0; if (pointCount < 2) return NULL; __disable_irq(); // disable IRQ and make a copy of the data we're going to interpolate. int lastPoint = nextPosition - 1; int prevPoint = nextPosition - 2; if (lastPoint<0) lastPoint+=posHistoryLen; if (prevPoint<0) prevPoint+=posHistoryLen; memcpy(&lastPos,&lastPositions[lastPoint], sizeof(struct posAndTime_s)); memcpy(&prevPos,&lastPositions[prevPoint], sizeof(struct posAndTime_s)); __enable_irq(); // calculate timestamps as a function of time since last frame uint64_t LastTimeMark = lastPos.time; uint64_t PrevTimeMark = prevPos.time; uint64_t timeValueUnwrap = timeValue; if (PrevTimeMark > LastTimeMark) LastTimeMark += ((uint64_t)1)<<32; if (LastTimeMark > timeValueUnwrap) timeValueUnwrap += ((uint64_t)1)<<32; outputPosition.time = timeValueUnwrap-PrevTimeMark; // should be between 10,000 and 20,000 // interpolate uses the position times. Replace them with the internal clock counts. lastPos.pos.time = LastTimeMark-PrevTimeMark; // should be very close to 10,000 prevPos.pos.time = 0; // interpolate position to requested time. if (position::interp(&outputPosition, &(lastPos.pos), &(prevPos.pos))) { return &outputPosition; } // interpolation failed. Return most recent location return &lastPos.pos; } void VIPSSerial::smoothOutputPacket(position *posPtr) { xFilter.addPoint(posPtr->X); yFilter.addPoint(posPtr->Y); zFilter.addPoint(posPtr->Height); if (hyperSmoothEnabled) { if (!SmoothRunning) { XSmoothTotal = posPtr->X * (SmoothBy - 1); YSmoothTotal = posPtr->Y * (SmoothBy - 1); ZSmoothTotal = posPtr->Height * (SmoothBy - 1); SmoothRunning = true; //pc.write("Seeded Filter\r\n",11); } //smooth the KF_X and KF_Y positions XSmoothTotal += posPtr->X; posPtr->X = XSmoothTotal / SmoothBy; XSmoothTotal -= posPtr->X; YSmoothTotal += posPtr->Y; posPtr->Y = YSmoothTotal / SmoothBy; YSmoothTotal -= posPtr->Y; ZSmoothTotal += posPtr->Height; posPtr->Height = ZSmoothTotal / SmoothBy; ZSmoothTotal -= posPtr->Height; } else { SmoothRunning = false; // pc.printf("filterX = %f\r\n",xFilter.lastValue()); posPtr->X = xFilter.lastValue(); posPtr->Y = yFilter.lastValue(); posPtr->Height = zFilter.lastValue(); } posPtr->roll = rollFilter.addPoint(posPtr->roll); posPtr->pitch = pitchFilter.addPoint(posPtr->pitch); posPtr->yaw = yawFilter.addPoint(posPtr->yaw); } bool VIPSSerial::setFilters(struct UserSettings_s *settings) { if (settings->FilterXY) { if (!xFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate)) return false; if (!yFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate)) return false; } else { xFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate); yFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate); } if (settings->FilterZ) { if (!zFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate)) return false; } else { zFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate); } if (settings->FilterRoll) { if (!rollFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate)) return false; } else { rollFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate); } if (settings->FilterPitch) { if (!pitchFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate)) return false; } else { pitchFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate); } if (settings->FilterYaw) { if (!yawFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate)) return false; } else { yawFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate); } return true; }