Diff: FIZ_readers/FIZDigiPowerActive.cpp

Revision:

71:7305a35cee58

Parent:

26:7f66ac76cd5d

Child:

80:0b7f1b85b626

--- a/FIZ_readers/FIZDigiPowerActive.cpp	Thu Feb 03 11:51:25 2022 +0000
+++ b/FIZ_readers/FIZDigiPowerActive.cpp	Tue Feb 08 09:17:48 2022 +0000
@@ -4,23 +4,30 @@
 {
     inputPtr = 0;
     newData = false;
+    initialising = true; //not used as lens parameter reading is disabled
     _port.baud(38400);
+//    read_lens_parameters();
     _port.attach(callback(this, &FIZDigiPowerActive::OnRx));
     missedPackets=0;
+    min_zoom = 0;
+    max_zoom = 0;
 
 }
 
 void FIZDigiPowerActive::requestCurrent(void)
 {
+//    if (initialising) {
+//        requestLensParameters();
+//        return;
     if (missedPackets>5) {
         // configure output format
-        _port.putc(04);
-        _port.putc(0x70);
-        _port.putc(0x30);
-        _port.putc(0x31);
-        _port.putc(0x32);
-        _port.putc(0x53);
-        _port.putc(0xA6);
+        _port.putc(04); //data length (number of paramters)
+        _port.putc(0x70); //multi data setting
+        _port.putc(0x30); //iris
+        _port.putc(0x31); //zoom
+        _port.putc(0x32); //focus
+        _port.putc(0x53); //Extender
+        _port.putc(0xA6); //checksum (mod 256)
         missedPackets = 0;
     }
 
@@ -30,12 +37,121 @@
     missedPackets++;
 }
 
+//void FIZDigiPowerActive::requestLensParameters(void)
+//{
+//    if (min_zoom) {
+//        _port.putc(00);
+//        _port.putc(0x15);
+//        _port.putc(0xEB);
+//    } else {
+//        _port.putc(00);
+//        _port.putc(0x14);
+//        _port.putc(0xEC);
+//    missedPackets++;
+//}
+
+
+void FIZDigiPowerActive::read_lens_parameters()
+{
+    _port.putc(00);
+    _port.putc(0x15); //0x16 for MOD
+    _port.putc(0xEB); //checksum is EA for MOD
+    wait_ms(50);
+    uint16_t raw_val = 0;
+    uint8_t tries = 5;
+    while (tries) {
+        tries--;
+        uint8_t dIn =_port.getc();
+        inputBuffer[inputPtr] = dIn;
+//        printf("%02x ", dIn);
+        if (inputPtr==0) {
+            if (dIn == 0x02) { // correct length
+                inputPtr++;
+            }
+        } else if (inputPtr == 1) {
+            if (dIn == 0x15) { // correct ID
+                inputPtr++;
+            } else { // wrong ID
+                if (dIn == 0x02) {
+                    inputBuffer[0] = 7;
+                    inputPtr = 1;
+                } else {
+                    inputPtr = 0;
+                }
+            }
+        } else { // waiting for data.
+            inputPtr++;
+            if (inputPtr == (2+2+1)) { // full packet = 2 byte header, 2 byte data, 1 byte cs
+                raw_val = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3];
+                min_zoom = (uint16_t) (FIZDigiPowerActive::get_16bit_float(raw_val)*1000);
+                pc.printf("Min Focus: %d\r\n", min_zoom);
+                break;
+            }
+        }
+    }
+    inputPtr = 0;
+    _port.putc(00);
+    _port.putc(0x14);
+    _port.putc(0xEC); //ec for 0x14
+    wait_ms(50);
+    raw_val = 0;
+    tries = 5;
+    while (tries) {
+        tries--;
+        uint8_t dIn =_port.getc();
+        inputBuffer[inputPtr] = dIn;
+//        printf("%02x ", dIn);
+        if (inputPtr==0) {
+            if (dIn == 0x02) { // correct length
+                inputPtr++;
+            }
+        } else if (inputPtr == 1) {
+            if (dIn == 0x14) { // correct ID
+                inputPtr++;
+            } else { // wrong ID
+                if (dIn == 0x02) {
+                    inputBuffer[0] = 7;
+                    inputPtr = 1;
+                } else {
+                    inputPtr = 0;
+                }
+            }
+        } else { // waiting for data.
+            inputPtr++;
+            if (inputPtr == (2+2+1)) { // full packet = 2 byte header, 2 byte data, 1 byte cs
+                raw_val = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3];
+                max_zoom = (uint16_t) (FIZDigiPowerActive::get_16bit_float(raw_val)*1000);
+                pc.printf("Max Focus %d\r\n", max_zoom);
+                break;
+            }
+        }
+    }
+
+//quantise it
+
+
+
+}
+
+float FIZDigiPowerActive::get_16bit_float(uint16_t val)
+{
+    //Non Standard 16bit float, 4 bit Exponent, 12bit mantissa
+    int expo = val >> 12;
+    if (expo & 0x8)
+        expo|= ~0xF;
+    float ret = (float) ((val &0xFFF) * pow((double)10.0, (double)expo));
+    return ret;
+}
+
 //  expect hex data format:
 //  [dat len] [command] [data] [cs]
-// only command we care about is multi reply which is 7 bytes of data - zoom, focus, iris, switch 4
-// 07 60 [zoom1][zoom2] .... [cs]
+// only command we care about is multi reply which is 7 bytes of data - iris, zoom, focus, switch 4
+// 07 60 [iris1][iris2] .... [cs]
 void FIZDigiPowerActive::OnRx(void)
 {
+//    if (initialising){
+//        read_lens_parameters();
+//        return;        
     uint8_t dIn = _port.getc();
 //    pc.printf("R");
     inputBuffer[inputPtr] = dIn;
@@ -83,14 +199,14 @@
         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];
+    uint16_t iris_Position = ((uint16_t)inputBuffer[2])<<8 | inputBuffer[3];
+    uint16_t zoom_Position = ((uint16_t)inputBuffer[4])<<8 | inputBuffer[5];
+    uint16_t focus_Position = ((uint16_t)inputBuffer[6])<<8 | inputBuffer[7];
 
 // MAY NEED TO SCALE THESE
-    _focus = focus_Position;
-    _iris = iris_Position;
-    _zoom = zoom_Position;
+    _focus = (uint32_t)(focus_Position * UserSettings.focus_scale) + UserSettings.focus_offset;
+    _iris = (uint16_t) (iris_Position * UserSettings.iris_scale) + UserSettings.iris_offset;
+    _zoom = (uint16_t) (zoom_Position * UserSettings.zoom_scale) + UserSettings.zoom_offset;
 
     newData = true;
 }