From 7ae03089d2eb959c4d94cf5224b33cde459328b7 Mon Sep 17 00:00:00 2001
From: Fabrizio Monti <fabrizio.monti@skywarder.eu>
Date: Tue, 1 Aug 2023 09:15:14 +0000
Subject: [PATCH] [LSM6DSRX] Added driver for the LSM6DSRX 6 axis IMU
---
CMakeLists.txt | 3 +
cmake/boardcore.cmake | 1 +
src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp | 924 +++++++++++++++++++
src/shared/sensors/LSM6DSRX/LSM6DSRX.h | 242 +++++
src/shared/sensors/LSM6DSRX/LSM6DSRXConfig.h | 210 +++++
src/shared/sensors/LSM6DSRX/LSM6DSRXData.h | 63 ++
src/shared/sensors/LSM6DSRX/LSM6DSRXDefs.h | 188 ++++
src/tests/sensors/test-lsm6dsrx.cpp | 310 +++++++
8 files changed, 1941 insertions(+)
create mode 100644 src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
create mode 100644 src/shared/sensors/LSM6DSRX/LSM6DSRX.h
create mode 100644 src/shared/sensors/LSM6DSRX/LSM6DSRXConfig.h
create mode 100644 src/shared/sensors/LSM6DSRX/LSM6DSRXData.h
create mode 100644 src/shared/sensors/LSM6DSRX/LSM6DSRXDefs.h
create mode 100644 src/tests/sensors/test-lsm6dsrx.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5352d725d..fd08e56e3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -425,6 +425,9 @@ sbs_target(test-lps28dfw stm32f767zi_nucleo)
add_executable(test-lps22df src/tests/sensors/test-lps22df.cpp)
sbs_target(test-lps22df stm32f767zi_nucleo)
+add_executable(test-lsm6dsrx src/tests/sensors/test-lsm6dsrx.cpp)
+sbs_target(test-lsm6dsrx stm32f407vg_stm32f4discovery)
+
#-----------------------------------------------------------------------------#
# Tests - Utils #
#-----------------------------------------------------------------------------#
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index 3e29187ee..435e99a41 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -111,6 +111,7 @@ foreach(OPT_BOARD ${BOARDS})
${SBS_BASE}/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
${SBS_BASE}/src/shared/sensors/LPS28DFW/LPS28DFW.cpp
${SBS_BASE}/src/shared/sensors/LPS22DF/LPS22DF.cpp
+ ${SBS_BASE}/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
# Calibration
${SBS_BASE}/src/shared/sensors/calibration/BiasCalibration/BiasCalibration.cpp
diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp b/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
new file mode 100644
index 000000000..3294db578
--- /dev/null
+++ b/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
@@ -0,0 +1,924 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Fabrizio Monti
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "LSM6DSRX.h"
+
+#include <assert.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <utils/Constants.h>
+#include <utils/Debug.h>
+
+#include <cmath>
+
+namespace Boardcore
+{
+
+LSM6DSRX::LSM6DSRX(SPIBus& bus, miosix::GpioPin csPin,
+ SPIBusConfig busConfiguration, LSM6DSRXConfig& configuration)
+ : spiSlave(bus, csPin, busConfiguration), config(configuration)
+{
+ isInit = false;
+
+ // check that the watermark value is suitable
+ config.fifoWatermark = std::min(config.fifoWatermark, uint16_t(170));
+
+ /**
+ * watermark value is multiplied by 3, because every sample is composed of 3
+ * data: timestamp, accelerometer data, gyroscope data. The sensor's fifo
+ * contains all these data one by one. These data are then collected and
+ * crafted inside a single sample by the driver.
+ */
+ config.fifoWatermark *= 3;
+
+ // check that ACC_ODR is set to HZ_1_6 only if OPERATING_MODE is equal to
+ // NORMAL
+ if (config.odrAcc == LSM6DSRXConfig::ACC_ODR::HZ_1_6)
+ {
+ config.opModeAcc = LSM6DSRXConfig::OPERATING_MODE::NORMAL;
+ }
+}
+
+bool LSM6DSRX::init()
+{
+ if (isInit)
+ {
+ LOG_ERR(logger, "init() should be called once");
+ lastError = SensorErrors::ALREADY_INIT;
+ return false;
+ }
+
+ if (checkWhoAmI() == false)
+ {
+ LOG_ERR(logger, "Got bad CHIPID");
+ lastError = SensorErrors::INVALID_WHOAMI;
+ return false;
+ }
+
+ // Set BDU and multiple spi read/write
+ {
+ SPITransaction spiTransaction{spiSlave};
+
+ uint8_t value = static_cast<uint8_t>(config.bdu) << 6; // set bdu
+ value |= 1 << 2; // set multiple spi read/write
+
+ spiTransaction.writeRegister(LSM6DSRXDefs::REG_CTRL3_C, value);
+ }
+
+ // Setup accelerometer
+ initAccelerometer();
+
+ // Setup gyroscope
+ initGyroscope();
+
+ // setup Fifo
+ initFifo();
+
+ // enable timestamp
+ {
+ constexpr uint8_t REG_CTRL10_TIMESTAMP_EN = 1 << 5;
+
+ SPITransaction spiTransaction{spiSlave};
+ spiTransaction.writeRegister(LSM6DSRXDefs::REG_CTRL10_C,
+ REG_CTRL10_TIMESTAMP_EN);
+ }
+
+ // set interrupt
+ initInterrupts();
+
+ // set timestamps
+ correlateTimestamps();
+ sensorTimestampResolution =
+ getSensorTimestampResolution() *
+ 1000; // return value is in milliseconds, need microseconds.
+
+ isInit = true;
+ lastError = SensorErrors::NO_ERRORS;
+ return true;
+}
+
+void LSM6DSRX::initAccelerometer()
+{
+ uint8_t configByte = 0;
+ SPITransaction spiTransaction{spiSlave};
+
+ // Setup accelerometer
+
+ // set accelerometer odr, fullscale and high resolution
+ configByte = static_cast<uint8_t>(config.odrAcc) << 4 | // odr
+ static_cast<uint8_t>(config.fsAcc) << 2 | // fullscale
+ 0 << 1; // high resolution selection
+ spiTransaction.writeRegister(LSM6DSRXDefs::REG_CTRL1_XL, configByte);
+
+ // set accelerometer performance mode
+ configByte = static_cast<uint8_t>(config.opModeAcc) << 4;
+ spiTransaction.writeRegister(LSM6DSRXDefs::REG_CTRL6_C, configByte);
+
+ // set sensitivity
+ switch (config.fsAcc)
+ {
+ case LSM6DSRXConfig::ACC_FULLSCALE::G2:
+ sensitivityAcc = 0.061;
+ break;
+ case LSM6DSRXConfig::ACC_FULLSCALE::G4:
+ sensitivityAcc = 0.122;
+ break;
+ case LSM6DSRXConfig::ACC_FULLSCALE::G8:
+ sensitivityAcc = 0.244;
+ break;
+ case LSM6DSRXConfig::ACC_FULLSCALE::G16:
+ sensitivityAcc = 0.488;
+ break;
+ default:
+ config.fsAcc = LSM6DSRXConfig::ACC_FULLSCALE::G2;
+ 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()
+{
+ uint8_t configByte = 0;
+ SPITransaction spiTransaction{spiSlave};
+
+ // set odr and fullscale
+ configByte = static_cast<uint8_t>(config.odrGyr) << 4 | // odr
+ static_cast<uint8_t>(config.fsGyr); // fullscale
+ spiTransaction.writeRegister(LSM6DSRXDefs::REG_CTRL2_G, configByte);
+
+ // set performance mode
+ configByte = static_cast<uint8_t>(config.opModeGyr) << 7;
+ spiTransaction.writeRegister(LSM6DSRXDefs::REG_CTRL7_G, configByte);
+
+ // set sensitivity
+ switch (config.fsGyr)
+ {
+ case LSM6DSRXConfig::GYR_FULLSCALE::DPS_125:
+ sensitivityGyr = 4.375;
+ break;
+ case LSM6DSRXConfig::GYR_FULLSCALE::DPS_250:
+ sensitivityGyr = 8.75;
+ break;
+ case LSM6DSRXConfig::GYR_FULLSCALE::DPS_500:
+ sensitivityGyr = 17.5;
+ break;
+ case LSM6DSRXConfig::GYR_FULLSCALE::DPS_1000:
+ sensitivityGyr = 35.0;
+ break;
+ case LSM6DSRXConfig::GYR_FULLSCALE::DPS_2000:
+ sensitivityGyr = 70.0;
+ break;
+ case LSM6DSRXConfig::GYR_FULLSCALE::DPS_4000:
+ sensitivityGyr = 140.0;
+ break;
+ default:
+ config.fsGyr = LSM6DSRXConfig::GYR_FULLSCALE::DPS_125;
+ 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()
+{
+ // setup Fifo
+ uint8_t configByte = 0;
+ SPITransaction spiTransaction{spiSlave};
+
+ // select batch data rate in FIFO_CTRL3
+ configByte = static_cast<uint8_t>(config.odrAcc) | // accelerometer bdr
+ (static_cast<uint8_t>(config.odrGyr) << 4); // gyroscope bdr
+ spiTransaction.writeRegister(LSM6DSRXDefs::REG_FIFO_CTRL3, configByte);
+
+ // set fifo mode, batch data rate for temperature sensor and the decimation
+ // factor for timestamp batching
+ configByte = static_cast<uint8_t>(config.fifoMode) | // fifo operating mode
+ static_cast<uint8_t>(config.fifoTemperatureBdr)
+ << 4 | // batch data rate for temperature data
+ static_cast<uint8_t>(config.fifoTimestampDecimation)
+ << 6; // timestamp decimation
+ spiTransaction.writeRegister(LSM6DSRXDefs::REG_FIFO_CTRL4, configByte);
+}
+
+void LSM6DSRX::initInterrupts()
+{
+ uint8_t buf[] = {0, 0};
+ SPITransaction spi{spiSlave};
+
+ buf[0] = static_cast<uint8_t>(
+ config.int1InterruptSelection); // set interrupt on pin INT1
+ buf[1] = static_cast<uint8_t>(
+ config.int2InterruptSelection); // set interrupt on pin INT2
+ spi.writeRegisters(LSM6DSRXDefs::REG_INT1_CTRL, buf, 2);
+
+ // set watermark level
+ buf[0] = static_cast<uint8_t>(config.fifoWatermark &
+ 255); // the first 8bits of the number.
+ buf[1] = static_cast<uint8_t>((config.fifoWatermark >> 8) &
+ 0x01); // the 9th bit of the number.
+ spi.writeRegisters(LSM6DSRXDefs::REG_FIFO_CTRL1, buf, 2);
+}
+
+bool LSM6DSRX::selfTestAcc()
+{
+ bool returnValue = false;
+ uint8_t byteValue = 0; // used to read and write in registers
+ uint8_t idx = 0;
+ const uint8_t SIZE_DATA = 5; // number of sample for the test
+ SPITransaction spi{spiSlave};
+
+ // sleep time for data ready interrupt (150% odr during self-test)
+ // expressed in milliseconds.
+ // for this self-test the odr is 52Hz
+ const unsigned int dataReadyWaitTime = 29;
+
+ LSM6DSRXData averageSF; // average data during self test
+ LSM6DSRXData averageNormal; // average normal data
+
+ // set registers
+
+ // set odr=52Hz, fs=2g
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL1_XL, 0x30);
+
+ // power off the gyro
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL2_G, 0x00);
+
+ // set bdu to UPDATE_AFTER_READ
+ // register address automatically incremented during a multiple byte access
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL3_C, 0x44);
+
+ // disable gyro sleep mode
+ // disable data ready
+ // disable i2c
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL4_C, 0x00);
+
+ // disable accelerometer and gyro self test (for acc it will be enabled
+ // later on)
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL5_C, 0x00);
+
+ // more settings for the accelerometer (performance mode)
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL6_C, 0x00);
+
+ // gyro settings
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL7_G, 0x00);
+
+ // disables accelerometers additional filters
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL8_XL, 0x00);
+
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL9_XL, 0x00);
+
+ // disable timestamp counter
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL10_C, 0x00);
+
+ // wait for stable output
+ miosix::Thread::sleep(100);
+
+ // wait for accelerometer data ready
+ byteValue = spi.readRegister(LSM6DSRXDefs::REG_STATUS);
+ byteValue = byteValue & 0x01;
+ while (byteValue != 1)
+ {
+ miosix::Thread::sleep(dataReadyWaitTime);
+ byteValue = spi.readRegister(LSM6DSRXDefs::REG_STATUS);
+ byteValue = byteValue & 0x01;
+ }
+
+ // 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);
+
+ // read normal data (self test disabled)
+ for (idx = 0; idx < SIZE_DATA; ++idx)
+ {
+ // wait for accelerometer data ready
+ byteValue = spi.readRegister(LSM6DSRXDefs::REG_STATUS);
+ byteValue = byteValue & 0x01;
+ while (byteValue != 1)
+ {
+ miosix::Thread::sleep(dataReadyWaitTime);
+ byteValue = spi.readRegister(LSM6DSRXDefs::REG_STATUS);
+ byteValue = byteValue & 0x01;
+ }
+
+ // 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 /= SIZE_DATA;
+ averageNormal.accelerationY /= SIZE_DATA;
+ averageNormal.accelerationZ /= SIZE_DATA;
+
+ // enable accelerometer positive sign self test
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL5_C, 0x01);
+
+ // wait for stable output
+ miosix::Thread::sleep(100);
+
+ // wait for accelerometer data ready
+ byteValue = spi.readRegister(LSM6DSRXDefs::REG_STATUS);
+ byteValue = byteValue & 0x01;
+ while (byteValue != 1)
+ {
+ miosix::Thread::sleep(dataReadyWaitTime);
+ byteValue = spi.readRegister(LSM6DSRXDefs::REG_STATUS);
+ byteValue = byteValue & 0x01;
+ }
+
+ // 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);
+
+ // read self test data
+ for (idx = 0; idx < SIZE_DATA; ++idx)
+ {
+ // wait for accelerometer data ready
+ byteValue = spi.readRegister(LSM6DSRXDefs::REG_STATUS);
+ byteValue = byteValue & 0x01;
+ while (byteValue != 1)
+ {
+ miosix::Thread::sleep(dataReadyWaitTime);
+ byteValue = spi.readRegister(LSM6DSRXDefs::REG_STATUS);
+ byteValue = byteValue & 0x01;
+ }
+
+ // 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 /= SIZE_DATA;
+ averageSF.accelerationY /= SIZE_DATA;
+ averageSF.accelerationZ /= SIZE_DATA;
+
+ if (LSM6DSRXDefs::ACC_SELF_TEST_MIN <=
+ std::abs(averageSF.accelerationX - averageNormal.accelerationX) &&
+ std::abs(averageSF.accelerationX - averageNormal.accelerationX) <=
+ LSM6DSRXDefs::ACC_SELF_TEST_MAX &&
+ LSM6DSRXDefs::ACC_SELF_TEST_MIN <=
+ std::abs(averageSF.accelerationY - averageNormal.accelerationY) &&
+ std::abs(averageSF.accelerationY - averageNormal.accelerationY) <=
+ LSM6DSRXDefs::ACC_SELF_TEST_MAX &&
+ LSM6DSRXDefs::ACC_SELF_TEST_MIN <=
+ std::abs(averageSF.accelerationZ - averageNormal.accelerationZ) &&
+ std::abs(averageSF.accelerationZ - averageNormal.accelerationZ) <=
+ LSM6DSRXDefs::ACC_SELF_TEST_MAX)
+ {
+ returnValue = true;
+ }
+ else
+ {
+ LOG_ERR(logger, "Accelerometer self-test failed!");
+ returnValue = false;
+ }
+
+ // Disable self-test
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL5_C, 0x00);
+ // Disable sensor
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL1_XL, 0x00);
+
+ return returnValue;
+}
+
+bool LSM6DSRX::selfTestGyr()
+{
+ bool returnValue = true;
+ uint8_t byteValue = 0;
+ uint8_t idx = 0;
+ const uint8_t SIZE_DATA = 5;
+ SPITransaction spi{spiSlave};
+
+ // sleep time for data ready interrupt (150% odr during self-test)
+ // expressed in milliseconds.
+ // for this self-test the odr is 208Hz
+ const unsigned int dataReadyWaitTime = 8;
+
+ LSM6DSRXData averageSF; // average data during self test
+ LSM6DSRXData averageNormal; // average normal data
+
+ // set registers
+
+ // disable accelerometer (power down)
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL1_XL, 0x00);
+
+ // set odr and fullscale for the gyro
+ // odr: 208Hz
+ // fs: 2000 dps
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL2_G, 0x5C);
+
+ // set bdu to UPDATE_AFTER_READ
+ // register address automatically incremented during a multiple byte access
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL3_C, 0x44);
+
+ // disable gyro sleep mode
+ // disable data ready
+ // disable i2c
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL4_C, 0x00);
+
+ // disable accelerometer and gyro self test
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL5_C, 0x00);
+
+ // more settings for the accelerometer (performance mode)
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL6_C, 0x00);
+
+ // enables high performance mode for the gyro
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL7_G, 0x00);
+
+ // disables accelerometer's filters
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL8_XL, 0x00);
+
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL9_XL, 0x00);
+
+ // disable timestamp counter
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL10_C, 0x00);
+
+ // sleep for stable output
+ miosix::Thread::sleep(100);
+
+ // wait for gyroscope data ready
+ byteValue = spi.readRegister(LSM6DSRXDefs::REG_STATUS);
+ byteValue = (byteValue & 0x02) >> 1;
+ while (byteValue != 1)
+ {
+ miosix::Thread::sleep(dataReadyWaitTime);
+ byteValue = spi.readRegister(LSM6DSRXDefs::REG_STATUS);
+ 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);
+
+ // read normal data (self test disabled)
+ for (idx = 0; idx < SIZE_DATA; ++idx)
+ {
+ // wait for gyroscope data ready
+ byteValue = spi.readRegister(LSM6DSRXDefs::REG_STATUS);
+ byteValue = (byteValue & 0x02) >> 1;
+ while (byteValue != 1)
+ {
+ miosix::Thread::sleep(dataReadyWaitTime);
+ byteValue = spi.readRegister(LSM6DSRXDefs::REG_STATUS);
+ byteValue = (byteValue & 0x02) >> 1;
+ }
+
+ // 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 /= SIZE_DATA;
+ averageNormal.angularSpeedY /= SIZE_DATA;
+ averageNormal.angularSpeedZ /= SIZE_DATA;
+
+ // enable gyroscope positive sign self test
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL5_C, 0x04);
+
+ // wait for stable output
+ miosix::Thread::sleep(100);
+
+ // wait for gyroscope data ready
+ byteValue = spi.readRegister(LSM6DSRXDefs::REG_STATUS);
+ byteValue = (byteValue & 0x02) >> 1;
+ while (byteValue != 1)
+ {
+ miosix::Thread::sleep(dataReadyWaitTime);
+ byteValue = spi.readRegister(LSM6DSRXDefs::REG_STATUS);
+ 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);
+
+ // read self test data
+ for (idx = 0; idx < SIZE_DATA; ++idx)
+ {
+ // wait for gyroscope data ready
+ byteValue = spi.readRegister(LSM6DSRXDefs::REG_STATUS);
+ byteValue = (byteValue & 0x02) >> 1;
+ while (byteValue != 1)
+ {
+ miosix::Thread::sleep(dataReadyWaitTime);
+ byteValue = spi.readRegister(LSM6DSRXDefs::REG_STATUS);
+ byteValue = (byteValue & 0x02) >> 1;
+ }
+
+ // 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 /= SIZE_DATA;
+ averageSF.angularSpeedY /= SIZE_DATA;
+ averageSF.angularSpeedZ /= SIZE_DATA;
+
+ if (LSM6DSRXDefs::GYR_SELF_TEST_MIN <=
+ std::abs(averageSF.angularSpeedX - averageNormal.angularSpeedX) &&
+ std::abs(averageSF.angularSpeedX - averageNormal.angularSpeedX) <=
+ LSM6DSRXDefs::GYR_SELF_TEST_MAX &&
+ LSM6DSRXDefs::GYR_SELF_TEST_MIN <=
+ std::abs(averageSF.angularSpeedY - averageNormal.angularSpeedY) &&
+ std::abs(averageSF.angularSpeedY - averageNormal.angularSpeedY) <=
+ LSM6DSRXDefs::GYR_SELF_TEST_MAX &&
+ LSM6DSRXDefs::GYR_SELF_TEST_MIN <=
+ std::abs(averageSF.angularSpeedZ - averageNormal.angularSpeedZ) &&
+ std::abs(averageSF.angularSpeedZ - averageNormal.angularSpeedZ) <=
+ LSM6DSRXDefs::GYR_SELF_TEST_MAX)
+ {
+ returnValue = true;
+ }
+ else
+ {
+ LOG_ERR(logger, "Gyroscope self-test failed!");
+ returnValue = false;
+ }
+
+ // Disable self test
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL5_C, 0x00);
+ // Disable sensor
+ spi.writeRegister(LSM6DSRXDefs::REG_CTRL2_G, 0x00);
+
+ return returnValue;
+}
+
+bool LSM6DSRX::checkWhoAmI()
+{
+ uint8_t regValue = 0;
+ {
+ SPITransaction transaction{spiSlave};
+ regValue = transaction.readRegister(LSM6DSRXDefs::REG_WHO_AM_I);
+ }
+
+ return regValue == LSM6DSRXDefs::WHO_AM_I_VALUE;
+}
+
+int16_t LSM6DSRX::combineHighLowBits(uint8_t low, uint8_t high)
+{
+ int16_t ret = high;
+ ret <<= 8;
+ ret |= low;
+ return ret;
+}
+
+uint16_t LSM6DSRX::combineHighLowBitsUnsigned(uint8_t low, uint8_t high)
+{
+ uint16_t sample = high;
+ sample <<= 8;
+ sample |= low;
+ return sample;
+}
+
+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);
+}
+
+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);
+}
+
+uint32_t LSM6DSRX::getSensorTimestamp()
+{
+ SPITransaction spi{spiSlave};
+
+ uint32_t value = spi.readRegister(LSM6DSRXDefs::REG_TIMESTAMP0);
+ value |= spi.readRegister(LSM6DSRXDefs::REG_TIMESTAMP1) << 8;
+ value |= spi.readRegister(LSM6DSRXDefs::REG_TIMESTAMP2) << 16;
+ value |= spi.readRegister(LSM6DSRXDefs::REG_TIMESTAMP3) << 24;
+
+ return value;
+}
+
+LSM6DSRXData LSM6DSRX::getSensorData()
+{
+ D(assert(isInit && "init() was not called"));
+
+ LSM6DSRXData data;
+
+ getAccelerometerData(data);
+ getGyroscopeData(data);
+
+ return data;
+}
+
+bool LSM6DSRX::selfTest()
+{
+ D(assert(isInit && "init() was not called"));
+
+ isInit = false;
+
+ if (!selfTestAcc() || !selfTestGyr())
+ {
+ lastError = SensorErrors::SELF_TEST_FAIL;
+ return false;
+ }
+
+ return init();
+}
+
+LSM6DSRXData LSM6DSRX::sampleImpl()
+{
+ D(assert(isInit && "init() was not called"));
+ // Reset any errors.
+ lastError = SensorErrors::NO_ERRORS;
+
+ if (config.fifoMode == LSM6DSRXConfig::FIFO_MODE::BYPASS)
+ {
+ return getSensorData();
+ }
+
+ readFromFifo();
+
+ if (lastFifoLevel > 0)
+ {
+ return lastFifo[lastFifoLevel - 1];
+ }
+ else
+ {
+ // no new data
+ lastError = SensorErrors::NO_NEW_DATA;
+ return lastValidSample;
+ }
+}
+
+float LSM6DSRX::getAxisData(LSM6DSRXDefs::Registers lowReg,
+ LSM6DSRXDefs::Registers highReg, float sensitivity)
+{
+ int8_t low = 0, high = 0;
+ int16_t sample = 0;
+
+ SPITransaction transaction{spiSlave};
+
+ high = transaction.readRegister(highReg);
+ low = transaction.readRegister(lowReg);
+
+ sample = combineHighLowBits(low, high);
+
+ float ret = static_cast<float>(sample) * sensitivity;
+ return ret;
+}
+
+int16_t LSM6DSRX::getAxisData(LSM6DSRXDefs::Registers lowReg,
+ LSM6DSRXDefs::Registers highReg)
+{
+ int8_t low = 0, high = 0;
+ int16_t sample = 0;
+
+ SPITransaction transaction{spiSlave};
+
+ high = transaction.readRegister(highReg);
+ low = transaction.readRegister(lowReg);
+
+ sample = combineHighLowBits(low, high);
+
+ return sample;
+}
+
+uint64_t LSM6DSRX::convertTimestamp(const uint64_t sensorTimestamp)
+{
+ uint64_t deltaSensor = 0; // difference between 2 sensor timestamps.
+ if (sensorTimestamp >= sensorTimestamp0)
+ {
+ deltaSensor = sensorTimestamp - sensorTimestamp0;
+ }
+ else
+ {
+ deltaSensor =
+ static_cast<uint64_t>(-1) - sensorTimestamp0 + sensorTimestamp + 1;
+ }
+
+ // delta evaluated from TimestampTimer point of view.
+ uint64_t delta = deltaSensor * sensorTimestampResolution;
+
+ return timestamp0 + delta;
+}
+
+void LSM6DSRX::correlateTimestamps()
+{
+ timestamp0 = TimestampTimer::getTimestamp();
+ sensorTimestamp0 = getSensorTimestamp();
+}
+
+float LSM6DSRX::getSensorTimestampResolution()
+{
+ SPITransaction spi{spiSlave};
+
+ uint8_t value = spi.readRegister(LSM6DSRXDefs::REG_INTERNAL_FREQ_FINE);
+
+ // TS_Res = 1 / (40000 + (0.0015 * INTERNAL_FREQ_FINE * 40000))
+ float TS_Res = 1 / (40000 + (0.0015 * value * 40000));
+ return TS_Res * 1000;
+}
+
+void LSM6DSRX::readFromFifo()
+{
+ SPITransaction spi{spiSlave};
+
+ // get number of sample to read
+ const uint16_t numSamples = unreadDataInFifo();
+
+ /**
+ * Data has 2bits tags that determins the corresponding time slot.
+ *
+ * 00 -> first element of the array (timestamps[0])
+ * 11 -> last element of the array (timestamps[3])
+ *
+ * When a new timestamp is received from the fifo the array is updated.
+ * NOTE: those timestamps are already converted from sensor ones to
+ * TimestampTimer class.
+ */
+ LSM6DSRXDefs::FifoTimeslotData
+ timestamps[LSM6DSRXDefs::FIFO_TIMESLOT_NUMBER] = {LSM6DSRXData(), false,
+ false};
+
+ // read samples from the sensors
+ spi.readRegisters(LSM6DSRXDefs::REG_FIFO_DATA_OUT_TAG,
+ reinterpret_cast<uint8_t*>(rawFifo.data()),
+ numSamples * sizeof(LSM6DSRXDefs::RawFifoData));
+
+ // not all data extracted from fifo is sample data, timestamps are not
+ // saved.
+ // --> `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;
+
+ switch (sensorTag)
+ {
+ case 0x01:
+ // Gyroscope data
+
+ // Set data
+ timestamps[timeslotTag].data.angularSpeedX =
+ static_cast<float>(combineHighLowBits(xl, xh)) *
+ sensitivityGyr;
+ timestamps[timeslotTag].data.angularSpeedY =
+ static_cast<float>(combineHighLowBits(yl, yh)) *
+ sensitivityGyr;
+ timestamps[timeslotTag].data.angularSpeedZ =
+ static_cast<float>(combineHighLowBits(zl, zh)) *
+ sensitivityGyr;
+
+ // Set flag
+ timestamps[timeslotTag].gyrPresent = true;
+
+ // Check if we can push into fifo (both samples are present)
+ if (timestamps[timeslotTag].accPresent)
+ {
+ pushIntoFifo(timestamps[timeslotTag], idxFifo);
+ }
+
+ break;
+ case 0x02:
+ // Accelerometer data
+ timestamps[timeslotTag].data.accelerationX =
+ static_cast<float>(combineHighLowBits(xl, xh)) *
+ sensitivityAcc;
+ timestamps[timeslotTag].data.accelerationY =
+ static_cast<float>(combineHighLowBits(yl, yh)) *
+ sensitivityAcc;
+ timestamps[timeslotTag].data.accelerationZ =
+ static_cast<float>(combineHighLowBits(zl, zh)) *
+ sensitivityAcc;
+
+ timestamps[timeslotTag].accPresent = true;
+
+ // Check if we can push into fifo (both samples are present)
+ if (timestamps[timeslotTag].gyrPresent)
+ {
+ pushIntoFifo(timestamps[timeslotTag], idxFifo);
+ }
+
+ break;
+ case 0x04:
+ // timestamp data --> update timestamps
+
+ uint32_t t =
+ static_cast<uint32_t>(combineHighLowBitsUnsigned(xl, xh));
+ t |= static_cast<uint32_t>(combineHighLowBitsUnsigned(yl, yh))
+ << 16;
+
+ // Set new data
+ timestamps[timeslotTag].data.accelerationTimestamp =
+ convertTimestamp(static_cast<uint64_t>(t));
+ timestamps[timeslotTag].data.angularSpeedTimestamp =
+ timestamps[timeslotTag].data.accelerationTimestamp;
+
+ timestamps[timeslotTag].accPresent = false;
+ timestamps[timeslotTag].gyrPresent = false;
+
+ break;
+ }
+ }
+
+ // update timestamp base values
+ correlateTimestamps();
+
+ lastFifoLevel = idxFifo;
+}
+
+void LSM6DSRX::pushIntoFifo(LSM6DSRXDefs::FifoTimeslotData& timeslot,
+ uint16_t& fifoIdx)
+{
+ // reset flags (done even if data gets discarded)
+ timeslot.accPresent = false;
+ timeslot.gyrPresent = false;
+
+ // check if data can be pushed
+ if ((fifoIdx > 0 && timeslot.data.accelerationTimestamp ==
+ lastFifo[fifoIdx - 1].accelerationTimestamp) ||
+ timeslot.data.accelerationTimestamp == 0)
+ {
+ // the new sample has the same timestamp of the previous one or
+ // timestamp is 0 --> discarded
+ return;
+ }
+
+ // push into fifo and update index
+ lastFifo[fifoIdx] = timeslot.data;
+ ++fifoIdx;
+
+ // update lastValidSample
+ lastValidSample = timeslot.data;
+}
+
+uint16_t LSM6DSRX::unreadDataInFifo()
+{
+ uint16_t ris = 0;
+ SPITransaction spi{spiSlave};
+
+ ris = spi.readRegister(LSM6DSRXDefs::REG_FIFO_STATUS1);
+ ris = ris | (static_cast<uint16_t>(
+ spi.readRegister(LSM6DSRXDefs::REG_FIFO_STATUS2) & 3)
+ << 8);
+ return ris;
+}
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRX.h b/src/shared/sensors/LSM6DSRX/LSM6DSRX.h
new file mode 100644
index 000000000..a13b6802c
--- /dev/null
+++ b/src/shared/sensors/LSM6DSRX/LSM6DSRX.h
@@ -0,0 +1,242 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Fabrizio Monti
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <diagnostic/PrintLogger.h>
+#include <drivers/spi/SPIDriver.h>
+#include <miosix.h>
+#include <sensors/Sensor.h>
+
+#include "LSM6DSRXConfig.h"
+#include "LSM6DSRXData.h"
+#include "LSM6DSRXDefs.h"
+
+namespace Boardcore
+{
+
+/**
+ * @brief LSM6DSRX Driver.
+ */
+class LSM6DSRX : public SensorFIFO<LSM6DSRXData, LSM6DSRXDefs::FIFO_SIZE>
+{
+public:
+ /**
+ * @brief LSM6DSRX constructor.
+ *
+ * @param bus SPI bus.
+ * @param csPin SPI chip select pin.
+ * @param busConfiguration SPI bus configuration.
+ * @param config LSM6DSRX configuration.
+ */
+ LSM6DSRX(SPIBus& bus, miosix::GpioPin csPin, SPIBusConfig busConfiguration,
+ LSM6DSRXConfig& config);
+
+ /**
+ * @brief Initialize the sensor.
+ */
+ bool init() override;
+
+ /**
+ * @brief Performs self test for the sensor.
+ * @return Return true if the test was successful.
+ */
+ bool selfTest() override;
+
+ /**
+ * @brief Returns data from the sensors.
+ */
+ LSM6DSRXData getSensorData();
+
+ /**
+ * @brief Gather data from FIFO/data registers.
+ */
+ LSM6DSRXData sampleImpl() override;
+
+private:
+ bool isInit = false;
+ SPISlave spiSlave;
+ LSM6DSRXConfig config;
+
+ float sensitivityAcc = 0.0; ///< Sensitivity value for the accelerator.
+ float sensitivityGyr = 0.0; ///< Sensitivity value for the gyroscope.
+
+ /**
+ * @brief These two values represent the same instant from the two different
+ * clocks: the one on the sensor and the one from TimestampTimer. They are
+ * used to convert a sensor timestamp to a TimestampTimer one.
+ */
+ uint64_t timestamp0 = 0; ///< Timestamp given by TimestampTimer class.
+ uint32_t sensorTimestamp0 = 0; ///< Timestamp given by the sensor.
+
+ float sensorTimestampResolution =
+ 0.0; ///< Resolution of the sensor's timestamps in microseconds.
+
+ /**
+ * @brief When extracting data from fifo i get data only from one sensor,
+ * but the struct LSM6DSRXData is made to have data from both sensors. The
+ * idea is to copy the value from the last valid sample for the sensor that
+ * hasn't received data.
+ */
+ LSM6DSRXData lastValidSample;
+
+ /**
+ * @brief When extracting samples from fifo data is first read and saved
+ * inside this array, then it get processed and stored inside `lastFifo`.
+ */
+ std::array<LSM6DSRXDefs::RawFifoData, LSM6DSRXDefs::SENSOR_FIFO_SIZE>
+ rawFifo;
+
+ PrintLogger logger = Logging::getLogger("lsm6dsrx");
+
+ /**
+ * @brief Check who_am_i register for validity.
+ *
+ * @return Returns false if not valid.
+ */
+ bool checkWhoAmI();
+
+ /**
+ * @brief Performs a really simple reading from the FIFO buffer.
+ */
+ void readFromFifo();
+
+ /**
+ * @brief Utility function to handle the saving of a sample inside the fifo
+ * `lastFifo`. This operation could fail if the data to be pushed has the
+ * same timestamp of the last value pushed inside `lastFifo` or the
+ * timestamp is 0: in this case nothing happens to the fifo, data inside
+ * `timeslot` is discarded.
+ *
+ * @param timeslot The timeslot containing data to be pushed inside the
+ * fifo.
+ * @param fifoIdx The current index of the fifo, pointing to the current
+ * writable cell. Updated if operation is successful, unchanged otherwise.
+ */
+ void pushIntoFifo(LSM6DSRXDefs::FifoTimeslotData& timeslot,
+ uint16_t& fifoIdx);
+
+ /**
+ * @brief Returns the number of unread data in the fifo.
+ */
+ uint16_t unreadDataInFifo();
+
+ /**
+ * @brief Utility for combining two 8 bits numbers in one 16 bits number.
+ * @param low Low bits of the 16 bit number.
+ * @param high High bits of the 16 bit number.
+ */
+ int16_t combineHighLowBits(uint8_t low, uint8_t high);
+
+ /**
+ * @brief Utility for combining two 8 bits numbers in one 16 bits number.
+ * @param low Low bits of the 16 bit number.
+ * @param high High bits of the 16 bit number.
+ */
+ uint16_t combineHighLowBitsUnsigned(uint8_t low, uint8_t high);
+
+ /**
+ * @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);
+
+ /**
+ * @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);
+
+ /**
+ * @brief Initialize the accelerometer.
+ */
+ void initAccelerometer();
+
+ /**
+ * @brief Initialize the gyroscope.
+ */
+ void initGyroscope();
+
+ /**
+ * @brief Initialize fifo.
+ */
+ void initFifo();
+
+ /**
+ * @brief Initialize interrupts.
+ */
+ void initInterrupts();
+
+ /**
+ * @brief Performs self test for the accelerometer.
+ * @return Return true if successful.
+ */
+ bool selfTestAcc();
+
+ /**
+ * @brief Performs self test for the gyroscope.
+ * @return Return true if successful.
+ */
+ bool selfTestGyr();
+
+ /**
+ * @brief Retrieves data from the accelerometer.
+ * @param data The structure where data from the sensor is to be saved.
+ */
+ void getAccelerometerData(LSM6DSRXData& data);
+
+ /**
+ * @brief Retrieves data from the gyroscope.
+ * @param data The structure where data from the sensor is to be saved.
+ */
+ void getGyroscopeData(LSM6DSRXData& data);
+
+ /**
+ * @brief Converts timestamp from the value given by the sensor to the one
+ * given by TimestampTimer class.
+ * @param sensorTimestamp The timestamp from the sensor to be converted into
+ * a TimestampTimer one.
+ */
+ uint64_t convertTimestamp(const uint64_t sensorTimestamp);
+
+ /**
+ * @brief Utility to set timestamp values for conversion.
+ */
+ void correlateTimestamps();
+
+ /**
+ * @brief Returns the timestamp from the sensor.
+ */
+ uint32_t getSensorTimestamp();
+
+ /**
+ * @brief Returns the timestamp resolution in milliseconds.
+ */
+ float getSensorTimestampResolution();
+};
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRXConfig.h b/src/shared/sensors/LSM6DSRX/LSM6DSRXConfig.h
new file mode 100644
index 000000000..2db415c0f
--- /dev/null
+++ b/src/shared/sensors/LSM6DSRX/LSM6DSRXConfig.h
@@ -0,0 +1,210 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Fabrizio Monti
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+namespace Boardcore
+{
+
+struct LSM6DSRXConfig
+{
+
+ /**
+ * @brief Output data rate definitions for the accelerometer.
+ *
+ * If OPERATING_MODE is set to NORMAL, the power mode depends on the choosen
+ * odr.
+ */
+ enum class ACC_ODR : uint8_t
+ {
+ POWER_DOWN = 0, ///< Disabled
+ HZ_1_6 = 11, ///< ODR of 1.6 Hz - AVAILABLE ONLY IN LOW POWER MODE
+ HZ_12_5 = 1, ///< ODR of 12.5 Hz - low power
+ HZ_26 = 2, ///< ODR of 26 Hz - low power
+ HZ_52 = 3, ///< ODR of 52 Hz - low power
+ HZ_104 = 4, ///< ODR of 104 Hz - normal
+ HZ_208 = 5, ///< ODR of 208 Hz - normal
+ HZ_416 = 6, ///< ODR of 416 Hz - high performance
+ HZ_833 = 7, ///< ODR of 833 Hz - high performance
+ HZ_1660 = 8, ///< ODR of 1.66 kHz - high performance
+ HZ_3330 = 9, ///< ODR of 3.33 kHz - high performance
+ HZ_6660 = 10, ///< ODR of 6.66 kHz - high performance
+ };
+
+ /**
+ * @brief Output data rate definitions for the gyroscope.
+ *
+ * If OPERATING_MODE is set to NORMAL, the power mode depends on the choosen
+ * odr.
+ */
+ enum class GYR_ODR : uint8_t
+ {
+ POWER_DOWN = 0, ///< Disabled
+ HZ_12_5 = 1, ///< ODR of 12.5 Hz - low power
+ HZ_26 = 2, ///< ODR of 26 Hz - low power
+ HZ_52 = 3, ///< ODR of 52 Hz - low power
+ HZ_104 = 4, ///< ODR of 104 Hz - normal
+ HZ_208 = 5, ///< ODR of 208 Hz - normal
+ HZ_416 = 6, ///< ODR of 416 Hz - high performance
+ HZ_833 = 7, ///< ODR of 833 Hz - high performance
+ HZ_1660 = 8, ///< ODR of 1.66 kHz - high performance
+ HZ_3330 = 9, ///< ODR of 3.33 kHz - high performance
+ HZ_6660 = 10, ///< ODR of 6.66 kHz - high performance
+ };
+
+ /**
+ * @brief Fullscale values for the accelerometer.
+ */
+ enum class ACC_FULLSCALE : uint8_t
+ {
+ G2 = 0,
+ G4 = 2,
+ G8 = 3,
+ G16 = 1,
+ };
+
+ /**
+ * @brief Fullscale values for the gyroscope.
+ */
+ enum class GYR_FULLSCALE : uint8_t
+ {
+ DPS_125 = 0b0010,
+ DPS_250 = 0,
+ DPS_500 = 0b0100,
+ DPS_1000 = 0b1000,
+ DPS_2000 = 0b1100,
+ DPS_4000 = 0b0001,
+ };
+
+ /**
+ * @brief Data update mode for the sensor.
+ */
+ enum class BDU : uint8_t
+ {
+ CONTINUOUS_UPDATE = 0,
+ UPDATE_AFTER_READ = 1, ///< Output registers are not updated until MSB
+ ///< and LSB have been read
+ };
+
+ /**
+ * @brief Operating mode for the sensor.
+ *
+ * The sensor operates in 4 modes: power_down, low_power, normal,
+ * high_performance. high_performance is valid for all the odr values. If
+ * NORMAL mode is selected, the sensor behaviour depends on the selected
+ * odr.
+ */
+ enum class OPERATING_MODE : uint8_t
+ {
+ HIGH_PERFORMANCE = 0, ///< Valid for all odrs
+ NORMAL = 1, ///< Works in low power or normal mode depending on the odr
+ };
+
+ /**
+ * @brief Fifo operating mode.
+ */
+ enum class FIFO_MODE : uint8_t
+ {
+ BYPASS = 0, ///< Fifo disabled.
+ FIFO = 1, ///< Stops collecting data when FIFO is full.
+ CONTINUOUS = 6, ///< If the FIFO is full, the new sample overwrites the
+ ///< older one.
+ };
+
+ /**
+ * @brief Selects decimation for timestamp batching in FIFO. Write rate will
+ * be the maximum rate between accelerometer and gyro BDR divided by
+ * decimation decoder.
+ */
+ enum class FIFO_TIMESTAMP_DECIMATION : uint8_t
+ {
+ DEC_1 = 1, ///< max(BDR_XL[Hz],BDR_GY[Hz]) [Hz]
+
+ // Those options below are disabled, because without DEC_1 the driver
+ // would not work properly (multiple samples would share the same
+ // timestamp or have none)
+
+ // DISABLED = 0, ///< Timestamp not batched in FIFO.
+ // DEC_8 = 2, ///< max(BDR_XL[Hz],BDR_GY[Hz]) / 8 [Hz]
+ // DEC_32 = 3, ///< max(BDR_XL[Hz],BDR_GY[Hz]) / 32 [Hz]
+ };
+
+ /**
+ * @brief Selects batch data rate in FIFO for temperature data.
+ */
+ 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
+ };
+
+ /**
+ * @brief Selectable interrupts. You can combine multiple interrupts with
+ * bitwise or.
+ */
+ enum class INTERRUPT : uint8_t
+ {
+ NOTHING = 0, ///< no interrupt selected
+ ACC_DRDY = 1, ///< Accelerometer data ready.
+ GYR_DRDY = 2, ///< Gyroscope data ready.
+ FIFO_THRESHOLD = 8, ///< FIFO threshold interrupt.
+ FIFO_OVERRUN = 16, ///< FIFO overrun interrupt.
+ FIFO_FULL = 32, ///< FIFO full interrupt.
+ };
+
+ BDU bdu; ///< Data update mode.
+
+ // accelerometer
+ ACC_ODR odrAcc; ///< Accelerometer odr.
+ OPERATING_MODE opModeAcc; ///< Operating mode for the accelerometer.
+ ACC_FULLSCALE fsAcc; ///< Fullscale selection for the accelerometer.
+
+ // gyroscope
+ GYR_ODR odrGyr; ///< Accelerometer odr.
+ OPERATING_MODE opModeGyr; ///< Operating mode for the accelerometer.
+ GYR_FULLSCALE fsGyr; ///< Fullscale selection for the accelerometer.
+
+ // fifo
+ FIFO_MODE fifoMode; ///< Fifo operating mode.
+ FIFO_TIMESTAMP_DECIMATION
+ fifoTimestampDecimation; ///< decimation for timestamp sampling in FIFO.
+ FIFO_TEMPERATURE_BDR
+ fifoTemperatureBdr; ///< batch data rate for temperature data in FIFO.
+
+ // interrupt
+ INTERRUPT int1InterruptSelection; ///< interrupts selected on INT1 pin.
+ INTERRUPT int2InterruptSelection; ///< interrupts selected on INT2 pin.
+
+ /**
+ * @brief Fifo watermark level, expressed as number of samples.
+ * Max value is 170.
+ */
+ uint16_t fifoWatermark;
+};
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRXData.h b/src/shared/sensors/LSM6DSRX/LSM6DSRXData.h
new file mode 100644
index 000000000..0cd5083fb
--- /dev/null
+++ b/src/shared/sensors/LSM6DSRX/LSM6DSRXData.h
@@ -0,0 +1,63 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Fabrizio Monti
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <sensors/SensorData.h>
+
+namespace Boardcore
+{
+
+struct LSM6DSRXData : public AccelerometerData, public GyroscopeData
+{
+ LSM6DSRXData()
+ : AccelerometerData{0, 0.0, 0.0, 0.0}, GyroscopeData{0, 0.0, 0.0, 0.0}
+ {
+ }
+
+ static std::string header()
+ {
+ return "accelerationTimestamp,accelerationX,accelerationY,"
+ "accelerationZ,angularSpeedTimestamp,angularSpeedX,"
+ "angularSpeedY,angularSpeedZ\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ 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";
+ }
+};
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/LSM6DSRX/LSM6DSRXDefs.h b/src/shared/sensors/LSM6DSRX/LSM6DSRXDefs.h
new file mode 100644
index 000000000..43d0dd63a
--- /dev/null
+++ b/src/shared/sensors/LSM6DSRX/LSM6DSRXDefs.h
@@ -0,0 +1,188 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Fabrizio Monti
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+namespace Boardcore
+{
+
+namespace LSM6DSRXDefs
+{
+
+/**
+ * @brief Driver's fifo size expressed as number of samples.
+ */
+const uint16_t FIFO_SIZE = 200;
+
+/**
+ * @brief Size of the sensor's internal fifo.
+ */
+const uint16_t SENSOR_FIFO_SIZE = 512;
+
+/**
+ * @brief Sensor who_am_i register value.
+ */
+const uint8_t WHO_AM_I_VALUE = 0x6B;
+
+/**
+ * @brief Lower bound value for accelerometer self test.
+ */
+const float ACC_SELF_TEST_MIN = 40.0;
+
+/**
+ * @brief Upper bound value for accelerometer self test.
+ */
+const float ACC_SELF_TEST_MAX = 1700.0;
+
+/**
+ * @brief Lower bound value for gyroscope self test.
+ * Valid for self test fullscale = 2000dps.
+ */
+const float GYR_SELF_TEST_MIN = 150.0;
+
+/**
+ * @brief Upper bound value for gyroscope self test.
+ * Valid for self test fullscale = 2000dps.
+ */
+const float GYR_SELF_TEST_MAX = 700.0;
+
+/**
+ * @brief Number of timeslots available when extracting
+ * data from fifo.
+ */
+const uint8_t FIFO_TIMESLOT_NUMBER = 4;
+
+/**
+ * @brief Internal registers definitions.
+ */
+enum Registers
+{
+ REG_WHO_AM_I = 0x0F, ///< who_am_i register
+
+ REG_CTRL1_XL = 0x10, ///< accelerometer control register
+ REG_CTRL2_G = 0x11, ///< gyroscope control register
+ REG_CTRL3_C = 0x12, ///< set bdu
+ REG_CTRL6_C = 0x15, ///< enable/disable high performance mode for the
+ ///< accelerometer
+ REG_CTRL7_G = 0x16, ///< enable/disable high performance mode for the
+ ///< gyroscope
+
+ REG_FIFO_CTRL1 = 0x07,
+ REG_FIFO_CTRL2 = 0x08,
+
+ REG_FIFO_CTRL3 = 0x09, ///< fifo control register 3 (select batch data
+ ///< rate for gyro and acc)
+ REG_FIFO_CTRL4 = 0x0A, ///< fifo control register 4 (select fifo mode,
+ ///< batch data rate for temperature sensor and the
+ ///< decimation factor for timestamp batching)
+
+ REG_FIFO_STATUS1 =
+ 0x3A, ///< Gives number of unread sensor data stored in FIFO.
+ REG_FIFO_STATUS2 = 0x3B, ///< Gives number of unread sensor data and
+ ///< the current status (watermark, overrun,
+ ///< full, BDR counter) of the FIFO.
+
+ REG_STATUS = 0x1E, ///< data ready register.
+ REG_CTRL4_C = 0x13,
+ REG_CTRL5_C = 0x14,
+ REG_CTRL8_XL = 0x17,
+ REG_CTRL9_XL = 0x18,
+ REG_CTRL10_C = 0x19,
+
+ REG_FIFO_DATA_OUT_TAG = 0x78,
+ REG_FIFO_DATA_OUT_X_L = 0x79,
+ REG_FIFO_DATA_OUT_X_H = 0x7A,
+ REG_FIFO_DATA_OUT_Y_L = 0x7B,
+ REG_FIFO_DATA_OUT_Y_H = 0x7C,
+ REG_FIFO_DATA_OUT_Z_L = 0x7D,
+ REG_FIFO_DATA_OUT_Z_H = 0x7E,
+
+ REG_OUTX_L_A =
+ 0x28, ///< Low bits output register for the accelerometer (x axis)
+ REG_OUTX_H_A =
+ 0x29, ///< High bits output register for the accelerometer (x axis)
+ REG_OUTY_L_A =
+ 0x2A, ///< Low bits output register for the accelerometer (y axis)
+ REG_OUTY_H_A =
+ 0x2B, ///< High bits output register for the accelerometer (y axis)
+ REG_OUTZ_L_A =
+ 0x2C, ///< Low bits output register for the accelerometer (z axis)
+ REG_OUTZ_H_A =
+ 0x2D, ///< High bits output register for the accelerometer (z axis)
+
+ REG_OUTX_L_G =
+ 0x22, ///< Low bits output register for the gyroscope (x axis)
+ REG_OUTX_H_G =
+ 0x23, ///< High bits output register for the gyroscope (x axis)
+ REG_OUTY_L_G =
+ 0x24, ///< Low bits output register for the gyroscope (y axis)
+ REG_OUTY_H_G =
+ 0x25, ///< High bits output register for the gyroscope (y axis)
+ REG_OUTZ_L_G =
+ 0x26, ///< Low bits output register for the gyroscope (z axis)
+ REG_OUTZ_H_G =
+ 0x27, ///< High bits output register for the gyroscope (z axis)
+
+ REG_TIMESTAMP0 = 0x40,
+ REG_TIMESTAMP1 = 0x41,
+ REG_TIMESTAMP2 = 0x42,
+ REG_TIMESTAMP3 = 0x43,
+
+ REG_INTERNAL_FREQ_FINE = 0x63,
+
+ REG_INT1_CTRL = 0x0D, ///< set interrupts on INT1 pin.
+ REG_INT2_CTRL = 0x0E, ///< set interrupts on INT2 pin.
+};
+
+/**
+ * @brief Structure used to store data from fifo before being processed.
+ */
+struct RawFifoData
+{
+ uint8_t sampleTag; ///< Tag used to identify data
+ uint8_t xl; ///< low part of DATA_OUT_X register
+ uint8_t xh; ///< high part of DATA_OUT_X register
+ uint8_t yl; ///< low part of DATA_OUT_Y register
+ uint8_t yh; ///< high part of DATA_OUT_Y register
+ uint8_t zl; ///< low part of DATA_OUT_Z register
+ uint8_t zh; ///< high part of DATA_OUT_Z register
+};
+
+/**
+ * @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.
+ */
+struct FifoTimeslotData
+{
+ LSM6DSRXData data;
+ bool accPresent = false;
+ bool gyrPresent = false;
+};
+
+} // namespace LSM6DSRXDefs
+
+} // namespace Boardcore
diff --git a/src/tests/sensors/test-lsm6dsrx.cpp b/src/tests/sensors/test-lsm6dsrx.cpp
new file mode 100644
index 000000000..ebaa8d668
--- /dev/null
+++ b/src/tests/sensors/test-lsm6dsrx.cpp
@@ -0,0 +1,310 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Fabrizio Monti
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <drivers/spi/SPIDriver.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <miosix.h>
+#include <sensors/LSM6DSRX/LSM6DSRX.h>
+#include <utils/Debug.h>
+
+#include <algorithm>
+#include <iostream>
+#include <memory>
+#include <vector>
+
+using namespace Boardcore;
+using namespace miosix;
+
+/**
+ * @brief Test how much time it takes to fill completely the fifo from empty.
+ */
+void testFifoFillingTime(SPIBus& bus, miosix::GpioPin csPin,
+ SPIBusConfig busConfiguration, LSM6DSRXConfig& config,
+ miosix::GpioPin intPin);
+
+/**
+ * @brief Test the execution time of sampleImpl().
+ */
+void testSampleImplTime(SPIBus& bus, miosix::GpioPin csPin,
+ SPIBusConfig busConfiguration, LSM6DSRXConfig& config);
+
+/**
+ * @brief Test fifo read.
+ */
+void testFifoRead(SPIBus& bus, miosix::GpioPin csPin,
+ SPIBusConfig busConfiguration, LSM6DSRXConfig& config,
+ miosix::GpioPin intPin);
+
+int main()
+{
+
+ SPIBus bus(SPI3);
+
+ GpioPin csPin(GPIOE_BASE, 3); // PE3 CS
+ csPin.mode(Mode::OUTPUT);
+
+ GpioPin clockPin(GPIOB_BASE, 3); // PB3 CK (SCL)
+ clockPin.mode(Mode::ALTERNATE);
+ clockPin.alternateFunction(6);
+ GpioPin misoPin(GPIOB_BASE, 4); // PB4 MISO (SDO)
+ misoPin.alternateFunction(6);
+ misoPin.mode(Mode::ALTERNATE);
+ GpioPin mosiPin(GPIOB_BASE, 5); // PB5 MOSI (SDA)
+ mosiPin.alternateFunction(6);
+ mosiPin.mode(Mode::ALTERNATE);
+
+ GpioPin int1Pin(GPIOC_BASE, 15); // PC15 interrupt pin 1
+ int1Pin.mode(Mode::INPUT);
+ GpioPin int2Pin(GPIOC_BASE, 13); // PC13 interrupt pin 2
+ int2Pin.mode(Mode::INPUT);
+
+ SPIBusConfig busConfiguration; // Bus configuration for the sensor
+ busConfiguration.clockDivider = SPI::ClockDivider::DIV_2;
+ busConfiguration.mode =
+ SPI::Mode::MODE_0; // Set clock polarity to 0 and phase to 1
+
+ LSM6DSRXConfig sensConfig;
+ sensConfig.bdu = LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE;
+
+ // acc
+ sensConfig.fsAcc = LSM6DSRXConfig::ACC_FULLSCALE::G2;
+ sensConfig.odrAcc = LSM6DSRXConfig::ACC_ODR::HZ_833;
+ sensConfig.opModeAcc = LSM6DSRXConfig::OPERATING_MODE::NORMAL;
+
+ // gyr
+ sensConfig.fsGyr = LSM6DSRXConfig::GYR_FULLSCALE::DPS_125;
+ sensConfig.odrGyr = LSM6DSRXConfig::GYR_ODR::HZ_833;
+ 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;
+
+ // interrupt
+ sensConfig.int1InterruptSelection = LSM6DSRXConfig::INTERRUPT::NOTHING;
+ sensConfig.int2InterruptSelection =
+ LSM6DSRXConfig::INTERRUPT::FIFO_THRESHOLD;
+ sensConfig.fifoWatermark = 170;
+
+ testFifoRead(bus, csPin, busConfiguration, sensConfig, int2Pin);
+
+ // testSampleImplTime(bus, csPin, busConfiguration, sensConfig);
+
+ // testFifoFillingTime(bus, csPin, busConfiguration, sensConfig, int2Pin);
+
+ while (true)
+ {
+ Thread::sleep(5000);
+ }
+
+ return 0;
+}
+
+void testFifoFillingTime(SPIBus& bus, miosix::GpioPin csPin,
+ SPIBusConfig busConfiguration,
+ LSM6DSRXConfig& sensConfig, miosix::GpioPin intPin)
+{
+ struct odrs
+ {
+ LSM6DSRXConfig::ACC_ODR acc;
+ LSM6DSRXConfig::GYR_ODR gyr;
+ };
+
+ odrs arr[] = {
+ {LSM6DSRXConfig::ACC_ODR::HZ_12_5, LSM6DSRXConfig::GYR_ODR::HZ_12_5},
+ {LSM6DSRXConfig::ACC_ODR::HZ_26, LSM6DSRXConfig::GYR_ODR::HZ_26},
+ {LSM6DSRXConfig::ACC_ODR::HZ_52, LSM6DSRXConfig::GYR_ODR::HZ_52},
+ {LSM6DSRXConfig::ACC_ODR::HZ_104, LSM6DSRXConfig::GYR_ODR::HZ_104},
+ {LSM6DSRXConfig::ACC_ODR::HZ_208, LSM6DSRXConfig::GYR_ODR::HZ_208},
+ {LSM6DSRXConfig::ACC_ODR::HZ_416, LSM6DSRXConfig::GYR_ODR::HZ_416},
+ {LSM6DSRXConfig::ACC_ODR::HZ_833, LSM6DSRXConfig::GYR_ODR::HZ_833},
+ {LSM6DSRXConfig::ACC_ODR::HZ_1660, LSM6DSRXConfig::GYR_ODR::HZ_1660},
+ {LSM6DSRXConfig::ACC_ODR::HZ_3330, LSM6DSRXConfig::GYR_ODR::HZ_3330},
+ {LSM6DSRXConfig::ACC_ODR::HZ_6660, LSM6DSRXConfig::GYR_ODR::HZ_6660},
+ };
+
+ for (unsigned int odrIdx = 0; odrIdx < sizeof(arr) / sizeof(odrs); ++odrIdx)
+ {
+ sensConfig.odrAcc = arr[odrIdx].acc;
+ sensConfig.odrGyr = arr[odrIdx].gyr;
+
+ std::unique_ptr<LSM6DSRX> sens = std::make_unique<LSM6DSRX>(
+ bus, csPin, busConfiguration, sensConfig);
+ if (sens->init() == false)
+ {
+ while (true)
+ {
+ TRACE("Error, sensor not initialized at index %d\n\n", odrIdx);
+ Thread::sleep(2000);
+ }
+ }
+
+ // empty the fifo
+ sens->sampleImpl();
+
+ // test time needed to fill the fifo
+ uint64_t t0 = TimestampTimer::getTimestamp();
+ int dataReady = intPin.value();
+ while (dataReady != 1)
+ {
+ Thread::sleep(1);
+ dataReady = intPin.value();
+ }
+ uint64_t t1 = TimestampTimer::getTimestamp();
+
+ uint64_t diff = t1 - t0;
+
+ std::cout << odrIdx << ") Filling time(us): " << diff << "\n\n";
+ }
+}
+
+void testSampleImplTime(SPIBus& bus, miosix::GpioPin csPin,
+ SPIBusConfig busConfiguration,
+ LSM6DSRXConfig& sensConfig)
+{
+ std::unique_ptr<LSM6DSRX> sens =
+ std::make_unique<LSM6DSRX>(bus, csPin, busConfiguration, sensConfig);
+
+ if (sens->init() == false)
+ {
+ while (true)
+ {
+ TRACE("Error, sensor not initialized\n\n");
+ Thread::sleep(2000);
+ }
+ }
+
+ if (sens->selfTest())
+ {
+ TRACE("Self test successful\n\n");
+ Thread::sleep(2000);
+ }
+ else
+ {
+ TRACE("Self test failed\n\n");
+ while (true)
+ {
+ Thread::sleep(2000);
+ }
+ }
+
+ std::cout << "sensor initialized\n";
+ while (true)
+ {
+ uint64_t t0 = TimestampTimer::getTimestamp();
+
+ auto d = sens->sampleImpl();
+
+ uint64_t t1 = TimestampTimer::getTimestamp();
+
+ uint64_t diff = t1 - t0;
+
+ std::cout << "sampleImpl() execution time(us): " << diff << "\n";
+ std::cout << "last fifo sample:\n";
+ d.print(std::cout);
+ std::cout << "\n\n\n";
+
+ miosix::Thread::sleep(1000);
+ }
+}
+
+void testFifoRead(SPIBus& bus, miosix::GpioPin csPin,
+ SPIBusConfig busConfiguration, LSM6DSRXConfig& config,
+ miosix::GpioPin intPin)
+{
+ std::unique_ptr<LSM6DSRX> sens =
+ std::make_unique<LSM6DSRX>(bus, csPin, busConfiguration, config);
+
+ if (sens->init() == false)
+ {
+ while (true)
+ {
+ std::cout << "Error, sensor not initialized\n\n";
+ Thread::sleep(2000);
+ }
+ }
+
+ if (sens->selfTest())
+ {
+ std::cout << "Self test successful\n\n";
+ Thread::sleep(2000);
+ }
+ else
+ {
+ std::cout << "Self test failed\n\n";
+ while (true)
+ {
+ Thread::sleep(2000);
+ }
+ }
+
+ while (true)
+ {
+ // wait for fifo full interrupt
+ int dataReady = intPin.value();
+ while (dataReady != 1)
+ {
+ Thread::sleep(20);
+ dataReady = intPin.value();
+ }
+
+ sens->sample();
+
+ const std::array<LSM6DSRXData, LSM6DSRXDefs::FIFO_SIZE>& buf =
+ sens->getLastFifo();
+
+ // Print fifo
+ std::cout << "last fifo element:\n";
+ buf[sens->getLastFifoSize() - 1].print(std::cout);
+ std::cout << "last fifo size: " << sens->getLastFifoSize() << "\n";
+
+ // Check fifo data
+ for (uint16_t i = 1; i < sens->getLastFifoSize(); ++i)
+ {
+ // Check for accelerometer timestamps
+ if (buf[i].accelerationTimestamp <=
+ buf[i - 1].accelerationTimestamp)
+ {
+ std::cout << "Error, accelerometer data not ordered\n\n";
+ }
+
+ // Check for gyroscope timestamps
+ if (buf[i].angularSpeedTimestamp <=
+ buf[i - 1].angularSpeedTimestamp)
+ {
+ std::cout << "Error, gyroscope data not ordered\n\n";
+ }
+
+ // Check that gyr and acc timestamps are equal
+ if (buf[i].accelerationTimestamp != buf[i].angularSpeedTimestamp)
+ {
+ std::cout << "Error, timestamps not equal\n\n";
+ }
+ }
+
+ std::cout << "Extraction completed\n\n" << std::endl;
+ Thread::sleep(1000);
+ }
+}
--
GitLab