Add CFG_GEOFENCE

ubx.c

@@ -664,6 +664,39 @@ extern UBXMsgBuffer getCFG_ESRC(UBXU1_t version,
     return buffer;
 }
 
+extern UBXMsgBuffer getCFG_GEOFENCE_POLL()
+{
+    int payloadSize = 0;
+    UBXMsgBuffer buffer = createBuffer(payloadSize);
+    UBXMsg* msg = (UBXMsg*)buffer.data;
+    initMsg(msg, payloadSize, UBXMsgClassCFG, UBXMsgIdCFG_GEOFENCE);
+    completeMsg(&buffer, payloadSize);
+    return buffer;
+}
+
+extern UBXMsgBuffer getCFG_GEOFENCE(UBXU1_t version,
+                                    UBXU1_t numFences,
+                                    UBXU1_t confLvl,
+                                    UBXU1_t pioEnabled,
+                                    UBXU1_t pinPolarity,
+                                    UBXU1_t pin,
+                                    UBXCFG_GEOFENCE_PART* fences)
+{
+    int payloadSize = sizeof(UBXCFG_GEOFENCE) + sizeof(UBXCFG_GEOFENCE_PART) * numFences;
+    UBXMsgBuffer buffer = createBuffer(payloadSize);
+    UBXMsg* msg = (UBXMsg*)buffer.data;
+    initMsg(msg, payloadSize, UBXMsgClassCFG, UBXMsgIdCFG_ESRC);
+    msg->payload.CFG_GEOFENCE.version = version;
+    msg->payload.CFG_GEOFENCE.numFences = numFences;
+    msg->payload.CFG_GEOFENCE.confLvl = confLvl;
+    msg->payload.CFG_GEOFENCE.pioEnabled = pioEnabled;
+    msg->payload.CFG_GEOFENCE.pinPolarity = pinPolarity;
+    msg->payload.CFG_GEOFENCE.pin = pin;
+    memcpy(msg->payload.CFG_GEOFENCE.fence, fences, numFences * sizeof(UBXCFG_GEOFENCE_PART));
+    completeMsg(&buffer, payloadSize);
+    return buffer;
+}
+
 UBXMsgBuffer getCFG_GNSS_POLL()
 {
     int payloadSize = 0;

ubxcfg.h

@@ -72,6 +72,14 @@ extern UBXMsgBuffer getCFG_ESRC_POLL();
 extern UBXMsgBuffer getCFG_ESRC(UBXU1_t version,
                                 UBXU1_t numSources,
                                 UBXCFG_ESRC_CFG* esrcCfg);
+extern UBXMsgBuffer getCFG_GEOFENCE_POLL();
+extern UBXMsgBuffer getCFG_GEOFENCE(UBXU1_t version,
+                                    UBXU1_t numFences,
+                                    UBXU1_t confLvl,
+                                    UBXU1_t pioEnabled,
+                                    UBXU1_t pinPolarity,
+                                    UBXU1_t pin,
+                                    UBXCFG_GEOFENCE_PART* fences);
 extern UBXMsgBuffer getCFG_GNSS_POLL();
 extern UBXMsgBuffer getCFG_GNSS(UBXU1_t msgVer,
                                 UBXU1_t numTrkChHw,

ubxmessage.h

@@ -1155,6 +1155,24 @@ typedef struct {
     UBXCFG_ESRC_CFG cfg[0];
 } UBXCFG_ESRC;
 
+typedef struct {
+    UBXI4_t lat;
+    UBXI4_t lon;
+    UBXU4_t radius;
+} UBXCFG_GEOFENCE_PART;
+
+typedef struct {
+    UBXU1_t version;
+    UBXU1_t numFences;
+    UBXU1_t confLvl;
+    UBXU1_t reserved1[1];
+    UBXU1_t pioEnabled;
+    UBXU1_t pinPolarity;
+    UBXU1_t pin;
+    UBXU1_t reserved2[1];
+    UBXCFG_GEOFENCE_PART fence[0];
+} UBXCFG_GEOFENCE;
+
 //typedef struct {
     //No payload
 //} UBXCFG_GNSS_POLL;
@@ -2205,6 +2223,7 @@ typedef union
     UBXCFG_DGNSS CFG_DGNSS;
     UBXCFG_DOSC CFG_DOSC;
     UBXCFG_ESRC CFG_ESRC;
+    UBXCFG_GEOFENCE CFG_GEOFENCE;
     UBXCFG_GNSS CFG_GNSS;
     UBXCFG_INF_POLL CFG_INF_POLL;
     UBXCFG_INF CFG_INF;