From 0192f7e481d39cfc704f92cf40a04e9fcd5c5ec4 Mon Sep 17 00:00:00 2001
From: Fabrizio Monti <fabrizio.monti@skywarder.eu>
Date: Mon, 26 Jun 2023 10:19:36 +0200
Subject: [PATCH] [LSM6DSRX] Modified samples units of measurement to meters
 per second squared and radians per second.

---
 src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp | 21 +++++++++++++++++++++
 src/shared/sensors/LSM6DSRX/LSM6DSRX.h   |  9 +++++++++
 2 files changed, 30 insertions(+)

diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp b/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
index 153fbc3d2..58a67b1e3 100644
--- a/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
+++ b/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
@@ -24,6 +24,7 @@
 
 #include <assert.h>
 #include <drivers/timer/TimestampTimer.h>
+#include <utils/Constants.h>
 #include <utils/Debug.h>
 
 #include <cmath>
@@ -598,6 +599,8 @@ LSM6DSRXData LSM6DSRX::getSensorData()
     getAccelerometerData(data);
     getGyroscopeData(data);
 
+    convertSampleMeasurementUnit(data);
+
     return data;
 }
 
@@ -844,6 +847,8 @@ void LSM6DSRX::pushIntoFifo(LSM6DSRXDefs::FifoTimeslotData& timeslot,
         return;
     }
 
+    convertSampleMeasurementUnit(timeslot.data);
+
     // push into fifo and update index
     lastFifo[fifoIdx] = timeslot.data;
     ++fifoIdx;
@@ -864,4 +869,20 @@ uint16_t LSM6DSRX::unreadDataInFifo()
     return ris;
 }
 
+void LSM6DSRX::convertSampleMeasurementUnit(LSM6DSRXData& sample)
+{
+    // convert accelerometer data from milli-g to meters (per second squared)
+    constexpr float accelerationConversion = Constants::g / 1000.0;
+    sample.accelerationX *= accelerationConversion;
+    sample.accelerationY *= accelerationConversion;
+    sample.accelerationZ *= accelerationConversion;
+
+    // converts gyroscope data from milli-degree per second to radiants per
+    // second
+    constexpr float angularConversion = Constants::DEGREES_TO_RADIANS / 1000.0;
+    sample.angularSpeedX *= angularConversion;
+    sample.angularSpeedY *= angularConversion;
+    sample.angularSpeedZ *= angularConversion;
+}
+
 }  // namespace Boardcore
diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRX.h b/src/shared/sensors/LSM6DSRX/LSM6DSRX.h
index a13b6802c..6cdd1edab 100644
--- a/src/shared/sensors/LSM6DSRX/LSM6DSRX.h
+++ b/src/shared/sensors/LSM6DSRX/LSM6DSRX.h
@@ -237,6 +237,15 @@ private:
      * @brief Returns the timestamp resolution in milliseconds.
      */
     float getSensorTimestampResolution();
+
+    /**
+     * @brief Converts the sample from the sensor's unit of measurement (milli-g
+     * for accelerometer data, milli-degree per second for gyroscope data) to
+     * our units (meters per second squared and radiants per second).
+     *
+     * @param sample The sample to be converted.
+     */
+    void convertSampleMeasurementUnit(LSM6DSRXData& sample);
 };
 
 }  // namespace Boardcore
-- 
GitLab