Diff: FIZ_readers/FIZ_digiPower.cpp

Revision:

16:a8d3a0dbe4bf

Child:

17:5ce3fe98e76d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FIZ_readers/FIZ_digiPower.cpp	Fri Apr 30 11:26:34 2021 +0000
@@ -0,0 +1,70 @@
+#include "FIZ_digiPower.h"
+#include "LTCApp.h"
+FIZDigiPower::FIZDigiPower(const PinName Tx, const PinName Rx) : FIZReader(Tx,Rx)
+{
+    inputPtr = 0;
+    newData = false;
+    _port.baud(38400);
+    _port.attach(callback(this, &FIZDigiPower::OnRx));
+}
+
+void FIZDigiPower::requestCurrent(void)
+{
+// is listen only
+}
+
+//  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]
+void FIZDigiPower::OnRx(void)
+{
+    uint8_t dIn = _port.getc();
+    inputBuffer[inputPtr] = dIn;
+    if (inputPtr==0) {
+        if (dIn == 0x07) // correct length
+            inputPtr++;
+    } else if (inputPtr == 1) {
+        if (dIn == 0x60) // correct ID
+            inputPtr++;
+        else { // wrong ID
+            if (dIn == 0x07) {
+                inputBuffer[0] = 7;
+                inputPtr = 1;
+            } else {
+                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++;
+    }
+}
+
+void FIZDigiPower::parsePacket()
+{    
+    if (inputBuffer[0] != 0x07)
+        return;
+    if (inputBuffer[1] != 0x60)
+        return;
+    // checksum is sum of all bytes mod 256 = 00      
+    int cs = 0;
+    for (int i=0;i<inputPtr;i++) {
+        cs += inputBuffer[i];
+        }
+    if (cs & 0x00ff) // cs & 0x000000ff should give 0
+    return;
+    
+    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];
+
+// NEED TO SCALE THESE
+    _focus = focus_Position;
+    _iris = iris_Position;
+    _zoom = zoom_Position;
+    
+    newData = true;
+}