[LSM6DSRX] Modified samples units of measurement to meters per second squared...

[LSM6DSRX] Modified samples units of measurement to meters per second squared and radians per second.

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

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