[LIS2MDL] Added selfTest, applyConfig, sampleImpl

src/shared/sensors/LIS2MDL/LIS2MDL.cpp

@@ -34,8 +34,6 @@ LIS2MDL::LIS2MDL(SPIBusInterface& bus, miosix::GpioPin pin,
 {
 }
 
-bool LIS2MDL::applyConfig(Config config) { return true; }
-
 bool LIS2MDL::init()
 {
     if (isInitialized)
@@ -69,8 +67,154 @@ bool LIS2MDL::init()
     return applyConfig(mConfig);
 }
 
-bool LIS2MDL::selfTest() { return true; }
+bool LIS2MDL::selfTest()
+{
+    if (!isInitialized)
+    {
+        LOG_ERR(logger, "Invoked selfTest() but sensor was uninitialized");
+        lastError = NOT_INIT;
+        return false;
+    }
+    return true;
+
+    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 deltaRange[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(CFG_REG_C, 4);
+    }
+
+    for (int i = 0; i < NUM_SAMPLES; ++i)
+    {
+        miosix::Thread::sleep(SLEEP_TIME);
+
+        LIS2MDLData lastData = sampleImpl();
+        avgX += lastData.magneticFieldX;
+        avgY += lastData.magneticFieldY;
+        avgZ += lastData.magneticFieldZ;
+    }
+
+    avgX /= NUM_SAMPLES;
+    avgY /= NUM_SAMPLES;
+    avgZ /= NUM_SAMPLES;
+
+    // Deltas: absolute difference between the values measured before and after
+    float deltas[3];
+
+    miosix::Thread::sleep(SLEEP_TIME);
+
+    LIS2MDLData lastData = sampleImpl();
+    deltas[0]            = std::abs(lastData.magneticFieldX - avgX);
+    deltas[1]            = std::abs(lastData.magneticFieldY - avgY);
+    deltas[2]            = std::abs(lastData.magneticFieldZ - avgZ);
+
+    bool passed = true;
+    for (int j = 0; j < 3; ++j)
+        if (deltas[j] < (deltaRange[j][0] - t) ||
+            deltas[j] > (deltaRange[j][1] + t))
+            passed = false;
+
+    if (!passed)
+    {
+        // reset configuration, then return
+        applyConfig(mConfig);
+
+        lastError = SELF_TEST_FAIL;
+        return false;
+    }
+
+    return applyConfig(mConfig);
+}
+
+bool LIS2MDL::applyConfig(Config config)
+{
+    SPITransaction spi(mSlave);
+    uint8_t reg = 0, err = 0;
+
+    // CFG_REG_A
+    reg |= config.odr << 2;
+    reg |= config.deviceMode;
+    reg |= (spi.readRegister(CFG_REG_A) & 0b11110000);
+    spi.writeRegister(CFG_REG_A, reg);
+
+    if (err)
+    {
+        LOG_ERR(logger, "Spi error");
+        lastError = BUS_FAULT;
+        return false;
+    }
+
+    return true;
+}
+
+LIS2MDLData LIS2MDL::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;
+    }
 
-LIS2MDLData LIS2MDL::sampleImpl() { return LIS2MDLData{}; }
+    // Reset any error
+    lastError = SensorErrors::NO_ERRORS;
+
+    int16_t val;
+    LIS2MDLData newData{};
+
+    if (mConfig.temperatureDivider != 0)
+    {
+        if (currDiv == 0)
+        {
+            val = spi.readRegister(TEMP_OUT_L_REG);
+            val |= spi.readRegister(TEMP_OUT_H_REG) << 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.magneticFieldTimestamp = TimestampTimer::getTimestamp();
+
+    val = spi.readRegister(OUTX_L_REG);
+    val |= spi.readRegister(OUTX_H_REG) << 8;
+    newData.magneticFieldX = mUnit * val;
+
+    val = spi.readRegister(OUTY_L_REG);
+    val |= spi.readRegister(OUTY_H_REG) << 8;
+    newData.magneticFieldY = mUnit * val;
+
+    val = spi.readRegister(OUTZ_L_REG);
+    val |= spi.readRegister(OUTY_H_REG) << 8;
+    newData.magneticFieldZ = mUnit * val;
+
+    return newData;
+}
 
 }  // namespace Boardcore
\ No newline at end of file

src/tests/sensors/test-lis2mdl.cpp

@@ -49,7 +49,7 @@ int main()
 
     SPIBusConfig busConfig;
     busConfig.clockDivider = SPI::ClockDivider::DIV_256;
-    busConfig.mode         = SPI::Mode::MODE_0;
+    busConfig.mode         = SPI::Mode::MODE_3;
 
     LIS2MDL::Config config;
     config.odr = LIS2MDL::ODR_10_HZ;
@@ -64,4 +64,22 @@ int main()
         return 1;
     }
     TRACE("LIS2MDL: Init done");
+
+    TRACE("Doing self test!\n");
+    if (!sensor.selfTest())
+    {
+        TRACE("Error: selfTest() returned false!\n");
+    }
+    TRACE("selfTest returned true\n");
+    TRACE("Now printing some sensor data:\n");
+    Thread::sleep(100);
+
+    while (true)
+    {
+        sensor.sample();
+        LIS2MDLData data __attribute__((unused)) = sensor.getLastSample();
+        TRACE("%f C | x: %f | y: %f | z %f\n", data.temperature,
+              data.magneticFieldX, data.magneticFieldY, data.magneticFieldZ);
+        miosix::Thread::sleep(2000);
+    }
 }