diff --git a/src/collectors/m10spg_clctr.c b/src/collectors/m10spg_clctr.c index bada946..8ee9a98 100644 --- a/src/collectors/m10spg_clctr.c +++ b/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); } 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 */ diff --git a/src/logging.h b/src/logging.h index 3849480..9e96238 100644 --- a/src/logging.h +++ b/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_