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