From 9eb183f438306cb9e3e1c9d90c3ec5c095484239 Mon Sep 17 00:00:00 2001
From: giovannaloro <giovanni.annaloro@skywarder.eu>
Date: Sat, 14 Dec 2024 19:19:38 +0100
Subject: [PATCH] [SensorFIFO] Make the API thread-safe
The following sensor's drivers have been updated:
* bmx160
* l3gd20
* lsm6dsrx
---
src/entrypoints/bmx160-calibration-entry.cpp | 13 +++---
src/shared/sensors/BMX160/BMX160.cpp | 3 +-
src/shared/sensors/BMX160/BMX160.h | 4 +-
src/shared/sensors/BMX160/BMX160Defs.h | 5 +++
.../sensors/BMX160/BMX160WithCorrection.cpp | 20 ++++-----
src/shared/sensors/L3GD20/L3GD20.h | 9 ++--
src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp | 17 ++++---
src/shared/sensors/LSM6DSRX/LSM6DSRX.h | 2 +
src/shared/sensors/Sensor.h | 45 ++++++++++++-------
src/tests/hil/Sensors/Sensors.cpp | 4 +-
.../sensors/test-bmx160-with-correction.cpp | 10 ++---
src/tests/sensors/test-bmx160.cpp | 9 ++--
src/tests/sensors/test-l3gd20-fifo.cpp | 8 ++--
src/tests/sensors/test-lsm6dsrx.cpp | 12 +++--
src/tests/test-sensormanager.cpp | 7 +--
15 files changed, 95 insertions(+), 73 deletions(-)
diff --git a/src/entrypoints/bmx160-calibration-entry.cpp b/src/entrypoints/bmx160-calibration-entry.cpp
index 2d36db1e9..05f100a16 100644
--- a/src/entrypoints/bmx160-calibration-entry.cpp
+++ b/src/entrypoints/bmx160-calibration-entry.cpp
@@ -238,8 +238,8 @@ void calibrateAccelerometer()
{
bmx160->sample();
- uint8_t fifoSize = bmx160->getLastFifoSize();
- auto& fifo = bmx160->getLastFifo();
+ uint16_t fifoSize;
+ const auto fifo = bmx160->getLastFifo(fifoSize);
for (uint8_t ii = 0; ii < fifoSize; ii++)
{
@@ -333,9 +333,8 @@ void calibrateMagnetometer()
[&]()
{
bmx160->sample();
-
- uint8_t fifoSize = bmx160->getLastFifoSize();
- auto& fifo = bmx160->getLastFifo();
+ uint16_t fifoSize;
+ const auto fifo = bmx160->getLastFifo(fifoSize);
for (uint8_t i = 0; i < fifoSize; i++)
{
@@ -409,8 +408,8 @@ void calibrateGyroscope()
{
bmx160->sample();
- uint8_t fifoSize = bmx160->getLastFifoSize();
- auto& fifo = bmx160->getLastFifo();
+ uint16_t fifoSize;
+ const auto fifo = bmx160->getLastFifo(fifoSize);
for (uint8_t i = 0; i < fifoSize; i++)
{
diff --git a/src/shared/sensors/BMX160/BMX160.cpp b/src/shared/sensors/BMX160/BMX160.cpp
index ccf1e0fef..f175ad873 100644
--- a/src/shared/sensors/BMX160/BMX160.cpp
+++ b/src/shared/sensors/BMX160/BMX160.cpp
@@ -750,8 +750,9 @@ void BMX160::readFifo(bool headerless)
spi.readRegisters(BMX160Defs::REG_FIFO_DATA, buf, len);
uint64_t timestamp = 0;
uint64_t watermarkTimestamp = 0;
+ int idx = 0;
+ miosix::Lock<miosix::FastMutex> l(fifoMutex);
- int idx = 0;
while (idx < len && buf[idx] != BMX160Defs::FIFO_STOP_BYTE)
{
if (headerless)
diff --git a/src/shared/sensors/BMX160/BMX160.h b/src/shared/sensors/BMX160/BMX160.h
index b4a3864ab..17a84f360 100644
--- a/src/shared/sensors/BMX160/BMX160.h
+++ b/src/shared/sensors/BMX160/BMX160.h
@@ -41,7 +41,7 @@ namespace Boardcore
/**
* @brief BMX160 Driver.
*/
-class BMX160 : public SensorFIFO<BMX160Data, 200>
+class BMX160 : public SensorFIFO<BMX160Data, BMX160Defs::FIFO_SIZE>
{
public:
/**
@@ -117,6 +117,8 @@ private:
/**
* @brief Push a sample into the FIFO.
*
+ * @note The fifo mutex must be locked prior to calling this function
+ *
* @param sample Sample to be pushed.
*/
void pushSample(BMX160Data sample);
diff --git a/src/shared/sensors/BMX160/BMX160Defs.h b/src/shared/sensors/BMX160/BMX160Defs.h
index 0d14e7fb6..907b0ef82 100644
--- a/src/shared/sensors/BMX160/BMX160Defs.h
+++ b/src/shared/sensors/BMX160/BMX160Defs.h
@@ -33,6 +33,11 @@ namespace Boardcore
namespace BMX160Defs
{
+/**
+ * @brief Driver's fifo size expressed as number of samples.
+ */
+const uint16_t FIFO_SIZE = 200;
+
/**
* @brief Temperature sensor sensibility.
*/
diff --git a/src/shared/sensors/BMX160/BMX160WithCorrection.cpp b/src/shared/sensors/BMX160/BMX160WithCorrection.cpp
index ba68500bb..1d2500f7f 100644
--- a/src/shared/sensors/BMX160/BMX160WithCorrection.cpp
+++ b/src/shared/sensors/BMX160/BMX160WithCorrection.cpp
@@ -71,16 +71,16 @@ BMX160WithCorrectionData BMX160WithCorrection::sampleImpl()
}
Eigen::Vector3f avgAccel{0, 0, 0}, avgMag{0, 0, 0}, avgGyro{0, 0, 0}, vec;
- BMX160Data fifoElement;
BMX160WithCorrectionData result;
-
- uint8_t fifoSize = bmx160->getLastFifoSize();
+ BMX160Data fifoElement;
+ uint16_t lastFifoSize;
+ const auto lastFifo = bmx160->getLastFifo(
+ lastFifoSize); // get last fifo and save last fifo size in fifoSize
// Read all data in the fifo
- for (int i = 0; i < fifoSize; i++)
+ for (int i = 0; i < lastFifoSize; i++)
{
- fifoElement = bmx160->getFifoElement(i);
-
+ fifoElement = lastFifo[i];
// Read acceleration data
static_cast<AccelerometerData>(fifoElement) >> vec;
avgAccel += vec;
@@ -95,7 +95,7 @@ BMX160WithCorrectionData BMX160WithCorrection::sampleImpl()
}
// Average the samples
- if (fifoSize == 0)
+ if (lastFifoSize == 0)
{
static_cast<AccelerometerData>(bmx160->getLastSample()) >> avgAccel;
static_cast<MagnetometerData>(bmx160->getLastSample()) >> avgMag;
@@ -104,9 +104,9 @@ BMX160WithCorrectionData BMX160WithCorrection::sampleImpl()
}
else
{
- avgAccel /= fifoSize;
- avgMag /= fifoSize;
- avgGyro /= fifoSize;
+ avgAccel /= lastFifoSize;
+ avgMag /= lastFifoSize;
+ avgGyro /= lastFifoSize;
}
// Correct the measurements
diff --git a/src/shared/sensors/L3GD20/L3GD20.h b/src/shared/sensors/L3GD20/L3GD20.h
index 802b0ed2f..5d50608e8 100644
--- a/src/shared/sensors/L3GD20/L3GD20.h
+++ b/src/shared/sensors/L3GD20/L3GD20.h
@@ -221,11 +221,13 @@ public:
int16_t z = buf[4] | buf[5] << 8;
Eigen::Vector3f rads = toRadiansPerSecond(x, y, z);
- lastFifo[0] = {lastSampleTimestamp, rads(0), rads(1), rads(2)};
+ return {lastSampleTimestamp, rads(0), rads(1), rads(2)};
}
-
else // FIFO is enabled
{
+ // Lock mutex for thread safe Fifo reading
+ miosix::Lock<miosix::FastMutex> l(fifoMutex);
+
SPITransaction spi(spislave);
// Read last fifo level
uint8_t fifoSrc = spi.readRegister(REG_FIFO_SRC);
@@ -287,9 +289,8 @@ public:
}
lastFifoLevel = fifoLevel - duplicates;
+ return lastFifo[lastFifoLevel - 1];
}
-
- return lastFifo[lastFifoLevel - 1];
}
private:
diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp b/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
index 9508ad896..aa7d51c9b 100644
--- a/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
+++ b/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
@@ -765,6 +765,9 @@ float LSM6DSRX::getSensorTimestampResolution()
void LSM6DSRX::readFromFifo()
{
+ // Lock mutex for thread safe Fifo reading
+ miosix::Lock<miosix::FastMutex> l(fifoMutex);
+
SPITransaction spi{spiSlave};
// get number of sample to read
@@ -794,17 +797,17 @@ void LSM6DSRX::readFromFifo()
// --> `i` keeps count of the number of elements in `rawFifo`
// `idxFifo` keeps track of the samples saved inside `lastFifo`
uint16_t idxFifo = 0;
+
for (uint16_t i = 0; i < numSamples; ++i)
{
const uint8_t sensorTag = (rawFifo[i].sampleTag >> 3) & 31;
const uint8_t timeslotTag = (rawFifo[i].sampleTag & 6) >> 1;
-
- const uint8_t xl = rawFifo[i].xl;
- const uint8_t xh = rawFifo[i].xh;
- const uint8_t yl = rawFifo[i].yl;
- const uint8_t yh = rawFifo[i].yh;
- const uint8_t zl = rawFifo[i].zl;
- const uint8_t zh = rawFifo[i].zh;
+ const uint8_t xl = rawFifo[i].xl;
+ const uint8_t xh = rawFifo[i].xh;
+ const uint8_t yl = rawFifo[i].yl;
+ const uint8_t yh = rawFifo[i].yh;
+ const uint8_t zl = rawFifo[i].zl;
+ const uint8_t zh = rawFifo[i].zh;
switch (sensorTag)
{
diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRX.h b/src/shared/sensors/LSM6DSRX/LSM6DSRX.h
index a13b6802c..45d99ddf6 100644
--- a/src/shared/sensors/LSM6DSRX/LSM6DSRX.h
+++ b/src/shared/sensors/LSM6DSRX/LSM6DSRX.h
@@ -127,6 +127,8 @@ private:
* timestamp is 0: in this case nothing happens to the fifo, data inside
* `timeslot` is discarded.
*
+ * @note The fifo mutex must be locked prior to calling this function
+ *
* @param timeslot The timeslot containing data to be pushed inside the
* fifo.
* @param fifoIdx The current index of the fifo, pointing to the current
diff --git a/src/shared/sensors/Sensor.h b/src/shared/sensors/Sensor.h
index bfbae2740..11cbf3709 100644
--- a/src/shared/sensors/Sensor.h
+++ b/src/shared/sensors/Sensor.h
@@ -137,36 +137,47 @@ public:
/**
* @brief Interface for sensor that implement a FIFO.
+ *
+ * @note This class is move constructible, but the move operation is not thread
+ * safe. The moved-to object's mutex is initialized to the unlocked state, no
+ * matter the moved-from object's mutex state. The move constructor is only
+ * needed for HIL.
+ *
*/
template <typename Data, uint32_t FifoSize>
class SensorFIFO : public Sensor<Data>
{
protected:
std::array<Data, FifoSize> lastFifo;
- uint16_t lastFifoLevel = 1; ///< number of samples in lastFifo
-
+ uint16_t lastFifoLevel = 1; //< number of samples in lastFifo
uint64_t lastInterruptTimestamp = 0;
uint64_t interruptTimestampDelta =
- 0; ///< delta between previous interrupt timestamp and the last
- ///< received one
+ 0; //< delta between previous interrupt
+ // timestamp and the last received one
+ miosix::FastMutex fifoMutex; // thread safe mutex to read FIFO
public:
- /**
- * @return last FIFO sampled from the sensor
- */
- const std::array<Data, FifoSize>& getLastFifo() { return lastFifo; }
-
- /**
- * @param i index of the requested item inside the FIFO
- *
- * @return the i-th element of the FIFO
- */
- const Data& getFifoElement(uint32_t i) const { return lastFifo[i]; }
+ SensorFIFO() {}
+
+ SensorFIFO(SensorFIFO&& other)
+ : lastFifo{std::move(other.lastFifo)},
+ lastFifoLevel{std::move(other.lastFifoLevel)},
+ lastInterruptTimestamp{std::move(other.lastInterruptTimestamp)},
+ interruptTimestampDelta{std::move(other.interruptTimestampDelta)},
+ fifoMutex{}
+ {
+ }
/**
- * @return number of elements in the last FIFO sampled from the sensor
+ * @param lastFifoSize output parameter for last FIFO size
+ * @return last FIFO sampled from the sensor
*/
- uint16_t getLastFifoSize() const { return lastFifoLevel; }
+ const std::array<Data, FifoSize> getLastFifo(uint16_t& lastFifoSize)
+ {
+ miosix::Lock<miosix::FastMutex> l(fifoMutex);
+ lastFifoSize = lastFifoLevel;
+ return lastFifo;
+ }
/**
* @brief Called by the interrupt handling routine: provides the timestamp
diff --git a/src/tests/hil/Sensors/Sensors.cpp b/src/tests/hil/Sensors/Sensors.cpp
index bc1edb3d9..888c07aac 100644
--- a/src/tests/hil/Sensors/Sensors.cpp
+++ b/src/tests/hil/Sensors/Sensors.cpp
@@ -675,8 +675,8 @@ void Sensors::ubxgpsCallback()
}
void Sensors::lsm6dsrxCallback()
{
- auto& fifo = lsm6dsrx->getLastFifo();
- uint16_t fifoSize = lsm6dsrx->getLastFifoSize();
+ uint16_t fifoSize;
+ const auto fifo = lsm6dsrx->getLastFifo(fifoSize);
// For every instance inside the fifo log the sample
for (uint16_t i = 0; i < fifoSize; i++)
diff --git a/src/tests/sensors/test-bmx160-with-correction.cpp b/src/tests/sensors/test-bmx160-with-correction.cpp
index db14f2c96..b39cff4a9 100644
--- a/src/tests/sensors/test-bmx160-with-correction.cpp
+++ b/src/tests/sensors/test-bmx160-with-correction.cpp
@@ -209,8 +209,8 @@ SixParametersCorrector calibrateMagnetometer(SixParametersCorrector oldCorr)
{
bmx160->sample();
- uint8_t fifoSize = bmx160->getLastFifoSize();
- auto& fifo = bmx160->getLastFifo();
+ uint16_t fifoSize;
+ const auto fifo = bmx160->getLastFifo(fifoSize);
for (uint8_t i = 0; i < fifoSize; i++)
{
@@ -294,8 +294,8 @@ SixParametersCorrector calibrateGyroscope(SixParametersCorrector corr)
{
bmx160->sample();
- uint8_t fifoSize = bmx160->getLastFifoSize();
- auto& fifo = bmx160->getLastFifo();
+ uint16_t fifoSize;
+ const auto fifo = bmx160->getLastFifo(fifoSize);
for (uint8_t i = 0; i < fifoSize; i++)
{
@@ -436,7 +436,7 @@ SixParametersCorrector calibrateGyroscope(SixParametersCorrector corr)
// bmx160->sample();
// avgAccel = {0, 0, 0};
-// // Read all the data contained in the fifo
+// // Read all the data contaigetLastFifoned in the fifo
// uint8_t size = bmx160->getLastFifoSize();
// fifoAccSampleCount = 0;
// for (int ii = 0; ii < size; ii++)
diff --git a/src/tests/sensors/test-bmx160.cpp b/src/tests/sensors/test-bmx160.cpp
index 6557645cd..8be612fd4 100644
--- a/src/tests/sensors/test-bmx160.cpp
+++ b/src/tests/sensors/test-bmx160.cpp
@@ -94,18 +94,19 @@ int main()
}
uint64_t now = TimestampTimer::getTimestamp();
+ uint16_t fifoSize;
+ const auto lastFifo = sensor->getLastFifo(fifoSize);
+ uint16_t len = std::min(fifoSize, (uint16_t)5);
printf("Tick: %.4f s, Now: %.4f s\n", tick / 1000000.0f,
now / 1000000.0f);
printf("Temp: %.2f deg\n", sensor->getTemperature().temperature);
- printf("Fill: %d\n", sensor->getLastFifoSize());
-
+ printf("Fill: %d\n", fifoSize);
printf("----------------------------\n");
- uint16_t len = std::min(sensor->getLastFifoSize(), (uint16_t)5);
for (uint16_t i = 0; i < len; i++)
{
- BMX160Data data = sensor->getFifoElement(i);
+ BMX160Data data = lastFifo[i];
printf("Mag [%.4f s]:\t%.2f\t%.2f\t%.2f\n",
data.magneticFieldTimestamp / 1000000.0f,
data.magneticFieldX, data.magneticFieldY,
diff --git a/src/tests/sensors/test-l3gd20-fifo.cpp b/src/tests/sensors/test-l3gd20-fifo.cpp
index 76e9186c0..aad44c5c4 100644
--- a/src/tests/sensors/test-l3gd20-fifo.cpp
+++ b/src/tests/sensors/test-l3gd20-fifo.cpp
@@ -174,11 +174,9 @@ int main()
// Measure how long we take to read the fifo
update = TimestampTimer::getTimestamp() - update;
- uint8_t level =
- gyro->getLastFifoSize(); // Current number of samples in the FIFO
-
- // Obtain the FIFO
- const array<L3GD20Data, L3GD20_FIFO_SIZE>& fifo = gyro->getLastFifo();
+ uint16_t level;
+ // Obtain the FIFO and save the last fifo size
+ const auto fifo = gyro->getLastFifo(level);
// Store everything in the data buffer
for (int i = 0; i < level; i++)
diff --git a/src/tests/sensors/test-lsm6dsrx.cpp b/src/tests/sensors/test-lsm6dsrx.cpp
index a979fe0f4..3b3c037bf 100644
--- a/src/tests/sensors/test-lsm6dsrx.cpp
+++ b/src/tests/sensors/test-lsm6dsrx.cpp
@@ -264,17 +264,15 @@ void testFifoRead(SPIBus& bus, miosix::GpioPin csPin,
}
sens->sample();
-
- const std::array<LSM6DSRXData, LSM6DSRXDefs::FIFO_SIZE>& buf =
- sens->getLastFifo();
-
+ uint16_t fifoSize;
+ const auto buf = sens->getLastFifo(fifoSize);
// Print fifo
std::cout << "last fifo element:\n";
- buf[sens->getLastFifoSize() - 1].print(std::cout);
- std::cout << "last fifo size: " << sens->getLastFifoSize() << "\n";
+ buf[fifoSize - 1].print(std::cout);
+ std::cout << "last fifo size: " << fifoSize << "\n";
// Check fifo data
- for (uint16_t i = 1; i < sens->getLastFifoSize(); ++i)
+ for (uint16_t i = 1; i < fifoSize; ++i)
{
// Check for accelerometer timestamps
if (buf[i].accelerationTimestamp <=
diff --git a/src/tests/test-sensormanager.cpp b/src/tests/test-sensormanager.cpp
index b67326b5b..8fcbec7df 100644
--- a/src/tests/test-sensormanager.cpp
+++ b/src/tests/test-sensormanager.cpp
@@ -158,7 +158,8 @@ public:
FIFOData sampleImpl()
{
index = 0;
- return sensor->getFifoElement(index);
+ uint16_t actualFifoSize;
+ return sensor->getLastFifo(actualFifoSize)[index];
}
FIFOData getLastSample() override
@@ -167,8 +168,8 @@ public:
index++;
TRACE("Index : %d \n", index);
-
- return sensor->getFifoElement(index);
+ uint16_t actualFifoSize;
+ return sensor->getLastFifo(actualFifoSize)[index];
}
private:
--
GitLab