Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

LSM6DSO32 interface change #42

Merged
merged 9 commits into from
May 22, 2024
Merged

LSM6DSO32 interface change #42

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
5f7c09c
Create helper functions and types in the IMU interface.
linguini1 May 20, 2024
27dd860
Update collector to use new interfaces.
linguini1 May 20, 2024
53e9581
Update collector to configure ODR and perform conversions.
linguini1 May 20, 2024
f993abb
Added WHOAMI for sanity checks.
linguini1 May 20, 2024
9887aee
Made OSR and FSR setting clearer.
linguini1 May 20, 2024
2e6f63f
Fixed temperature calculation.
linguini1 May 20, 2024
59aeaf4
Fixed conversion of data types according to typical specifications fr…
linguini1 May 21, 2024
07c7f27
Add a memory reboot.
linguini1 May 21, 2024
5064afe
Added high performance mode toggle.
linguini1 May 21, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
107 changes: 84 additions & 23 deletions src/collectors/lsm6dso32_clctr.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@
#include "sensor_api.h"
#include <stdio.h>

#define return_err(err) return (void *)((uint64_t)errno)

/**
* Collector thread for the LSM6DSO32 sensor.
* @param args Arguments in the form of `collector_args_t`
Expand All @@ -15,45 +17,104 @@ void *lsm6dso32_collector(void *args) {
if (sensor_q == -1) {
fprintf(stderr, "LSM6DSO32 collector could not open message queue '%s': '%s' \n", SENSOR_QUEUE,
strerror(errno));
return (void *)((uint64_t)errno);
return_err(err);
}

/* Create LSM6DO32 instance */
Sensor lsm6dso32;
lsm6dso32_init(&lsm6dso32, clctr_args(args)->bus, clctr_args(args)->addr, PRECISION_HIGH);
SensorLocation loc = {
.addr = {.addr = clctr_args(args)->addr, .fmt = I2C_ADDRFMT_7BIT},
.bus = clctr_args(args)->bus,
};

int err;
err = lsm6dso32_reset(&loc);
if (err != EOK) {
fprintf(stderr, "Failed to reset LSM6DSO32: %s\n", strerror(err));
return_err(err);
}

uint8_t lsm6dso32_context[sensor_get_ctx_size(lsm6dso32)];
sensor_set_ctx(&lsm6dso32, lsm6dso32_context);
errno_t err = sensor_open(lsm6dso32);
err = lsm6dso32_mem_reboot(&loc);
if (err != EOK) {
fprintf(stderr, "%s\n", strerror(err));
return (void *)((uint64_t)err);
fprintf(stderr, "Failed to reboot LSM6DSO32 memory content: %s\n", strerror(err));
return_err(err);
}

size_t nbytes;
uint8_t data[sensor_max_dsize(&lsm6dso32) + 1];
usleep(100);

err = lsm6dso32_high_performance(&loc, true);
if (err != EOK) {
fprintf(stderr, "Failed to set LSM6DSO32 to high performance mode: %s\n", strerror(err));
return_err(err);
}

err = lsm6dso32_set_acc_fsr(&loc, LA_FS_32G);
if (err != EOK) {
fprintf(stderr, "Failed to set LSM6DSO32 accelerometer FSR: %s\n", strerror(err));
return_err(err);
}

err = lsm6dso32_set_gyro_fsr(&loc, G_FS_500);
if (err != EOK) {
fprintf(stderr, "Failed to set LSM6DSO32 gyroscope FSR: %s\n", strerror(err));
return_err(err);
}

err = lsm6dso32_set_acc_odr(&loc, LA_ODR_6664);
if (err != EOK) {
fprintf(stderr, "Failed to set LSM6DSO32 accelerometer ODR: %s\n", strerror(err));
return_err(err);
}

err = lsm6dso32_set_gyro_odr(&loc, G_ODR_6664);
if (err != EOK) {
fprintf(stderr, "Failed to set LSM6DSO32 gyroscope ODR: %s\n", strerror(err));
return_err(err);
}

uint8_t data[sizeof(vec3d_t) + 1];
int16_t temperature;
int16_t x;
int16_t y;
int16_t z;

for (;;) {

// Read temperature
sensor_read(lsm6dso32, TAG_TEMPERATURE, &data[1], &nbytes);
data[0] = TAG_TEMPERATURE;
if (mq_send(sensor_q, (char *)data, sizeof(data), 0) == -1) {
fprintf(stderr, "LSM6DSO32 couldn't send message: %s\n", strerror(errno));
err = lsm6dso32_get_temp(&loc, &temperature);
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) {
fprintf(stderr, "LSM6DSO32 couldn't send message: %s\n", strerror(errno));
}
}

// Read linear acceleration
sensor_read(lsm6dso32, TAG_LINEAR_ACCEL_REL, &data[1], &nbytes);
data[0] = TAG_LINEAR_ACCEL_REL;
if (mq_send(sensor_q, (char *)data, sizeof(data), 0) == -1) {
fprintf(stderr, "LSM6DSO32 couldn't send message: %s\n", strerror(errno));
err = lsm6dso32_get_accel(&loc, &x, &y, &z);
if (err != EOK) {
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) {
fprintf(stderr, "LSM6DSO32 couldn't send message: %s\n", strerror(errno));
}
}

// Read angular velocity
sensor_read(lsm6dso32, TAG_ANGULAR_VEL, &data[1], &nbytes);
data[0] = TAG_ANGULAR_VEL;
if (mq_send(sensor_q, (char *)data, sizeof(data), 0) == -1) {
fprintf(stderr, "LSM6DSO32 couldn't send message: %s\n", strerror(errno));
err = lsm6dso32_get_angular_vel(&loc, &x, &y, &z);

if (err != EOK) {
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) {
fprintf(stderr, "LSM6DSO32 couldn't send message: %s\n", strerror(errno));
}
}
}
}
Loading
Loading