Skip to content

Commit

Permalink
Merge pull request #43 from CarletonURocketry/gps-suggestions
Browse files Browse the repository at this point in the history 
GPS suggestions
  • Loading branch information
@linguini1
linguini1 authored May 25, 2024
2 parents 258d39e + 17c5575 commit d40b374
Show file tree
Hide file tree
Showing 7 changed files with 128 additions and 109 deletions.
108 changes: 58 additions & 50 deletions src/collectors/m10spg_clctr.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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);
}
}
27 changes: 15 additions & 12 deletions src/drivers/m10spg/m10spg.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,14 @@
#include <errno.h>
#include <hw/i2c.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <unistd.h>

#ifdef __M10SPG_DEBUG__
#include <stdio.h>
#endif // __M10SPG_DEBUG__

/** Macro to early return an error. */
#define return_err(err) \
if (err != EOK) return err
Expand All @@ -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
Expand Down Expand Up @@ -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));
Expand All @@ -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];
Expand Down Expand Up @@ -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)];
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -384,4 +385,6 @@ static int debug_dump_buffer(const SensorLocation *loc, size_t bytes) {
}
putchar('\n');
return EOK;
}
}

#endif // __M10SPG_DEBUG__
7 changes: 4 additions & 3 deletions src/drivers/m10spg/m10spg.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,15 +11,16 @@
#include "../sensor_api.h"
#include <stdint.h>

/** 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_
52 changes: 26 additions & 26 deletions src/drivers/m10spg/ubx_def.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_
#endif // _UBX_DEF_
23 changes: 14 additions & 9 deletions src/drivers/sensor_api.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand All @@ -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}, */
};

/**
Expand Down Expand Up @@ -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;
}
}
Loading

0 comments on commit d40b374

Please sign in to comment.