Added high performance mode toggle.

CarletonURocketry · May 21, 2024 · 5064afe · 5064afe
1 parent 07c7f27
commit 5064afe
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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);