Revision 44:fe7fcf62c75d, committed 2021-08-12

Comitter:

AndyA

Date:

Thu Aug 12 11:30:14 2021 +0000

Parent:

40:dafa2b1a1630

Child:

45:03d255642704

Commit message:

Make VIPS format output flexible;

Changed in this revision

--- a/Filter/LowPassFilter.cpp	Tue Aug 10 14:55:37 2021 +0000
+++ b/Filter/LowPassFilter.cpp	Thu Aug 12 11:30:14 2021 +0000
@@ -143,7 +143,7 @@
 float LowPassFilter::addPoint(float input)
 {
     m_lastIn = input;
-    if (m_enable) {
+    if (!m_enable) {
         return input;
     }
 

--- a/LTCApp.h	Tue Aug 10 14:55:37 2021 +0000
+++ b/LTCApp.h	Thu Aug 12 11:30:14 2021 +0000
@@ -42,6 +42,7 @@
     bool FilterRoll;
     bool FilterPitch;
     bool FilterYaw;
+    bool FlexibleVIPSOut;
 } UserSettings_t;
 
 #endif

--- a/VIPSSerialProtocol.cpp	Tue Aug 10 14:55:37 2021 +0000
+++ b/VIPSSerialProtocol.cpp	Thu Aug 12 11:30:14 2021 +0000
@@ -282,6 +282,9 @@
     lastPositions[nextPosition].pos.Y = *(double *)(messageInBuffer+20);
     lastPositions[nextPosition].pos.Height = *(float *)(messageInBuffer+28);
 
+    if (mask & 0x0001)
+        lastPositions[nextPosition].pos.LLAPosition = true;
+
     if (mask & 0x0002) { // parse status
         lastPositions[nextPosition].pos.beacons = messageInBuffer[offset++];
         lastPositions[nextPosition].pos.solutionType = messageInBuffer[offset++];
@@ -323,22 +326,28 @@
         offset+=4;
     }
     if (mask & 0x0080) {  // raw UWB
-
         offset+=24;
     }
     if (mask & 0x0100) { // raw IMU
-
         offset+=24;
     }
 
     if (mask & 0x0200) {// rover info
-
         offset+=4;
     }
     if (mask & 0x0400) {// FIZ data
-
         offset+=4;
     }
+    if (mask & 0x0800) {// Origin
+        offset+=24;
+    }
+    if (mask & 0x1000) {// Beacon list
+        memcpy(lastPositions[nextPosition].pos.UsedBeacons, messageInBuffer+offset, 12);
+        lastPositions[nextPosition].pos.UsedBeaconsValid = true;
+        offset+=12;
+    } else
+        lastPositions[nextPosition].pos.UsedBeaconsValid = false;
+
 
     /*if ( ( ( lastPositions[nextPosition].pos.KFStatus & 0xE634) == 0x0400) && (!hyperSmoothEnabled)) {
         EnableSmoothing(true);
@@ -436,7 +445,7 @@
         ZSmoothTotal -= posPtr->Height;
     } else {
         SmoothRunning = false;
- //       pc.printf("filterX = %f\r\n",xFilter.lastValue());
+//       pc.printf("filterX = %f\r\n",xFilter.lastValue());
         posPtr->X = xFilter.lastValue();
         posPtr->Y = yFilter.lastValue();
         posPtr->Height = zFilter.lastValue();

--- a/main.cpp	Tue Aug 10 14:55:37 2021 +0000
+++ b/main.cpp	Thu Aug 12 11:30:14 2021 +0000
@@ -51,15 +51,16 @@
 NOTE-The filter will add latency so a filtered channel will be delayed relative to an unfiltered one.
 
 
+ExtendedOutput=1
+Enable extra fields in VIPS output.
+
+
 All settings are case sensitive.
 Do NOT include spaces in the options lines.
 All options default to a value of 0 is omitted from the file.
 
 */
 
-
-
-
 #include "mbed.h"
 #include "LTCApp.h"
 #include "EthernetInterface.h"
@@ -162,27 +163,9 @@
 
 UserSettings_t UserSettings;
 
-struct outputFormat_s {
-    uint32_t header; // 2 byte header + 2 byte length
-    uint32_t mask;
-    uint32_t time;
-    double x;
-    double y;
-    float z;
-    uint8_t beacons;
-    uint8_t solutionType;
-    uint16_t kfStatus;
-    float roll;
-    float pitch;
-    float yaw;
-    uint8_t accuracy[4];
-    uint32_t focus;
-    uint16_t iris;
-    uint16_t zoom;
-    uint16_t checksum;
-} __attribute__((packed)) ;
+uint8_t FlexibleVIPSOut[200];
+int VIPSOutSize;
 
-struct outputFormat_s packetOut;
 struct D1MsgFormat_s fdPacket;
 
 volatile bool EthTxNow = false;
@@ -299,16 +282,8 @@
 
 void prepPacketOut()
 {
-    uint8_t bytes[4];
-    bytes[0]=0x24;
-    bytes[1]=0xd9;
-    *(uint16_t*)(bytes+2) = sizeof(struct outputFormat_s);
-    packetOut.header = *(uint32_t*)bytes;
-    packetOut.mask = 0x0446;
-    packetOut.accuracy[0] = 0;
-    packetOut.accuracy[1] = 0;
-    packetOut.accuracy[2] = 0;
-    packetOut.accuracy[3] = 0;
+    FlexibleVIPSOut[0]=0x24;
+    FlexibleVIPSOut[1]=0xd9;
 }
 
 void onOutputTxTime(void)
@@ -322,8 +297,8 @@
         break;
         case mode_VIPS:
         default: {
-            char *dataPtr = (char *)&packetOut;
-            for (int byte=0; byte<sizeof(struct outputFormat_s); byte++)
+            char *dataPtr = (char *)FlexibleVIPSOut;
+            for (int byte=0; byte<VIPSOutSize; byte++)
                 COM1.putc(*(dataPtr+byte));
         }
         break;
@@ -395,41 +370,68 @@
 
 void sendVIPSPacket(position *posPtr)
 {
+    int byteCount = 4;
+    uint32_t mask = 0x0446; // Status,orientation,accuracy,FIZ
+    if (posPtr && UserSettings.FlexibleVIPSOut) {
+        if (posPtr->UsedBeaconsValid)
+            mask |= 0x1000;
+        if (posPtr->LLAPosition)
+            mask |= 0x0001;
+    }
+
+    *((uint32_t*)(FlexibleVIPSOut + byteCount)) = mask;
+    byteCount +=4;
+
+    *((uint32_t*)(FlexibleVIPSOut + byteCount)) = movieTime.getTimeMS();
+    byteCount +=4;
+
     if (posPtr) {
-        packetOut.x = posPtr->X;
-        packetOut.y = posPtr->Y;
-        packetOut.z = posPtr->Height;
-        packetOut.beacons = posPtr->beacons;
-        packetOut.solutionType = posPtr->solutionType;
-        packetOut.kfStatus = posPtr->KFStatus;
-        packetOut.roll = posPtr->roll;
-        packetOut.pitch = posPtr->pitch;
-        packetOut.yaw = posPtr->yaw;
-        packetOut.accuracy[3] = posPtr->ID;
-        if (packetOut.kfStatus < 0x200) {
+        *((double*)(FlexibleVIPSOut + byteCount)) = posPtr->X;
+        byteCount +=8;
+        *((double*)(FlexibleVIPSOut + byteCount)) = posPtr->Y;
+        byteCount +=8;
+        *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->Height;
+        byteCount +=4;
+        FlexibleVIPSOut[  byteCount++] =   posPtr->beacons;
+        FlexibleVIPSOut[  byteCount++] =   posPtr->solutionType;
+        *((uint16_t*)(FlexibleVIPSOut + byteCount)) = posPtr->KFStatus;
+        byteCount +=2;
+        *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->roll;
+        byteCount +=4;
+        *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->pitch;
+        byteCount +=4;
+        *((float*)(FlexibleVIPSOut + byteCount)) = posPtr->yaw;
+        byteCount +=4;
+    } else {
+        int blankBytes = 8+8+4+1+1+2+4+4+4;
+        memset(FlexibleVIPSOut + byteCount,0,blankBytes);
+        byteCount += blankBytes;
+    }
+
+    FlexibleVIPSOut[  byteCount++] = 0;
+    FlexibleVIPSOut[  byteCount++] = 0;
+    FlexibleVIPSOut[  byteCount++] = 0;
+    FlexibleVIPSOut[  byteCount++] = posPtr?posPtr->ID:0;
+
+    FIZPort->getMostRecent((uint32_t*)(FlexibleVIPSOut + byteCount), (uint16_t*)(FlexibleVIPSOut + byteCount+4), (uint16_t*)(FlexibleVIPSOut + byteCount+6));
+    byteCount+=8;
+
+    if (mask | 0x1000) // pos must be valid for this to be set
+        memcpy(FlexibleVIPSOut + byteCount,posPtr->UsedBeacons,12);
+
+    // set length to current size plus checksum.
+    VIPSOutSize = byteCount+2;
+    *((uint16_t*)(FlexibleVIPSOut + 2)) = VIPSOutSize;
+
+    VIPSSerial::getCRC(FlexibleVIPSOut, byteCount, (void *)(FlexibleVIPSOut+byteCount));
+
+    if (posPtr && (posPtr->KFStatus < 0x200)) {
             led2 = 0;
             RedLED = LED_ON;
         } else {
             led2 = 1;
             RedLED = LED_OFF;
         }
-    } else {
-        packetOut.x = 0;
-        packetOut.y = 0;
-        packetOut.z = 0;
-        packetOut.roll = 0;
-        packetOut.pitch = 0;
-        packetOut.yaw = 0;
-        packetOut.accuracy[3] = 0;
-        packetOut.beacons = 0;
-        packetOut.solutionType = 0;
-        packetOut.kfStatus = 0;
-        led2 = 1;
-        RedLED = LED_OFF;
-    }
-    packetOut.time = movieTime.getTimeMS();
-    FIZPort->getMostRecent(&packetOut.focus, &packetOut.iris, &packetOut.zoom);
-    VIPSSerial::getCRC((void *)&packetOut, sizeof(struct outputFormat_s)-2, (void *)&packetOut.checksum);
 
 #ifdef Delay_TX_By
     OutputDelayTimer.attach_us(&onOutputTxTime,Delay_TX_By*1000);
@@ -437,7 +439,7 @@
     onOutputTxTime();//    COM1.write(&packetOut, sizeof(struct outputFormat_s));
 #endif
     if (logging) {
-        if (!fwrite(&packetOut, sizeof(struct outputFormat_s), 1, logFile)) { // write failed
+        if (!fwrite(FlexibleVIPSOut, VIPSOutSize, 1, logFile)) { // write failed
             GreenLED = LED_OFF;
             logging = false;
             fclose(logFile);
@@ -589,7 +591,7 @@
     UserSettings.FilterRoll = false;
     UserSettings.FilterPitch = false;
     UserSettings.FilterYaw = false;
-
+    UserSettings.FlexibleVIPSOut = false;
     LocalFileSystem localFS("local");
     FILE *LSFile= fopen("/local/settings.txt","r");
     char lineBuffer[64];
@@ -651,6 +653,10 @@
                     pc.printf("Got FilterYaw value from file of %d\r\n",valueIn);
                     UserSettings.FilterYaw = (valueIn==1);
                 }
+                if (sscanf(lineBuffer,"ExtendedOutput=%d",&valueIn) == 1) {
+                    pc.printf("Got ExtendedOutput value from file of %d\r\n",valueIn);
+                    UserSettings.FlexibleVIPSOut = (valueIn==1);
+                }
             }
         }
         fclose(LSFile);
@@ -777,7 +783,7 @@
 
     if (!VIPS.setFilters(&UserSettings))
         pc.puts("Failed to create VIPS filters\r\n");
-    
+
     VIPS.run();
 
     pc.printf("System init complete\r\n");

--- a/position.cpp	Tue Aug 10 14:55:37 2021 +0000
+++ b/position.cpp	Thu Aug 12 11:30:14 2021 +0000
@@ -76,6 +76,7 @@
     output->beacons = pos1->beacons;
     output->solutionType = pos1->solutionType;
     output->KFStatus = pos1->KFStatus;
+    memcpy(output->UsedBeacons,pos1->UsedBeacons,12);
 
     return true;
 }

--- a/position.h	Tue Aug 10 14:55:37 2021 +0000
+++ b/position.h	Thu Aug 12 11:30:14 2021 +0000
@@ -19,7 +19,10 @@
   int beacons;
   int solutionType;
   int KFStatus;
-
+  uint8_t UsedBeacons[12];
+  
+  bool UsedBeaconsValid;
+  bool LLAPosition;
     static bool interp(position* output, position *pos1, position *pos2);
     static float interpAngle(float value1, float value2, float gradientWeight);
 

--- a/settings.txt	Tue Aug 10 14:55:37 2021 +0000
+++ b/settings.txt	Thu Aug 12 11:30:14 2021 +0000
@@ -54,6 +54,14 @@
 
 # NOTE-The filter will add latency so a filtered channel will be delayed relative to an unfiltered one.
  
+[Additional outputs]
+# By default FIZ box VIPS format output only output the fields required by unreal
+# even if the VIPS itself is configured to output more data. This is intended to minimise radio traffic
+# while allowing debug data to be avalible when connecting directly to VIPS.
+# Setting this option to 1 allows additional fields to be passed through the FIZ box.
+# Currently supported fields are:
+#   Used Beacon List (mask value 0x1000)
+//ExtendedOutput=1
  
 [NOTES]
 - All settings are case sensitive.