Add back in checking fix type

src/collectors/m10spg_clctr.c

@@ -9,6 +9,14 @@ union read_buffer {
     UBXNavStatusPayload stat;
 };
 
+/**
+ * Helper function to simplify sending a message on the message queue
+ */
+#define send_msg(sensor_q, msg)                                                                                        \
+    if (mq_send(sensor_q, (char *)&msg, sizeof(msg), 0) == -1) {                                                       \
+        fetcher_log(stderr, LOG_WARN, "M10SPG couldn't send message: %s.", strerror(errno));                           \
+    }
+
 void *m10spg_collector(void *args) {
 
     /* Open message queue. */
@@ -24,52 +32,70 @@ void *m10spg_collector(void *args) {
         .addr = {.addr = (clctr_args(args)->addr), .fmt = I2C_ADDRFMT_7BIT},
     };
 
-    int err = m10spg_open(&loc);
-    if (err != EOK) {
-        fetcher_log(stderr, LOG_ERROR, "Could not open M10SPG: %s", strerror(err));
-        return (void *)((uint64_t)err);
-    }
+    int err;
+    do {
+        err = m10spg_open(&loc);
+        if (err != EOK) {
+            fetcher_log(stderr, LOG_ERROR, "Could not open M10SPG: %s", strerror(err));
+            return (void *)((uint64_t)err);
+        }
+    } while (err != EOK);
 
     for (;;) {
         // TODO - Don't read if the next epoch hasn't happened
         union read_buffer buf;
+        GPSFixType fix_type = GPS_NO_FIX;
         common_t msg;
         err = m10spg_send_command(&loc, UBX_NAV_STAT, &buf, sizeof(UBXNavStatusPayload));
-        // Check if we have a valid fix, no point logging bad data */
-        if (err == EOK) {
-            // Make sure that the fix is valid (has a reasonable value and is not a no-fix) */
-            // if ((buf.stat.flags & 0x01) && buf.stat.gpsFix) {
+
+        // Check if we could send command
+        if (err) {
+            fetcher_log(stderr, LOG_ERROR, "Could not send command to M10SPG: %s", strerror(err));
+            continue;
+        }
+
+        // Make sure that the fix is valid
+        if (buf.stat.flags & 0x1) {
             msg.type = TAG_FIX;
             msg.data.U8 = buf.stat.gpsFix;
-            if (mq_send(sensor_q, (char *)&msg, sizeof(msg), 0) == -1) {
-                fetcher_log(stderr, LOG_ERROR, "M10SPG couldn't send message: %s.", strerror(errno));
-            }
-            // The else here is commented out so you can see the data processing is working even while an invalid
-            // fix is held
-            //}  else {
-            //    // Instead of doing a continue, should sleep until the next epoch */
-            //    printf("Bad GPS fix, skipping\n"); */
-            //    continue;
-            //}
+            fix_type = buf.stat.gpsFix;
+            send_msg(sensor_q, msg);
         } else {
-            fetcher_log(stderr, LOG_ERROR, "M10SPG failed to read status: %s", strerror(err));
+            // Send a no-fix so that it's clear we're not getting anything
+            msg.type = TAG_FIX;
+            msg.data.U8 = GPS_NO_FIX;
+            send_msg(sensor_q, msg);
+        }
+
+        // Don't bother reading any information if there's no fix
+        if (fix_type == GPS_NO_FIX) {
+            fetcher_log(stderr, LOG_WARN, "M10SPG could not get fix.");
             continue;
         }
 
         // Read position
         err = m10spg_send_command(&loc, UBX_NAV_POSLLH, &buf, sizeof(UBXNavPositionPayload));
         if (err == EOK) {
-            msg.type = TAG_COORDS;
-            msg.data.VEC2D_I32.x = buf.pos.lat;
-            msg.data.VEC2D_I32.y = buf.pos.lon;
-
-            if (mq_send(sensor_q, (char *)&msg, sizeof(msg), 0) == -1) {
-                fetcher_log(stderr, LOG_ERROR, "M10SPG couldn't send message: %s.", strerror(errno));
-            }
-            msg.type = TAG_ALTITUDE_SEA;
-            msg.data.FLOAT = (((float)buf.pos.hMSL) / ALT_SCALE_TO_METERS);
-            if (mq_send(sensor_q, (char *)&msg, sizeof(msg), 0) == -1) {
-                fetcher_log(stderr, LOG_ERROR, "M10SPG couldn't send message: %s.", strerror(errno));
+            switch (fix_type) {
+            case GPS_3D_FIX:
+                msg.type = TAG_ALTITUDE_SEA;
+                msg.data.FLOAT = (((float)buf.pos.hMSL) / ALT_SCALE_TO_METERS);
+                send_msg(sensor_q, msg);
+                // FALL THROUGH
+            case GPS_FIX_DEAD_RECKONING:
+                // FALL THROUGH
+            case GPS_2D_FIX:
+                // FALL THROUGH
+            case GPS_DEAD_RECKONING:
+                msg.type = TAG_COORDS;
+                msg.data.VEC2D_I32.x = buf.pos.lat;
+                msg.data.VEC2D_I32.y = buf.pos.lon;
+                send_msg(sensor_q, msg);
+                break;
+            case GPS_TIME_ONLY:
+                break;
+            default:
+                break;
             }
         } else {
             fetcher_log(stderr, LOG_ERROR, "M10SPG failed to read position: %s", strerror(err));
@@ -93,7 +119,7 @@ void *m10spg_collector(void *args) {
         /*     fprintf(stderr, "M10SPG failed to read velocity: %s\n", strerror(err)); */
         /*     continue; */
         /* } */
+        fetcher_log(stderr, LOG_ERROR, "%s", strerror(err));
+        return (void *)((uint64_t)err);
     }
-    fetcher_log(stderr, LOG_ERROR, "%s", strerror(err));
-    return (void *)((uint64_t)err);
 }

src/drivers/m10spg/ubx_def.h

@@ -76,6 +76,16 @@ typedef struct {
     uint32_t msss;   /**< Milliseconds since startup */
 } UBXNavStatusPayload;
 
+/** A struct representing the different fix types the GPS can have */
+typedef enum {
+    GPS_NO_FIX = 0x00,             /**< The gps has no fix, do not use data */
+    GPS_DEAD_RECKONING = 0x01,     /**< Dead reckoning only (uses previous velocity and position information) */
+    GPS_2D_FIX = 0x02,             /**< Two dimensional fix (no altitude) */
+    GPS_3D_FIX = 0x03,             /**< Three dimensional fix */
+    GPS_FIX_DEAD_RECKONING = 0x04, /**< Dead reckoning and gps combined */
+    GPS_TIME_ONLY = 0x05,          /**< Time solution only, do not use other data */
+} GPSFixType;
+
 /** A struct representing the UBX-NAV-POSLLH (position and height) payload */
 typedef struct {
     uint32_t iTOW;  /**< The GPS time of week of the navigation epoch that created this payload */

src/logging.h

@@ -15,6 +15,6 @@ void _fetcher_log(FILE *stream, log_level_e lvl, const char *file, const char *f
                   ...);
 
 #define fetcher_log(stream, lvl, fmt_string, ...)                                                                      \
-    _fetcher_log((stream), (lvl), __FILE__, __FUNCTION__, __LINE__, (fmt_string), __VA_ARGS__)
+    _fetcher_log((stream), (lvl), __FILE__, __FUNCTION__, __LINE__, (fmt_string), ##__VA_ARGS__)
 
 #endif // _LOGGING_H_