Update files

SerialPasstrough.h

@@ -1,44 +1,120 @@
 #include "TransferDataStructure.h"
 
-dataStruct2 GetSerialData(dataStruct2 dataIn, bool debug = false)
+String getValue(String data, char separator, int index)
 {
-  if (Serial.available())
+  int found = 0;
+  int strIndex[] = { 0, -1 };
+  int maxIndex = data.length() - 1;
+
+  for (int i = 0; i <= maxIndex && found <= index; i++) {
+    if (data.charAt(i) == separator || i == maxIndex) {
+      found++;
+      strIndex[0] = strIndex[1] + 1;
+      strIndex[1] = (i == maxIndex) ? i + 1 : i;
+    }
+  }
+  return found > index ? data.substring(strIndex[0], strIndex[1]) : "";
+}
+
+dataStruct2 GetSerialData(dataStruct2 &dataIn, bool debug = false)
+{
+  if (Serial.available() && Serial.peek() != -1)
   {
-    String data = Serial.readStringUntil('\r');
-    int maxIndex = data.length() - 1;
-    int index = 0;
-    int next_index;
-    String data_word;
-    String channel;
-    String angle;
-    do {
-      next_index = data.indexOf(',', index);
+    //    String data = Serial.readStringUntil('\r');
+    //    while (Serial.read() >= 0);
+    //    int maxIndex = data.length() - 1;
+    //    int index = 0;
+    //    int next_index;
+    //    String data_word;
+    //    String channel;
+    //    String angle;
+    //    do {
+    //      next_index = data.indexOf(',', index);
+    //
+    //      data_word = data.substring(index, next_index);
+    //      channel = data_word.substring(0, data_word.indexOf('_'));
+    //      angle = data_word.substring(data_word.indexOf('_') + 1);
+    //
+    //      if (isDigit(angle[0])) // prevent error overwriting first entry.
+    //        dataIn.servoPositions[channel.toInt()] = angle.toFloat();
+    //
+    //      index = next_index + 1;
+    //    } while ((next_index != -1) && (next_index < maxIndex));
 
-      data_word = data.substring(index, next_index);
-      channel = data_word.substring(0, data_word.indexOf('_'));
-      angle = data_word.substring(data_word.indexOf('_') + 1);
 
-      if (isDigit(angle[0])) // prevent error overwriting first entry.
-        dataIn.servoPositions[channel.toInt()] = angle.toFloat();
-
-      index = next_index + 1;
-    } while ((next_index != -1) && (next_index < maxIndex));
-
-    dataIn.remotePasstroughControl = true;
-
-    if (debug)
-    {
-      Serial.println("Enabled Servo Passtrough mode!");
-
-      Serial.println("Output line: ");
+    /*String s = Serial.readStringUntil('s');
       for (int i = 0; i < 18; i++)
       {
-        Serial.print(i);
-        Serial.print(", ");
-        Serial.print(dataIn.servoPositions[i]);
+      Serial.setTimeout(10);
+      String c = Serial.readStringUntil('_');
+      String v = Serial.readStringUntil(',');
+      String cc = Serial.readStringUntil('=');
+      int channel = c.toInt();
+      int value = v.toInt();
+      int crc = cc.toInt();
+
+      if ((crc == (channel) * value) && value >= 0 && value <= 180)
+        dataIn.servoPositions[channel] = value;*/
+    //byte recv[18 + 1];
+    if (Serial.available() >= 18 + 1 + 2 + 30)
+    {
+      bool recc = false;
+      byte recv[256] = {0};
+      //Serial.readBytesUntil(0xffffff, recv, 255);
+      Serial.readBytesUntil(0xff, recv, 255);
+      for (int i = 0; i < 255 - 19; i++)
+      {
+        //  if (recv[i + 1] == 0xff && recv[i + 2] == 0xff && recv[i + 3] == 0xff)
+        if (recv[i + 19] == 0xfe)
+        {
+          for (int j = 0; j < 18 + 1; j++)
+          {
+            recv[j] = recv[(i) + j];
+          }
+          recc = true;
+
+          break;
+        }
+      }
+
+
+      //s.getBytes(recv, s.length());
+      byte crc = 0x00;
+      for (int i = 0; i < 18; i++)
+      {
+        crc += recv[i];
+      }
+
+
+      if (crc == recv[18])
+      {
+
+        if (recc)
+        {
+          for (int i = 0; i < 18; i++)
+          {
+            dataIn.servoPositions[i] = recv[i];
+          }
+        }
+      }
+      //}
+
+      dataIn.remotePasstroughControl = true;
+
+      if (debug)
+      {
+        Serial.println("Enabled Servo Passtrough mode!");
+
+        Serial.println("Output line: ");
+        for (int i = 0; i < 18; i++)
+        {
+          Serial.print(i);
+          Serial.print(", ");
+          Serial.print(dataIn.servoPositions[i]);
+          Serial.println("");
+        }
         Serial.println("");
       }
-      Serial.println("");
     }
   }