Diff: main.cpp

Revision:

3:14d241e29be3

Parent:

2:a79201e302d7

Child:

4:6cd904bff1bd

--- a/main.cpp	Mon Jan 18 09:15:04 2021 +0000
+++ b/main.cpp	Thu Jan 28 15:13:05 2021 +0000
@@ -8,7 +8,7 @@
 BufferedSerial pc(USBTX, USBRX);
 VIPSSerial VIPS(p28, p27);
 BufferedSerial COM1(p13, p14);
-BufferedSerial COM3(p9, p10);
+FIZReader FIZPort(p9, p10);
 
 DigitalOut led1(LED1);
 DigitalOut PPS(p12);
@@ -56,6 +56,12 @@
 float remainingClockError;
 bool ppmCorrection;
 
+struct FIZData_s {
+    uint32_t focus;
+    uint16_t iris;
+    uint16_t zoom;
+};
+
 struct outputFormat_s {
     uint32_t header; // 2 byte header + 2 byte length
     uint32_t mask;
@@ -67,23 +73,27 @@
     float pitch;
     float yaw;
     uint8_t accuracy[4];
+    uint32_t focus;
+    uint16_t iris;
+    uint16_t zoom;
     uint16_t checksum;
 } __attribute__((packed)) ;
 
 struct outputFormat_s packetOut;
+struct FIZData_s lastFiz;
 
 void prepPacketOut()
 {
     uint8_t bytes[4];
     bytes[0]=0x24;
-    bytes[1]=0x9d;
+    bytes[1]=0xd9;
     *(uint16_t*)(bytes+2) = sizeof(struct outputFormat_s);
     packetOut.header = *(uint32_t*)bytes;
-    packetOut.mask = 0x44;
-packetOut.accuracy[0] = 0;
-packetOut.accuracy[1] = 0;
-packetOut.accuracy[2] = 0;
-packetOut.accuracy[3] = 0;
+    packetOut.mask = 0x0444;
+    packetOut.accuracy[0] = 0;
+    packetOut.accuracy[1] = 0;
+    packetOut.accuracy[2] = 0;
+    packetOut.accuracy[3] = 0;
 }
 
 void sendPosition(position *posPtr)
@@ -97,9 +107,14 @@
         packetOut.pitch = posPtr->pitch;
         packetOut.yaw = posPtr->yaw;
         packetOut.accuracy[3] = posPtr->ID;
+        packetOut.focus = lastFiz.focus;
+        packetOut.iris = lastFiz.iris;
+        packetOut.zoom = lastFiz.zoom;
+        FIZPort.getMostRecent(&  packetOut.focus,  & packetOut.iris,&packetOut.zoom);
         VIPSSerial::getCRC((void *)&packetOut, sizeof(struct outputFormat_s)-2, (void *)&packetOut.checksum);
+//        COM1.printf("send\r\n");
         COM1.write(&packetOut, sizeof(struct outputFormat_s));
-        COM3.write(&packetOut, sizeof(struct outputFormat_s));
+//        COM3.write(&packetOut, sizeof(struct outputFormat_s));
 //      printf("send\r\n");
     }
 }
@@ -251,6 +266,7 @@
 {
     sendPosition(VIPS.sendPositionForTime(TimeSinceLastFrame.read_us()));
     TimeSinceLastFrame.reset();
+    FIZPort.requestCurrent();
 }
 
 // called by background loop at the end of each frame
@@ -366,7 +382,6 @@
 {
     pc.baud(115200);
     COM1.baud(115200);
-    COM3.baud(115200);
     inputTimer.reset();
     inputTimer.start();
 
@@ -374,8 +389,11 @@
     led1 = 1;
 
     prepPacketOut();
+
     LTCInput.setInputTimer(&inputTimer);
+
     PPFin.rise(callback(&OnPPFInputStartup));
+
     VIPS.run();
 
     pc.printf("armed\r\n");