Skip to content
Snippets Groups Projects
Commit 653d1adb authored by Giulia Ghirardini's avatar Giulia Ghirardini Committed by Alberto Nidasio
Browse files

[LIS2MDL] Added selfTest, applyConfig, sampleImpl

parent 943fa0ba
Branches
No related tags found
No related merge requests found
......@@ -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
......@@ -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);
}
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment