DFM: Add XDATA support

einergehtnochrein · Feb 23, 2022 · a308ed1 · a308ed1
1 parent 383a007
commit a308ed1
Show file tree

Hide file tree

Showing 3 changed files with 65 additions and 0 deletions.
diff --git a/src/dfm/dfm.c b/src/dfm/dfm.c
@@ -369,6 +369,30 @@ LPCLIB_Result DFM_processBlock (
 
                 result = LPCLIB_SUCCESS;
             }
+
+            /* Send XDATA if available */
+            if (handle->instance->haveXdata) {
+                handle->instance->haveXdata = false;
+
+                char xdata[80];
+                snprintf(xdata, sizeof(xdata), "%s,2,2,%04"PRIX16
+                                               "%08"PRIX32"%04"PRIX16
+                                               "%08"PRIX32"%04"PRIX16
+                                               "%08"PRIX32"%04"PRIX16
+                                               "%08"PRIX32"%04"PRIX16,
+                            handle->instance->name,
+                            handle->instance->xdata.header,
+                            handle->instance->xdata.x0_32,
+                            handle->instance->xdata.x0_16,
+                            handle->instance->xdata.x1_32,
+                            handle->instance->xdata.x1_16,
+                            handle->instance->xdata.x2_32,
+                            handle->instance->xdata.x2_16,
+                            handle->instance->xdata.x3_32,
+                            handle->instance->xdata.x3_16
+                            );
+                SYS_send2Host(HOST_CHANNEL_INFO, xdata);
+            }
         }
 
         /* Remember RX frequency (difference to nominal sonde frequency will be reported as frequency offset) */

diff --git a/src/dfm/dfmgps.c b/src/dfm/dfmgps.c
@@ -261,6 +261,33 @@ static void _DFM_processGps (DFM_InstanceData *instance, struct _DFM_GpsDetect *
 }
 
 
+/* Process XDATA information in GPS subframe */
+static void _DFM_processXdata (DFM_InstanceData *instance, struct _DFM_GpsDetect *pDetect)
+{
+    _Bool haveXdata = false;
+
+    /* XDATA is available in GPS mode 4 only */
+    if (instance->gps.mode == 4) {
+        /* Need a non-zero header */
+        instance->xdata.header = pDetect->fragment[3].i16;
+        if (instance->xdata.header != 0) {
+            instance->xdata.x0_32 = pDetect->fragment[4].i32;
+            instance->xdata.x0_16 = pDetect->fragment[4].i16;
+            instance->xdata.x1_32 = pDetect->fragment[5].i32;
+            instance->xdata.x1_16 = pDetect->fragment[5].i16;
+            instance->xdata.x2_32 = pDetect->fragment[6].i32;
+            instance->xdata.x2_16 = pDetect->fragment[6].i16;
+            instance->xdata.x3_32 = pDetect->fragment[7].i32;
+            instance->xdata.x3_16 = pDetect->fragment[7].i16;
+
+            haveXdata = true;
+        }
+    }
+
+    instance->haveXdata = haveXdata;
+}
+
+
 LPCLIB_Result _DFM_processGpsBlock (
         const DFM_SubFrameGps *rawGps,
         DFM_InstanceData **instancePointer,
@@ -321,6 +348,7 @@ LPCLIB_Result _DFM_processGpsBlock (
             }
 
             _DFM_processGps(instance, p);
+            _DFM_processXdata(instance, p);
         }
 
         /* This GPS frame has been processed */

diff --git a/src/dfm/dfmprivate.h b/src/dfm/dfmprivate.h
@@ -165,6 +165,19 @@ typedef struct _DFM_InstanceData {
     DFM_CookedConfig config;
     DFM_CookedMetrology metro;
     DFM_CookedGps gps;
+
+    _Bool haveXdata;
+    struct {
+        uint16_t header;
+        uint32_t x0_32;
+        uint16_t x0_16;
+        uint32_t x1_32;
+        uint16_t x1_16;
+        uint32_t x2_32;
+        uint16_t x2_16;
+        uint32_t x3_32;
+        uint16_t x3_16;
+    } xdata;
 } DFM_InstanceData;