diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9f728f7790aed5d2023be436c06bd86f82cd5f14..07850a8b5ec88a9efc3c68736ecacaa79bee2b2e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -377,7 +377,7 @@ add_executable(test-vn100 src/tests/sensors/test-vn100.cpp)
sbs_target(test-vn100 stm32f407vg_stm32f4discovery)
add_executable(test-lis2mdl src/tests/sensors/test-lis2mdl.cpp)
-sbs_target(test-lis2mdl stm32f407vg_stm32f4discovery)
+sbs_target(test-lis2mdl stm32f429zi_stm32f4discovery)
#-----------------------------------------------------------------------------#
diff --git a/src/shared/sensors/BMX160/BMX160Defs.h b/src/shared/sensors/BMX160/BMX160Defs.h
index 661930d0d06c29b23e7a03608b76d5b1f1d5c990..0d14e7fb696c615853dea67c944203f9bb6fb405 100644
--- a/src/shared/sensors/BMX160/BMX160Defs.h
+++ b/src/shared/sensors/BMX160/BMX160Defs.h
@@ -41,7 +41,7 @@ const float TEMP_SENSIBILITY = 64.0f / 32768.0f;
/**
* @brief Magnetometer fixed sensibility.
*/
-const float MAG_SENSIBILITY = 0.3f;
+const float MAG_SENSIBILITY = 0.003; // [Gauss]
/**
* @brief BMX160 Chip Id.
diff --git a/src/shared/sensors/LIS2MDL/LIS2MDL.cpp b/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
index 15bab01737118e118b49204001271d61fa112858..ae2bb907a9087b2476481dbf54b4b5971f66ab19 100644
--- a/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
+++ b/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
@@ -31,8 +31,7 @@ namespace Boardcore
LIS2MDL::LIS2MDL(SPIBusInterface& bus, miosix::GpioPin pin,
SPIBusConfig spiConfig, Config config)
- : mSlave(bus, pin, spiConfig), mConfig(config), currDiv(0),
- isInitialized(false)
+ : slave(bus, pin, spiConfig), configuration(config)
{
}
@@ -46,13 +45,12 @@ bool LIS2MDL::init()
}
{
- // selfTest is disabled
- SPITransaction spi(mSlave);
+ SPITransaction spi(slave);
spi.writeRegister(CFG_REG_C, ENABLE_4WSPI | I2C_DISABLE);
}
{
- SPITransaction spi(mSlave);
+ SPITransaction spi(slave);
uint8_t res = spi.readRegister(WHO_AM_I);
if (res != WHO_AM_I_VALUE)
@@ -67,7 +65,7 @@ bool LIS2MDL::init()
}
isInitialized = true;
- return applyConfig(mConfig);
+ return applyConfig(configuration);
}
bool LIS2MDL::selfTest()
@@ -79,7 +77,7 @@ bool LIS2MDL::selfTest()
return false;
}
- constexpr int NUM_SAMPLES = 50;
+ constexpr int NUM_SAMPLES = 5;
constexpr int SLEEP_TIME = 50;
// Absolute value of extra tolerance
@@ -94,7 +92,7 @@ bool LIS2MDL::selfTest()
// Set configuration for selfTest procedure. selfTest still not enabled
{
- SPITransaction spi(mSlave);
+ SPITransaction spi(slave);
spi.writeRegister(CFG_REG_A,
ENABLE_TEMPERATURE_COMP | ODR_100_HZ | MD_CONTINUOUS);
spi.writeRegister(CFG_REG_B,
@@ -119,7 +117,7 @@ bool LIS2MDL::selfTest()
{
// selfTest is enabled
- SPITransaction spi(mSlave);
+ SPITransaction spi(slave);
spi.writeRegister(CFG_REG_C,
spi.readRegister(CFG_REG_C) | ENABLE_SELF_TEST);
}
@@ -138,8 +136,8 @@ bool LIS2MDL::selfTest()
if (deltas[j] < (ST_max - t) && deltas[j] > (ST_min + t))
passed = false;
- // reset configuration, then return
- applyConfig(mConfig);
+ // Reset configuration, then return
+ applyConfig(configuration);
if (!passed)
{
@@ -149,7 +147,7 @@ bool LIS2MDL::selfTest()
{
// Disable selfTest
- SPITransaction spi(mSlave);
+ SPITransaction spi(slave);
spi.writeRegister(CFG_REG_C,
spi.readRegister(CFG_REG_C) & ~ENABLE_SELF_TEST);
}
@@ -159,7 +157,7 @@ bool LIS2MDL::selfTest()
bool LIS2MDL::applyConfig(Config config)
{
- SPITransaction spi(mSlave);
+ SPITransaction spi(slave);
uint8_t reg = 0;
// CFG_REG_A: configuration register
@@ -181,53 +179,38 @@ LIS2MDLData LIS2MDL::sampleImpl()
return lastSample;
}
- SPITransaction spi(mSlave);
- // Check STATUS_REG (Zyxda) to see if new data is available.
- if (!(spi.readRegister(STATUS_REG) | (1 << 4)))
- {
- lastError = NO_NEW_DATA;
- return lastSample;
- }
-
- // Reset any error
- lastError = SensorErrors::NO_ERRORS;
+ SPITransaction spi(slave);
+ LIS2MDLData newData;
- int16_t val;
- LIS2MDLData newData{};
-
- if (mConfig.temperatureDivider != 0)
+ // Check if the temperature has to be sampled
+ tempCounter++;
+ if (configuration.temperatureDivider != 0 &&
+ tempCounter % configuration.temperatureDivider == 0)
{
- if (currDiv == 0)
- {
- val = spi.readRegister(TEMP_OUT_L_REG);
- val |= ((uint16_t)spi.readRegister(TEMP_OUT_H_REG)) << 8;
+ uint8_t values[2];
+ spi.readRegisters(TEMP_OUT_L_REG, values, sizeof(values));
- newData.temperatureTimestamp = TimestampTimer::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;
+ int16_t outTemp = values[1] << 8 | values[0];
+ newData.temperatureTimestamp = TimestampTimer::getTimestamp();
+ newData.temperature = DEG_PER_LSB * outTemp;
+ newData.temperature += REFERENCE_TEMPERATURE;
+ }
+ else
+ {
+ newData.temperature = lastSample.temperature;
}
- newData.magneticFieldTimestamp = TimestampTimer::getTimestamp();
-
- val = spi.readRegister(OUTX_L_REG);
- val |= ((uint16_t)spi.readRegister(OUTX_H_REG)) << 8;
- newData.magneticFieldX = mUnit * val;
+ uint8_t values[6];
+ spi.readRegisters(OUTX_L_REG, values, sizeof(values));
- val = spi.readRegister(OUTY_L_REG);
- val |= ((uint16_t)spi.readRegister(OUTY_H_REG)) << 8;
- newData.magneticFieldY = mUnit * val;
+ int16_t outX = values[1] << 8 | values[0];
+ int16_t outY = values[3] << 8 | values[2];
+ int16_t outZ = values[5] << 8 | values[4];
- val = spi.readRegister(OUTZ_L_REG);
- val |= ((uint16_t)spi.readRegister(OUTZ_H_REG)) << 8;
- newData.magneticFieldZ = mUnit * val;
+ newData.magneticFieldTimestamp = TimestampTimer::getTimestamp();
+ newData.magneticFieldX = GAUSS_PER_LSB * outX;
+ newData.magneticFieldY = GAUSS_PER_LSB * outY;
+ newData.magneticFieldZ = GAUSS_PER_LSB * outZ;
return newData;
}
diff --git a/src/shared/sensors/LIS2MDL/LIS2MDL.h b/src/shared/sensors/LIS2MDL/LIS2MDL.h
index 0bde9df3e88878a92d68dae5cac2b5e49bc1493c..7e82c8fd4b16b35f1a8743a72caeb02ab00b9b05 100644
--- a/src/shared/sensors/LIS2MDL/LIS2MDL.h
+++ b/src/shared/sensors/LIS2MDL/LIS2MDL.h
@@ -74,14 +74,6 @@ public:
* disable it completely.
*/
unsigned temperatureDivider = 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 continuously updated.
- */
- bool doBlockDataUpdate = false;
};
LIS2MDL(SPIBusInterface& bus, miosix::GpioPin pin,
@@ -110,12 +102,11 @@ public:
private:
LIS2MDLData sampleImpl() override;
- SPISlave mSlave;
- Config mConfig;
+ SPISlave slave;
+ Config configuration;
- unsigned currDiv;
- bool isInitialized;
- float mUnit = 1; // TODO
+ unsigned tempCounter = 0;
+ bool isInitialized = false;
enum Registers : uint8_t
{
@@ -153,16 +144,16 @@ private:
static constexpr uint32_t CONTINUOS_CONVERSION = 0x0;
static constexpr float REFERENCE_TEMPERATURE = 25;
- static constexpr float LSB_PER_CELSIUS = 8;
- static constexpr float LSB_PER_GAUSS_MIN = 0.001395;
- static constexpr float LSB_PER_GAUSS_MAX = 0.001605;
-
- static constexpr uint32_t ENABLE_TEMPERATURE_COMP = (1 << 7);
- static constexpr uint32_t ENABLE_SELF_TEST = (1 << 1);
- static constexpr uint32_t ENABLE_BDU = (1 << 4);
- static constexpr uint32_t ENABLE_4WSPI = (1 << 2);
- static constexpr uint32_t OFFSET_CANCELLATION = (1 << 1);
- static constexpr uint32_t I2C_DISABLE = (1 << 5);
+ static constexpr float DEG_PER_LSB = 0.125;
+
+ static constexpr float GAUSS_PER_LSB = 0.0015;
+
+ static constexpr uint32_t ENABLE_TEMPERATURE_COMP = 1 << 7;
+ static constexpr uint32_t ENABLE_SELF_TEST = 1 << 1;
+ static constexpr uint32_t ENABLE_BDU = 1 << 4;
+ static constexpr uint32_t ENABLE_4WSPI = 1 << 2;
+ static constexpr uint32_t OFFSET_CANCELLATION = 1 << 1;
+ static constexpr uint32_t I2C_DISABLE = 1 << 5;
PrintLogger logger = Logging::getLogger("lis2mdl");
};
diff --git a/src/shared/sensors/LIS3MDL/LIS3MDL.cpp b/src/shared/sensors/LIS3MDL/LIS3MDL.cpp
index 2299478117f95de5a6dab82bfe03f489fbac8da6..2d9183b678484102aeba1c108ff95dde2ab32ea1 100644
--- a/src/shared/sensors/LIS3MDL/LIS3MDL.cpp
+++ b/src/shared/sensors/LIS3MDL/LIS3MDL.cpp
@@ -29,8 +29,7 @@ namespace Boardcore
LIS3MDL::LIS3MDL(SPIBusInterface& bus, miosix::GpioPin pin,
SPIBusConfig spiConfig, Config config)
- : mSlave(bus, pin, spiConfig), mConfig(config), currDiv(0),
- isInitialized(false)
+ : slave(bus, pin, spiConfig), configuration(config)
{
}
@@ -44,7 +43,7 @@ bool LIS3MDL::init()
}
{
- SPITransaction spi(mSlave);
+ SPITransaction spi(slave);
uint8_t res = spi.readRegister(WHO_AM_I);
if (res != WHO_AM_I_VALUE)
@@ -59,7 +58,7 @@ bool LIS3MDL::init()
}
isInitialized = true;
- return applyConfig(mConfig);
+ return applyConfig(configuration);
}
bool LIS3MDL::selfTest()
@@ -84,7 +83,7 @@ bool LIS3MDL::selfTest()
float avgX = 0.f, avgY = 0.f, avgZ = 0.f;
{
- SPITransaction spi(mSlave);
+ SPITransaction spi(slave);
spi.writeRegister(CTRL_REG2, FS_12_GAUSS);
}
updateUnit(FS_12_GAUSS);
@@ -105,7 +104,7 @@ bool LIS3MDL::selfTest()
// Setting up the sensor settings for proper usage of the self test mode.
{
- SPITransaction spi(mSlave);
+ SPITransaction spi(slave);
spi.writeRegister(CTRL_REG1, ODR_20_HZ | ENABLE_SELF_TEST |
(OM_ULTRA_HIGH_POWER << 4));
@@ -129,26 +128,25 @@ bool LIS3MDL::selfTest()
deltas[j] > (deltaRange[j][1] + t))
passed = false;
+ // Reset configuration, then return
+ applyConfig(configuration);
+
if (!passed)
{
- // reset configuration, then return
- applyConfig(mConfig);
-
lastError = SELF_TEST_FAIL;
return false;
}
- return applyConfig(mConfig);
+ return applyConfig(configuration);
}
bool LIS3MDL::applyConfig(Config config)
{
- SPITransaction spi(mSlave);
+ SPITransaction spi(slave);
uint8_t reg = 0, err = 0;
- mConfig = config;
- currDiv = 0;
+ configuration = config;
// CTRL_REG1
if (config.temperatureDivider != 0)
@@ -209,53 +207,37 @@ LIS3MDLData LIS3MDL::sampleImpl()
return lastSample;
}
- SPITransaction spi(mSlave);
+ SPITransaction spi(slave);
+ LIS3MDLData newData;
- if (!spi.readRegister(STATUS_REG))
+ tempCounter++;
+ if (configuration.temperatureDivider != 0 &&
+ tempCounter % configuration.temperatureDivider == 0)
{
- lastError = NO_NEW_DATA;
- return lastSample;
- }
-
- // Reset any error
- lastError = SensorErrors::NO_ERRORS;
+ uint8_t values[2];
+ spi.readRegisters(TEMP_OUT_L, values, sizeof(values));
- int16_t val;
- LIS3MDLData newData{};
-
- if (mConfig.temperatureDivider != 0)
+ int16_t outTemp = values[1] << 8 | values[0];
+ newData.temperatureTimestamp = TimestampTimer::getTimestamp();
+ newData.temperature = DEG_PER_LSB * outTemp;
+ newData.temperature += REFERENCE_TEMPERATURE;
+ }
+ else
{
- if (currDiv == 0)
- {
- val = spi.readRegister(TEMP_OUT_L);
- val |= spi.readRegister(TEMP_OUT_H) << 8;
-
- newData.temperatureTimestamp = TimestampTimer::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.temperature = lastSample.temperature;
}
- newData.magneticFieldTimestamp = TimestampTimer::getTimestamp();
-
- val = spi.readRegister(OUT_X_L);
- val |= spi.readRegister(OUT_X_H) << 8;
- newData.magneticFieldX = mUnit * val;
+ uint8_t values[6];
+ spi.readRegisters(OUT_X_L, values, sizeof(values));
- val = spi.readRegister(OUT_Y_L);
- val |= spi.readRegister(OUT_Y_H) << 8;
- newData.magneticFieldY = mUnit * val;
+ int16_t outX = values[1] << 8 | values[0];
+ int16_t outY = values[3] << 8 | values[2];
+ int16_t outZ = values[5] << 8 | values[4];
- val = spi.readRegister(OUT_Z_L);
- val |= spi.readRegister(OUT_Z_H) << 8;
- newData.magneticFieldZ = mUnit * val;
+ newData.magneticFieldTimestamp = TimestampTimer::getTimestamp();
+ newData.magneticFieldX = currentUnit * outX;
+ newData.magneticFieldY = currentUnit * outY;
+ newData.magneticFieldZ = currentUnit * outZ;
return newData;
}
@@ -265,19 +247,19 @@ void LIS3MDL::updateUnit(FullScale fs)
switch (fs)
{
case FS_4_GAUSS:
- mUnit = 1.f / LSB_PER_GAUSS_FS_4;
+ currentUnit = GAUSS_PER_LSB_FS_4;
break;
case FS_8_GAUSS:
- mUnit = 1.f / LSB_PER_GAUSS_FS_8;
+ currentUnit = GAUSS_PER_LSB_FS_8;
break;
case FS_12_GAUSS:
- mUnit = 1.f / LSB_PER_GAUSS_FS_12;
+ currentUnit = GAUSS_PER_LSB_FS_12;
break;
case FS_16_GAUSS:
- mUnit = 1.f / LSB_PER_GAUSS_FS_16;
+ currentUnit = GAUSS_PER_LSB_FS_16;
break;
}
};
diff --git a/src/shared/sensors/LIS3MDL/LIS3MDL.h b/src/shared/sensors/LIS3MDL/LIS3MDL.h
index efc9d1e25c6ac1275e0692cdfc22553d98f6fecf..a60e02f8403ffd0e511f6f86d1f145cf2229ab60 100644
--- a/src/shared/sensors/LIS3MDL/LIS3MDL.h
+++ b/src/shared/sensors/LIS3MDL/LIS3MDL.h
@@ -32,7 +32,7 @@ namespace Boardcore
{
/**
- * Driver for LIS3MDL, a three-axis magnetic sensor.
+ * @brief Driver for LIS3MDL, a three-axis magnetic sensor.
*/
class LIS3MDL : public Sensor<LIS3MDLData>
{
@@ -40,7 +40,7 @@ public:
/**
* @brief Constants for Output Data Rate configuration.
*
- * Note: constants values already include axis operative mode selection and
+ * 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.
*/
@@ -150,12 +150,12 @@ private:
void updateUnit(FullScale fs);
- SPISlave mSlave;
- Config mConfig;
+ SPISlave slave;
+ Config configuration;
- unsigned currDiv;
- bool isInitialized;
- float mUnit = 0;
+ unsigned tempCounter = 0;
+ bool isInitialized = false;
+ float currentUnit = 0;
enum Registers : uint8_t
{
@@ -183,28 +183,25 @@ private:
INT_THS_H = 0x33,
};
- enum Constants : unsigned
- {
- WHO_AM_I_VALUE = 0x3d,
+ static constexpr uint32_t WHO_AM_I_VALUE = 0x3d;
+ static constexpr uint32_t CONTINUOS_CONVERSION = 0x0;
- CONTINUOS_CONVERSION = 0x0,
- REFERENCE_TEMPERATURE = 25,
- LSB_PER_CELSIUS = 8,
+ static constexpr uint32_t REFERENCE_TEMPERATURE = 25;
+ static constexpr float DEG_PER_LSB = 0.125;
- LSB_PER_GAUSS_FS_4 = 6842,
- LSB_PER_GAUSS_FS_8 = 3421,
- LSB_PER_GAUSS_FS_12 = 2281,
- LSB_PER_GAUSS_FS_16 = 1711,
+ static constexpr float GAUSS_PER_LSB_FS_4 = 0.000146156;
+ static constexpr float GAUSS_PER_LSB_FS_8 = 0.000292312;
+ static constexpr float GAUSS_PER_LSB_FS_12 = 0.000438404;
+ static constexpr float GAUSS_PER_LSB_FS_16 = 0.000584454;
- ENABLE_TEMPERATURE = 0x80,
- ENABLE_SELF_TEST = 0x01,
- ENABLE_BDU = 0x40,
+ static constexpr uint32_t ENABLE_TEMPERATURE = 1 << 7;
+ static constexpr uint32_t ENABLE_SELF_TEST = 1 << 0;
+ static constexpr uint32_t ENABLE_BDU = 1 << 6;
- ENABLE_INT_PIN = 0x01,
- ENABLE_INT_X = 0x80,
- ENABLE_INT_Y = 0x40,
- ENABLE_INT_Z = 0x20,
- };
+ static constexpr uint32_t ENABLE_INT_PIN = 1 << 0;
+ static constexpr uint32_t ENABLE_INT_X = 1 << 7;
+ static constexpr uint32_t ENABLE_INT_Y = 1 << 6;
+ static constexpr uint32_t ENABLE_INT_Z = 1 << 5;
PrintLogger logger = Logging::getLogger("lis3mdl");
};
diff --git a/src/tests/sensors/test-lis2mdl.cpp b/src/tests/sensors/test-lis2mdl.cpp
index 87353379647449e94a024ea0cd51926e78a25afd..bcab4bc630b9bd214ab011a830344cf23c4df33b 100644
--- a/src/tests/sensors/test-lis2mdl.cpp
+++ b/src/tests/sensors/test-lis2mdl.cpp
@@ -29,34 +29,24 @@
using namespace Boardcore;
using namespace miosix;
+GpioPin clk(GPIOA_BASE, 5);
+GpioPin miso(GPIOA_BASE, 6);
+GpioPin mosi(GPIOA_BASE, 7);
+GpioPin cs(GPIOA_BASE, 15);
+
int main()
{
- GpioPin cs(GPIOA_BASE, 15), miso(GPIOA_BASE, 6), mosi(GPIOA_BASE, 7),
- clk(GPIOA_BASE, 5), tx(GPIOA_BASE, 2), rx(GPIOA_BASE, 3);
-
- cs.mode(Mode::OUTPUT);
- cs.high();
-
clk.mode(Mode::ALTERNATE);
clk.alternateFunction(5);
clk.speed(Speed::_100MHz);
-
miso.mode(Mode::ALTERNATE);
miso.alternateFunction(5);
-
mosi.mode(Mode::ALTERNATE);
mosi.alternateFunction(5);
-
- // Serial data write on Matlab
- rx.mode(Mode::ALTERNATE);
- rx.alternateFunction(7);
-
- tx.mode(Mode::ALTERNATE);
- tx.alternateFunction(7);
+ cs.mode(Mode::OUTPUT);
+ cs.high();
SPIBus bus(SPI1);
- USART usart(USART2, USARTInterface::Baudrate::B115200);
- usart.init();
SPIBusConfig busConfig;
busConfig.clockDivider = SPI::ClockDivider::DIV_256;
@@ -69,33 +59,29 @@ int main()
LIS2MDL sensor(bus, cs, busConfig, config);
+ printf("Starting...\n");
+
if (!sensor.init())
{
- TRACE("LIS2MDL: Init failed");
+ printf("LIS2MDL: Init failed\n");
return 1;
}
- TRACE("LIS2MDL: Init done\n");
+ printf("LIS2MDL: Init done\n");
- TRACE("Doing self test!\n");
+ printf("Doing self test!\n");
if (!sensor.selfTest())
{
- TRACE("Error: selfTest() returned false!\n");
+ printf("Error: selfTest() returned false!\n");
}
- TRACE("selfTest returned true\n");
- TRACE("Now printing some sensor data:\n");
- Thread::sleep(3000);
- float sensorData[3];
+ printf("selfTest returned true\n");
+ printf("Now printing some sensor data:\n");
while (true)
{
sensor.sample();
LIS2MDLData data = sensor.getLastSample();
- TRACE("%f C | x: %f | y: %f | z: %f\n", data.temperature,
- data.magneticFieldX, data.magneticFieldY, data.magneticFieldZ);
- sensorData[0] = data.magneticFieldX;
- sensorData[1] = data.magneticFieldY;
- sensorData[2] = data.magneticFieldZ;
- usart.write(sensorData, 3 * sizeof(float));
+ printf("%f C | x: %f | y: %f | z: %f\n", data.temperature,
+ data.magneticFieldX, data.magneticFieldY, data.magneticFieldZ);
miosix::Thread::sleep(10);
}
}