diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp b/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp index 58a67b1e31fade041ae6925448eac1ebc3a5063c..b0e22f15b7bda173018b58efa0f18cf39f3398f0 100644 --- a/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp +++ b/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp @@ -152,6 +152,11 @@ void LSM6DSRX::initAccelerometer() sensitivityAcc = 0.061; break; }; + + // the sensor's unit of measurement is milli-g, we need to convert + // to meters per second squared + constexpr float accelerationConversion = Constants::g / 1000.0; + sensitivityAcc *= accelerationConversion; } void LSM6DSRX::initGyroscope() @@ -194,6 +199,11 @@ void LSM6DSRX::initGyroscope() sensitivityGyr = 4.375; break; } + + // the sensor's unit of measurement is milli-degree per second, we need + // to convert to radians per second + constexpr float angularConversion = Constants::DEGREES_TO_RADIANS / 1000.0; + sensitivityGyr *= angularConversion; } void LSM6DSRX::initFifo() @@ -599,8 +609,6 @@ LSM6DSRXData LSM6DSRX::getSensorData() getAccelerometerData(data); getGyroscopeData(data); - convertSampleMeasurementUnit(data); - return data; } @@ -847,8 +855,6 @@ void LSM6DSRX::pushIntoFifo(LSM6DSRXDefs::FifoTimeslotData& timeslot, return; } - convertSampleMeasurementUnit(timeslot.data); - // push into fifo and update index lastFifo[fifoIdx] = timeslot.data; ++fifoIdx; @@ -869,20 +875,4 @@ 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 6cdd1edabfc1d0e4dfa7c39d1fc0a6b9063348c1..a13b6802c92dcf8bc838dcbf84cebe8dc4121ce3 100644 --- a/src/shared/sensors/LSM6DSRX/LSM6DSRX.h +++ b/src/shared/sensors/LSM6DSRX/LSM6DSRX.h @@ -237,15 +237,6 @@ 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