Add back in checking fix type

CarletonURocketry · Jun 8, 2024 · 01c7753 · 01c7753
1 parent ec8afca
commit 01c7753
Show file tree

Hide file tree

Showing 2 changed files with 61 additions and 29 deletions.
diff --git a/src/collectors/m10spg_clctr.c b/src/collectors/m10spg_clctr.c
@@ -8,6 +8,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) {                                                       \
+        fprintf(stderr, "M10SPG couldn't send message: %s.\n", strerror(errno));                                       \
+    }
+
 void *m10spg_collector(void *args) {
 
     /* Open message queue. */
@@ -22,52 +30,66 @@ void *m10spg_collector(void *args) {
         .addr = {.addr = (clctr_args(args)->addr), .fmt = I2C_ADDRFMT_7BIT},
     };
 
-    int err = m10spg_open(&loc);
-    if (err != EOK) {
-        fprintf(stderr, "Could not open M10SPG: %s\n", strerror(err));
-        return (void *)((uint64_t)err);
-    }
+    int err;
+    do {
+        err = m10spg_open(&loc);
+        if (err != EOK) {
+            fprintf(stderr, "Could not open M10SPG: %s, retrying\n", strerror(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) {
-            msg.type = TAG_FIX;
-            msg.data.U8 = buf.stat.gpsFix;
-            if (mq_send(sensor_q, (char *)&msg, sizeof(msg), 0) == -1) {
-                fprintf(stderr, "M10SPG couldn't send message: %s.\n", strerror(errno));
+            // Make sure that the fix is valid
+            if (buf.stat.flags & 0x1) {
+                msg.type = TAG_FIX;
+                msg.data.U8 = buf.stat.gpsFix;
+                fix_type = buf.stat.gpsFix;
+                send_msg(sensor_q, msg);
+            } else {
+                // 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);
             }
-            // 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;
-            //}
         } else {
             fprintf(stderr, "M10SPG failed to read status: %s\n", strerror(err));
             continue;
         }
 
+        // Don't bother reading any information if there's no fix
+        if (fix_type == GPS_NO_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) {
-                fprintf(stderr, "M10SPG couldn't send message: %s.\n", 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) {
-                fprintf(stderr, "M10SPG couldn't send message: %s.\n", 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 {
             fprintf(stderr, "M10SPG failed to read position: %s\n", strerror(err));

diff --git a/src/drivers/m10spg/ubx_def.h b/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 */