Diff: FIZ_readers/FIZCanon.cpp

Revision:

39:3cd9e498b5c6

Parent:

31:2eec69c777a7

--- a/FIZ_readers/FIZCanon.cpp	Mon Aug 09 10:37:51 2021 +0000
+++ b/FIZ_readers/FIZCanon.cpp	Tue Aug 10 14:48:56 2021 +0000
@@ -63,60 +63,60 @@
 
 void FIZCanon::parsePacket()
 {
-    //pc.puts("FIZ parse\r\n");
+//    pc.puts("FIZ parse\r\n");
+//    pc.putc('P');
     int read_pos = 0;
     bool valid = false;
 
-
     while (read_pos != inputPtr) {
-        switch(inputBuffer[read_pos]) {
-            case CANON_START:
+        if (!valid) {
+            if (inputBuffer[read_pos] == CANON_START) {
                 valid = true;
-                break;
-            case CANON_ZOOM:
-                if (valid) {
-                    _zoom = (((uint32_t)inputBuffer[read_pos+1])<<14 | inputBuffer[read_pos +2]<<7 | inputBuffer[read_pos +3]);
+            }
+        } else {
+            switch(inputBuffer[read_pos]) {
+                case CANON_ZOOM:
+//                    pc.putc('Z');
+                    _zoom = (((uint32_t)inputBuffer[read_pos+1])<<14 | inputBuffer[read_pos +2]<<7 | inputBuffer[read_pos +3])/60;
 //                pc.putc(0x01);
 //                pc.putc(inputBuffer[read_pos+1]);
 //                pc.putc(inputBuffer[read_pos+2]);
 //                pc.putc(inputBuffer[read_pos+3]);
-                    read_pos += 5; //START BYTE(0xC1), 3 BYTES OF DATA, END BYTE (0xBF)
-                    valid = false;
-                }
-                break;
-            case CANON_FOCUS:
-                if (valid) {
-                    _focus = (((uint32_t)inputBuffer[read_pos+1])<<14 | inputBuffer[read_pos +2]<<7 | inputBuffer[read_pos +3]);
+                    read_pos += 4; // type + 3 BYTES OF DATA
+                    break;
+                case CANON_FOCUS:
+//                    pc.putc('F');
+                    _focus = 1000 - (((uint32_t)inputBuffer[read_pos+1])<<14 | inputBuffer[read_pos +2]<<7 | inputBuffer[read_pos +3])/60;
 //                pc.putc(0x02);
 //                pc.putc(inputBuffer[read_pos+1]);
 //                pc.putc(inputBuffer[read_pos+2]);
 //                pc.putc(inputBuffer[read_pos+3]);
-                    read_pos += 5;
-                    valid = false;
-                }
-                break;
-            case CANON_IRIS:
-                if (valid) {
+                    read_pos += 4;
+                    break;
+                case CANON_IRIS:
+                    //IRIS NOT SCALED CORRECTLY -> 0x8000 - 0xD000, 0x1000 between FStops -> Fix Later
+//                    pc.putc('I');
                     _iris = (((uint32_t)inputBuffer[read_pos +1])<<14 | inputBuffer[read_pos +2]<<7 | inputBuffer[read_pos +3]);
 //                pc.putc(0x03);
 //                pc.putc(inputBuffer[read_pos+1]);
 //                pc.putc(inputBuffer[read_pos+2]);
 //                pc.putc(inputBuffer[read_pos+3]);
-                    read_pos += 5;
-                    valid = false;
-                }
-                break;
-            default:
-                read_pos++;
-                valid = false;
-                break;
-        }
+                    read_pos += 4;
+                    break;
+                default:
+                    //pc.putc('D');
+                    break;
+            }
+            valid = false;
+        } // if valid
+        read_pos++;
+    } // while
 
 //// MAY NEED TO SCALE THESE
 //        _focus = focus_Position;
 //        _iris = iris_Position;
 //        _zoom = zoom_Position;
 //
-    }
+
     newData = true;
-}
+}
\ No newline at end of file