Diff: FIZ_readers/FIZ_DISNEY.cpp

Revision:

16:a8d3a0dbe4bf

Child:

17:5ce3fe98e76d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FIZ_readers/FIZ_DISNEY.cpp	Fri Apr 30 11:26:34 2021 +0000
@@ -0,0 +1,63 @@
+#include "FIZ_DISNEY.h"
+#include "LTCApp.h"
+FIZDisney::FIZDisney(const PinName Tx, const PinName Rx) : FIZReader(Tx,Rx)
+{
+    inputPtr = 0;
+    newData = false;
+    _port.baud(115200);
+    _port.attach(callback(this, &FIZDisney::OnRx));
+}
+
+void FIZDisney::requestCurrent(void)
+{
+    _port.putc(0x02);
+    _port.putc('0');
+    _port.putc('C');
+    _port.putc('0');
+    _port.putc('0');
+    _port.putc('D');
+    _port.putc('5');
+    _port.putc(0x03);
+}
+
+void FIZDisney::OnRx(void)
+{
+        inputBuffer[inputPtr] = _port.getc();
+        if (inputPtr==0) {
+            if (inputBuffer[inputPtr] == 0x02)
+                inputPtr++;
+        } else if (inputBuffer[inputPtr] == 0x03) {
+            parsePacket();
+            inputPtr = 0;
+        } else {
+            inputPtr++;
+            if (inputPtr == InBufferSize)
+                inputPtr = 0;
+        }
+}
+
+void FIZDisney::parsePacket()
+{
+//  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;
+}