Enable Gyro when data is not present

- detect that gyro data received from Steam Deck controller is 0 - when lack of gyro data detected, send a command via hidraw to enable it again Closes #64 Fixes #45 - workaround no longer needed.
kmicki · Nov 10, 2023 · 70a2c04 · 70a2c04
1 parent fa420f9
commit 70a2c04
Show file tree

Hide file tree

Showing 8 changed files with 85 additions and 5 deletions.
diff --git a/inc/hiddev/hidapidev.h b/inc/hiddev/hidapidev.h
@@ -18,6 +18,9 @@ namespace kmicki::hiddev
         int Read(std::vector<char> & data);
         bool Close();
         bool IsOpen();
+        bool EnableGyro();
+        bool Write(std::vector<char> & data);
+        bool Write(std::vector<unsigned char> & data);
 
         private:
         uint16_t vId;

diff --git a/inc/hiddev/hiddevreader.h b/inc/hiddev/hiddevreader.h
@@ -81,6 +81,8 @@ namespace kmicki::hiddev
         // Is grabbing frames being stopped right now?
         bool IsStopping();
 
+        void SetNoGyro(SignalOut& _noGyro);
+
         private:
 
         // Pipeline threads
@@ -128,6 +130,8 @@ namespace kmicki::hiddev
             ReadDataApi(uint16_t const& vId, uint16_t const& pId, const int& _interfaceNumber, int const& _frameLen, int const& _scanTimeUs);
             ~ReadDataApi();
 
+            void SetNoGyro(SignalOut& _noGyro);
+
             protected:
 
             void Execute() override;
@@ -137,6 +141,8 @@ namespace kmicki::hiddev
             uint16_t pId;
             int interfaceNumber;
             int timeout;
+
+            SignalOut *noGyro;
         };
 
         class ProcessData : public Thread
@@ -197,6 +203,7 @@ namespace kmicki::hiddev
         std::vector<std::unique_ptr<Thread>> pipeline;
         ServeFrame * serve;
         ReadData* readData;
+        ReadDataApi* readDataApi;
 
         // Mutex
         std::mutex startStopMutex;

diff --git a/inc/sdgyrodsu/cemuhookadapter.h b/inc/sdgyrodsu/cemuhookadapter.h
@@ -5,6 +5,7 @@
 #include "cemuhook/cemuhookprotocol.h"
 #include "hiddev/hiddevreader.h"
 #include "pipeline/serve.h"
+#include "pipeline/signalout.h"
 
 namespace kmicki::sdgyrodsu
 {
@@ -28,6 +29,8 @@ namespace kmicki::sdgyrodsu
         cemuhook::protocol::MotionData GetMotionData(SdHidFrame const& frame, float &lastAccelRtL, float &lastAccelFtB, float &lastAccelTtB);
         static void SetMotionData(SdHidFrame const& frame, cemuhook::protocol::MotionData &data, float &lastAccelRtL, float &lastAccelFtB, float &lastAccelTtB);
 
+        SignalOut NoGyro;
+
         private:
         bool ignoreFirst;
         bool isPersistent;
@@ -43,6 +46,7 @@ namespace kmicki::sdgyrodsu
         float lastAccelTtB;
 
         int toReplicate;
+        int noGyroCooldown;
 
         pipeline::Serve<hiddev::HidDevReader::frame_t> * frameServe;
     };

diff --git a/src/hiddev/hidapidev.cpp b/src/hiddev/hidapidev.cpp
@@ -87,4 +87,35 @@ namespace kmicki::hiddev
 
         return readCnt;
     }
+
+    bool HidApiDev::Write(std::vector<char> & data)
+    {
+        if(dev == nullptr)
+            return false;
+
+        auto writeCnt = hid_write(dev,(unsigned char*)(data.data()),data.size());
+
+        return writeCnt == data.size();
+    }
+
+    bool HidApiDev::Write(std::vector<unsigned char> & data)
+    {
+        if(dev == nullptr)
+            return false;
+
+        auto writeCnt = hid_write(dev,data.data(),data.size());
+
+        return writeCnt == data.size();
+    }
+
+    bool HidApiDev::EnableGyro()
+    {                              
+        std::vector<unsigned char> cmd = {   0x00 // TUTAJ
+                                    , 0x87, 0x0f, 0x30, 0x18, 0x00, 0x07, 0x07, 0x00, 0x08, 0x07, 0x00, 0x31, 0x02, 0x00, 0x18, 0x00
+                                    , 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+                                    , 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+                                    , 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+
+        return Write(cmd);
+    }
 }
diff --git a/src/hiddev/hiddevreader.cpp b/src/hiddev/hiddevreader.cpp
@@ -76,7 +76,7 @@ namespace kmicki::hiddev
     }
 
     HidDevReader::HidDevReader(int const& hidNo, int const& _frameLen, int const& scanTimeUs) 
-    : frameLen(_frameLen), startStopMutex()
+    : frameLen(_frameLen), startStopMutex(), readDataApi(nullptr)
     {
         if(hidNo < 0) throw std::invalid_argument("hidNo");
 
@@ -92,8 +92,9 @@ namespace kmicki::hiddev
     HidDevReader::HidDevReader(uint16_t const& vId, uint16_t const& pId, int const& interfaceNumber ,int const& _frameLen, int const& scanTimeUs) 
     : frameLen(_frameLen), startStopMutex()
     {
-        auto* readDataOp = new ReadDataApi(vId, pId, interfaceNumber, _frameLen, scanTimeUs);
-        ConstructPipeline(readDataOp, _frameLen, scanTimeUs,false);
+        readDataApi = new ReadDataApi(vId, pId, interfaceNumber, _frameLen, scanTimeUs);
+
+        ConstructPipeline(readDataApi, _frameLen, scanTimeUs,false);
     }
 
 
@@ -163,4 +164,10 @@ namespace kmicki::hiddev
 
         return false;
     }
+
+    void HidDevReader::SetNoGyro(SignalOut &_noGyro)
+    {
+        if(readDataApi)
+            readDataApi->SetNoGyro(_noGyro);
+    }
 }
diff --git a/src/hiddev/hiddevreader/readdataapi.cpp b/src/hiddev/hiddevreader/readdataapi.cpp
@@ -11,8 +11,13 @@ namespace kmicki::hiddev
 
     // Definition - ReadDataApi
     HidDevReader::ReadDataApi::ReadDataApi(uint16_t const& _vId, uint16_t const& _pId, const int& _interfaceNumber, int const& _frameLen, int const& _scanTimeUs)
-    : vId(_vId), pId(_pId), ReadData(_frameLen), timeout(cApiScanTimeToTimeout*_scanTimeUs/1000),interfaceNumber(_interfaceNumber)
+    : vId(_vId), pId(_pId), ReadData(_frameLen), timeout(cApiScanTimeToTimeout*_scanTimeUs/1000),interfaceNumber(_interfaceNumber),noGyro(nullptr)
     { }
+
+    void HidDevReader::ReadDataApi::SetNoGyro(SignalOut &_noGyro)
+    {
+        noGyro = &_noGyro;
+    }
 
     void HidDevReader::ReadDataApi::Execute()
     {
@@ -31,6 +36,15 @@ namespace kmicki::hiddev
             if(!ShouldContinue())
                 break;
 
+            if(noGyro && noGyro->TrySignal())
+            {
+                if(dev.EnableGyro())
+                    Log("HidDevReader::ReadDataApi: Gyro reenabled.");
+                else
+                    Log("HidDevReader::ReadDataApi: Gyro reenaling failed.");
+                continue;
+            }
+
             auto readCnt = dev.Read(*data);
 
             if(readCnt == 0)

diff --git a/src/main.cpp b/src/main.cpp
@@ -123,6 +123,7 @@ int main()
     reader.SetStartMarker({ 0x01, 0x00, 0x09, 0x40 }); // Beginning of every Steam Decks' HID frame
 
     CemuhookAdapter adapter(reader);
+    reader.SetNoGyro(adapter.NoGyro);
     Server server(adapter);
 
     uint32_t lastInc = 0;

diff --git a/src/sdgyrodsu/cemuhookadapter.cpp b/src/sdgyrodsu/cemuhookadapter.cpp
@@ -99,7 +99,7 @@ namespace kmicki::sdgyrodsu
     : reader(_reader),
       lastInc(0),
       lastAccelRtL(0.0),lastAccelFtB(0.0),lastAccelTtB(0.0),
-      isPersistent(persistent), toReplicate(0)
+      isPersistent(persistent), toReplicate(0), noGyroCooldown(0)
     {
         Log("CemuhookAdapter: Initialized. Waiting for start of frame grab.",LogLevelDebug);
     }
@@ -116,6 +116,9 @@ namespace kmicki::sdgyrodsu
     int const& CemuhookAdapter::SetMotionDataNewFrame(MotionData &motion)
     {
         static const int64_t cMaxDiffReplicate = 100;
+        static const int cNoGyroCooldownFrames = 250;
+
+        if(noGyroCooldown > 0) --noGyroCooldown;
 
         auto const& dataFrame = frameServe->GetPointer();
 
@@ -133,6 +136,16 @@ namespace kmicki::sdgyrodsu
                 auto lock = frameServe->GetConsumeLock();
                 //Log("CONSUME LOCK ACQUIRED.");
                 auto const& frame = GetSdFrame(*dataFrame);
+
+                if( noGyroCooldown <= 0
+                    &&  frame.AccelAxisFrontToBack == 0 && frame.AccelAxisRightToLeft == 0 
+                    &&  frame.AccelAxisTopToBottom == 0 && frame.GyroAxisFrontToBack == 0 
+                    &&  frame.GyroAxisRightToLeft == 0 && frame.GyroAxisTopToBottom == 0)
+                {
+                    NoGyro.SendSignal();
+                    noGyroCooldown = cNoGyroCooldownFrames;
+                }
+
                 int64_t diff = (int64_t)frame.Increment - (int64_t)lastInc;
 
                 if(lastInc != 0 && diff < 1 && diff > -100)