Parcourir la source

Fixed FrSky protocal parsing bug

Thomas Buck il y a 7 ans
Parent
révision
c8f3d02ee0
1 fichiers modifiés avec 35 ajouts et 2 suppressions
  1. 35
    2
      frsky.cpp

+ 35
- 2
frsky.cpp Voir le fichier

@@ -32,19 +32,51 @@ void FrSky::poll() {
32 32
 
33 33
     uint8_t c = serial->read();
34 34
     if (c == delimiter) {
35
-        if (bufferIndex < minPacketSize) {
35
+#ifdef DEBUG_OUTPUT
36
+        DEBUG_OUTPUT.print("Got delimiter at ");
37
+        DEBUG_OUTPUT.println(bufferIndex);
38
+#endif
39
+        if (bufferIndex < (minPacketSize - 1)) {
40
+#ifdef DEBUG_OUTPUT
41
+            DEBUG_OUTPUT.print("Reset to 0: ");
42
+            DEBUG_OUTPUT.print(bufferIndex);
43
+            DEBUG_OUTPUT.print(" ! <= ");
44
+            DEBUG_OUTPUT.println(minPacketSize - 1);
45
+#endif
36 46
             bufferIndex = 0;
37 47
         }
38 48
         if (bufferIndex >= bufferSize) {
49
+#ifdef DEBUG_OUTPUT
50
+            DEBUG_OUTPUT.print("too large: ");
51
+            DEBUG_OUTPUT.print(bufferIndex);
52
+            DEBUG_OUTPUT.print(" / ");
53
+            DEBUG_OUTPUT.println(bufferSize);
54
+#endif
39 55
             bufferIndex = bufferSize - 1;
40 56
         }
41 57
         buffer[bufferIndex++] = c;
42
-        if (bufferIndex > minPacketSize) {
58
+        if (bufferIndex >= minPacketSize) {
59
+#ifdef DEBUG_OUTPUT
60
+            DEBUG_OUTPUT.println("Handling...");
61
+#endif
43 62
             handleMessage();
44 63
             bufferIndex = 0;
45 64
         }
46 65
     } else if ((bufferIndex > 0) && (bufferIndex < bufferSize)) {
47 66
         buffer[bufferIndex++] = c;
67
+#ifdef DEBUG_OUTPUT
68
+        DEBUG_OUTPUT.print("Got ");
69
+        DEBUG_OUTPUT.print(c);
70
+        DEBUG_OUTPUT.print(" at ");
71
+        DEBUG_OUTPUT.println(bufferIndex - 1);
72
+#endif
73
+    } else {
74
+#ifdef DEBUG_OUTPUT
75
+        DEBUG_OUTPUT.print("Invalid: ");
76
+        DEBUG_OUTPUT.print(bufferIndex);
77
+        DEBUG_OUTPUT.print(" / ");
78
+        DEBUG_OUTPUT.println(bufferSize);
79
+#endif
48 80
     }
49 81
 }
50 82
 
@@ -85,6 +117,7 @@ void FrSky::handleMessage() {
85 117
     DEBUG_OUTPUT.println("FrSky::handleMessage()");
86 118
     for (uint8_t i = 0; i < bufferIndex; i++) {
87 119
         DEBUG_OUTPUT.print(buffer[i], HEX);
120
+        DEBUG_OUTPUT.print(" ");
88 121
     }
89 122
     DEBUG_OUTPUT.println();
90 123
 #endif

Chargement…
Annuler
Enregistrer