diff --git a/.vscode/settings.json b/.vscode/settings.json
index 1641f81b44c6020c1fa98d5444302f7a55bc6e5c..072d70fa988631832257ac7032e75f479dc907ec 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -150,6 +150,7 @@
"cwise",
"cyaw",
"DATABUS",
+ "datasheet",
"deleteme",
"DMEIE",
"Doxyfile",
diff --git a/src/shared/sensors/LIS2MDL/LIS2MDL.cpp b/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
index 9d8ab34794b58844f20bc56470b017d1b7ec98ef..9017aaa365ce084fa2e52e955cab6a972234fc37 100644
--- a/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
+++ b/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
@@ -24,8 +24,13 @@
#include <drivers/timer/TimestampTimer.h>
#include <miosix.h>
+#include <sensors/calibration/SensorDataExtra/SensorDataExtra.h>
#include <utils/Debug.h>
+#include <iostream>
+
+using namespace Eigen;
+
namespace Boardcore
{
@@ -86,20 +91,18 @@ bool LIS2MDL::selfTest()
return false;
}
- constexpr int NUM_SAMPLES = 5;
- 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 ST_min = 0.015;
- constexpr float ST_max = 0.500;
+ /**
+ * The device has to be kept still while the self-test is ongoing.
+ * See AN5069 for further details on the self-test procedure.
+ */
- float avgX = 0.f, avgY = 0.f, avgZ = 0.f;
+ static constexpr int NUM_SAMPLES = 50; // 50 samples suggested by AN5069
+ static constexpr int SLEEP_TIME = 10; // 100Hz -> 10ms between samples
+ Vector3f avgPreTest = Vector3f::Zero();
+ Vector3f avgPostTest = Vector3f::Zero();
+ Vector3f tmp;
- // Set configuration for selfTest procedure. selfTest still not enabled
+ // 1. Set configuration for selfTest procedure. selfTest still not enabled
{
SPITransaction spi(slave);
spi.writeRegister(CFG_REG_A,
@@ -108,60 +111,80 @@ bool LIS2MDL::selfTest()
spi.readRegister(CFG_REG_B) | OFFSET_CANCELLATION);
spi.writeRegister(CFG_REG_C, spi.readRegister(CFG_REG_C) | ENABLE_BDU);
}
+
+ // Wait for power up, ~20ms for a stable output
miosix::Thread::sleep(20);
- for (int i = 0; i < NUM_SAMPLES; ++i)
+ // 2. Averaging fifty samples before enabling the self-test
{
- miosix::Thread::sleep(SLEEP_TIME);
+ for (int i = 0; i < NUM_SAMPLES - 1; i++)
+ {
+ tmp << static_cast<MagnetometerData>(sampleImpl());
+ avgPreTest += tmp;
- LIS2MDLData lastData = sampleImpl();
- avgX += lastData.magneticFieldX;
- avgY += lastData.magneticFieldY;
- avgZ += lastData.magneticFieldZ;
- }
+ miosix::Thread::sleep(SLEEP_TIME);
+ }
+ tmp << static_cast<MagnetometerData>(sampleImpl());
+ avgPreTest += tmp;
- avgX /= NUM_SAMPLES;
- avgY /= NUM_SAMPLES;
- avgZ /= NUM_SAMPLES;
+ // Compute average
+ avgPreTest /= NUM_SAMPLES;
+ }
+ // 3. Enable self-test
{
- // selfTest is enabled
SPITransaction spi(slave);
spi.writeRegister(CFG_REG_C,
spi.readRegister(CFG_REG_C) | ENABLE_SELF_TEST);
}
- miosix::Thread::sleep(60);
- // Deltas: absolute difference between the values measured before and after
- float deltas[3];
-
- LIS2MDLData lastData = sampleImpl();
- deltas[0] = std::abs(lastData.magneticFieldX - avgX);
- deltas[1] = std::abs(lastData.magneticFieldY - avgY);
- deltas[2] = std::abs(lastData.magneticFieldZ - avgZ);
+ // Wait 60ms (suggested in AN)
+ miosix::Thread::sleep(60);
- bool passed = true;
- for (int j = 0; j < 3; ++j)
- if (deltas[j] < (ST_max - t) && deltas[j] > (ST_min + t))
- passed = false;
+ // 4. Averaging fifty samples after enabling the self-test
+ {
+ for (int i = 0; i < NUM_SAMPLES - 1; i++)
+ {
+ tmp << static_cast<MagnetometerData>(sampleImpl());
+ avgPostTest += tmp;
- // Reset configuration, then return
- applyConfig(configuration);
+ miosix::Thread::sleep(SLEEP_TIME);
+ }
+ tmp << static_cast<MagnetometerData>(sampleImpl());
+ avgPostTest += tmp;
- if (!passed)
- {
- lastError = SELF_TEST_FAIL;
- return false;
+ // Compute average
+ avgPostTest /= NUM_SAMPLES;
}
+ // 5. Computing the difference in the module for each axis and verifying
+ // that is falls in the given range: the min and max value are provided
+ // in the datasheet.
{
- // Disable selfTest
- SPITransaction spi(slave);
- spi.writeRegister(CFG_REG_C,
- spi.readRegister(CFG_REG_C) & ~ENABLE_SELF_TEST);
- }
+ Vector3f deltas = (avgPostTest - avgPreTest).cwiseAbs();
- return true;
+ // Range which delta must be between, one for axis and expressed as
+ // {min, max}. The unit is gauss.
+ static constexpr float ST_MIN = 0.015; // [Gauss]
+ static constexpr float ST_MAX = 0.500; // [Gauss]
+
+ bool passed =
+ (ST_MIN < deltas.array()).all() && (deltas.array() < ST_MAX).all();
+
+ // Reset configuration, then return
+ applyConfig(configuration);
+
+ if (!passed)
+ {
+ LOG_ERR(logger, "selfTest() failed");
+ lastError = SELF_TEST_FAIL;
+ return false;
+ }
+ else
+ {
+ return true;
+ }
+ }
}
bool LIS2MDL::applyConfig(Config config)
@@ -170,7 +193,7 @@ bool LIS2MDL::applyConfig(Config config)
uint8_t reg = 0;
// CFG_REG_A: configuration register
- reg |= config.odr << 2;
+ reg |= config.odr;
reg |= config.deviceMode;
reg |= (1 << 7);
reg |= (spi.readRegister(CFG_REG_A) & 0b01110000);
diff --git a/src/shared/sensors/LIS2MDL/LIS2MDL.h b/src/shared/sensors/LIS2MDL/LIS2MDL.h
index 3fdaf088700edde9513c1da38354c8a42cf0f4a1..77227a9f22638a84799210af4973be53eb543be9 100644
--- a/src/shared/sensors/LIS2MDL/LIS2MDL.h
+++ b/src/shared/sensors/LIS2MDL/LIS2MDL.h
@@ -40,9 +40,9 @@ public:
enum ODR : uint8_t
{
ODR_10_HZ = 0x00, ///< 10 Hz
- ODR_20_HZ = 0x01, ///< 20 Hz
- ODR_50_HZ = 0x02, ///< 50 Hz
- ODR_100_HZ = 0x03, ///< 100 Hz
+ ODR_20_HZ = 0x04, ///< 20 Hz
+ ODR_50_HZ = 0x08, ///< 50 Hz
+ ODR_100_HZ = 0x18, ///< 100 Hz
};
enum OperativeMode : uint8_t