Skip to content

Commit

Permalink
Merge pull request #48 from CarletonURocketry/m10spg-suggestions
Browse files Browse the repository at this point in the history 
Add some remaining M10SPG suggestions
  • Loading branch information
@linguini1
linguini1 authored Jun 7, 2024
2 parents 7081ba2 + 95534fb commit ec8afca
Show file tree
Hide file tree
Showing 13 changed files with 305 additions and 311 deletions.
117 changes: 60 additions & 57 deletions .gitignore
View file
Original file line number Diff line number Diff line change
@@ -1,57 +1,60 @@
# Documentation
/docs/html/
*.pinfo

# Object files
*.o
*.ko
/obj/
*.obj
*.elf

# Linker output
*.ilk
*.map
*.exp

# Precompiled Headers
*.gch
*.pch

# Libraries
*.lib
*.a
*.la
*.lo

# Shared objects (inc. Windows DLLs)
*.dll
*.so
*.so.*
*.dylib

# Executables
fetcher
*.exe
*.out
*.app
*.i*86
*.x86_64
*.hex

# Debug files
*.dSYM/
*.su
*.idb
*.pdb
compile_commands.json
.cache

# Kernel Module Compile Results
*.mod*
*.cmd
.tmp_versions/
modules.order
Module.symvers
Mkfile.old
dkms.conf
# Documentation
/docs/html/
*.pinfo

# Object files
*.o
*.ko
/obj/
*.obj
*.elf

# Linker output
*.ilk
*.map
*.exp

# Precompiled Headers
*.gch
*.pch

# Libraries
*.lib
*.a
*.la
*.lo

# Shared objects (inc. Windows DLLs)
*.dll
*.so
*.so.*
*.dylib

# Executables
fetcher
*.exe
*.out
*.app
*.i*86
*.x86_64
*.hex

# Debug files
*.dSYM/
*.su
*.idb
*.pdb
compile_commands.json
.cache

# Kernel Module Compile Results
*.mod*
*.cmd
.tmp_versions/
modules.order
Module.symvers
Mkfile.old
dkms.conf

# Editor files
*.code-workspace
22 changes: 11 additions & 11 deletions src/collectors/lsm6dso32_clctr.c
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
#include "../drivers/lsm6dso32/lsm6dso32.h"
#include "../drivers/sensor_api.h"
#include "collectors.h"
#include "sensor_api.h"
#include <stdio.h>

#define return_err(err) return (void *)((uint64_t)errno)
Expand Down Expand Up @@ -70,7 +70,7 @@ void *lsm6dso32_collector(void *args) {
return_err(err);
}

uint8_t data[sizeof(vec3d_t) + 1];
common_t msg;
int16_t temperature;
int16_t x;
int16_t y;
Expand All @@ -83,9 +83,9 @@ void *lsm6dso32_collector(void *args) {
if (err != EOK) {
fprintf(stderr, "LSM6DSO32 could not read temperature: %s\n", strerror(errno));
} else {
data[0] = TAG_TEMPERATURE;
*((float *)(data + 1)) = (float)temperature;
if (mq_send(sensor_q, (char *)data, sizeof(data), 0) == -1) {
msg.type = TAG_TEMPERATURE;
msg.data.FLOAT = (float)temperature;
if (mq_send(sensor_q, (char *)&msg, sizeof(msg), 0) == -1) {
fprintf(stderr, "LSM6DSO32 couldn't send message: %s\n", strerror(errno));
}
}
Expand All @@ -96,9 +96,9 @@ void *lsm6dso32_collector(void *args) {
fprintf(stderr, "LSM6DSO32 could not read linear acceleration: %s\n", strerror(errno));
} else {
lsm6dso32_convert_accel(LA_FS_32G, &x, &y, &z);
data[0] = TAG_LINEAR_ACCEL_REL;
*((vec3d_t *)(data + 1)) = (vec3d_t){.x = x, .y = y, .z = z};
if (mq_send(sensor_q, (char *)data, sizeof(data), 0) == -1) {
msg.type = TAG_LINEAR_ACCEL_REL;
msg.data.VEC3D = (vec3d_t){.x = x, .y = y, .z = z};
if (mq_send(sensor_q, (char *)&msg, sizeof(msg), 0) == -1) {
fprintf(stderr, "LSM6DSO32 couldn't send message: %s\n", strerror(errno));
}
}
Expand All @@ -110,9 +110,9 @@ void *lsm6dso32_collector(void *args) {
fprintf(stderr, "LSM6DSO32 could not read angular velocity: %s\n", strerror(errno));
} else {
lsm6dso32_convert_angular_vel(G_FS_500, &x, &y, &z);
data[0] = TAG_ANGULAR_VEL;
*((vec3d_t *)(data + 1)) = (vec3d_t){.x = x, .y = y, .z = z};
if (mq_send(sensor_q, (char *)data, sizeof(data), 0) == -1) {
msg.type = TAG_ANGULAR_VEL;
msg.data.VEC3D = (vec3d_t){.x = x, .y = y, .z = z};
if (mq_send(sensor_q, (char *)&msg, sizeof(msg), 0) == -1) {
fprintf(stderr, "LSM6DSO32 couldn't send message: %s\n", strerror(errno));
}
}
Expand Down
208 changes: 97 additions & 111 deletions src/collectors/m10spg_clctr.c
View file
Original file line number Diff line number Diff line change
@@ -1,111 +1,97 @@
#include "../drivers/m10spg/m10spg.h"
#include "../drivers/m10spg/ubx_def.h"
#include "collectors.h"

union read_buffer {
UBXNavPositionPayload pos;
UBXNavVelocityPayload vel;
UBXNavStatusPayload stat;
};

/** Type for sending measurements over the message queue. */
typedef struct {
uint8_t type; /**< Measurement type */
union {
uint32_t U32;
int32_t I32;
uint8_t U8;
};
} __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},
};

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_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_send_command(&loc, UBX_NAV_POSLLH, &buf, sizeof(UBXNavPositionPayload));
if (err == EOK) {

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) {
fprintf(stderr, "M10SPG couldn't send message: %s.\n", strerror(errno));
}
} else {
fprintf(stderr, "M10SPG failed to read position: %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);
}
#include "../drivers/m10spg/m10spg.h"
#include "../drivers/m10spg/ubx_def.h"
#include "collectors.h"

union read_buffer {
UBXNavPositionPayload pos;
UBXNavVelocityPayload vel;
UBXNavStatusPayload stat;
};

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},
};

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;
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));
}
// 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_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));
}
} else {
fprintf(stderr, "M10SPG failed to read position: %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);
}
Loading

0 comments on commit ec8afca

Please sign in to comment.