diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp b/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
index aa7d51c9b6763b28696728c23019a22dd5fd56eb..9beacbe95f6d58cde5dbe06c80a8c86b915932df 100644
--- a/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
+++ b/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
@@ -309,9 +309,9 @@ bool LSM6DSRX::selfTestAcc()
}
// read and discard data
- getAxisData(LSM6DSRXDefs::REG_OUTX_L_A, LSM6DSRXDefs::REG_OUTX_H_A);
- getAxisData(LSM6DSRXDefs::REG_OUTY_L_A, LSM6DSRXDefs::REG_OUTY_H_A);
- getAxisData(LSM6DSRXDefs::REG_OUTZ_L_A, LSM6DSRXDefs::REG_OUTZ_H_A);
+ readFloat16(LSM6DSRXDefs::REG_OUTX_L_A, LSM6DSRXDefs::REG_OUTX_H_A);
+ readFloat16(LSM6DSRXDefs::REG_OUTY_L_A, LSM6DSRXDefs::REG_OUTY_H_A);
+ readFloat16(LSM6DSRXDefs::REG_OUTZ_L_A, LSM6DSRXDefs::REG_OUTZ_H_A);
// read normal data (self test disabled)
for (idx = 0; idx < SIZE_DATA; ++idx)
@@ -327,12 +327,15 @@ bool LSM6DSRX::selfTestAcc()
}
// read data
- averageNormal.accelerationX += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTX_L_A, LSM6DSRXDefs::REG_OUTX_H_A, 0.061));
- averageNormal.accelerationY += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTY_L_A, LSM6DSRXDefs::REG_OUTY_H_A, 0.061));
- averageNormal.accelerationZ += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTZ_L_A, LSM6DSRXDefs::REG_OUTZ_H_A, 0.061));
+ averageNormal.accelerationX += readFloat16(LSM6DSRXDefs::REG_OUTX_L_A,
+ LSM6DSRXDefs::REG_OUTX_H_A) *
+ 0.061;
+ averageNormal.accelerationY += readFloat16(LSM6DSRXDefs::REG_OUTY_L_A,
+ LSM6DSRXDefs::REG_OUTY_H_A) *
+ 0.061;
+ averageNormal.accelerationZ += readFloat16(LSM6DSRXDefs::REG_OUTZ_L_A,
+ LSM6DSRXDefs::REG_OUTZ_H_A) *
+ 0.061;
}
averageNormal.accelerationX /= SIZE_DATA;
averageNormal.accelerationY /= SIZE_DATA;
@@ -355,9 +358,9 @@ bool LSM6DSRX::selfTestAcc()
}
// read and discard data
- getAxisData(LSM6DSRXDefs::REG_OUTX_L_A, LSM6DSRXDefs::REG_OUTX_H_A);
- getAxisData(LSM6DSRXDefs::REG_OUTY_L_A, LSM6DSRXDefs::REG_OUTY_H_A);
- getAxisData(LSM6DSRXDefs::REG_OUTZ_L_A, LSM6DSRXDefs::REG_OUTZ_H_A);
+ readFloat16(LSM6DSRXDefs::REG_OUTX_L_A, LSM6DSRXDefs::REG_OUTX_H_A);
+ readFloat16(LSM6DSRXDefs::REG_OUTY_L_A, LSM6DSRXDefs::REG_OUTY_H_A);
+ readFloat16(LSM6DSRXDefs::REG_OUTZ_L_A, LSM6DSRXDefs::REG_OUTZ_H_A);
// read self test data
for (idx = 0; idx < SIZE_DATA; ++idx)
@@ -373,12 +376,15 @@ bool LSM6DSRX::selfTestAcc()
}
// read data
- averageSF.accelerationX += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTX_L_A, LSM6DSRXDefs::REG_OUTX_H_A, 0.061));
- averageSF.accelerationY += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTY_L_A, LSM6DSRXDefs::REG_OUTY_H_A, 0.061));
- averageSF.accelerationZ += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTZ_L_A, LSM6DSRXDefs::REG_OUTZ_H_A, 0.061));
+ averageSF.accelerationX += readFloat16(LSM6DSRXDefs::REG_OUTX_L_A,
+ LSM6DSRXDefs::REG_OUTX_H_A) *
+ 0.061;
+ averageSF.accelerationY += readFloat16(LSM6DSRXDefs::REG_OUTY_L_A,
+ LSM6DSRXDefs::REG_OUTY_H_A) *
+ 0.061;
+ averageSF.accelerationZ += readFloat16(LSM6DSRXDefs::REG_OUTZ_L_A,
+ LSM6DSRXDefs::REG_OUTZ_H_A) *
+ 0.061;
}
averageSF.accelerationX /= SIZE_DATA;
averageSF.accelerationY /= SIZE_DATA;
@@ -478,9 +484,9 @@ bool LSM6DSRX::selfTestGyr()
byteValue = (byteValue & 0x02) >> 1;
}
// read and discard data
- getAxisData(LSM6DSRXDefs::REG_OUTX_L_G, LSM6DSRXDefs::REG_OUTX_H_G);
- getAxisData(LSM6DSRXDefs::REG_OUTY_L_G, LSM6DSRXDefs::REG_OUTY_H_G);
- getAxisData(LSM6DSRXDefs::REG_OUTZ_L_G, LSM6DSRXDefs::REG_OUTZ_H_G);
+ readFloat16(LSM6DSRXDefs::REG_OUTX_L_G, LSM6DSRXDefs::REG_OUTX_H_G);
+ readFloat16(LSM6DSRXDefs::REG_OUTY_L_G, LSM6DSRXDefs::REG_OUTY_H_G);
+ readFloat16(LSM6DSRXDefs::REG_OUTZ_L_G, LSM6DSRXDefs::REG_OUTZ_H_G);
// read normal data (self test disabled)
for (idx = 0; idx < SIZE_DATA; ++idx)
@@ -496,12 +502,15 @@ bool LSM6DSRX::selfTestGyr()
}
// read data
- averageNormal.angularSpeedX += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTX_L_G, LSM6DSRXDefs::REG_OUTX_H_G, 0.070));
- averageNormal.angularSpeedY += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTY_L_G, LSM6DSRXDefs::REG_OUTY_H_G, 0.070));
- averageNormal.angularSpeedZ += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTZ_L_G, LSM6DSRXDefs::REG_OUTZ_H_G, 0.070));
+ averageNormal.angularSpeedX += readFloat16(LSM6DSRXDefs::REG_OUTX_L_G,
+ LSM6DSRXDefs::REG_OUTX_H_G) *
+ 0.070;
+ averageNormal.angularSpeedY += readFloat16(LSM6DSRXDefs::REG_OUTY_L_G,
+ LSM6DSRXDefs::REG_OUTY_H_G) *
+ 0.070;
+ averageNormal.angularSpeedZ += readFloat16(LSM6DSRXDefs::REG_OUTZ_L_G,
+ LSM6DSRXDefs::REG_OUTZ_H_G) *
+ 0.070;
}
averageNormal.angularSpeedX /= SIZE_DATA;
averageNormal.angularSpeedY /= SIZE_DATA;
@@ -523,9 +532,9 @@ bool LSM6DSRX::selfTestGyr()
byteValue = (byteValue & 0x02) >> 1;
}
// read and discard data
- getAxisData(LSM6DSRXDefs::REG_OUTX_L_G, LSM6DSRXDefs::REG_OUTX_H_G);
- getAxisData(LSM6DSRXDefs::REG_OUTY_L_G, LSM6DSRXDefs::REG_OUTY_H_G);
- getAxisData(LSM6DSRXDefs::REG_OUTZ_L_G, LSM6DSRXDefs::REG_OUTZ_H_G);
+ readFloat16(LSM6DSRXDefs::REG_OUTX_L_G, LSM6DSRXDefs::REG_OUTX_H_G);
+ readFloat16(LSM6DSRXDefs::REG_OUTY_L_G, LSM6DSRXDefs::REG_OUTY_H_G);
+ readFloat16(LSM6DSRXDefs::REG_OUTZ_L_G, LSM6DSRXDefs::REG_OUTZ_H_G);
// read self test data
for (idx = 0; idx < SIZE_DATA; ++idx)
@@ -541,12 +550,15 @@ bool LSM6DSRX::selfTestGyr()
}
// read data
- averageSF.angularSpeedX += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTX_L_G, LSM6DSRXDefs::REG_OUTX_H_G, 0.070));
- averageSF.angularSpeedY += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTY_L_G, LSM6DSRXDefs::REG_OUTY_H_G, 0.070));
- averageSF.angularSpeedZ += static_cast<float>(getAxisData(
- LSM6DSRXDefs::REG_OUTZ_L_G, LSM6DSRXDefs::REG_OUTZ_H_G, 0.070));
+ averageSF.angularSpeedX += readFloat16(LSM6DSRXDefs::REG_OUTX_L_G,
+ LSM6DSRXDefs::REG_OUTX_H_G) *
+ 0.070;
+ averageSF.angularSpeedY += readFloat16(LSM6DSRXDefs::REG_OUTY_L_G,
+ LSM6DSRXDefs::REG_OUTY_H_G) *
+ 0.070;
+ averageSF.angularSpeedZ += readFloat16(LSM6DSRXDefs::REG_OUTZ_L_G,
+ LSM6DSRXDefs::REG_OUTZ_H_G) *
+ 0.070;
}
averageSF.angularSpeedX /= SIZE_DATA;
averageSF.angularSpeedY /= SIZE_DATA;
@@ -610,26 +622,36 @@ uint16_t LSM6DSRX::combineHighLowBitsUnsigned(uint8_t low, uint8_t high)
void LSM6DSRX::getAccelerometerData(LSM6DSRXData& data)
{
- data.accelerationTimestamp = TimestampTimer::getTimestamp();
-
- data.accelerationX = getAxisData(
- LSM6DSRXDefs::REG_OUTX_L_A, LSM6DSRXDefs::REG_OUTX_H_A, sensitivityAcc);
- data.accelerationY = getAxisData(
- LSM6DSRXDefs::REG_OUTY_L_A, LSM6DSRXDefs::REG_OUTY_H_A, sensitivityAcc);
- data.accelerationZ = getAxisData(
- LSM6DSRXDefs::REG_OUTZ_L_A, LSM6DSRXDefs::REG_OUTZ_H_A, sensitivityAcc);
+ data.accelerationX =
+ readFloat16(LSM6DSRXDefs::REG_OUTX_L_A, LSM6DSRXDefs::REG_OUTX_H_A) *
+ sensitivityAcc;
+ data.accelerationY =
+ readFloat16(LSM6DSRXDefs::REG_OUTY_L_A, LSM6DSRXDefs::REG_OUTY_H_A) *
+ sensitivityAcc;
+ data.accelerationZ =
+ readFloat16(LSM6DSRXDefs::REG_OUTZ_L_A, LSM6DSRXDefs::REG_OUTZ_H_A) *
+ sensitivityAcc;
}
void LSM6DSRX::getGyroscopeData(LSM6DSRXData& data)
{
- data.angularSpeedTimestamp = TimestampTimer::getTimestamp();
-
- data.angularSpeedX = getAxisData(
- LSM6DSRXDefs::REG_OUTX_L_G, LSM6DSRXDefs::REG_OUTX_H_G, sensitivityGyr);
- data.angularSpeedY = getAxisData(
- LSM6DSRXDefs::REG_OUTY_L_G, LSM6DSRXDefs::REG_OUTY_H_G, sensitivityGyr);
- data.angularSpeedZ = getAxisData(
- LSM6DSRXDefs::REG_OUTZ_L_G, LSM6DSRXDefs::REG_OUTZ_H_G, sensitivityGyr);
+ data.angularSpeedX =
+ readFloat16(LSM6DSRXDefs::REG_OUTX_L_G, LSM6DSRXDefs::REG_OUTX_H_G) *
+ sensitivityGyr;
+ data.angularSpeedY =
+ readFloat16(LSM6DSRXDefs::REG_OUTY_L_G, LSM6DSRXDefs::REG_OUTY_H_G) *
+ sensitivityGyr;
+ data.angularSpeedZ =
+ readFloat16(LSM6DSRXDefs::REG_OUTZ_L_G, LSM6DSRXDefs::REG_OUTZ_H_G) *
+ sensitivityGyr;
+}
+
+void LSM6DSRX::getTemperatureData(LSM6DSRXData& data)
+{
+ data.temperature = readFloat16(LSM6DSRXDefs::REG_OUT_TEMP_L,
+ LSM6DSRXDefs::REG_OUT_TEMP_H) /
+ 256;
+ data.temperature += 25.0;
}
uint32_t LSM6DSRX::getSensorTimestamp()
@@ -650,8 +672,13 @@ LSM6DSRXData LSM6DSRX::getSensorData()
LSM6DSRXData data;
+ data.accelerationTimestamp = TimestampTimer::getTimestamp();
+ data.angularSpeedTimestamp = data.accelerationTimestamp;
+ data.temperatureTimestamp = data.accelerationTimestamp;
+
getAccelerometerData(data);
getGyroscopeData(data);
+ getTemperatureData(data);
return data;
}
@@ -694,8 +721,8 @@ LSM6DSRXData LSM6DSRX::sampleImpl()
}
}
-float LSM6DSRX::getAxisData(LSM6DSRXDefs::Registers lowReg,
- LSM6DSRXDefs::Registers highReg, float sensitivity)
+float LSM6DSRX::readFloat16(LSM6DSRXDefs::Registers lowReg,
+ LSM6DSRXDefs::Registers highReg)
{
int8_t low = 0, high = 0;
int16_t sample = 0;
@@ -707,12 +734,12 @@ float LSM6DSRX::getAxisData(LSM6DSRXDefs::Registers lowReg,
sample = combineHighLowBits(low, high);
- float ret = static_cast<float>(sample) * sensitivity;
+ float ret = static_cast<float>(sample);
return ret;
}
-int16_t LSM6DSRX::getAxisData(LSM6DSRXDefs::Registers lowReg,
- LSM6DSRXDefs::Registers highReg)
+int16_t LSM6DSRX::readInt16(LSM6DSRXDefs::Registers lowReg,
+ LSM6DSRXDefs::Registers highReg)
{
int8_t low = 0, high = 0;
int16_t sample = 0;
@@ -851,6 +878,15 @@ void LSM6DSRX::readFromFifo()
if (timestamps[timeslotTag].gyrPresent)
pushIntoFifo(timestamps[timeslotTag], idxFifo);
+ break;
+ case 0x03:
+ // Temperature data
+ timestamps[timeslotTag].data.temperature =
+ static_cast<float>(combineHighLowBits(xl, xh)) / 256;
+ timestamps[timeslotTag].data.temperature += 25.0;
+
+ timestamps[timeslotTag].tempPresent = true;
+
break;
case 0x04:
// timestamp data --> update timestamps
@@ -865,6 +901,8 @@ void LSM6DSRX::readFromFifo()
convertTimestamp(static_cast<uint64_t>(t));
timestamps[timeslotTag].data.angularSpeedTimestamp =
timestamps[timeslotTag].data.accelerationTimestamp;
+ timestamps[timeslotTag].data.temperatureTimestamp =
+ timestamps[timeslotTag].data.accelerationTimestamp;
timestamps[timeslotTag].accPresent = false;
timestamps[timeslotTag].gyrPresent = false;
@@ -882,9 +920,23 @@ void LSM6DSRX::readFromFifo()
void LSM6DSRX::pushIntoFifo(LSM6DSRXDefs::FifoTimeslotData& timeslot,
uint16_t& fifoIdx)
{
+ // Check if temperature is present
+ if (!timeslot.tempPresent)
+ {
+ /**
+ * Temperature maximum sampling rate is 52Hz, but
+ * accelerometer and gyroscope can handle higher
+ * sampling rates. Therefore, some samples might
+ * not contain a new temperature value. In case
+ * temperature is missing, the last sample is used.
+ */
+ timeslot.data.temperature = lastValidSample.temperature;
+ }
+
// reset flags (done even if data gets discarded)
- timeslot.accPresent = false;
- timeslot.gyrPresent = false;
+ timeslot.accPresent = false;
+ timeslot.gyrPresent = false;
+ timeslot.tempPresent = false;
// check if data can be pushed
if ((fifoIdx > 0 && timeslot.data.accelerationTimestamp ==
diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRX.h b/src/shared/sensors/LSM6DSRX/LSM6DSRX.h
index 45d99ddf6fbb235012ffa71662f6fb2205924743..3f7e466095303a74a6d49a507f3500ed53b8a03a 100644
--- a/src/shared/sensors/LSM6DSRX/LSM6DSRX.h
+++ b/src/shared/sensors/LSM6DSRX/LSM6DSRX.h
@@ -67,6 +67,7 @@ public:
*/
LSM6DSRXData getSensorData();
+protected:
/**
* @brief Gather data from FIFO/data registers.
*/
@@ -160,18 +161,17 @@ private:
* @brief Reads 16-bits float data from the specified registers.
* @param lowReg Register containing the low bits of the output.
* @param highReg Register containing the high bits of the output.
- * @param sensitivity Sensitivity value for the sample.
*/
- float getAxisData(LSM6DSRXDefs::Registers lowReg,
- LSM6DSRXDefs::Registers highReg, float sensitivity);
+ float readFloat16(LSM6DSRXDefs::Registers lowReg,
+ LSM6DSRXDefs::Registers highReg);
/**
* @brief Reads 16-bits data from the specified registers.
* @param lowReg Register containing the low bits of the output.
* @param highReg Register containing the high bits of the output.
*/
- int16_t getAxisData(LSM6DSRXDefs::Registers lowReg,
- LSM6DSRXDefs::Registers highReg);
+ int16_t readInt16(LSM6DSRXDefs::Registers lowReg,
+ LSM6DSRXDefs::Registers highReg);
/**
* @brief Initialize the accelerometer.
@@ -217,6 +217,12 @@ private:
*/
void getGyroscopeData(LSM6DSRXData& data);
+ /**
+ * @brief Retrieves temperature data.
+ * @param data The structure where data from the sensor is to be saved.
+ */
+ void getTemperatureData(LSM6DSRXData& data);
+
/**
* @brief Converts timestamp from the value given by the sensor to the one
* given by TimestampTimer class.
diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRXConfig.h b/src/shared/sensors/LSM6DSRX/LSM6DSRXConfig.h
index 70d4d692ca136d01fea3f42321919cd6baf47e2d..905e2dd513fc4d375f43f1120a91b63e87c397cc 100644
--- a/src/shared/sensors/LSM6DSRX/LSM6DSRXConfig.h
+++ b/src/shared/sensors/LSM6DSRX/LSM6DSRXConfig.h
@@ -155,11 +155,9 @@ struct LSM6DSRXConfig
enum class FIFO_TEMPERATURE_BDR : uint8_t
{
DISABLED = 0, ///< Temperature not batched in FIFO.
-
- // commented so that temperature doesn't get sampled in fifo.
- // HZ_1_6 = 1, ///< 1.6 Hz
- // HZ_12_5 = 2, ///< 12.5 Hz
- // HZ_52 = 3, ///< 52 Hz
+ HZ_1_6 = 1, ///< 1.6 Hz
+ HZ_12_5 = 2, ///< 12.5 Hz
+ HZ_52 = 3, ///< 52 Hz
};
/**
diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRXData.h b/src/shared/sensors/LSM6DSRX/LSM6DSRXData.h
index 0cd5083fbeafcc30dac38152bb1706ea096b4526..db95c60828e8974dd623a9b556012dca11cff8ac 100644
--- a/src/shared/sensors/LSM6DSRX/LSM6DSRXData.h
+++ b/src/shared/sensors/LSM6DSRX/LSM6DSRXData.h
@@ -27,10 +27,13 @@
namespace Boardcore
{
-struct LSM6DSRXData : public AccelerometerData, public GyroscopeData
+struct LSM6DSRXData : public AccelerometerData,
+ public GyroscopeData,
+ public TemperatureData
{
LSM6DSRXData()
- : AccelerometerData{0, 0.0, 0.0, 0.0}, GyroscopeData{0, 0.0, 0.0, 0.0}
+ : AccelerometerData{0, 0.0, 0.0, 0.0}, GyroscopeData{0, 0.0, 0.0, 0.0},
+ TemperatureData{0, 0.0}
{
}
@@ -38,7 +41,7 @@ struct LSM6DSRXData : public AccelerometerData, public GyroscopeData
{
return "accelerationTimestamp,accelerationX,accelerationY,"
"accelerationZ,angularSpeedTimestamp,angularSpeedX,"
- "angularSpeedY,angularSpeedZ\n";
+ "angularSpeedY,angularSpeedZ,temperatureTimestamp,temperature\n";
}
void print(std::ostream& os) const
@@ -46,17 +49,8 @@ struct LSM6DSRXData : public AccelerometerData, public GyroscopeData
os << accelerationTimestamp << "," << accelerationX << ","
<< accelerationY << "," << accelerationZ << ","
<< angularSpeedTimestamp << "," << angularSpeedX << ","
- << angularSpeedY << "," << angularSpeedZ << "\n";
- }
-};
-
-struct LSM6DSRXTemperature : public TemperatureData
-{
- static std::string header() { return "temperatureTimestamp,temperature\n"; }
-
- void print(std::ostream& os) const
- {
- os << temperatureTimestamp << "," << temperature << "\n";
+ << angularSpeedY << "," << angularSpeedZ << ","
+ << temperatureTimestamp << "," << temperature << "\n";
}
};
diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRXDefs.h b/src/shared/sensors/LSM6DSRX/LSM6DSRXDefs.h
index 43d0dd63ad01785c68148387df61edfacd23155f..5952a7ff925fc9a1a1134893462c31f0bdaef4fe 100644
--- a/src/shared/sensors/LSM6DSRX/LSM6DSRXDefs.h
+++ b/src/shared/sensors/LSM6DSRX/LSM6DSRXDefs.h
@@ -118,6 +118,9 @@ enum Registers
REG_FIFO_DATA_OUT_Z_L = 0x7D,
REG_FIFO_DATA_OUT_Z_H = 0x7E,
+ REG_OUT_TEMP_L = 0x20, ///< Low bits temperature output register
+ REG_OUT_TEMP_H = 0x21, ///< High bits temperature output register
+
REG_OUTX_L_A =
0x28, ///< Low bits output register for the accelerometer (x axis)
REG_OUTX_H_A =
@@ -173,14 +176,15 @@ struct RawFifoData
* @brief Temporary struct used to store data extracted from fifo, before
* turning it into LSM6DSRXData.
*
- * accPresent and gyrPresent are flags that are set when a sample from the
- * corresponding sensor is pushed inside the timeslot.
+ * accPresent, gyrPresent and tempPresent are flags that are set when a sample
+ * from the corresponding sensor is pushed inside the timeslot.
*/
struct FifoTimeslotData
{
LSM6DSRXData data;
- bool accPresent = false;
- bool gyrPresent = false;
+ bool accPresent = false;
+ bool gyrPresent = false;
+ bool tempPresent = false;
};
} // namespace LSM6DSRXDefs
diff --git a/src/tests/sensors/test-lsm6dsrx.cpp b/src/tests/sensors/test-lsm6dsrx.cpp
index 3b3c037bfadf0771195ab864298be9c93350cce5..21bbde0824be69a255886eed308cd190e685297d 100644
--- a/src/tests/sensors/test-lsm6dsrx.cpp
+++ b/src/tests/sensors/test-lsm6dsrx.cpp
@@ -42,10 +42,10 @@ void testFifoFillingTime(SPIBus& bus, miosix::GpioPin csPin,
miosix::GpioPin intPin);
/**
- * @brief Test the execution time of sampleImpl().
+ * @brief Test the execution time of sample().
*/
-void testSampleImplTime(SPIBus& bus, miosix::GpioPin csPin,
- SPIBusConfig busConfiguration, LSM6DSRXConfig& config);
+void testSampleTime(SPIBus& bus, miosix::GpioPin csPin,
+ SPIBusConfig busConfiguration, LSM6DSRXConfig& config);
/**
* @brief Test fifo read.
@@ -77,7 +77,7 @@ int main()
int2Pin.mode(Mode::INPUT);
SPIBusConfig busConfiguration; // Bus configuration for the sensor
- busConfiguration.clockDivider = SPI::ClockDivider::DIV_2;
+ busConfiguration.clockDivider = SPI::ClockDivider::DIV_64;
busConfiguration.mode =
SPI::Mode::MODE_0; // Set clock polarity to 0 and phase to 1
@@ -86,20 +86,19 @@ int main()
// acc
sensConfig.fsAcc = LSM6DSRXConfig::ACC_FULLSCALE::G2;
- sensConfig.odrAcc = LSM6DSRXConfig::ACC_ODR::HZ_833;
+ sensConfig.odrAcc = LSM6DSRXConfig::ACC_ODR::HZ_52;
sensConfig.opModeAcc = LSM6DSRXConfig::OPERATING_MODE::NORMAL;
// gyr
sensConfig.fsGyr = LSM6DSRXConfig::GYR_FULLSCALE::DPS_125;
- sensConfig.odrGyr = LSM6DSRXConfig::GYR_ODR::HZ_833;
+ sensConfig.odrGyr = LSM6DSRXConfig::GYR_ODR::HZ_52;
sensConfig.opModeGyr = LSM6DSRXConfig::OPERATING_MODE::NORMAL;
// fifo
sensConfig.fifoMode = LSM6DSRXConfig::FIFO_MODE::CONTINUOUS;
sensConfig.fifoTimestampDecimation =
LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1;
- sensConfig.fifoTemperatureBdr =
- LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED;
+ sensConfig.fifoTemperatureBdr = LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::HZ_52;
// interrupt
sensConfig.int1InterruptSelection = LSM6DSRXConfig::INTERRUPT::NOTHING;
@@ -109,7 +108,7 @@ int main()
testFifoRead(bus, csPin, busConfiguration, sensConfig, int2Pin);
- // testSampleImplTime(bus, csPin, busConfiguration, sensConfig);
+ // testSampleTime(bus, csPin, busConfiguration, sensConfig);
// testFifoFillingTime(bus, csPin, busConfiguration, sensConfig, int2Pin);
@@ -159,7 +158,7 @@ void testFifoFillingTime(SPIBus& bus, miosix::GpioPin csPin,
}
// empty the fifo
- sens->sampleImpl();
+ sens->sample();
// test time needed to fill the fifo
uint64_t t0 = TimestampTimer::getTimestamp();
@@ -177,9 +176,8 @@ void testFifoFillingTime(SPIBus& bus, miosix::GpioPin csPin,
}
}
-void testSampleImplTime(SPIBus& bus, miosix::GpioPin csPin,
- SPIBusConfig busConfiguration,
- LSM6DSRXConfig& sensConfig)
+void testSampleTime(SPIBus& bus, miosix::GpioPin csPin,
+ SPIBusConfig busConfiguration, LSM6DSRXConfig& sensConfig)
{
std::unique_ptr<LSM6DSRX> sens =
std::make_unique<LSM6DSRX>(bus, csPin, busConfiguration, sensConfig);
@@ -210,13 +208,15 @@ void testSampleImplTime(SPIBus& bus, miosix::GpioPin csPin,
{
uint64_t t0 = TimestampTimer::getTimestamp();
- auto d = sens->sampleImpl();
+ sens->sample();
uint64_t t1 = TimestampTimer::getTimestamp();
+ auto d = sens->getLastSample();
+
uint64_t diff = t1 - t0;
- std::cout << "sampleImpl() execution time(us): " << diff << "\n";
+ std::cout << "sample() execution time(us): " << diff << "\n";
std::cout << "last fifo sample:\n";
d.print(std::cout);
std::cout << "\n\n\n";