diff --git a/src/entrypoints/bmx160-calibration-entry.cpp b/src/entrypoints/bmx160-calibration-entry.cpp
index 2d36db1e9760a947bfff29dbd324c7a0647c8bcb..05f100a16d3796c4ce18e4ac8fee1aec2cad51b6 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 ccf1e0fef7650d5b95ab8b6be540411b98daaf13..f175ad87342fb1dbbd7309c69b9a6ab683e20ad4 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 b4a3864ab7ccdf1a2f6c9bddb90dbe7663d382aa..17a84f360df643d3b31c0a9817afc809146ba170 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 0d14e7fb696c615853dea67c944203f9bb6fb405..907b0ef82b1a3f29233e56fb1505d1e1629fa348 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 ba68500bb4214faab3e5f48e8323bc92310a629b..1d2500f7fda10b32766b8c054bef2e537024185a 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 802b0ed2fefad0331afcd5da5598178984e4eadc..5d50608e82ef884ae3ab6352f6580393339b2b5c 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 9508ad8969c422598b3489f355db80acc69e17b5..aa7d51c9b6763b28696728c23019a22dd5fd56eb 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 a13b6802c92dcf8bc838dcbf84cebe8dc4121ce3..45d99ddf6fbb235012ffa71662f6fb2205924743 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 bfbae274092d983a504ddc459b9f49b8a78fc5c9..11cbf3709151a2509c243c5e2d34d86154381978 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 bc1edb3d9f9aaab9ecdf0d0da2095538ba12d4a3..888c07aacf77d0dea7ce83a68c70d6428938758d 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 db14f2c9679bb72a0dad6953101cfc8d2b80984f..b39cff4a9274e467372ac5c75313b6841cf4cadc 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 6557645cd6c98f9c40d26bae285fd82bbd488292..8be612fd4d69282806e653bd0d70295465f2529e 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 76e9186c0d8068f57181890359a866c4f1c7436c..aad44c5c40dfae6705afe9418869f6a80504e548 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 a979fe0f49b67a0a4cfd2bcc423cb85ecf73d1e7..3b3c037bfadf0771195ab864298be9c93350cce5 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 b67326b5b3a2875a20457f95c1bf458b43a3a3d6..8fcbec7df56fd1d8dcd6b7a626075825c6aacb8b 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: