Diff: VIPSSerialProtocol.cpp

Revision:

35:7ecf25d9c414

Parent:

28:7600b18ccb7a

Child:

36:fff2a71bbf86

--- a/VIPSSerialProtocol.cpp	Wed Aug 04 16:43:45 2021 +0000
+++ b/VIPSSerialProtocol.cpp	Thu Aug 05 13:44:45 2021 +0000
@@ -402,13 +402,17 @@
 
 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);            
+            pc.write("Seeded Filter\r\n",11);
         }
         //smooth the KF_X and KF_Y positions
         XSmoothTotal += posPtr->X;
@@ -424,5 +428,54 @@
         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;
+}
\ No newline at end of file