Diff: FIZReader.cpp

Revision:

5:7994913a15fe

Parent:

3:14d241e29be3

Child:

6:61274e214f46

--- a/FIZReader.cpp	Fri Jan 29 14:50:21 2021 +0000
+++ b/FIZReader.cpp	Mon Feb 01 17:29:57 2021 +0000
@@ -14,9 +14,12 @@
 void FIZReader::requestCurrent(void)
 {
     _port.putc(0x02);
-    _port.putc(0x0c);
-    _port.putc(0x00);
-    _port.putc(0xD5);
+    _port.putc('0');
+    _port.putc('C');
+    _port.putc('0');
+    _port.putc('0');
+    _port.putc('D');
+    _port.putc('5');
     _port.putc(0x03);
 }
 
@@ -37,25 +40,41 @@
     while (_port.readable()) {
         inputBuffer[inputPtr] = _port.getc();
         if (inputPtr==0) {
-            if (inputBuffer[0] == 0x0C)
-                inputPtr++;
-        } else if (inputPtr==1) {
-            if (inputBuffer[1] == 0x07)
+            if (inputBuffer[inputPtr] == 0x02)
                 inputPtr++;
-            else
-                inputPtr = 0;
-        }
-        if (inputPtr == 9) {
+        } else if (inputBuffer[inputPtr] == 0x03) {
             parsePacket();
             inputPtr = 0;
-        } else
+        } else {
             inputPtr++;
+            if (inputPtr == InBufferSize)
+                inputPtr = 0;
+        }
     }
 }
+
 void FIZReader::parsePacket()
 {
-    _focus = inputBuffer[4] | ((uint32_t)inputBuffer[3])<<8 | ((uint32_t)inputBuffer[2])<<16;
-    _iris = inputBuffer[6] | ((uint16_t)inputBuffer[5])<<8;
-    _zoom = inputBuffer[8] | ((uint16_t)inputBuffer[7])<<8;
+//  expect ASCII string of "0C07ffffffiiiizzzzcs" where ffffff is 6 char hex value for focus
+//  iiii is 4 char hex value for tstop, zzzz is 4 char hex value for zoom and cs is 2 char checksum
+    if (inputPtr != 21)
+        return;
+    if (inputBuffer[1] != '0')
+        return;
+    if (inputBuffer[2] != 'C')
+        return;
+        _focus = 0;
+        _iris = 0;
+        _zoom = 0;
+    for (int count = 0;count<6;count++) {
+        _focus*=16;
+        _focus += hexValue(inputBuffer[5+count]);
+    }
+    for (int count = 0;count<4;count++) {
+        _iris*=16;
+        _iris += hexValue(inputBuffer[11+count]);
+        _zoom*=16;
+        _zoom += hexValue(inputBuffer[15+count]);
+    }
     newData = true;
 }