Diff: FIZ_readers/FIZ_digiPower.cpp

Revision:

18:ad407a2ed4c9

Parent:

17:5ce3fe98e76d

--- a/FIZ_readers/FIZ_digiPower.cpp	Fri Apr 30 12:21:01 2021 +0000
+++ b/FIZ_readers/FIZ_digiPower.cpp	Mon May 17 11:41:57 2021 +0000
@@ -22,12 +22,13 @@
     uint8_t dIn = _port.getc();
     inputBuffer[inputPtr] = dIn;
     if (inputPtr==0) {
-        if (dIn == 0x07) // correct length
+        if (dIn == 0x07) { // correct length
             inputPtr++;
+        }
     } else if (inputPtr == 1) {
-        if (dIn == 0x60) // correct ID
+        if (dIn == 0x60) { // correct ID
             inputPtr++;
-        else { // wrong ID
+        } else { // wrong ID
             if (dIn == 0x07) {
                 inputBuffer[0] = 7;
                 inputPtr = 1;
@@ -35,28 +36,33 @@
                 inputPtr = 0;
             }
         }
-    } else if (inputPtr == (2+7+1)) { // full packet = 2 byte header, 7 byte data, 1 byte cs
-        parsePacket();
-        inputPtr = 0;
     } else { // waiting for data.
         inputPtr++;
+        if (inputPtr == (2+7+1)) { // full packet = 2 byte header, 7 byte data, 1 byte cs
+            parsePacket();
+            inputPtr = 0;
+        }
     }
 }
 
 void FIZDigiPower::parsePacket()
-{    
+{
+//    pc.puts("FIZ parse\r\n");
     if (inputBuffer[0] != 0x07)
         return;
     if (inputBuffer[1] != 0x60)
         return;
-    // checksum is sum of all bytes mod 256 = 00      
+    // checksum is sum of all bytes mod 256 = 00
     int cs = 0;
-    for (int i=0;i<inputPtr;i++) {
+    for (int i=0; i<inputPtr; i++) {
         cs += inputBuffer[i];
-        }
-    if (cs & 0x00ff) // cs & 0x000000ff should give 0
-    return;
-    
+//        pc.printf("byte 0x%02X cs 0x%04X\r\n",inputBuffer[i],cs);
+    }
+    if (cs & 0x00ff) { // cs & 0x000000ff should give 0
+//       pc.printf("FIZ Checksum Fail 0x%04X\r\n",cs);
+        return;
+    }
+//    pc.puts("FIZ good\r\n");
     uint16_t zoom_Position = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3];
     uint16_t focus_Position = ((uint16_t)inputBuffer[4])<<8 | inputBuffer[5];
     uint16_t iris_Position = ((uint16_t)inputBuffer[6])<<8 | inputBuffer[7];
@@ -65,6 +71,6 @@
     _focus = focus_Position;
     _iris = iris_Position;
     _zoom = zoom_Position;
-    
+
     newData = true;
 }