diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index a38c686066fed81df16df3fd247bf487af8f56e9..c8a098bfa534801c17a0535e20143685b34fbdda 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -75,6 +75,7 @@ foreach(OPT_BOARD ${BOARDS})
${SBS_BASE}/src/shared/sensors/BMX160/BMX160.cpp
${SBS_BASE}/src/shared/sensors/BMX160/BMX160WithCorrection.cpp
${SBS_BASE}/src/shared/sensors/HX711/HX711.cpp
+ ${SBS_BASE}/src/shared/sensors/LIS3MDL/LIS3MDL.cpp
${SBS_BASE}/src/shared/sensors/calibration/SensorDataExtra.cpp
${SBS_BASE}/src/shared/sensors/MAX6675/MAX6675.cpp
${SBS_BASE}/src/shared/sensors/MAX31855/MAX31855.cpp
diff --git a/src/shared/sensors/BMP280/BMP280.h b/src/shared/sensors/BMP280/BMP280.h
index 8b3263dfaa376c6108bec7bf03ef154bace53423..5786a0c4854d033077e8f91249c6fec5b740e0d0 100644
--- a/src/shared/sensors/BMP280/BMP280.h
+++ b/src/shared/sensors/BMP280/BMP280.h
@@ -242,7 +242,7 @@ private:
*/
bool checkWhoAmI();
- enum BMP280Registers : uint8_t
+ enum Registers : uint8_t
{
REG_CALIB_0 = 0x88,
// Calibration register 1-25
diff --git a/src/shared/sensors/LIS3MDL/LIS3MDL.cpp b/src/shared/sensors/LIS3MDL/LIS3MDL.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..10e257b61c226c10acbf6d51cb85b1d79825970a
--- /dev/null
+++ b/src/shared/sensors/LIS3MDL/LIS3MDL.cpp
@@ -0,0 +1,325 @@
+/* Copyright (c) 2020 Skyward Experimental Rocketry
+ * Author: Riccardo Musso
+ *
+ * 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 "LIS3MDL.h"
+
+#include <drivers/timer/TimestampTimer.h>
+
+namespace Boardcore
+{
+
+LIS3MDL::LIS3MDL(SPIBusInterface& bus, miosix::GpioPin pin,
+ SPIBusConfig spiConfig, Config config)
+ : mSlave(bus, pin, spiConfig), mConfig(config), currDiv(0),
+ isInitialized(false)
+{
+}
+
+bool LIS3MDL::init()
+{
+ if (isInitialized)
+ {
+ LOG_ERR(logger, "Attempted to initialized sensor twice but failed");
+ lastError = ALREADY_INIT;
+ return false;
+ }
+
+ {
+ SPITransaction spi(mSlave);
+ uint8_t res = spi.readRegister(WHO_AM_I);
+
+ if (res != WHO_AM_I_VALUE)
+ {
+ LOG_ERR(logger,
+ "WHO_AM_I value differs from expectation: read 0x{02x} "
+ "but expected 0x{02x}",
+ res, WHO_AM_I_VALUE);
+ lastError = INVALID_WHOAMI;
+ return false;
+ }
+ }
+
+ isInitialized = true;
+ return applyConfig(mConfig);
+}
+
+bool LIS3MDL::selfTest()
+{
+ if (!isInitialized)
+ {
+ LOG_ERR(logger, "Invoked selfTest() but sensor was uninitialized");
+ lastError = NOT_INIT;
+ return false;
+ }
+
+ // NUM_SAMPLES: # of samples used to take the average value before tests
+ constexpr int NUM_SAMPLES = 5;
+
+ // NUM_TESTS: # of actual tests
+ constexpr int NUM_TESTS = 5;
+
+ // SLEEP_TIME: milliseconds between samples/tests
+ constexpr int SLEEP_TIME = 50;
+
+ // Absolute value of extra tolerance
+ constexpr float t = 0.1f;
+
+ // Range which delta must be between, one for axis and expressed as {min,
+ // max}. The unit is gauss.
+ constexpr float r[3][2] = {{1.f, 3.f}, {1.f, 3.f}, {0.1f, 1.f}};
+
+ float avgX = 0.f, avgY = 0.f, avgZ = 0.f;
+
+ {
+ SPITransaction spi(mSlave);
+ spi.writeRegister(CTRL_REG2, FS_12_GAUSS);
+ }
+ updateUnit(FS_12_GAUSS);
+
+ for (int i = 0; i < NUM_SAMPLES; ++i)
+ {
+ miosix::Thread::sleep(SLEEP_TIME);
+
+ LIS3MDLData lastData = sampleImpl();
+ avgX += lastData.magneticFieldX;
+ avgY += lastData.magneticFieldY;
+ avgZ += lastData.magneticFieldZ;
+ }
+
+ avgX /= NUM_SAMPLES;
+ avgY /= NUM_SAMPLES;
+ avgZ /= NUM_SAMPLES;
+
+ // Setting up the sensor settings for proper usage of the self test mode.
+ {
+ SPITransaction spi(mSlave);
+
+ spi.writeRegister(CTRL_REG1, ODR_20_HZ | ENABLE_SELF_TEST |
+ (OM_ULTRA_HIGH_POWER << 4));
+ spi.writeRegister(CTRL_REG2, FS_12_GAUSS);
+ spi.writeRegister(CTRL_REG4, OM_ULTRA_HIGH_POWER << 2);
+ }
+
+ // Deltas: absolute difference between the values measured before and after
+ float d[3];
+
+ for (int i = 0; i < NUM_TESTS; ++i)
+ {
+ miosix::Thread::sleep(SLEEP_TIME);
+
+ LIS3MDLData lastData = sampleImpl();
+ d[0] = std::abs(lastData.magneticFieldX - avgX);
+ d[1] = std::abs(lastData.magneticFieldY - avgY);
+ d[2] = std::abs(lastData.magneticFieldZ - avgZ);
+
+ bool passed = true;
+ for (int j = 0; j < 3; ++j)
+ if (d[j] < (r[j][0] - t) || d[j] > (r[j][1] + t))
+ passed = false;
+
+ if (!passed)
+ {
+ // reset configuration, then return
+ applyConfig(mConfig);
+
+ lastError = SELF_TEST_FAIL;
+ return false;
+ }
+ }
+
+ return applyConfig(mConfig);
+}
+
+bool LIS3MDL::applyConfig(Config config)
+{
+
+ SPITransaction spi(mSlave);
+ uint8_t reg = 0, err = 0;
+
+ mConfig = config;
+ currDiv = 0;
+
+ // CTRL_REG1
+ if (config.enableTemperature)
+ {
+ reg = ENABLE_TEMPERATURE;
+ }
+ reg |= config.odr;
+
+ // odr <= 80Hz
+ if (!(config.odr & FAST_ODR_BIT))
+ reg |= config.xyMode << 4;
+ spi.writeRegister(CTRL_REG1, reg);
+ err |= spi.readRegister(CTRL_REG1) != reg;
+
+ // CTRL_REG2
+ reg = config.scale;
+ spi.writeRegister(CTRL_REG2, reg);
+ err |= spi.readRegister(CTRL_REG2) != reg;
+
+ // CTRL_REG3
+ reg = CONTINUOS_CONVERSION;
+ spi.writeRegister(CTRL_REG3, reg);
+ err |= spi.readRegister(CTRL_REG3) != reg;
+
+ // CTRL_REG4
+ reg = config.zMode << 2;
+ spi.writeRegister(CTRL_REG4, reg);
+ err |= spi.readRegister(CTRL_REG4) != reg;
+
+ // CTRL_REG5
+ if (config.doBlockDataUpdate)
+ reg = ENABLE_BDU;
+ else
+ reg = 0;
+
+ spi.writeRegister(CTRL_REG5, reg);
+ err |= spi.readRegister(CTRL_REG5) != reg;
+
+ // INT_CFG
+ if (config.enableInterrupt[0])
+ reg = ENABLE_INT_X;
+ else
+ reg = 0;
+
+ if (config.enableInterrupt[1])
+ reg |= ENABLE_INT_Y;
+ if (config.enableInterrupt[2])
+ reg |= ENABLE_INT_Z;
+
+ // The interrupt of at least one axis is enabled
+ if (reg)
+ reg |= ENABLE_INT_PIN;
+
+ reg |= 0x08;
+ spi.writeRegister(INT_CFG, reg);
+ err |= spi.readRegister(INT_CFG) != reg;
+
+ // INT_THS
+ uint16_t val = static_cast<uint16_t>(std::abs(config.threshold / mUnit));
+ reg = static_cast<uint8_t>(0xff & val);
+ spi.writeRegister(INT_THS_L, reg);
+ err |= spi.readRegister(INT_THS_L) != reg;
+
+ reg = static_cast<uint8_t>(val >> 8);
+ reg &= 0x7f; // Remove MSB (according to the datasheet, it must be zero)
+ spi.writeRegister(INT_THS_H, reg);
+ err |= spi.readRegister(INT_THS_H) != reg;
+
+ // Set mUnit according to scale
+ updateUnit(config.scale);
+
+ if (err)
+ {
+ LOG_ERR(logger, "Spi error");
+ lastError = BUS_FAULT;
+ return false;
+ }
+
+ return true;
+}
+
+LIS3MDLData LIS3MDL::sampleImpl()
+{
+ if (!isInitialized)
+ {
+ LOG_ERR(logger, "Invoked sampleImpl() but sensor was uninitialized");
+ lastError = NOT_INIT;
+ return lastSample;
+ }
+
+ SPITransaction spi(mSlave);
+
+ if (!spi.readRegister(STATUS_REG))
+ {
+ lastError = NO_NEW_DATA;
+ return lastSample;
+ }
+
+ // Reset any error
+ lastError = SensorErrors::NO_ERRORS;
+
+ int16_t val;
+ LIS3MDLData newData{};
+
+ if (mConfig.enableTemperature)
+ {
+ if (currDiv == 0)
+ {
+ val = spi.readRegister(TEMP_OUT_L);
+ val |= spi.readRegister(TEMP_OUT_H) << 8;
+
+ newData.temperatureTimestamp =
+ TimestampTimer::getInstance().getTimestamp();
+ newData.temperature = static_cast<float>(val) / LSB_PER_CELSIUS +
+ REFERENCE_TEMPERATURE;
+ }
+ else
+ {
+ // Keep old value
+ newData.temperature = lastSample.temperature;
+ }
+
+ currDiv = (currDiv + 1) % mConfig.temperatureDivider;
+ }
+
+ newData.magneticFieldTimestamp =
+ TimestampTimer::getInstance().getTimestamp();
+
+ val = spi.readRegister(OUT_X_L);
+ val |= spi.readRegister(OUT_X_H) << 8;
+ newData.magneticFieldX = mUnit * val;
+
+ val = spi.readRegister(OUT_Y_L);
+ val |= spi.readRegister(OUT_Y_H) << 8;
+ newData.magneticFieldY = mUnit * val;
+
+ val = spi.readRegister(OUT_Z_L);
+ val |= spi.readRegister(OUT_Z_H) << 8;
+ newData.magneticFieldZ = mUnit * val;
+
+ return newData;
+}
+
+void LIS3MDL::updateUnit(FullScale fs)
+{
+ switch (fs)
+ {
+ case FS_4_GAUSS:
+ mUnit = 1.f / LSB_PER_GAUSS_FS_4;
+ break;
+
+ case FS_8_GAUSS:
+ mUnit = 1.f / LSB_PER_GAUSS_FS_8;
+ break;
+
+ case FS_12_GAUSS:
+ mUnit = 1.f / LSB_PER_GAUSS_FS_12;
+ break;
+
+ case FS_16_GAUSS:
+ mUnit = 1.f / LSB_PER_GAUSS_FS_16;
+ break;
+ }
+};
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/LIS3MDL/LIS3MDL.h b/src/shared/sensors/LIS3MDL/LIS3MDL.h
index 5038c79167cf32e0a4c3f3cf60b1a2182acb6f9f..698432e934826d9c7358fa81adbbcd2b151dbd22 100644
--- a/src/shared/sensors/LIS3MDL/LIS3MDL.h
+++ b/src/shared/sensors/LIS3MDL/LIS3MDL.h
@@ -24,11 +24,9 @@
#include <diagnostic/PrintLogger.h>
#include <drivers/spi/SPIDriver.h>
-#include <drivers/timer/TimestampTimer.h>
#include <sensors/Sensor.h>
#include "LIS3MDLData.h"
-#include "miosix.h"
namespace Boardcore
{
@@ -40,12 +38,11 @@ class LIS3MDL : public Sensor<LIS3MDLData>
{
public:
/**
- * Constants for Output Data Rate configuration.
+ * @brief Constants for Output Data Rate configuration.
*
- * Note: constants values already include
- * axis operative mode selection and FAST_ODR
- * options, so they are ready to be put inside
- * CTRL_REG1 along with TEMP_EN and ST
+ * Note: constants values already include axis operative mode selection and
+ * FAST_ODR options, so they are ready to be put inside CTRL_REG1 along with
+ * TEMP_EN and ST.
*/
enum ODR : uint8_t
{
@@ -72,17 +69,17 @@ public:
enum FullScale : uint8_t
{
- FS_4_GAUSS = 0x00, //!< +/- 4 gauss
- FS_8_GAUSS = 0x20, //!< +/- 8 gauss
- FS_12_GAUSS = 0x40, //!< +/- 12 gauss
- FS_16_GAUSS = 0x60, //!< +/- 16 gauss
+ FS_4_GAUSS = 0x00, ///< +/- 4 gauss
+ FS_8_GAUSS = 0x20, ///< +/- 8 gauss
+ FS_12_GAUSS = 0x40, ///< +/- 12 gauss
+ FS_16_GAUSS = 0x60, ///< +/- 16 gauss
};
/**
- * @brief Operative mode constants
+ * @brief Operative mode constants.
*
- * Operative mode (that is power consumpion) options.
- * Higher power implies better performance.
+ * Operative mode (that is power consumption) options. Higher power implies
+ * better performance.
*/
enum OperativeMode : uint8_t
{
@@ -102,14 +99,11 @@ public:
*/
struct Config
{
- /**
- * @brief Constructor
- * Creates an instance with the default settings.
- */
Config() {}
/**
- * Full scale setting
+ * @brief Full scale setting.
+ *
* @see LIS3MDL::FullScale
*/
FullScale scale = FS_8_GAUSS;
@@ -119,21 +113,18 @@ public:
*
* Default: 40 Hz
*
- * Important: if ODR is set more than 80 Hz,
- * operative mode of x and y axis will be set
- * accordingly.
+ * Important: if ODR is set more than 80 Hz, operative mode of x and y
+ * axis will be set accordingly.
*
* @see LIS3MDL::ODR
*/
ODR odr = ODR_40_HZ;
/**
- * Operative mode for x and y axis.
- * Note: if ODR is greater than 80 Hz,
- * this setting will be ignored and actual
- * operative mode will be set depending of
- * the chosen frequency.
- * Default: ultra high performance
+ * @brief Operative mode for x and y axis.
+ *
+ * Note: if ODR is greater than 80 Hz, this setting will be ignored and
+ * actual operative mode will be set depending of the chosen frequency.
*
* @see LIS3MDL::OperativeMode
*/
@@ -141,7 +132,6 @@ public:
/**
* Operative mode for z axis.
- * Default: ultra high performance
*
* @see LIS3MDL::OM
*/
@@ -162,404 +152,60 @@ public:
* to sampleImpl(), so for example:
* 2 -> updated half the times,
* 1 -> updated every time.
- * By default the divider is set to 1
- *
*/
unsigned temperatureDivider = 1;
/**
* @brief Enables interrupts
*
- * Whether are interrupts enabled respectively
- * on the x, y and z axis. If it is set to true
- * on at least one axis, the interrupts will be
- * generated otherwise, they will be completely
- * disabled by the driver.
- *
- * Default: disabled on all axis.
+ * Whether are interrupts enabled respectively on the x, y and z axis.
+ * If it is set to true on at least one axis, the interrupts will be
+ * generated otherwise, they will be completely disabled by the driver.
*/
bool enableInterrupt[3] = {false, false, false};
/**
* Absolute value of the threshold that triggers the interrupt
- * (expressed in gauss). Default: 0
+ * (expressed in gauss).
*/
float threshold = 0;
/**
* @brief BDU setting
*
- * If set to true, the sensor won't update the data
- * until it is read, if false the sensor data will be
- * continously updated. Default: false
+ * If set to true, the sensor won't update the data until it is read, if
+ * false the sensor data will be continuously updated.
*/
bool doBlockDataUpdate = false;
};
- /**
- * Constructs the default config for SPI Bus.
- *
- * @returns the default SPIBusConfig
- */
- static SPIBusConfig getDefaultSPIConfig()
- {
- SPIBusConfig config{};
- config.clockDivider = SPI::ClockDivider::DIV_32;
- return config;
- }
-
- /**
- * @brief The constructor.
- *
- * Takes all relevant SPI info and the settings
- * for the sensor. If no SPIBusConfig is given, it will
- * be used the default configuration.
- *
- * @param bus The SPI interface
- * @param pin The CS pin
- * @param spiConfig SPI configuration, optional
- * @param config Driver configuration, optional
- *
- * @see LIS3MDL::Config
- */
LIS3MDL(SPIBusInterface& bus, miosix::GpioPin pin,
- SPIBusConfig spiConfig = getDefaultSPIConfig(), Config config = {})
- : mSlave{bus, pin, spiConfig}, mConfig(config), currDiv(0),
- isInitialized(false)
- {
- }
-
- bool init() override
- {
- if (isInitialized)
- {
- LOG_ERR(logger, "Attempted to initialized sensor twice but failed");
- lastError = ALREADY_INIT;
- return false;
- }
-
- {
- SPITransaction spi(mSlave);
- uint8_t res = spi.readRegister(WHO_AM_I);
-
- if (res != WHO_AM_I_VALUE)
- {
- LOG_ERR(logger,
- "WHO_AM_I value differs from expectation: read 0x{02x} "
- "but expected 0x{02x}",
- res, WHO_AM_I_VALUE);
- lastError = INVALID_WHOAMI;
- return false;
- }
- }
-
- isInitialized = true;
- return applyConfig(mConfig);
- }
-
- /**
- * @brief Executes self test
- *
- * The init() method must have been called before.
- *
- * @returns false if the sensor failed the self test, true otherwise.
- */
- bool selfTest() override
- {
- if (!isInitialized)
- {
- LOG_ERR(logger, "Invoked selfTest() but sensor was unitialized");
- lastError = NOT_INIT;
- return false;
- }
-
- /*
- * NUM_SAMPLES: number of samples used
- * to take the average value before tests.
- * NUM_TESTS: number of actual tests.
- * SLEEP_TIME: millis between samples/tests
- */
- constexpr int NUM_SAMPLES = 5, NUM_TESTS = 5, SLEEP_TIME = 50;
+ SPIBusConfig spiConfig = {}, Config config = {});
- /*
- * Absolute value of extra tolerance
- */
- constexpr float t = 0.1f;
-
- /*
- * Range which delta must be between, one for
- * axis and expressed as {min, max}. The unit is gauss
- */
- constexpr float r[3][2] = {{1.f, 3.f}, {1.f, 3.f}, {0.1f, 1.f}};
+ bool init() override;
- float avgX = 0.f, avgY = 0.f, avgZ = 0.f;
-
- {
- SPITransaction spi(mSlave);
- spi.writeRegister(CTRL_REG2, FS_12_GAUSS);
- }
- updateUnit(FS_12_GAUSS);
-
- for (int i = 0; i < NUM_SAMPLES; ++i)
- {
- miosix::Thread::sleep(SLEEP_TIME);
-
- LIS3MDLData lastData = sampleImpl();
- avgX += lastData.magneticFieldX;
- avgY += lastData.magneticFieldY;
- avgZ += lastData.magneticFieldZ;
- }
-
- avgX /= NUM_SAMPLES;
- avgY /= NUM_SAMPLES;
- avgZ /= NUM_SAMPLES;
-
- /*
- * Setting up the sensor settings for
- * proper usage of the self test mode.
- */
- {
- SPITransaction spi(mSlave);
-
- spi.writeRegister(CTRL_REG1, ODR_20_HZ | ENABLE_SELF_TEST |
- (OM_ULTRA_HIGH_POWER << 4));
- spi.writeRegister(CTRL_REG2, FS_12_GAUSS);
- spi.writeRegister(CTRL_REG4, OM_ULTRA_HIGH_POWER << 2);
- }
-
- /*
- * Deltas: absolute difference between
- * the values measured before and during
- * selftest
- */
- float d[3];
-
- for (int i = 0; i < NUM_TESTS; ++i)
- {
- miosix::Thread::sleep(SLEEP_TIME);
-
- LIS3MDLData lastData = sampleImpl();
- d[0] = std::abs(lastData.magneticFieldX - avgX);
- d[1] = std::abs(lastData.magneticFieldY - avgY);
- d[2] = std::abs(lastData.magneticFieldZ - avgZ);
-
- bool passed = true;
- for (int j = 0; j < 3; ++j)
- {
- if (d[j] < (r[j][0] - t) || d[j] > (r[j][1] + t))
- passed = false;
- }
-
- if (!passed)
- {
- // reset configuration, then return
- applyConfig(mConfig);
-
- lastError = SELF_TEST_FAIL;
- return false;
- }
- }
-
- return applyConfig(mConfig);
- }
+ bool selfTest() override;
/**
* @brief Overwrites the sensor settings.
*
- * Writes a certain config to the sensor
- * registers. This method is automatically
- * called in LIS3MDL::init() using as parameter
- * the configuration given in the constructor.
+ * Writes a certain config to the sensor registers. This method is
+ * automatically called in LIS3MDL::init() using as parameter the
+ * configuration given in the constructor.
*
- * This method checks if the values were actually
- * written in the sensor's registers and returns false
- * if at least one of them was not as expected.
+ * This method checks if the values were actually written in the sensor's
+ * registers and returns false if at least one of them was not as expected.
*
- * @param config The configuration to be applied
- * @returns true if the configuration was applied successfully,
+ * @param config The configuration to be applied.
+ * @returns True if the configuration was applied successfully,
* false otherwise.
*/
- bool applyConfig(Config config)
- {
-
- SPITransaction spi(mSlave);
- uint8_t reg = 0, err = 0;
-
- mConfig = config;
- currDiv = 0;
-
- /* -- CTRL_REG1 -- */
- if (config.enableTemperature)
- {
- reg = ENABLE_TEMPERATURE;
- }
- reg |= config.odr;
-
- // odr <= 80Hz
- if (!(config.odr & FAST_ODR_BIT))
- reg |= config.xyMode << 4;
- spi.writeRegister(CTRL_REG1, reg);
- err |= spi.readRegister(CTRL_REG1) != reg;
-
- /* -- CTRL_REG2 -- */
- reg = config.scale;
- spi.writeRegister(CTRL_REG2, reg);
- err |= spi.readRegister(CTRL_REG2) != reg;
-
- /* -- CTRL_REG3 -- */
- reg = CONTINOUS_CONVERSION;
- spi.writeRegister(CTRL_REG3, reg);
- err |= spi.readRegister(CTRL_REG3) != reg;
-
- /* -- CTRL_REG4 -- */
- reg = config.zMode << 2;
- spi.writeRegister(CTRL_REG4, reg);
- err |= spi.readRegister(CTRL_REG4) != reg;
-
- /* -- CTRL_REG5 -- */
- if (config.doBlockDataUpdate)
- {
- reg = ENABLE_BDU;
- }
- else
- {
- reg = 0;
- }
-
- spi.writeRegister(CTRL_REG5, reg);
- err |= spi.readRegister(CTRL_REG5) != reg;
-
- /* -- INT_CFG -- */
- if (config.enableInterrupt[0])
- {
- reg = ENABLE_INT_X;
- }
- else
- {
- reg = 0;
- }
- if (config.enableInterrupt[1])
- {
- reg |= ENABLE_INT_Y;
- }
- if (config.enableInterrupt[2])
- {
- reg |= ENABLE_INT_Z;
- }
-
- // the interrupt of at least one axis is enabled
- if (reg)
- {
- reg |= ENABLE_INT_PIN;
- }
-
- reg |= 0x08;
- spi.writeRegister(INT_CFG, reg);
- err |= spi.readRegister(INT_CFG) != reg;
-
- /** INT_THS */
- uint16_t val =
- static_cast<uint16_t>(std::abs(config.threshold / mUnit));
- reg = static_cast<uint8_t>(0xff & val);
- spi.writeRegister(INT_THS_L, reg);
- err |= spi.readRegister(INT_THS_L) != reg;
-
- reg = static_cast<uint8_t>(val >> 8);
- reg &=
- 0x7f; // remove MSB (according to the datasheet, it must be zero)
- spi.writeRegister(INT_THS_H, reg);
- err |= spi.readRegister(INT_THS_H) != reg;
-
- /* Set mUnit according to scale */
- updateUnit(config.scale);
-
- if (err)
- {
- LOG_ERR(logger, "Spi error");
- lastError = BUS_FAULT;
- return false;
- }
-
- return true;
- }
+ bool applyConfig(Config config);
private:
- /**
- * @brief Reads data from the sensor
- *
- * The init method must have been called before.
- * Output data can be fetched with the methods
- * compassDataPtr() and tempDataPtr inherited respectevely
- * from CompassSensor and TemperatureSensor classes.
- * Important: the temperature will be taken only once in a while
- * according to the value of `temperatureDivider`
- *
- * @returns false if the sensor was unitialized, true otherwise.
- */
- LIS3MDLData sampleImpl() override
- {
- if (!isInitialized)
- {
- LOG_ERR(logger,
- "Invoked sampleImpl() but sensor was "
- "unitialized");
- lastError = NOT_INIT;
- return lastSample;
- }
-
- SPITransaction spi(mSlave);
-
- if (!spi.readRegister(STATUS_REG))
- {
- lastError = NO_NEW_DATA;
- return lastSample;
- }
-
- // Reset any error
- lastError = SensorErrors::NO_ERRORS;
-
- int16_t val;
- LIS3MDLData newData{};
-
- if (mConfig.enableTemperature)
- {
- if (currDiv == 0)
- {
- val = spi.readRegister(TEMP_OUT_L);
- val |= spi.readRegister(TEMP_OUT_H) << 8;
-
- newData.temperatureTimestamp =
- TimestampTimer::getInstance().getTimestamp();
- newData.temperature =
- static_cast<float>(val) / LSB_PER_CELSIUS +
- REFERENCE_TEMPERATURE;
- }
- else
- {
- // Keep old value
- newData.temperature = lastSample.temperature;
- }
-
- currDiv = (currDiv + 1) % mConfig.temperatureDivider;
- }
-
- newData.magneticFieldTimestamp =
- TimestampTimer::getInstance().getTimestamp();
-
- val = spi.readRegister(OUT_X_L);
- val |= spi.readRegister(OUT_X_H) << 8;
- newData.magneticFieldX = mUnit * val;
-
- val = spi.readRegister(OUT_Y_L);
- val |= spi.readRegister(OUT_Y_H) << 8;
- newData.magneticFieldY = mUnit * val;
-
- val = spi.readRegister(OUT_Z_L);
- val |= spi.readRegister(OUT_Z_H) << 8;
- newData.magneticFieldZ = mUnit * val;
-
- return newData;
- }
+ LIS3MDLData sampleImpl() override;
+
+ void updateUnit(FullScale fs);
SPISlave mSlave;
Config mConfig;
@@ -568,32 +214,7 @@ private:
bool isInitialized;
float mUnit = 0;
- void updateUnit(FullScale fs)
- {
- switch (fs)
- {
- case FS_4_GAUSS:
- mUnit = 1.f / LSB_PER_GAUSS_FS_4;
- break;
-
- case FS_8_GAUSS:
- mUnit = 1.f / LSB_PER_GAUSS_FS_8;
- break;
-
- case FS_12_GAUSS:
- mUnit = 1.f / LSB_PER_GAUSS_FS_12;
- break;
-
- case FS_16_GAUSS:
- mUnit = 1.f / LSB_PER_GAUSS_FS_16;
- break;
- }
- };
-
- /**
- * List of addresses of sensor registers
- */
- enum Reg : uint8_t
+ enum Registers : uint8_t
{
WHO_AM_I = 0x0f,
@@ -619,15 +240,11 @@ private:
INT_THS_H = 0x33,
};
- /**
- * Misc. constants used by the driver. They are not
- * particularly useful to the user.
- */
enum Constants : unsigned
{
WHO_AM_I_VALUE = 0x3d,
- CONTINOUS_CONVERSION = 0x0,
+ CONTINUOS_CONVERSION = 0x0,
REFERENCE_TEMPERATURE = 25,
LSB_PER_CELSIUS = 8,
diff --git a/src/shared/sensors/MS5803/MS5803.h b/src/shared/sensors/MS5803/MS5803.h
index 6065ebcbab862231cf4fcd37e12d857cb4119198..48c7af2c1ca147e62995b2067a21af8c970c8935 100644
--- a/src/shared/sensors/MS5803/MS5803.h
+++ b/src/shared/sensors/MS5803/MS5803.h
@@ -24,9 +24,9 @@
#include <diagnostic/PrintLogger.h>
#include <drivers/spi/SPIDriver.h>
-#include <sensors/MS5803/MS5803Data.h>
#include <sensors/Sensor.h>
-#include <utils/Debug.h>
+
+#include "MS5803Data.h"
namespace Boardcore
{