From 5f7c09cb7cda391e72558e2d8ba392a3ffd42ee7 Mon Sep 17 00:00:00 2001
From: Matteo Golin <matteo.golin@gmail.com>
Date: Mon, 20 May 2024 16:54:10 -0400
Subject: [PATCH 1/9] Create helper functions and types in the IMU interface.

---
 src/drivers/lsm6dso32/lsm6dso32.c | 320 ++++++++++++++++++++----------
 src/drivers/lsm6dso32/lsm6dso32.h |  62 +++++-
 2 files changed, 273 insertions(+), 109 deletions(-)

diff --git a/src/drivers/lsm6dso32/lsm6dso32.c b/src/drivers/lsm6dso32/lsm6dso32.c
index b71008a..962ea61 100644
--- a/src/drivers/lsm6dso32/lsm6dso32.c
+++ b/src/drivers/lsm6dso32/lsm6dso32.c
@@ -82,22 +82,18 @@ enum imu_reg {
 #define return_err(err)                                                                                                \
     if (err != EOK) return err
 
-/** A list of data types that can be read by the LSM6DSO32. */
-static const SensorTag TAGS[] = {TAG_TEMPERATURE, TAG_LINEAR_ACCEL_REL, TAG_ANGULAR_VEL};
-
 /**
  * Write data to a register of the LSM6DSO32.
  * WARNING: This function does not lock the I2C bus. It is meant to be called multiple times and therefore the bus must
  * be locked by the caller.
- * @param sensor A pointer to a LSM6DSO32 sensor instance.
+ * @param loc The location of the IMU on the I2C bus.
  * @param reg The address of the register to write to.
  * @param data The byte of data to write to the register.
  */
-static errno_t lsm6dso32_write_byte(Sensor const *sensor, const uint8_t reg, const uint8_t data) {
+static errno_t lsm6dso32_write_byte(SensorLocation const *loc, const uint8_t reg, const uint8_t data) {
 
     // Construct header
-    i2c_send_t header = {.slave = {0}, .stop = 1, .len = 2};
-    header.slave = sensor->loc.addr;
+    i2c_send_t header = {.slave = loc->addr, .stop = 1, .len = 2};
 
     // Create command
     uint8_t cmd[sizeof(header) + 2];
@@ -106,123 +102,247 @@ static errno_t lsm6dso32_write_byte(Sensor const *sensor, const uint8_t reg, con
     cmd[sizeof(header) + 1] = data;
 
     // Send command
-    return devctl(sensor->loc.bus, DCMD_I2C_SEND, cmd, sizeof(cmd), NULL);
+    return devctl(loc->bus, DCMD_I2C_SEND, cmd, sizeof(cmd), NULL);
 }
 
 /**
  * Read data from register address `reg`.
  * WARNING: This function does not lock the I2C bus, as it is meant to be used in a continuous stream of calls.
- * @param sensor A reference to an LSM6DSO32 sensor instance.
+ * @param loc The location of the IMU on the I2C bus.
  * @param reg The register address to read from.
  * @param buf The buffer to read data into.
  * @return EOK if the read was okay, otherwise the error status of the read command.
  */
-static errno_t lsm6dso32_read_byte(Sensor *sensor, uint8_t reg, uint8_t *buf) {
+static errno_t lsm6dso32_read_byte(SensorLocation const *loc, uint8_t reg, uint8_t *buf) {
 
-    i2c_sendrecv_t read_hdr = {.stop = 1, .send_len = 1, .recv_len = 1, .slave = {0}};
-    read_hdr.slave = sensor->loc.addr;
+    i2c_sendrecv_t read_hdr = {.stop = 1, .send_len = 1, .recv_len = 1, .slave = loc->addr};
 
     uint8_t read_cmd[sizeof(read_hdr) + 1];
     memcpy(read_cmd, &read_hdr, sizeof(read_hdr));
     read_cmd[sizeof(read_hdr)] = reg; // Data to be send is the register address
 
-    errno_t err = devctl(sensor->loc.bus, DCMD_I2C_SENDRECV, read_cmd, sizeof(read_cmd), NULL);
-    if (err != EOK) return err;
+    errno_t err = devctl(loc->bus, DCMD_I2C_SENDRECV, read_cmd, sizeof(read_cmd), NULL);
+    return_err(err);
 
     *buf = read_cmd[sizeof(read_hdr)]; // Received byte will be the last one
     return err;
 }
 
 /**
- * Reads the specified data from the LSM6DSO32.
- * @param sensor A reference to an LSM6DSO32 sensor.
- * @param tag The tag of the data type that should be read.
- * @param buf A pointer to the memory location to store the data.
- * @param nbytes The number of bytes that were written into the byte array buffer.
- * @return Error status of reading from the sensor. EOK if successful. EINVAL if unrecognized tag.
+ * Reads temperature from the IMU.
+ * @param loc The sensor location.
+ * @temperature A pointer to where to store the temperature in degrees Celsius.
+ * @return The error status of reading from the sensor. EOK if successful.
+ */
+int lsm6dso32_get_temp(SensorLocation const *loc, int16_t *temperature) {
+    int err = lsm6dso32_read_byte(loc, OUT_TEMP_L, (uint8_t *)(temperature)); // Read low byte
+    return_err(err);
+    err = lsm6dso32_read_byte(loc, OUT_TEMP_H, (uint8_t *)(temperature) + 1); // Read high byte
+    return_err(err);
+    *temperature /= 256.0f + 25.0f; // In degrees Celsius
+    return err;
+}
+
+/**
+ * Read the linear acceleration data from the IMU.
+ * @param x A pointer to where to store the X component of the acceleration in milli-Gs per LSB.
+ * @param y A pointer to where to store the Y component of the acceleration in milli-Gs per LSB.
+ * @param z A pointer to where to store the Z component of the acceleration in milli-Gs per LSB.
+ * @return Any error that occurred while reading the sensor, EOK if successful.
+ */
+int lsm6dso32_get_accel(SensorLocation const *loc, int16_t *x, int16_t *y, int16_t *z) {
+
+    int err = lsm6dso32_read_byte(loc, OUTX_L_A, (uint8_t *)(x)); // Read low byte
+    return_err(err);
+    err = lsm6dso32_read_byte(loc, OUTX_H_A, (uint8_t *)(x) + 1); // Read high byte
+    return_err(err);
+
+    err = lsm6dso32_read_byte(loc, OUTY_L_A, (uint8_t *)(y));
+    return_err(err);
+    err = lsm6dso32_read_byte(loc, OUTY_H_A, (uint8_t *)(y) + 1);
+    return_err(err);
+
+    err = lsm6dso32_read_byte(loc, OUTZ_L_A, (uint8_t *)(z));
+    return_err(err);
+    err = lsm6dso32_read_byte(loc, OUTZ_H_A, (uint8_t *)(z) + 1);
+    return err;
+}
+
+/**
+ * Read the linear angular velocity data from the IMU.
+ * @param x A pointer to where to store the X component of the angular velocity in millidegrees per LSB.
+ * @param y A pointer to where to store the Y component of the angular velocity in millidegrees per LSB.
+ * @param z A pointer to where to store the Z component of the angular velocity in millidegrees per LSB.
+ * @return Any error that occurred while reading the sensor, EOK if successful.
+ */
+int lsm6dso32_get_angular_vel(SensorLocation const *loc, int16_t *x, int16_t *y, int16_t *z) {
+    int err = lsm6dso32_read_byte(loc, OUTX_L_G, (uint8_t *)(x)); // Read low byte
+    return_err(err);
+    err = lsm6dso32_read_byte(loc, OUTX_H_G, (uint8_t *)(x) + 1); // Read high byte
+    return_err(err);
+
+    err = lsm6dso32_read_byte(loc, OUTY_L_G, (uint8_t *)(y));
+    return_err(err);
+    err = lsm6dso32_read_byte(loc, OUTY_H_G, (uint8_t *)(y) + 1);
+    return_err(err);
+
+    err = lsm6dso32_read_byte(loc, OUTZ_L_G, (uint8_t *)(z));
+    return_err(err);
+    err = lsm6dso32_read_byte(loc, OUTZ_H_G, (uint8_t *)(z) + 1);
+    return err;
+}
+
+/**
+ * Converts acceleration in milli-Gs per LSB to meters per second squared. Results are stored back in the pointers
+ * themselves. Passing NULL as any of the pointers will result in the calculation being skipped.
+ * @param acc_fsr The full scale range of the accelerometer.
+ * @param x A pointer to the X component of the acceleration in milli-Gs per LSB.
+ * @param y A pointer to the Y component of the acceleration in milli-Gs per LSB.
+ * @param z A pointer to the Z component of the acceleration in milli-Gs per LSB.
+ */
+void lsm6dso32_convert_accel(accel_fsr_e acc_fsr, int16_t *x, int16_t *y, int16_t *z) {
+    float conversion_factor = acc_fsr * MILLI_UNIT_PER_LSB_TO_UNIT * GRAVIT_ACC;
+    if (x) (*x) *= conversion_factor;
+    if (y) (*y) *= conversion_factor;
+    if (z) (*z) *= conversion_factor;
+}
+
+/**
+ * Converts angular velocity in millidegrees per LSB to meters per second squared. Results are stored back in the
+ * pointers themselves. Passing NULL as any of the pointers will result in the calculation being skipped.
+ * @param gyro_fsr The full scale range of the gyroscope.
+ * @param x A pointer to the X component of the angular velocity in millidegrees per LSB.
+ * @param y A pointer to the Y component of the angular velocity in millidegrees per LSB.
+ * @param z A pointer to the Z component of the angular velocity in millidegrees per LSB.
+ */
+void lsm6dso32_convert_angular_vel(gyro_fsr_e gyro_fsr, int16_t *x, int16_t *y, int16_t *z) {
+    float conversion_factor = gyro_fsr * MILLI_UNIT_PER_LSB_TO_UNIT;
+    if (x) (*x) *= conversion_factor;
+    if (y) (*y) *= conversion_factor;
+    if (z) (*z) *= conversion_factor;
+}
+
+/**
+ * Performs a software reset of the LSM6DSO32.
+ * @param loc The location of the IMU on the I2C bus.
+ * @return Any error which occurred while resetting the IMU, EOK if successful.
  */
-static errno_t lsm6dso32_read(Sensor *sensor, const SensorTag tag, void *buf, size_t *nbytes) {
-    errno_t err;
-
-    switch (tag) {
-    case TAG_TEMPERATURE: {
-        int16_t temp;
-        err = lsm6dso32_read_byte(sensor, OUT_TEMP_L, (uint8_t *)(&temp)); // Read low byte
-        return_err(err);
-        err = lsm6dso32_read_byte(sensor, OUT_TEMP_H, (uint8_t *)(&temp) + 1); // Read high byte
-        return_err(err);
-
-        *(float *)buf = (float)(temp) / 256.0f + 25.0f; // In degrees Celsius
-        *nbytes = sizeof(float);
+int lsm6dso32_reset(SensorLocation const *loc) { return lsm6dso32_write_byte(loc, CTRL3_C, 0x01); }
+
+/**
+ * Sets the accelerometer full scale range.
+ * @param loc The location of the IMU on the I2C bus.
+ * @param fsr The FSR to set.
+ * @return Any error which occurred communicating with the IMU, EOK if successful, EINVAL if bad fsr.
+ */
+int lsm6dso32_set_acc_fsr(SensorLocation const *loc, accel_fsr_e fsr) {
+
+    uint8_t reg_val;
+    int err = lsm6dso32_read_byte(loc, CTRL1_XL, &reg_val); // Don't overwrite other configurations
+    reg_val &= ~((1 << 3) | (1 << 2));                      // Clear FSR selection bits
+    return_err(err);
+
+    switch (fsr) {
+    case LA_FS_4G:
+        // All zeroes
         break;
-    }
-    case TAG_LINEAR_ACCEL_REL: {
-
-        int16_t x;
-        err = lsm6dso32_read_byte(sensor, OUTX_L_A, (uint8_t *)(&x)); // Read low byte
-        return_err(err);
-        err = lsm6dso32_read_byte(sensor, OUTX_H_A, (uint8_t *)(&x) + 1); // Read high byte
-        return_err(err);
-
-        int16_t y;
-        err = lsm6dso32_read_byte(sensor, OUTY_L_A, (uint8_t *)(&y));
-        return_err(err);
-        err = lsm6dso32_read_byte(sensor, OUTY_H_A, (uint8_t *)(&y) + 1);
-        return_err(err);
-
-        int16_t z;
-        err = lsm6dso32_read_byte(sensor, OUTZ_L_A, (uint8_t *)(&z));
-        return_err(err);
-        err = lsm6dso32_read_byte(sensor, OUTZ_H_A, (uint8_t *)(&z) + 1);
-        return_err(err);
-
-        // Converts milli-Gs per LSB to m/s^2
-        float conversion_factor =
-            (float)((LSM6DSO32Context *)(sensor->context.data))->acc_fsr * MILLI_UNIT_PER_LSB_TO_UNIT * GRAVIT_ACC;
-
-        ((vec3d_t *)(buf))->x = (float)(x)*conversion_factor;
-        ((vec3d_t *)(buf))->y = (float)(y)*conversion_factor;
-        ((vec3d_t *)(buf))->z = (float)(z)*conversion_factor;
-        *nbytes = sizeof(vec3d_t);
+    case LA_FS_8G:
+        reg_val |= (0x2 << 3);
         break;
-    }
-    case TAG_ANGULAR_VEL: {
-
-        int16_t x;
-        err = lsm6dso32_read_byte(sensor, OUTX_L_G, (uint8_t *)(&x)); // Read low byte
-        return_err(err);
-        err = lsm6dso32_read_byte(sensor, OUTX_H_G, (uint8_t *)(&x) + 1); // Read high byte
-        return_err(err);
-
-        int16_t y;
-        err = lsm6dso32_read_byte(sensor, OUTY_L_G, (uint8_t *)(&y));
-        return_err(err);
-        err = lsm6dso32_read_byte(sensor, OUTY_H_G, (uint8_t *)(&y) + 1);
-        return_err(err);
-
-        int16_t z;
-        err = lsm6dso32_read_byte(sensor, OUTZ_L_G, (uint8_t *)(&z));
-        return_err(err);
-        err = lsm6dso32_read_byte(sensor, OUTZ_H_G, (uint8_t *)(&z) + 1);
-        return_err(err);
-
-        // Converts millidegrees per second per LSB to degrees per second
-        float conversion_factor =
-            (float)((LSM6DSO32Context *)(sensor->context.data))->gyro_fsr * MILLI_UNIT_PER_LSB_TO_UNIT;
-
-        ((vec3d_t *)(buf))->x = (float)(x)*conversion_factor;
-        ((vec3d_t *)(buf))->y = (float)(y)*conversion_factor;
-        ((vec3d_t *)(buf))->z = (float)(z)*conversion_factor;
-        *nbytes = sizeof(vec3d_t);
+    case LA_FS_16G:
+        reg_val |= (0x3 << 3);
+        break;
+    case LA_FS_32G:
+        reg_val |= (0x1 << 3);
         break;
+    default:
+        return EINVAL;
     }
+    return lsm6dso32_write_byte(loc, CTRL1_XL, reg_val);
+}
+
+/**
+ * Sets the gyroscope full scale range.
+ * @param loc The location of the IMU on the I2C bus.
+ * @param fsr The FSR to set.
+ * @return Any error which occurred communicating with the IMU, EOK if successful, EINVAL if bad fsr.
+ */
+int lsm6dso32_set_gyro_fsr(SensorLocation const *loc, gyro_fsr_e fsr) {
+
+    uint8_t reg_val;
+    int err = lsm6dso32_read_byte(loc, CTRL2_G, &reg_val); // Don't overwrite other configurations
+    reg_val &= ~((1 << 3) | (1 << 2) | (1 << 1));          // Clear FSR selection bits
+    return_err(err);
+
+    switch (fsr) {
+    case G_FS_125:
+        reg_val |= (1 << 1);
+        break;
+    case G_FS_250:
+        // All zeroes
+        break;
+    case G_FS_500:
+        reg_val |= (1 << 2);
+        break;
+    case G_FS_1000:
+        reg_val |= (1 << 3);
+        break;
+    case G_FS_2000:
+        reg_val |= ((1 << 3) | (1 << 2));
+        break;
     default:
         return EINVAL;
     }
+    return lsm6dso32_write_byte(loc, CTRL2_G, reg_val);
+}
 
-    return err;
+/**
+ * Sets the accelerometer output data rate.
+ * NOTE: An ODR of 1.6Hz can only be set if high performance operating mode is disabled. Otherwise the effect will be
+ * 12.6Hz ODR. It is the responsibility of the caller to set disable the high performance operating mode before or after
+ * this function is called.
+ * @param loc The location of the IMU on the I2C bus.
+ * @param odr The ODR to set.
+ * @return Any error which occurred communicating with the IMU, EOK if successful.
+ */
+int lsm6dso32_set_acc_odr(SensorLocation const *loc, accel_odr_e odr) {
+    uint8_t reg_val;
+    int err = lsm6dso32_read_byte(loc, CTRL1_XL, &reg_val); // Don't overwrite other configurations
+    reg_val &= ~(0xF0);                                     // Clear ODR selection bits
+    reg_val |= (uint8_t)(odr);                              // Set ODR selection
+    return_err(err);
+    return lsm6dso32_write_byte(loc, CTRL1_XL, reg_val);
+}
+
+/**
+ * Sets the gyroscope output data rate.
+ * @param loc The location of the IMU on the I2C bus.
+ * @param odr The ODR to set.
+ * @return Any error which occurred communicating with the IMU, EOK if successful.
+ */
+int lsm6dso32_set_gyro_odr(SensorLocation const *loc, gyro_odr_e odr) {
+    uint8_t reg_val;
+    int err = lsm6dso32_read_byte(loc, CTRL2_G, &reg_val); // Don't overwrite other configurations
+    reg_val &= ~(0xF0);                                    // Clear ODR selection bits
+    reg_val |= (uint8_t)(odr);                             // Set ODR selection
+    return_err(err);
+    return lsm6dso32_write_byte(loc, CTRL2_G, reg_val);
 }
 
+/**
+ * Powers down the gyroscope.
+ * @param loc The location of the IMU on the I2C bus.
+ * @return Any error which occurred communicating with the IMU, EOK if successful.
+ */
+int lsm6dso32_disable_gyro(SensorLocation const *loc) { return lsm6dso32_set_gyro_odr(loc, 0); }
+
+/**
+ * Powers down the accelerometer.
+ * @param loc The location of the IMU on the I2C bus.
+ * @return Any error which occurred communicating with the IMU, EOK if successful.
+ */
+int lsm6dso32_disable_accel(SensorLocation const *loc) { return lsm6dso32_set_gyro_odr(loc, 0); }
+
 /**
  * Prepares the LSM6DSO32 for reading.
  * @param sensor A reference to an LSM6DSO32 sensor.
@@ -253,19 +373,3 @@ static errno_t lsm6dso32_open(Sensor *sensor) {
     // TODO: what filter configuration will give the best measurements?
     return err;
 }
-
-/**
- * Initializes a sensor struct with the interface to interact with the LSM6DSO32.
- * @param sensor The sensor interface to be initialized.
- * @param bus The file descriptor of the I2C bus.
- * @param addr The address of the sensor on the I2C bus.
- * @param precision The precision to read measurements with.
- */
-void lsm6dso32_init(Sensor *sensor, const int bus, const uint8_t addr, const SensorPrecision precision) {
-    sensor->precision = precision;
-    sensor->loc = (SensorLocation){.bus = bus, .addr = {.addr = (addr & 0x7F), .fmt = I2C_ADDRFMT_7BIT}};
-    sensor->tag_list = (SensorTagList){.tags = TAGS, .len = sizeof(TAGS) / sizeof(SensorTag)};
-    sensor->context.size = sizeof(LSM6DSO32Context);
-    sensor->open = &lsm6dso32_open;
-    sensor->read = &lsm6dso32_read;
-}
diff --git a/src/drivers/lsm6dso32/lsm6dso32.h b/src/drivers/lsm6dso32/lsm6dso32.h
index f79abf0..9242b46 100644
--- a/src/drivers/lsm6dso32/lsm6dso32.h
+++ b/src/drivers/lsm6dso32/lsm6dso32.h
@@ -12,6 +12,66 @@
 #include "../sensor_api.h"
 #include <stdint.h>
 
-void lsm6dso32_init(Sensor *sensor, const int bus, const uint8_t addr, const SensorPrecision precision);
+/** Represents the possible full scale range settings for linear acceleration in Gs (gravitational acceleration). */
+typedef enum {
+    LA_FS_4G = 4,   /**< Full scale range of +/- 4Gs */
+    LA_FS_8G = 8,   /**< Full scale range of +/- 8Gs */
+    LA_FS_16G = 16, /**< Full scale range of +/- 16Gs */
+    LA_FS_32G = 32, /**< Full scale range of +/- 32Gs */
+} accel_fsr_e;
+
+/** Represents the possible full scale range settings for the gyroscope in degrees per second. */
+typedef enum {
+    G_FS_125 = 125,   /**< Full scale range of +/-125 degrees per second. */
+    G_FS_250 = 250,   /**< Full scale range of +/-250 degrees per second. */
+    G_FS_500 = 500,   /**< Full scale range of +/-500 degrees per second. */
+    G_FS_1000 = 1000, /**< Full scale range of +/-1000 degrees per second. */
+    G_FS_2000 = 2000, /**< Full scale range of +/-2000 degrees per second. */
+} gyro_fsr_e;
+
+/** Possible output data rates for linear acceleration in Hz. */
+typedef enum {
+    LA_ODR_1_6 = 0xD0,  /** 1.6 Hz */
+    LA_ODR_12_5 = 0x10, /** 12.5 Hz */
+    LA_ODR_26 = 0x20,   /** 26 Hz */
+    LA_ODR_52 = 0x30,   /** 52 Hz */
+    LA_ODR_104 = 0x40,  /** 104 Hz */
+    LA_ODR_208 = 0x50,  /** 208 Hz */
+    LA_ODR_416 = 0x60,  /** 416 Hz */
+    LA_ODR_833 = 0x70,  /** 833 Hz */
+    LA_ODR_1666 = 0x80, /** 1666 Hz */
+    LA_ODR_3332 = 0x90, /** 3332 Hz */
+    LA_ODR_6664 = 0xA0, /** 6644 Hz */
+} accel_odr_e;
+
+/** Possible output data rates for the gyroscope in Hz. */
+typedef enum {
+    G_ODR_12_5, /** 12.5 Hz */
+    G_ODR_26,   /** 26 Hz */
+    G_ODR_52,   /** 52 Hz */
+    G_ODR_104,  /** 104 Hz */
+    G_ODR_208,  /** 208 Hz */
+    G_ODR_416,  /** 416 Hz */
+    G_ODR_833,  /** 833 Hz */
+    G_ODR_1666, /** 1666 Hz */
+    G_ODR_3332, /** 3332 Hz */
+    G_ODR_6664, /** 6644 Hz */
+} gyro_odr_e;
+
+int lsm6dso32_reset(SensorLocation const *loc);
+int lsm6dso32_disable_accel(SensorLocation const *loc);
+int lsm6dso32_disable_gyro(SensorLocation const *loc);
+
+int lsm6dso32_set_acc_fsr(SensorLocation const *loc, accel_fsr_e fsr);
+int lsm6dso32_set_gyro_fsr(SensorLocation const *loc, gyro_fsr_e fsr);
+int lsm6dso32_set_acc_odr(SensorLocation const *loc, accel_odr_e odr);
+int lsm6dso32_set_gyro_odr(SensorLocation const *loc, gyro_odr_e odr);
+
+int lsm6dso32_get_temp(SensorLocation const *loc, int16_t *temperature);
+int lsm6dso32_get_accel(SensorLocation const *loc, int16_t *x, int16_t *y, int16_t *z);
+int lsm6dso32_get_angular_vel(SensorLocation const *loc, int16_t *x, int16_t *y, int16_t *z);
+
+void lsm6dso32_convert_accel(accel_fsr_e acc_fsr, int16_t *x, int16_t *y, int16_t *z);
+void lsm6dso32_convert_angular_vel(gyro_fsr_e gyro_fsr, int16_t *x, int16_t *y, int16_t *z);
 
 #endif // _LSM6DSO32_

From 27dd8601f23a2eebd05253ba4c31b77e45e5bb56 Mon Sep 17 00:00:00 2001
From: Matteo Golin <matteo.golin@gmail.com>
Date: Mon, 20 May 2024 17:50:05 -0400
Subject: [PATCH 2/9] Update collector to use new interfaces.

---
 src/collectors/lsm6dso32_clctr.c  | 48 ++++++++++++++++++++++---------
 src/drivers/lsm6dso32/lsm6dso32.c | 39 -------------------------
 2 files changed, 34 insertions(+), 53 deletions(-)

diff --git a/src/collectors/lsm6dso32_clctr.c b/src/collectors/lsm6dso32_clctr.c
index 5ea6e03..a8f7a8c 100644
--- a/src/collectors/lsm6dso32_clctr.c
+++ b/src/collectors/lsm6dso32_clctr.c
@@ -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`
@@ -15,43 +17,61 @@ 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(errno));
+        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(errno));
+        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_set_gyro_fsr(&loc, G_FS_500);
     if (err != EOK) {
-        fprintf(stderr, "%s\n", strerror(err));
-        return (void *)((uint64_t)err);
+        fprintf(stderr, "Failed to set LSM6DSO32 gyroscope FSR: %s\n", strerror(errno));
+        return_err(err);
     }
 
-    size_t nbytes;
-    uint8_t data[sensor_max_dsize(&lsm6dso32) + 1];
+    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);
+        lsm6dso32_get_temp(&loc, &temperature);
         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);
+        lsm6dso32_get_accel(&loc, &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);
+        lsm6dso32_get_angular_vel(&loc, &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));
         }
diff --git a/src/drivers/lsm6dso32/lsm6dso32.c b/src/drivers/lsm6dso32/lsm6dso32.c
index 962ea61..edb1ae7 100644
--- a/src/drivers/lsm6dso32/lsm6dso32.c
+++ b/src/drivers/lsm6dso32/lsm6dso32.c
@@ -35,14 +35,6 @@
  */
 #define MILLI_UNIT_PER_LSB_TO_UNIT 2.0f / 65535.0f
 
-/** Type to store the context of the IMU. */
-typedef struct {
-    /** Acceleration full scale range. */
-    uint8_t acc_fsr;
-    /** Gyroscope full scale range. */
-    uint16_t gyro_fsr;
-} LSM6DSO32Context;
-
 /** The different registers that are present in the IMU (and used by this program). */
 enum imu_reg {
     WHO_AM_I = 0x0F,   /**< Returns the hard-coded address of the IMU on the I2C bus. */
@@ -342,34 +334,3 @@ int lsm6dso32_disable_gyro(SensorLocation const *loc) { return lsm6dso32_set_gyr
  * @return Any error which occurred communicating with the IMU, EOK if successful.
  */
 int lsm6dso32_disable_accel(SensorLocation const *loc) { return lsm6dso32_set_gyro_odr(loc, 0); }
-
-/**
- * Prepares the LSM6DSO32 for reading.
- * @param sensor A reference to an LSM6DSO32 sensor.
- * @return The error status of the call. EOK if successful.
- */
-static errno_t lsm6dso32_open(Sensor *sensor) {
-
-    // Perform software reset
-    errno_t err = lsm6dso32_write_byte(sensor, CTRL3_C, 0x01);
-    return_err(err);
-
-    // TODO: We will want to operate in continuous mode for our use case (polling)
-
-    // Set the operating mode of the accelerometer to ODR of 6.66kHz (high perf)
-    // TODO: set based on configured sensor performance
-    // Full scale range of +-32g (necessary for rocket)
-    ((LSM6DSO32Context *)(sensor->context.data))->acc_fsr = 32;
-    err = lsm6dso32_write_byte(sensor, CTRL1_XL, 0xA4);
-    return_err(err);
-
-    // Set the operating mode of the gyroscope to ODR of 6.66kHz (high perf)
-    // TODO: set based on configured sensor performance
-    // TODO: what full-scale selection? Keep 500 for now
-    ((LSM6DSO32Context *)(sensor->context.data))->gyro_fsr = 500;
-    err = lsm6dso32_write_byte(sensor, CTRL2_G, 0xA1);
-    return_err(err);
-
-    // TODO: what filter configuration will give the best measurements?
-    return err;
-}

From 53e95811416c57ceef41ec8307bf98ae3aa0c5ae Mon Sep 17 00:00:00 2001
From: Matteo Golin <matteo.golin@gmail.com>
Date: Mon, 20 May 2024 18:53:28 -0400
Subject: [PATCH 3/9] Update collector to configure ODR and perform
 conversions.

---
 src/collectors/lsm6dso32_clctr.c | 57 +++++++++++++++++++++++---------
 1 file changed, 42 insertions(+), 15 deletions(-)

diff --git a/src/collectors/lsm6dso32_clctr.c b/src/collectors/lsm6dso32_clctr.c
index a8f7a8c..1e602e5 100644
--- a/src/collectors/lsm6dso32_clctr.c
+++ b/src/collectors/lsm6dso32_clctr.c
@@ -44,6 +44,18 @@ void *lsm6dso32_collector(void *args) {
         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(errno));
+        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(errno));
+        return_err(err);
+    }
+
     uint8_t data[sizeof(vec3d_t) + 1];
     int16_t temperature;
     int16_t x;
@@ -53,27 +65,42 @@ void *lsm6dso32_collector(void *args) {
     for (;;) {
 
         // Read temperature
-        lsm6dso32_get_temp(&loc, &temperature);
-        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));
+        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
-        lsm6dso32_get_accel(&loc, &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));
+        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
-        lsm6dso32_get_angular_vel(&loc, &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));
+        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));
+            }
         }
     }
 }

From f993abbfcb882dd4b936ebab5e572a19d7b91381 Mon Sep 17 00:00:00 2001
From: Matteo Golin <matteo.golin@gmail.com>
Date: Mon, 20 May 2024 19:09:54 -0400
Subject: [PATCH 4/9] Added WHOAMI for sanity checks.

---
 src/collectors/lsm6dso32_clctr.c  |  2 ++
 src/drivers/lsm6dso32/lsm6dso32.c | 10 +++++++++-
 src/drivers/lsm6dso32/lsm6dso32.h |  4 ++++
 3 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/src/collectors/lsm6dso32_clctr.c b/src/collectors/lsm6dso32_clctr.c
index 1e602e5..ca4990d 100644
--- a/src/collectors/lsm6dso32_clctr.c
+++ b/src/collectors/lsm6dso32_clctr.c
@@ -31,6 +31,7 @@ void *lsm6dso32_collector(void *args) {
         fprintf(stderr, "Failed to reset LSM6DSO32: %s\n", strerror(errno));
         return_err(err);
     }
+    usleep(100);
 
     err = lsm6dso32_set_acc_fsr(&loc, LA_FS_32G);
     if (err != EOK) {
@@ -70,6 +71,7 @@ void *lsm6dso32_collector(void *args) {
             fprintf(stderr, "LSM6DSO32 could not read temperature: %s\n", strerror(errno));
         } else {
             data[0] = TAG_TEMPERATURE;
+            printf("===== %f\n", (float)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));
diff --git a/src/drivers/lsm6dso32/lsm6dso32.c b/src/drivers/lsm6dso32/lsm6dso32.c
index edb1ae7..e1a1a36 100644
--- a/src/drivers/lsm6dso32/lsm6dso32.c
+++ b/src/drivers/lsm6dso32/lsm6dso32.c
@@ -129,7 +129,7 @@ static errno_t lsm6dso32_read_byte(SensorLocation const *loc, uint8_t reg, uint8
 int lsm6dso32_get_temp(SensorLocation const *loc, int16_t *temperature) {
     int err = lsm6dso32_read_byte(loc, OUT_TEMP_L, (uint8_t *)(temperature)); // Read low byte
     return_err(err);
-    err = lsm6dso32_read_byte(loc, OUT_TEMP_H, (uint8_t *)(temperature) + 1); // Read high byte
+    err = lsm6dso32_read_byte(loc, OUT_TEMP_H, ((uint8_t *)(temperature) + 1)); // Read high byte
     return_err(err);
     *temperature /= 256.0f + 25.0f; // In degrees Celsius
     return err;
@@ -334,3 +334,11 @@ int lsm6dso32_disable_gyro(SensorLocation const *loc) { return lsm6dso32_set_gyr
  * @return Any error which occurred communicating with the IMU, EOK if successful.
  */
 int lsm6dso32_disable_accel(SensorLocation const *loc) { return lsm6dso32_set_gyro_odr(loc, 0); }
+
+/**
+ * Reads the "who am I" value from the sensor. Should always be equal to 0x6C.
+ * @param loc The location of the sensor on the I2C bus.
+ * @param val The value returned by the WHOAMI command.
+ * @return Any error which occurred communicating with the IMU, EOK if successful.
+ */
+int lsm6dso32_whoami(SensorLocation const *loc, uint8_t *val) { return lsm6dso32_read_byte(loc, WHO_AM_I, val); }
diff --git a/src/drivers/lsm6dso32/lsm6dso32.h b/src/drivers/lsm6dso32/lsm6dso32.h
index 9242b46..815349f 100644
--- a/src/drivers/lsm6dso32/lsm6dso32.h
+++ b/src/drivers/lsm6dso32/lsm6dso32.h
@@ -12,6 +12,9 @@
 #include "../sensor_api.h"
 #include <stdint.h>
 
+/** The value that should be returned from the WHOAMI register. */
+#define WHOAMI_VALUE 0x6C
+
 /** Represents the possible full scale range settings for linear acceleration in Gs (gravitational acceleration). */
 typedef enum {
     LA_FS_4G = 4,   /**< Full scale range of +/- 4Gs */
@@ -70,6 +73,7 @@ int lsm6dso32_set_gyro_odr(SensorLocation const *loc, gyro_odr_e odr);
 int lsm6dso32_get_temp(SensorLocation const *loc, int16_t *temperature);
 int lsm6dso32_get_accel(SensorLocation const *loc, int16_t *x, int16_t *y, int16_t *z);
 int lsm6dso32_get_angular_vel(SensorLocation const *loc, int16_t *x, int16_t *y, int16_t *z);
+int lsm6dso32_whoami(SensorLocation const *loc, uint8_t *val);
 
 void lsm6dso32_convert_accel(accel_fsr_e acc_fsr, int16_t *x, int16_t *y, int16_t *z);
 void lsm6dso32_convert_angular_vel(gyro_fsr_e gyro_fsr, int16_t *x, int16_t *y, int16_t *z);

From 9887aee772c4061e54730b706478915e6ff82cb0 Mon Sep 17 00:00:00 2001
From: Matteo Golin <matteo.golin@gmail.com>
Date: Mon, 20 May 2024 19:41:40 -0400
Subject: [PATCH 5/9] Made OSR and FSR setting clearer.

---
 src/drivers/lsm6dso32/lsm6dso32.c |  9 ++++-----
 src/drivers/lsm6dso32/lsm6dso32.h | 22 +++++++++++-----------
 2 files changed, 15 insertions(+), 16 deletions(-)

diff --git a/src/drivers/lsm6dso32/lsm6dso32.c b/src/drivers/lsm6dso32/lsm6dso32.c
index e1a1a36..3e8dfd5 100644
--- a/src/drivers/lsm6dso32/lsm6dso32.c
+++ b/src/drivers/lsm6dso32/lsm6dso32.c
@@ -14,7 +14,6 @@
 #include <hw/i2c.h>
 #include <stdbool.h>
 #include <stdint.h>
-#include <stdio.h>
 #include <string.h>
 #include <time.h>
 #include <unistd.h>
@@ -239,13 +238,13 @@ int lsm6dso32_set_acc_fsr(SensorLocation const *loc, accel_fsr_e fsr) {
         // All zeroes
         break;
     case LA_FS_8G:
-        reg_val |= (0x2 << 3);
+        reg_val |= (1 << 3);
         break;
     case LA_FS_16G:
-        reg_val |= (0x3 << 3);
+        reg_val |= ((1 << 3) | (1 << 2));
         break;
     case LA_FS_32G:
-        reg_val |= (0x1 << 3);
+        reg_val |= (1 << 2);
         break;
     default:
         return EINVAL;
@@ -263,7 +262,7 @@ int lsm6dso32_set_gyro_fsr(SensorLocation const *loc, gyro_fsr_e fsr) {
 
     uint8_t reg_val;
     int err = lsm6dso32_read_byte(loc, CTRL2_G, &reg_val); // Don't overwrite other configurations
-    reg_val &= ~((1 << 3) | (1 << 2) | (1 << 1));          // Clear FSR selection bits
+    reg_val &= ~(0x0F);                                    // Clear FSR selection bits
     return_err(err);
 
     switch (fsr) {
diff --git a/src/drivers/lsm6dso32/lsm6dso32.h b/src/drivers/lsm6dso32/lsm6dso32.h
index 815349f..a1965bd 100644
--- a/src/drivers/lsm6dso32/lsm6dso32.h
+++ b/src/drivers/lsm6dso32/lsm6dso32.h
@@ -34,7 +34,7 @@ typedef enum {
 
 /** Possible output data rates for linear acceleration in Hz. */
 typedef enum {
-    LA_ODR_1_6 = 0xD0,  /** 1.6 Hz */
+    LA_ODR_1_6 = 0xB0,  /** 1.6 Hz */
     LA_ODR_12_5 = 0x10, /** 12.5 Hz */
     LA_ODR_26 = 0x20,   /** 26 Hz */
     LA_ODR_52 = 0x30,   /** 52 Hz */
@@ -49,16 +49,16 @@ typedef enum {
 
 /** Possible output data rates for the gyroscope in Hz. */
 typedef enum {
-    G_ODR_12_5, /** 12.5 Hz */
-    G_ODR_26,   /** 26 Hz */
-    G_ODR_52,   /** 52 Hz */
-    G_ODR_104,  /** 104 Hz */
-    G_ODR_208,  /** 208 Hz */
-    G_ODR_416,  /** 416 Hz */
-    G_ODR_833,  /** 833 Hz */
-    G_ODR_1666, /** 1666 Hz */
-    G_ODR_3332, /** 3332 Hz */
-    G_ODR_6664, /** 6644 Hz */
+    G_ODR_12_5 = 0x10, /** 12.5 Hz */
+    G_ODR_26 = 0x20,   /** 26 Hz */
+    G_ODR_52 = 0x30,   /** 52 Hz */
+    G_ODR_104 = 0x40,  /** 104 Hz */
+    G_ODR_208 = 0x50,  /** 208 Hz */
+    G_ODR_416 = 0x60,  /** 416 Hz */
+    G_ODR_833 = 0x70,  /** 833 Hz */
+    G_ODR_1666 = 0x80, /** 1666 Hz */
+    G_ODR_3332 = 0x90, /** 3332 Hz */
+    G_ODR_6664 = 0xA0, /** 6644 Hz */
 } gyro_odr_e;
 
 int lsm6dso32_reset(SensorLocation const *loc);

From 2e6f63fbfb7c1a668b91b2cf46701fb145112ae4 Mon Sep 17 00:00:00 2001
From: Matteo Golin <matteo.golin@gmail.com>
Date: Mon, 20 May 2024 19:42:35 -0400
Subject: [PATCH 6/9] Fixed temperature calculation.

---
 src/collectors/lsm6dso32_clctr.c  | 1 -
 src/drivers/lsm6dso32/lsm6dso32.c | 2 +-
 2 files changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/collectors/lsm6dso32_clctr.c b/src/collectors/lsm6dso32_clctr.c
index ca4990d..86ac32f 100644
--- a/src/collectors/lsm6dso32_clctr.c
+++ b/src/collectors/lsm6dso32_clctr.c
@@ -71,7 +71,6 @@ void *lsm6dso32_collector(void *args) {
             fprintf(stderr, "LSM6DSO32 could not read temperature: %s\n", strerror(errno));
         } else {
             data[0] = TAG_TEMPERATURE;
-            printf("===== %f\n", (float)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));
diff --git a/src/drivers/lsm6dso32/lsm6dso32.c b/src/drivers/lsm6dso32/lsm6dso32.c
index 3e8dfd5..da6bfac 100644
--- a/src/drivers/lsm6dso32/lsm6dso32.c
+++ b/src/drivers/lsm6dso32/lsm6dso32.c
@@ -130,7 +130,7 @@ int lsm6dso32_get_temp(SensorLocation const *loc, int16_t *temperature) {
     return_err(err);
     err = lsm6dso32_read_byte(loc, OUT_TEMP_H, ((uint8_t *)(temperature) + 1)); // Read high byte
     return_err(err);
-    *temperature /= 256.0f + 25.0f; // In degrees Celsius
+    *temperature = (*temperature / 256.0f) + 25.0f; // In degrees Celsius
     return err;
 }
 

From 59aeaf43ea2cc295832303b241c008f2b6ddf1cb Mon Sep 17 00:00:00 2001
From: Matteo Golin <matteo.golin@gmail.com>
Date: Mon, 20 May 2024 22:07:53 -0400
Subject: [PATCH 7/9] Fixed conversion of data types according to typical
 specifications from the datasheet.

---
 src/drivers/lsm6dso32/lsm6dso32.c | 70 ++++++++++++++++++++-----------
 1 file changed, 46 insertions(+), 24 deletions(-)

diff --git a/src/drivers/lsm6dso32/lsm6dso32.c b/src/drivers/lsm6dso32/lsm6dso32.c
index da6bfac..7bf3a00 100644
--- a/src/drivers/lsm6dso32/lsm6dso32.c
+++ b/src/drivers/lsm6dso32/lsm6dso32.c
@@ -18,21 +18,8 @@
 #include <time.h>
 #include <unistd.h>
 
-/** Acceleration due to gravity in m/s^2. */
-#define GRAVIT_ACC 9.81f
-
-/**
- * All units being converted are being stored in milli-units.
- *
- * Ex:
- * Milli-Gs per least significant bit to Gs conversion rate using 32g full scale range.
- * 32g FSR * 2 = 64g range * 1000 = 64000mg range
- * Result is given in 16b integer, so 65535 possible different values for 64000mg
- * Conversion factor = 64000/65535 (mg per bit)
- * See https://ozzmaker.com/accelerometer-to-g/
- * We will return units in regular units instead of milli-units, so the multiplication of 1000 is removed.
- */
-#define MILLI_UNIT_PER_LSB_TO_UNIT 2.0f / 65535.0f
+/** The acceleration of 1 G in meters per second squared. */
+#define GRAVIT_ACC 9.81
 
 /** The different registers that are present in the IMU (and used by this program). */
 enum imu_reg {
@@ -192,14 +179,30 @@ int lsm6dso32_get_angular_vel(SensorLocation const *loc, int16_t *x, int16_t *y,
  * @param z A pointer to the Z component of the acceleration in milli-Gs per LSB.
  */
 void lsm6dso32_convert_accel(accel_fsr_e acc_fsr, int16_t *x, int16_t *y, int16_t *z) {
-    float conversion_factor = acc_fsr * MILLI_UNIT_PER_LSB_TO_UNIT * GRAVIT_ACC;
-    if (x) (*x) *= conversion_factor;
-    if (y) (*y) *= conversion_factor;
-    if (z) (*z) *= conversion_factor;
+
+    int16_t conversion_factor;
+    switch (acc_fsr) {
+    case LA_FS_4G:
+        conversion_factor = 8197;
+        break;
+    case LA_FS_8G:
+        conversion_factor = 4098;
+        break;
+    case LA_FS_16G:
+        conversion_factor = 2049;
+        break;
+    case LA_FS_32G:
+        conversion_factor = 1025;
+        break;
+    }
+
+    if (x) (*x) = ((*x) / conversion_factor) * (int16_t)GRAVIT_ACC;
+    if (y) (*y) = ((*y) / conversion_factor) * (int16_t)GRAVIT_ACC;
+    if (z) (*z) = ((*z) / conversion_factor) * (int16_t)GRAVIT_ACC;
 }
 
 /**
- * Converts angular velocity in millidegrees per LSB to meters per second squared. Results are stored back in the
+ * Converts angular velocity in millidegrees per LSB to degrees per second. Results are stored back in the
  * pointers themselves. Passing NULL as any of the pointers will result in the calculation being skipped.
  * @param gyro_fsr The full scale range of the gyroscope.
  * @param x A pointer to the X component of the angular velocity in millidegrees per LSB.
@@ -207,10 +210,29 @@ void lsm6dso32_convert_accel(accel_fsr_e acc_fsr, int16_t *x, int16_t *y, int16_
  * @param z A pointer to the Z component of the angular velocity in millidegrees per LSB.
  */
 void lsm6dso32_convert_angular_vel(gyro_fsr_e gyro_fsr, int16_t *x, int16_t *y, int16_t *z) {
-    float conversion_factor = gyro_fsr * MILLI_UNIT_PER_LSB_TO_UNIT;
-    if (x) (*x) *= conversion_factor;
-    if (y) (*y) *= conversion_factor;
-    if (z) (*z) *= conversion_factor;
+
+    int16_t conversion_factor;
+    switch (gyro_fsr) {
+    case G_FS_125:
+        conversion_factor = 229;
+        break;
+    case G_FS_250:
+        conversion_factor = 114;
+        break;
+    case G_FS_500:
+        conversion_factor = 57;
+        break;
+    case G_FS_1000:
+        conversion_factor = 29;
+        break;
+    case G_FS_2000:
+        conversion_factor = 14;
+        break;
+    }
+
+    if (x) (*x) /= conversion_factor;
+    if (y) (*y) /= conversion_factor;
+    if (z) (*z) /= conversion_factor;
 }
 
 /**

From 07c7f27872bc7b4b9e17c2c458da23323afbfb5f Mon Sep 17 00:00:00 2001
From: Matteo Golin <matteo.golin@gmail.com>
Date: Mon, 20 May 2024 22:22:21 -0400
Subject: [PATCH 8/9] Add a memory reboot.

---
 src/collectors/lsm6dso32_clctr.c  | 17 ++++++++++++-----
 src/drivers/lsm6dso32/lsm6dso32.c |  7 +++++++
 src/drivers/lsm6dso32/lsm6dso32.h |  1 +
 3 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/src/collectors/lsm6dso32_clctr.c b/src/collectors/lsm6dso32_clctr.c
index 86ac32f..4201bec 100644
--- a/src/collectors/lsm6dso32_clctr.c
+++ b/src/collectors/lsm6dso32_clctr.c
@@ -28,32 +28,39 @@ void *lsm6dso32_collector(void *args) {
     int err;
     err = lsm6dso32_reset(&loc);
     if (err != EOK) {
-        fprintf(stderr, "Failed to reset LSM6DSO32: %s\n", strerror(errno));
+        fprintf(stderr, "Failed to reset LSM6DSO32: %s\n", strerror(err));
         return_err(err);
     }
+
+    err = lsm6dso32_mem_reboot(&loc);
+    if (err != EOK) {
+        fprintf(stderr, "Failed to reboot LSM6DSO32 memory content: %s\n", strerror(err));
+        return_err(err);
+    }
+
     usleep(100);
 
     err = lsm6dso32_set_acc_fsr(&loc, LA_FS_32G);
     if (err != EOK) {
-        fprintf(stderr, "Failed to set LSM6DSO32 accelerometer FSR: %s\n", strerror(errno));
+        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(errno));
+        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(errno));
+        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(errno));
+        fprintf(stderr, "Failed to set LSM6DSO32 gyroscope ODR: %s\n", strerror(err));
         return_err(err);
     }
 
diff --git a/src/drivers/lsm6dso32/lsm6dso32.c b/src/drivers/lsm6dso32/lsm6dso32.c
index 7bf3a00..39aa99d 100644
--- a/src/drivers/lsm6dso32/lsm6dso32.c
+++ b/src/drivers/lsm6dso32/lsm6dso32.c
@@ -242,6 +242,13 @@ void lsm6dso32_convert_angular_vel(gyro_fsr_e gyro_fsr, int16_t *x, int16_t *y,
  */
 int lsm6dso32_reset(SensorLocation const *loc) { return lsm6dso32_write_byte(loc, CTRL3_C, 0x01); }
 
+/**
+ * Reboots the memory content of the LSM6DSO32.
+ * @param loc The location of the IMU on the I2C bus.
+ * @return Any error which occurred while rebooting the IMU, EOK if successful.
+ */
+int lsm6dso32_mem_reboot(SensorLocation const *loc) { return lsm6dso32_write_byte(loc, CTRL3_C, 0x80); }
+
 /**
  * Sets the accelerometer full scale range.
  * @param loc The location of the IMU on the I2C bus.
diff --git a/src/drivers/lsm6dso32/lsm6dso32.h b/src/drivers/lsm6dso32/lsm6dso32.h
index a1965bd..9286fbc 100644
--- a/src/drivers/lsm6dso32/lsm6dso32.h
+++ b/src/drivers/lsm6dso32/lsm6dso32.h
@@ -62,6 +62,7 @@ typedef enum {
 } gyro_odr_e;
 
 int lsm6dso32_reset(SensorLocation const *loc);
+int lsm6dso32_mem_reboot(SensorLocation const *loc);
 int lsm6dso32_disable_accel(SensorLocation const *loc);
 int lsm6dso32_disable_gyro(SensorLocation const *loc);
 

From 5064afe905e816835d3268aae97e849dddb77da7 Mon Sep 17 00:00:00 2001
From: Matteo Golin <matteo.golin@gmail.com>
Date: Mon, 20 May 2024 22:29:18 -0400
Subject: [PATCH 9/9] Added high performance mode toggle.

---
 src/collectors/lsm6dso32_clctr.c  |  6 ++++++
 src/drivers/lsm6dso32/lsm6dso32.c | 23 +++++++++++++++++++++--
 src/drivers/lsm6dso32/lsm6dso32.h |  2 ++
 3 files changed, 29 insertions(+), 2 deletions(-)

diff --git a/src/collectors/lsm6dso32_clctr.c b/src/collectors/lsm6dso32_clctr.c
index 4201bec..0a4b622 100644
--- a/src/collectors/lsm6dso32_clctr.c
+++ b/src/collectors/lsm6dso32_clctr.c
@@ -40,6 +40,12 @@ void *lsm6dso32_collector(void *args) {
 
     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));
diff --git a/src/drivers/lsm6dso32/lsm6dso32.c b/src/drivers/lsm6dso32/lsm6dso32.c
index 39aa99d..6f08d6f 100644
--- a/src/drivers/lsm6dso32/lsm6dso32.c
+++ b/src/drivers/lsm6dso32/lsm6dso32.c
@@ -12,8 +12,6 @@
 #include "sensor_api.h"
 #include <errno.h>
 #include <hw/i2c.h>
-#include <stdbool.h>
-#include <stdint.h>
 #include <string.h>
 #include <time.h>
 #include <unistd.h>
@@ -349,6 +347,27 @@ int lsm6dso32_set_gyro_odr(SensorLocation const *loc, gyro_odr_e odr) {
     return lsm6dso32_write_byte(loc, CTRL2_G, reg_val);
 }
 
+/**
+ * Allows high performance operating mode to be enabled/disabled.
+ * @param loc The location of the IMU on the I2C bus.
+ * @param on True to turn on high performance, false to turn it off.
+ * @return Any error which occurred communicating with the IMU, EOK if successful.
+ */
+int lsm6dso32_high_performance(SensorLocation const *loc, bool on) {
+
+    uint8_t reg_val;
+    int err = lsm6dso32_read_byte(loc, CTRL6_C, &reg_val);
+    return_err(err);
+
+    if (on) {
+        reg_val |= 0x10;
+    } else {
+        reg_val &= ~0x10;
+    }
+
+    return lsm6dso32_write_byte(loc, CTRL6_C, reg_val);
+}
+
 /**
  * Powers down the gyroscope.
  * @param loc The location of the IMU on the I2C bus.
diff --git a/src/drivers/lsm6dso32/lsm6dso32.h b/src/drivers/lsm6dso32/lsm6dso32.h
index 9286fbc..b93325b 100644
--- a/src/drivers/lsm6dso32/lsm6dso32.h
+++ b/src/drivers/lsm6dso32/lsm6dso32.h
@@ -10,6 +10,7 @@
 #define _LSM6DSO32_
 
 #include "../sensor_api.h"
+#include <stdbool.h>
 #include <stdint.h>
 
 /** The value that should be returned from the WHOAMI register. */
@@ -70,6 +71,7 @@ int lsm6dso32_set_acc_fsr(SensorLocation const *loc, accel_fsr_e fsr);
 int lsm6dso32_set_gyro_fsr(SensorLocation const *loc, gyro_fsr_e fsr);
 int lsm6dso32_set_acc_odr(SensorLocation const *loc, accel_odr_e odr);
 int lsm6dso32_set_gyro_odr(SensorLocation const *loc, gyro_odr_e odr);
+int lsm6dso32_high_performance(SensorLocation const *loc, bool on);
 
 int lsm6dso32_get_temp(SensorLocation const *loc, int16_t *temperature);
 int lsm6dso32_get_accel(SensorLocation const *loc, int16_t *x, int16_t *y, int16_t *z);