Diff: PS4Serial.cpp

Revision:

5:3b887515b46e

Parent:

2:ebf134f6c2ef

Child:

6:9d766e152bc5

--- a/PS4Serial.cpp	Fri Sep 07 01:50:08 2018 +0000
+++ b/PS4Serial.cpp	Fri Sep 28 11:22:44 2018 +0000
@@ -1,6 +1,6 @@
 #include "PS4Serial.h"
 
-PS4Serial::PS4Serial(PinName tx, PinName rx) : WirelessSerial(tx, rx, 115200), rp(0)
+PS4Serial::PS4Serial(PinName tx, PinName rx) : WirelessSerial(tx, rx, 115200), rp(0), check(0)
 {
     for(int i = 0; i < 8; i++) ControllerData[i] = 2 <= i && i <= 5 ? 127 : 0;
     WirelessSerial.attach(this, &PS4Serial::WirelessRecive, RawSerial::RxIrq);
@@ -13,12 +13,15 @@
     char data = WirelessSerial.getc();
     if(data == 0xff) {
         rp = 0;
+        check = 0;
         for (int i = 0; i < 8; i++) ControllerData_OLD[i] = ControllerData[i];
-    } else if(rp > 8) {
-        //nothing
+    } else if(rp >= 8) {
+        if((check & 0x7f) == data)
+            for (int i = 0; i < 8; i++) ControllerData[i] = Receive[i];
     } else {
-        ControllerData[rp] = data;
+        Receive[rp] = data;
         rp++;
+        check += data;
     }
     t.start();
 }