diff --git a/src/collectors/m10spg_clctr.c b/src/collectors/m10spg_clctr.c index 4dcda9f..678d16d 100644 --- a/src/collectors/m10spg_clctr.c +++ b/src/collectors/m10spg_clctr.c @@ -19,57 +19,64 @@ typedef struct { } __attribute__((packed)) message_t; void *m10spg_collector(void *args) { + /* Open message queue. */ mqd_t sensor_q = mq_open(SENSOR_QUEUE, O_WRONLY); if (sensor_q == -1) { fprintf(stderr, "M10SPG collector could not open message queue '%s': '%s' \n", SENSOR_QUEUE, strerror(errno)); return (void *)((uint64_t)errno); } - SensorLocation loc = {.bus = clctr_args(args)->bus, - .addr = {.addr = (clctr_args(args)->addr), .fmt = I2C_ADDRFMT_7BIT}}; + + SensorLocation loc = { + .bus = clctr_args(args)->bus, + .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); } + for (;;) { // TODO - Don't read if the next epoch hasn't happened union read_buffer buf; message_t msg; - err = m10spg_read(&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.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)); - } - // 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; - } + /* 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.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)); */ + /* } */ + // 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; */ + /* } */ + // Read position - err = m10spg_read(&loc, UBX_NAV_POSLLH, &buf, sizeof(UBXNavPositionPayload)); + err = m10spg_send_command(&loc, UBX_NAV_POSLLH, &buf, sizeof(UBXNavPositionPayload)); if (err == EOK) { - msg.type = TAG_LATITUDE; - msg.I32 = buf.pos.lat; - 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_LONGITUDE; - msg.I32 = buf.pos.lon; - if (mq_send(sensor_q, (char *)&msg, sizeof(msg), 0) == -1) { + + uint8_t coordinates[sizeof(vec2d_t) + 1]; + coordinates[0] = TAG_COORDS; + memcpy(&coordinates[1], &buf.pos.lat, sizeof(buf.pos.lat)); + memcpy(&coordinates[1 + sizeof(buf.pos.lat)], &buf.pos.lon, sizeof(buf.pos.lon)); + + if (mq_send(sensor_q, (char *)coordinates, sizeof(coordinates), 0) == -1) { fprintf(stderr, "M10SPG couldn't send message: %s.\n", strerror(errno)); } + msg.type = TAG_ALTITUDE_SEA; msg.I32 = buf.pos.hMSL; if (mq_send(sensor_q, (char *)&msg, sizeof(msg), 0) == -1) { @@ -79,25 +86,26 @@ void *m10spg_collector(void *args) { fprintf(stderr, "M10SPG failed to read position: %s\n", strerror(err)); continue; } + // Read velocity - err = m10spg_read(&loc, UBX_NAV_VELNED, &buf, sizeof(UBXNavVelocityPayload)); - if (err == EOK) { - msg.type = TAG_SPEED; - msg.U32 = buf.vel.gSpeed; - 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_COURSE; - msg.U32 = buf.vel.heading; - if (mq_send(sensor_q, (char *)&msg, sizeof(msg), 0) == -1) { - fprintf(stderr, "M10SPG couldn't send message: %s.\n", strerror(errno)); - } - } else { - fprintf(stderr, "M10SPG failed to read velocity: %s\n", strerror(err)); - continue; - } - // Read velocity + /* err = m10spg_send_command(&loc, UBX_NAV_VELNED, &buf, sizeof(UBXNavVelocityPayload)); */ + /* if (err == EOK) { */ + /* msg.type = TAG_SPEED; */ + /* msg.U32 = buf.vel.gSpeed; */ + /* 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_COURSE; */ + /* msg.U32 = buf.vel.heading; */ + /* if (mq_send(sensor_q, (char *)&msg, sizeof(msg), 0) == -1) { */ + /* fprintf(stderr, "M10SPG couldn't send message: %s.\n", strerror(errno)); */ + /* } */ + /* } else { */ + /* fprintf(stderr, "M10SPG failed to read velocity: %s\n", strerror(err)); */ + /* continue; */ + /* } */ } + fprintf(stderr, "%s\n", strerror(err)); return (void *)((uint64_t)err); -} \ No newline at end of file +} diff --git a/src/drivers/m10spg/m10spg.c b/src/drivers/m10spg/m10spg.c index 167e52d..d583d58 100644 --- a/src/drivers/m10spg/m10spg.c +++ b/src/drivers/m10spg/m10spg.c @@ -11,11 +11,14 @@ #include #include #include -#include #include #include #include +#ifdef __M10SPG_DEBUG__ +#include +#endif // __M10SPG_DEBUG__ + /** Macro to early return an error. */ #define return_err(err) \ if (err != EOK) return err @@ -37,11 +40,9 @@ static const UBXFrame PREMADE_MESSAGES[] = { [UBX_NAV_POSLLH] = {.header = {.class = 0x01, .id = 0x02, .length = 0x00}, .checksum_a = 0x03, .checksum_b = 0x0a}, [UBX_NAV_VELNED] = {.header = {.class = 0x01, .id = 0x12, .length = 0x00}, .checksum_a = 0x13, .checksum_b = 0x3a}, [UBX_NAV_STAT] = {.header = {.class = 0x01, .id = 0x03, .length = 0x00}, .checksum_a = 0x04, .checksum_b = 0x0d}, + [UBX_MON_VER] = {.header = {.class = 0x0A, .id = 0x04, .length = 0x00}, .checksum_a = 0x0E, .checksum_b = 0x34}, }; -// Extra pre-built messages, may be used in the future or for debugging -// [UBX_MON_VER] = {.header = {.class = 0x0A, .id = 0x04, .length = 0x00}, .checksum_a = 0x0E, .checksum_b = 0x34}, - /** * Gets the total length of a message, including checksums and sync characters * @return uint16_t @@ -139,14 +140,14 @@ static int add_valset_item(UBXFrame *msg, uint32_t key, const void *value, UBXVa */ static int m10spg_available_bytes(const SensorLocation *loc, uint16_t *result) { // Send the address of the first register, then the second byte read will be the next register (0xFE) - static i2c_send_t write_header = {.stop = 0, .len = 1}; + i2c_send_t write_header = {.stop = 0, .len = 1}; write_header.slave = loc->addr; uint8_t address_cmd[sizeof(write_header) + 1]; memcpy(address_cmd, &write_header, sizeof(write_header)); address_cmd[sizeof(write_header)] = 0xFD; - static i2c_recv_t read_header = {.stop = 1, .len = 2}; + i2c_recv_t read_header = {.stop = 1, .len = 2}; read_header.slave = loc->addr; uint8_t read_cmd[sizeof(read_header) + 2]; memcpy(read_cmd, &read_header, sizeof(read_header)); @@ -168,8 +169,8 @@ static int m10spg_available_bytes(const SensorLocation *loc, uint16_t *result) { * @param nbytes The number of bytes to read into the buffer * @return int The error status of the call. EOK if successful. */ -static int read_bytes(const SensorLocation *loc, void *buf, size_t nbytes) { - static i2c_recv_t header = {.stop = 1}; +int read_bytes(const SensorLocation *loc, void *buf, size_t nbytes) { + i2c_recv_t header = {.stop = 1}; header.slave = loc->addr; header.len = nbytes; uint8_t read_cmd[sizeof(header) + nbytes]; @@ -208,7 +209,7 @@ static int write_bytes(const SensorLocation *loc, void *buf, size_t nbytes) { * @return int Status of writing to the sensor, EOK if successful */ static int send_message(const SensorLocation *loc, const UBXFrame *msg) { - static i2c_send_t header = {.stop = 1}; + i2c_send_t header = {.stop = 1}; header.slave = loc->addr; header.len = ubx_message_length(msg); uint8_t data[sizeof(header) + ubx_message_length(msg)]; @@ -280,7 +281,7 @@ static int recv_message(const SensorLocation *loc, UBXFrame *msg, uint16_t max_p * @param size The maximum number of bytes to read into the response buffer * @return int The error status of the call. EOK if successful. */ -int m10spg_read(const SensorLocation *loc, M10spgCmd command, void *response, size_t size) { +int m10spg_send_command(const SensorLocation *loc, M10SPG_cmd_t command, void *response, size_t size) { int err = send_message(loc, &PREMADE_MESSAGES[command]); return_err(err); UBXFrame recv; @@ -328,7 +329,7 @@ int m10spg_open(const SensorLocation *loc) { return EINTR; } -// DEBUG FUNCTIONS +#ifdef __M10SPG_DEBUG__ /** * Prints a populated message structure to standard output, which is useful when creating a pre-built message @@ -384,4 +385,6 @@ static int debug_dump_buffer(const SensorLocation *loc, size_t bytes) { } putchar('\n'); return EOK; -} \ No newline at end of file +} + +#endif // __M10SPG_DEBUG__ diff --git a/src/drivers/m10spg/m10spg.h b/src/drivers/m10spg/m10spg.h index 0455894..f942d50 100644 --- a/src/drivers/m10spg/m10spg.h +++ b/src/drivers/m10spg/m10spg.h @@ -11,15 +11,16 @@ #include "../sensor_api.h" #include -/** An enum representing the commands that can be used for polling m10spg data */ +/** An enum representing the commands that can be used for polling M10SPG data. */ typedef enum { UBX_NAV_UTC, UBX_NAV_POSLLH, UBX_NAV_VELNED, UBX_NAV_STAT, -} M10spgCmd; + UBX_MON_VER, +} M10SPG_cmd_t; int m10spg_open(const SensorLocation *loc); -int m10spg_read(const SensorLocation *loc, M10spgCmd command, void *response, size_t size); +int m10spg_send_command(const SensorLocation *loc, M10SPG_cmd_t command, void *response, size_t size); #endif // _MAXM10S_ diff --git a/src/drivers/m10spg/ubx_def.h b/src/drivers/m10spg/ubx_def.h index f8f298e..6c2185a 100644 --- a/src/drivers/m10spg/ubx_def.h +++ b/src/drivers/m10spg/ubx_def.h @@ -74,43 +74,43 @@ typedef struct { /** A struct representing the UBX-NAV-STAT (navigation status) payload */ typedef struct { - uint32_t iTOW; /** The GPS time of week of the navigation epoch that created this payload */ - uint8_t gpsFix; /** The type of fix */ - uint8_t flags; /** Navigation status flags */ - uint8_t fixStat; /** The fix status */ - uint8_t flags2; /** More flags about navigation output */ - uint32_t ttff; /** The time to first fix, in milliseconds */ - uint32_t msss; /** Milliseconds since startup */ + uint32_t iTOW; /**< The GPS time of week of the navigation epoch that created this payload */ + uint8_t gpsFix; /**< The type of fix */ + uint8_t flags; /**< Navigation status flags */ + uint8_t fixStat; /**< The fix status */ + uint8_t flags2; /**< More flags about navigation output */ + uint32_t ttff; /**< The time to first fix, in milliseconds */ + uint32_t msss; /**< Milliseconds since startup */ } UBXNavStatusPayload; /** 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 */ - int32_t lon; /** Longitude, in 0.0000001 * degrees */ - int32_t lat; /** Latitude, in 0.0000001 * degrees */ - int32_t height; /** Height above ellipsoid in millimeters */ - int32_t hMSL; /** Height above mean sea level in millimeters */ - uint32_t hAcc; /** Horizontal accuracy measurement in millimeters */ - uint32_t vAcc; /** Vertical accuracy measurement in millimeters */ + uint32_t iTOW; /**< The GPS time of week of the navigation epoch that created this payload */ + int32_t lon; /**< Longitude, in 0.0000001 * degrees */ + int32_t lat; /**< Latitude, in 0.0000001 * degrees */ + int32_t height; /**< Height above ellipsoid in millimeters */ + int32_t hMSL; /**< Height above mean sea level in millimeters */ + uint32_t hAcc; /**< Horizontal accuracy measurement in millimeters */ + uint32_t vAcc; /**< Vertical accuracy measurement in millimeters */ } UBXNavPositionPayload; /** A struct representing the UBX-NAV-VELNED (velocity) payload */ typedef struct { - uint32_t iTOW; /** The GPS time of week of the navigation epoch that created this payload */ - int32_t velN; /** North velocity component, in cm/s */ - int32_t velE; /** East velocity component, in cm/s */ - int32_t velD; /** Down velocity component, in cm/s */ - uint32_t speed; /** Speed (3-D), in cm/s */ - uint32_t gSpeed; /** Ground speed (2-D), in cm/s */ - int32_t heading; /** Heading of motion (2-D), in 0.00001 * degrees */ - uint32_t sAcc; /** Speed accuracy estimate, in cm/s */ - uint32_t cAcc; /** Course/heading accuracy estimate, in 0.00001 * degrees */ + uint32_t iTOW; /**< The GPS time of week of the navigation epoch that created this payload */ + int32_t velN; /**< North velocity component, in cm/s */ + int32_t velE; /**< East velocity component, in cm/s */ + int32_t velD; /**< Down velocity component, in cm/s */ + uint32_t speed; /**< Speed (3-D), in cm/s */ + uint32_t gSpeed; /**< Ground speed (2-D), in cm/s */ + int32_t heading; /**< Heading of motion (2-D), in 0.00001 * degrees */ + uint32_t sAcc; /**< Speed accuracy estimate, in cm/s */ + uint32_t cAcc; /**< Course/heading accuracy estimate, in 0.00001 * degrees */ } UBXNavVelocityPayload; /** A struct representing the UBX-ACK-ACK/UBX-ACK-NACK (acknowledgement) payload */ typedef struct { - uint8_t clsId; /** The class ID of the acknowledged or not acknowledged message */ - uint8_t msgId; /** The message ID of the acknowledged or not acknowledged message */ + uint8_t clsId; /**< The class ID of the acknowledged or not acknowledged message */ + uint8_t msgId; /**< The message ID of the acknowledged or not acknowledged message */ } UBXAckPayload; -#endif // _UBX_DEF_ \ No newline at end of file +#endif // _UBX_DEF_ diff --git a/src/drivers/sensor_api.c b/src/drivers/sensor_api.c index 14cddf5..d123d3a 100644 --- a/src/drivers/sensor_api.c +++ b/src/drivers/sensor_api.c @@ -23,7 +23,7 @@ const SensorTagData SENSOR_TAG_DATA[] = { [TAG_ALTITUDE_REL] = {.name = "Altitude rel", .unit = "m", .fmt_str = "%.2f", .dsize = sizeof(float), .dtype = TYPE_FLOAT}, [TAG_ALTITUDE_SEA] = - {.name = "Altitude sea level", .unit = "mm", .fmt_str = "%d", .dsize = sizeof(int32_t), .dtype = TYPE_I32}, + {.name = "Altitude sea level", .unit = "m", .fmt_str = "%.2f", .dsize = sizeof(float), .dtype = TYPE_FLOAT}, [TAG_LINEAR_ACCEL_ABS] = {.name = "Absolute linear acceleration", .unit = "m/s^2", .fmt_str = "%.2fX, %.2fY, %.2fZ", @@ -39,14 +39,15 @@ const SensorTagData SENSOR_TAG_DATA[] = { .fmt_str = "%.2fX, %.2fY, %.2fZ", .dsize = sizeof(vec3d_t), .dtype = TYPE_VEC3D}, - [TAG_LONGITUDE] = - {.name = "Longitude", .unit = "0.1udeg", .fmt_str = "%d", .dsize = sizeof(int32_t), .dtype = TYPE_I32}, - [TAG_LATITUDE] = - {.name = "Latitude", .unit = "0.1udeg", .fmt_str = "%d", .dsize = sizeof(int32_t), .dtype = TYPE_I32}, - [TAG_SPEED] = - {.name = "Ground speed", .unit = "cm/s", .fmt_str = "%d", .dsize = sizeof(uint32_t), .dtype = TYPE_U32}, - [TAG_COURSE] = {.name = "Course", .unit = "10udeg", .fmt_str = "%d", .dsize = sizeof(uint32_t), .dtype = TYPE_U32}, - [TAG_FIX] = {.name = "Fix type", .unit = "", .fmt_str = "0x%x", .dsize = sizeof(uint8_t), .dtype = TYPE_U8}, + [TAG_COORDS] = + {.name = "Lat/Long", .unit = "deg", .fmt_str = "%.2fX, %.2fY", .dsize = sizeof(vec2d_t), .dtype = TYPE_VEC2D}, + /* [TAG_LATITUDE] = */ + /* {.name = "Latitude", .unit = "0.1udeg", .fmt_str = "%d", .dsize = sizeof(int32_t), .dtype = TYPE_I32}, */ + /* [TAG_SPEED] = */ + /* {.name = "Ground speed", .unit = "cm/s", .fmt_str = "%d", .dsize = sizeof(uint32_t), .dtype = TYPE_U32}, */ + /* [TAG_COURSE] = {.name = "Course", .unit = "10udeg", .fmt_str = "%d", .dsize = sizeof(uint32_t), .dtype = + TYPE_U32}, */ + /* [TAG_FIX] = {.name = "Fix type", .unit = "", .fmt_str = "0x%x", .dsize = sizeof(uint8_t), .dtype = TYPE_U8}, */ }; /** @@ -176,5 +177,9 @@ void sensor_write_data(FILE *stream, const SensorTag tag, const void *data) { fprintf(stream, format_str, SENSOR_TAG_DATA[tag].name, drefcast(const vec3d_t, data).x, drefcast(const vec3d_t, data).y, drefcast(const vec3d_t, data).z, SENSOR_TAG_DATA[tag].unit); break; + case TYPE_VEC2D: + fprintf(stream, format_str, SENSOR_TAG_DATA[tag].name, drefcast(const vec2d_t, data).x, + drefcast(const vec2d_t, data).y, SENSOR_TAG_DATA[tag].unit); + break; } } diff --git a/src/drivers/sensor_api.h b/src/drivers/sensor_api.h index 394a0a3..1bbc3e2 100644 --- a/src/drivers/sensor_api.h +++ b/src/drivers/sensor_api.h @@ -14,6 +14,14 @@ #include #include +/** Type for a 2 dimensional vector with x, y components. */ +typedef struct { + /** X component. */ + float x; + /** Y component. */ + float y; +} vec2d_t; + /** Type for a 3 dimensional vector with x, y, z components. */ typedef struct { /** X component. */ @@ -30,17 +38,12 @@ typedef enum { TAG_PRESSURE = 1, /**< Pressure in kilo Pascals */ TAG_HUMIDITY = 2, /**< Humidity in % relative humidity */ TAG_TIME = 3, /**< Time in milliseconds */ - TAG_ALTITUDE_SEA = 4, /**< Altitude above sea level in milimeters */ + TAG_ALTITUDE_SEA = 4, /**< Altitude above sea level in meters */ TAG_ALTITUDE_REL = 5, /**< Altitude above launch height in meters */ TAG_ANGULAR_VEL = 6, /**< Angular velocity in degrees per second */ TAG_LINEAR_ACCEL_REL = 7, /**< Relative linear acceleration in meters per second squared */ TAG_LINEAR_ACCEL_ABS = 8, /**< Absolute linear acceleration in meters per second squared */ - TAG_LATITUDE = 9, /**< Latitude in units of 0.1 microdegrees */ - TAG_LONGITUDE = 10, /**< Longitude in units of 0.1 microdegrees */ - TAG_ALTITUDE_MSL = 11, /**< The height above mean sea level in millimeters*/ - TAG_SPEED = 12, /**< Speed over the ground in cm/s */ - TAG_COURSE = 13, /**< Course over ground (heading) in units of 10 microdegrees */ - TAG_FIX = 14, /**< The type of GPS fix that is currently held, as a combination of flags */ + TAG_COORDS = 9, /**< Latitude and longitude in degrees */ } SensorTag; /** Describes the data type of the data associated with a tag. */ @@ -53,6 +56,7 @@ typedef enum { TYPE_I16, /**< int16_t */ TYPE_I8, /**< int8_t */ TYPE_VEC3D, /**< vec3d_t */ + TYPE_VEC2D, /**< vec2d_t */ } SensorTagDType; /** Stores information about each tag. */ diff --git a/src/main.c b/src/main.c index c06f0a5..a05e261 100644 --- a/src/main.c +++ b/src/main.c @@ -138,8 +138,6 @@ int main(int argc, char **argv) { .mq_flags = 0, .mq_maxmsg = 30, .mq_msgsize = 50, - .mq_recvwait = 1, - .mq_sendwait = 1, }; mqd_t sensor_q = mq_open(SENSOR_QUEUE, O_CREAT | O_RDONLY, S_IWOTH | S_IRUSR, &q_attr); if (sensor_q == -1) {