Changes in order to reduce jankiness.

Added gyro deadzone to adapter.
Added acceleration smoothing to adapter (no added delay, only minimum change).
Added generation of lost frames in order to try and keep constant packet frequency.
Prevented resending old packets in case of istream read block.

inc/hiddev/hiddevreader.h

@@ -78,6 +78,7 @@ namespace kmicki::hiddev
         protected:
 
         frame_t frame;
+
         int frameLen;
         std::string inputFilePath;
         bool preReadingLock;
@@ -113,7 +114,7 @@ namespace kmicki::hiddev
 
         std::mutex lockMutex, clientsMutex, startStopMutex;
 
-        void LossAnalysis();
+        void LossAnalysis(uint32_t diff);
 
         int lossPeriod;
         bool lossAnalysis;

inc/sdgyrodsu/cemuhookadapter.h

@@ -26,6 +26,7 @@ namespace kmicki::sdgyrodsu
         private:
         cemuhook::protocol::MotionData data;
         hiddev::HidDevReader & reader;
+
     };
 }
 

src/cemuhook/cemuhookserver.cpp

@@ -1,4 +1,5 @@
 #include "cemuhookserver.h"
+
 #include <sys/socket.h>
 #include <sys/types.h>
 #include <stdexcept>

src/hiddev/hiddevreader.cpp

@@ -1,4 +1,5 @@
 #include "hiddevreader.h"
+#include "cemuhookadapter.h"
 
 #include <iostream>
 #include <stdexcept>
@@ -100,7 +101,7 @@ namespace kmicki::hiddev
 
     HidDevReader::frame_t const& HidDevReader::GetNewFrame(void * client)
     {
-        while(clients.find(client) != clients.end() && !stopTask) std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        while(clients.find(client) != clients.end() && !stopTask) std::this_thread::sleep_for(std::chrono::microseconds(100));
         return GetFrame(client);
     }
 
@@ -149,15 +150,10 @@ namespace kmicki::hiddev
     // Extract HID frame from data read from /dev/usb/hiddevX
     void HidDevReader::processData(std::vector<char> const& bufIn) 
     {
-        clientsMutex.lock();
         // Each byte is encapsulated in an 8-byte long record
         for (int i = 0, j = BYTEPOS_INPUT; i < frame.size(); ++i,j+=INPUT_RECORD_LEN) {
             frame[i] = bufIn[j];
         }
-        writingLock = false;
-
-        clients.clear();
-        clientsMutex.unlock();
     }
 
     void HidDevReader::readTask(std::vector<char>** buf)
@@ -195,10 +191,8 @@ namespace kmicki::hiddev
         }
     }
 
-    void HidDevReader::LossAnalysis()
+    void HidDevReader::LossAnalysis(uint32_t diff)
     {
-        uint32_t const& newInc = *reinterpret_cast<uint32_t const*>(frame.data()+4);
-        uint32_t diff;
         if(lossAnalysis)
         {
             if(++lossPeriod > MAX_LOSS_PERIOD)
@@ -210,9 +204,8 @@ namespace kmicki::hiddev
 
             }
         }
-        if(lastInc > 0 && (diff = newInc - lastInc) > 1 && diff < 1000)
+        if(lastInc > 0 && diff > 1 && diff < 1000)
         {
-            std::cout << "Lost frames : " << diff-1 << " after " << lossPeriod << " frames" << std::endl;
             if(diff > 25)
             {
                 if(smallLossEncountered || !lossAnalysis)
@@ -232,15 +225,14 @@ namespace kmicki::hiddev
             }
             lossPeriod = 0;
         }
-        lastInc = newInc;
     }
 
     void HidDevReader::executeReadingTask()
     {
         auto& input = static_cast<std::ifstream&>(*inputStream);
-        std::vector<char> buf1(INPUT_RECORD_LEN*frameLen);
-        std::vector<char> buf2(INPUT_RECORD_LEN*frameLen);
-        std::vector<char>* buf = &buf1; // buffer swapper
+        std::vector<char> buf1_1(INPUT_RECORD_LEN*frameLen);
+        std::vector<char> buf1_2(INPUT_RECORD_LEN*frameLen);
+        std::vector<char>* buf = &buf1_1; // buffer swapper
 
         enter=exit=0;
         tick=rdy=wait=false;
@@ -303,16 +295,18 @@ namespace kmicki::hiddev
                 reconnectInput(input,inputFilePath);
             }
             else {
+                if( readingLock || preReadingLock )
+                    std::this_thread::sleep_for(std::chrono::microseconds(200));
                 // Start processing data in a separate thread unless previous processing has not finished
                 if( !readingLock && !preReadingLock )
                 {
                     writingLock = true;
                     auto bufProcess = buf;
                     // swap buffers
-                    if(buf == &buf1)
-                        buf = &buf2;
+                    if(buf == &buf1_1)
+                        buf = &buf1_2;
                     else
-                        buf = &buf1;
+                        buf = &buf1_1;
                     {
                         std::unique_lock<std::mutex> lock(m);
                         rdy = false;
@@ -321,7 +315,37 @@ namespace kmicki::hiddev
                     }
 
                     processData(*buf);
-                    LossAnalysis();
+
+                    uint32_t * newInc = reinterpret_cast<uint32_t *>(frame.data()+4);
+                    uint32_t diff = *newInc - lastInc;
+                    LossAnalysis(diff);
+
+                    if(diff > 1 && lastInc > 0 && diff <= 40)
+                    {
+                        *newInc = lastInc+1;
+                        // Fill with generated frames to avoid jitter
+                        auto fillPeriod = std::chrono::microseconds(3200/(diff-1));//3000/(diff-1));
+
+                        for (int i = 0; i < diff-1; i++)
+                        {
+                            writingLock=false;
+                            clientsMutex.lock();
+                            clients.clear();
+                            clientsMutex.unlock();
+                            std::this_thread::sleep_for(fillPeriod);
+                            if( !readingLock && !preReadingLock )
+                                writingLock = true;
+                            ++(*newInc);
+                        }
+                    }
+                    lastInc = *newInc;
+
+                    writingLock = false;
+
+                    clientsMutex.lock();
+                    clients.clear();
+                    clientsMutex.unlock();
+
                 }
             }
         }

src/main.cpp

@@ -11,6 +11,8 @@
 using namespace kmicki::sdgyrodsu;
 using namespace kmicki::hiddev;
 
+bool showIncrement = false;
+
 #define FRAME_LEN 64
 #define SCAN_PERIOD_US 3900
 

src/sdgyrodsu/cemuhookadapter.cpp

@@ -1,14 +1,19 @@
 #include "cemuhookadapter.h"
 #include "sdhidframe.h"
 
+#include <iostream>
+
 using namespace kmicki::cemuhook::protocol;
 
-#define SD_SCANTIME_US 4000;
-#define ACC_1G 16383;
-#define GYRO_1DEGPERSEC 16;
+#define SD_SCANTIME_US 4000
+#define ACC_1G 0x4000
+#define GYRO_1DEGPERSEC 16
+#define GYRO_DEADZONE 4
+#define ACCEL_SMOOTH 0x7F
 
 namespace kmicki::sdgyrodsu
 {
+
     MotionData GetMotionData(SdHidFrame const& frame)
     {
         MotionData data;
@@ -17,6 +22,17 @@ namespace kmicki::sdgyrodsu
 
         return data;
     }
+
+    int16_t SmoothAccel(int16_t &last, int16_t curr)
+    {
+        if(abs(curr - last) < ACCEL_SMOOTH)
+            return last;
+        else
+        {
+            last = curr;
+            return curr;
+        }
+    }
 
     void SetMotionData(SdHidFrame const& frame, MotionData &data)
     {
@@ -27,14 +43,29 @@ namespace kmicki::sdgyrodsu
 
         data.timestampL = (uint32_t)(timestamp & 0xFFFFFFFF);
         data.timestampH = (uint32_t)(timestamp >> 32);
+
+        static int16_t lastAccelRtL = 0;
+        static int16_t lastAccelFtB = 0;
+        static int16_t lastAccelTtB = 0;
 
-        data.accX = -(float)frame.AccelAxisRightToLeft/acc1G;
-        data.accY = -(float)frame.AccelAxisFrontToBack/acc1G;
-        data.accZ = (float)frame.AccelAxisTopToBottom/acc1G;
+        data.accX = -(float)SmoothAccel(lastAccelRtL,frame.AccelAxisRightToLeft)/acc1G;
+        data.accY = -(float)SmoothAccel(lastAccelFtB,frame.AccelAxisFrontToBack)/acc1G;
+        data.accZ = (float)SmoothAccel(lastAccelTtB,frame.AccelAxisTopToBottom)/acc1G;
+
+        auto gyroRtL = frame.GyroAxisRightToLeft;
+        auto gyroFtB = frame.GyroAxisFrontToBack;
+        auto gyroTtB = frame.GyroAxisTopToBottom;
 
-        data.pitch = (float)frame.GyroAxisRightToLeft/gyro1dps;
-        data.yaw = -(float)frame.GyroAxisFrontToBack/gyro1dps;
-        data.roll = (float)frame.GyroAxisTopToBottom/gyro1dps;
+        if(gyroRtL < GYRO_DEADZONE && gyroRtL > -GYRO_DEADZONE)
+            gyroRtL = 0;
+        if(gyroFtB < GYRO_DEADZONE && gyroFtB > -GYRO_DEADZONE)
+            gyroFtB = 0;
+        if(gyroTtB < GYRO_DEADZONE && gyroTtB > -GYRO_DEADZONE)
+            gyroTtB = 0;
+
+        data.pitch = (float)gyroRtL/gyro1dps;
+        data.yaw = -(float)gyroFtB/gyro1dps;
+        data.roll = (float)gyroTtB/gyro1dps;
     }
 
     CemuhookAdapter::CemuhookAdapter(hiddev::HidDevReader & _reader)
@@ -50,9 +81,26 @@ namespace kmicki::sdgyrodsu
 
     MotionData const& CemuhookAdapter::GetMotionDataNewFrame()
     {
-        SetMotionData(GetSdFrame(reader.GetNewFrame(this)),data);
-        reader.UnlockFrame(this);
-        return data;
+        static uint32_t lastInc = 0;
+        while(true)
+        {
+            auto const& frame = reader.GetNewFrame(this);
+
+            auto const& inc = *reinterpret_cast<uint32_t const*>(frame.data()+4);
+
+            if(inc <= lastInc && lastInc-inc < 40)
+            {
+                reader.UnlockFrame(this);
+            }
+            else
+            {
+                lastInc = inc;
+
+                SetMotionData(GetSdFrame(frame),data);
+                reader.UnlockFrame(this);
+                return data;
+            }
+        }
     }
 
     void CemuhookAdapter::StopFrameGrab()