Diff: VIPSSerialProtocol.cpp

Revision:

16:a8d3a0dbe4bf

Parent:

14:76083dc18b0d

Child:

18:ad407a2ed4c9

--- a/VIPSSerialProtocol.cpp	Mon Mar 08 17:29:31 2021 +0000
+++ b/VIPSSerialProtocol.cpp	Fri Apr 30 11:26:34 2021 +0000
@@ -26,6 +26,14 @@
     enableAllUpdates = false;
     newFormatMsg = false;
 
+    SmoothBy = 500;
+    XSmoothTotal = 0;
+    YSmoothTotal = 0;
+    ZSmoothTotal = 0;
+    SmoothRunning = false;
+
+    BypassMode = false;
+
     nextPosition= 0;
     _outputMask = 0x44;
     _port.baud(115200);
@@ -36,45 +44,54 @@
     _port.attach(callback(this, &VIPSSerial::onSerialRx));
 }
 
+void VIPSSerial::bypassTx(char byte)
+{
+    _port.putc(byte);
+}
+
 void VIPSSerial::onSerialRx(void)
 {
     while (_port.readable()) {
-        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)
+        if (BypassMode) {
+            vipsBypassRx(_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;
-                    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;
+                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;
+                    }
                 }
             }
         }
@@ -233,7 +250,7 @@
         return;
     }
 
-    led1=!led1;
+ //   led1=!led1;
 
     lastPositions[nextPosition].time = TimeSinceLastFrame.read_us();
     uint32_t mask = *(uint32_t*)(messageInBuffer+4);
@@ -302,6 +319,8 @@
         offset+=4;
     }
 
+    smoothOutputPacket(&(lastPositions[nextPosition].pos));
+
     if (enableAllUpdates) {
 //        printf("Add pos\r\n");
         outputPtr = &outputPosition;
@@ -315,6 +334,7 @@
     pointCount++;
 }
 
+
 // send a position output for the requested time. Times are based on the global TimeSinceLastFrame timer.
 position* VIPSSerial::sendPositionForTime(uint32_t timeValue)
 {
@@ -359,3 +379,28 @@
     return &lastPos.pos;
 }
 
+void VIPSSerial::smoothOutputPacket(position *posPtr)
+{
+    if (hyperSmoothEnabled) {
+        if (!SmoothRunning) {
+            XSmoothTotal = posPtr->X * (SmoothBy - 1);
+            YSmoothTotal = posPtr->Y * (SmoothBy - 1);
+            YSmoothTotal = posPtr->Height * (SmoothBy - 1);
+            SmoothRunning = true;
+        }
+        //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;
+    }
+}