diff --git a/src/collectors/m10spg_clctr.c b/src/collectors/m10spg_clctr.c index 1018c70..fdbdc5c 100644 --- 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 index 6e4e310..8b862e5 100644 --- 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 */