From cbce84309840b4ae92130a896032562e29bb1016 Mon Sep 17 00:00:00 2001
From: Fabrizio Monti <fabrizio.monti@skywarder.eu>
Date: Mon, 26 Jun 2023 12:16:26 +0200
Subject: [PATCH] [LSM6DSRX] Improved units conversion by reducing the number
of multiplications needed.
---
src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp | 30 ++++++++----------------
src/shared/sensors/LSM6DSRX/LSM6DSRX.h | 9 -------
2 files changed, 10 insertions(+), 29 deletions(-)
diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp b/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
index 58a67b1e3..b0e22f15b 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 6cdd1edab..a13b6802c 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
--
GitLab