From c26476d253e95fc61d2786e5a1cdf48a2a914102 Mon Sep 17 00:00:00 2001
From: Giulia <giulia.ghirardini@skywarder.eu>
Date: Fri, 17 Mar 2023 17:25:37 +0100
Subject: [PATCH] [LIS2MDL] Changed init and sampleImpl configuration
---
src/shared/sensors/LIS2MDL/LIS2MDL.cpp | 48 +++++++++++++++-----------
src/shared/sensors/LIS2MDL/LIS2MDL.h | 2 +-
src/tests/sensors/test-lis2mdl.cpp | 13 +++----
3 files changed, 35 insertions(+), 28 deletions(-)
diff --git a/src/shared/sensors/LIS2MDL/LIS2MDL.cpp b/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
index 4984c9d3b..bb69eafc4 100644
--- a/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
+++ b/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
@@ -23,6 +23,8 @@
#include "LIS2MDL.h"
#include <drivers/timer/TimestampTimer.h>
+#include <miosix.h>
+#include <utils/Debug.h>
namespace Boardcore
{
@@ -44,8 +46,11 @@ bool LIS2MDL::init()
}
{
+ // Important! It is imperative to get the 4WSPI enabled (set to the
+ // value of 1) due to the four-wire connection for SPI and the I2C_DIS
+ // must be disabled. In addiction, self test is enabled.
SPITransaction spi(mSlave);
- spi.writeRegister(CFG_REG_C, 4);
+ spi.writeRegister(CFG_REG_C, (1 << 2) | (1 << 5));
}
{
@@ -75,9 +80,8 @@ bool LIS2MDL::selfTest()
lastError = NOT_INIT;
return false;
}
- return true;
- constexpr int NUM_SAMPLES = 5;
+ constexpr int NUM_SAMPLES = 50;
constexpr int SLEEP_TIME = 50;
// Absolute value of extra tolerance
@@ -91,8 +95,10 @@ bool LIS2MDL::selfTest()
{
SPITransaction spi(mSlave);
- spi.writeRegister(CFG_REG_C, 4);
+ uint16_t temp = spi.readRegister(CFG_REG_C) | (1 << 1);
+ spi.writeRegister(CFG_REG_C, temp);
}
+ miosix::Thread::sleep(1);
for (int i = 0; i < NUM_SAMPLES; ++i)
{
@@ -124,36 +130,36 @@ bool LIS2MDL::selfTest()
deltas[j] > (deltaRange[j][1] + t))
passed = false;
- if (!passed)
{
- // reset configuration, then return
- applyConfig(mConfig);
+ SPITransaction spi(mSlave);
+ uint16_t temp = spi.readRegister(CFG_REG_C) & ~(1 << 1);
+ spi.writeRegister(CFG_REG_C, temp);
+ }
+ // reset configuration, then return
+ applyConfig(mConfig);
+
+ if (!passed)
+ {
lastError = SELF_TEST_FAIL;
return false;
}
- return applyConfig(mConfig);
+ return true;
}
bool LIS2MDL::applyConfig(Config config)
{
SPITransaction spi(mSlave);
- uint8_t reg = 0, err = 0;
+ uint8_t reg = 0;
// CFG_REG_A
reg |= config.odr << 2;
reg |= config.deviceMode;
- reg |= (spi.readRegister(CFG_REG_A) & 0b11110000);
+ reg |= (1 << 7);
+ reg |= (spi.readRegister(CFG_REG_A) & 0b01110000);
spi.writeRegister(CFG_REG_A, reg);
- if (err)
- {
- LOG_ERR(logger, "Spi error");
- lastError = BUS_FAULT;
- return false;
- }
-
return true;
}
@@ -185,7 +191,7 @@ LIS2MDLData LIS2MDL::sampleImpl()
if (currDiv == 0)
{
val = spi.readRegister(TEMP_OUT_L_REG);
- val |= spi.readRegister(TEMP_OUT_H_REG) << 8;
+ val |= ((uint16_t)spi.readRegister(TEMP_OUT_H_REG)) << 8;
newData.temperatureTimestamp = TimestampTimer::getTimestamp();
newData.temperature = static_cast<float>(val) / LSB_PER_CELSIUS +
@@ -203,15 +209,15 @@ LIS2MDLData LIS2MDL::sampleImpl()
newData.magneticFieldTimestamp = TimestampTimer::getTimestamp();
val = spi.readRegister(OUTX_L_REG);
- val |= spi.readRegister(OUTX_H_REG) << 8;
+ val |= ((uint16_t)spi.readRegister(OUTX_H_REG)) << 8;
newData.magneticFieldX = mUnit * val;
val = spi.readRegister(OUTY_L_REG);
- val |= spi.readRegister(OUTY_H_REG) << 8;
+ val |= ((uint16_t)spi.readRegister(OUTY_H_REG)) << 8;
newData.magneticFieldY = mUnit * val;
val = spi.readRegister(OUTZ_L_REG);
- val |= spi.readRegister(OUTY_H_REG) << 8;
+ val |= ((uint16_t)spi.readRegister(OUTZ_H_REG)) << 8;
newData.magneticFieldZ = mUnit * val;
return newData;
diff --git a/src/shared/sensors/LIS2MDL/LIS2MDL.h b/src/shared/sensors/LIS2MDL/LIS2MDL.h
index 15cc0136b..c65c07b97 100644
--- a/src/shared/sensors/LIS2MDL/LIS2MDL.h
+++ b/src/shared/sensors/LIS2MDL/LIS2MDL.h
@@ -115,7 +115,7 @@ private:
unsigned currDiv;
bool isInitialized;
- float mUnit = 0;
+ float mUnit = 1; // TODO
enum Registers : uint8_t
{
diff --git a/src/tests/sensors/test-lis2mdl.cpp b/src/tests/sensors/test-lis2mdl.cpp
index 011a7407e..d0032fa7f 100644
--- a/src/tests/sensors/test-lis2mdl.cpp
+++ b/src/tests/sensors/test-lis2mdl.cpp
@@ -30,7 +30,7 @@ using namespace miosix;
int main()
{
- GpioPin cs(GPIOA_BASE, 3), miso(GPIOA_BASE, 6), mosi(GPIOA_BASE, 7),
+ GpioPin cs(GPIOA_BASE, 15), miso(GPIOA_BASE, 6), mosi(GPIOA_BASE, 7),
clk(GPIOA_BASE, 5);
cs.mode(Mode::OUTPUT);
@@ -53,7 +53,8 @@ int main()
busConfig.mode = SPI::Mode::MODE_3;
LIS2MDL::Config config;
- config.odr = LIS2MDL::ODR_10_HZ;
+ config.odr = LIS2MDL::ODR_10_HZ;
+ config.deviceMode = LIS2MDL::MD_CONTINUOUS;
// config.scale = LIS2MDL::FS_16_GAUSS;
config.temperatureDivider = 5;
@@ -64,7 +65,7 @@ int main()
TRACE("LIS2MDL: Init failed");
return 1;
}
- TRACE("LIS2MDL: Init done");
+ TRACE("LIS2MDL: Init done\n");
TRACE("Doing self test!\n");
if (!sensor.selfTest())
@@ -78,9 +79,9 @@ int main()
while (true)
{
sensor.sample();
- LIS2MDLData data __attribute__((unused)) = sensor.getLastSample();
- TRACE("%f C | x: %f | y: %f | z %f\n", data.temperature,
+ LIS2MDLData data = sensor.getLastSample();
+ TRACE("%f C | x: %f | y: %f | z: %f\n", data.temperature,
data.magneticFieldX, data.magneticFieldY, data.magneticFieldZ);
- miosix::Thread::sleep(2000);
+ miosix::Thread::sleep(100);
}
}
--
GitLab