diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp index 94a8db6bef5fe0b1137663c84afbf7858f83ca66..743a26acc4044015518ab7d559e2926d6f0279d6 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp @@ -24,6 +24,12 @@ #include "LSM9DS1_Magneto.h" +#include <math.h> + +#include <iostream> + +#include "math/Stats.h" + using miosix::GpioPin; using std::vector; @@ -42,11 +48,10 @@ LSM9DS1_M::LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config, bool LSM9DS1_M::init() { - if (sensor_initialized) - { - TRACE("[LSM9DS1 MAG] init() : already initialized\n"); - return false; - } + +#ifdef DEBUG + assert(sensor_initialized == false); +#endif SPITransaction spi(spislave); @@ -61,11 +66,11 @@ bool LSM9DS1_M::init() } // X,Y axes in ultra-high performance mode, ODR defined by constructor - uint8_t CTRL_REG1_M_VAL = 0x60 | (int)odr << 2; + uint8_t CTRL_REG1_M_VAL = 0x60 | odr << 2; spi.write(regMapM::CTRL_REG1_M, CTRL_REG1_M_VAL); // FSR defined by constructor - uint8_t CTRL_REG2_M_VAL = (int)magFSR << 5; + uint8_t CTRL_REG2_M_VAL = magFSR << 5; spi.write(regMapM::CTRL_REG2_M, CTRL_REG2_M_VAL); // Z axis in ultra-high performance mode @@ -81,64 +86,184 @@ bool LSM9DS1_M::init() if (spi.read(regMapM::CTRL_REG1_M) != CTRL_REG1_M_VAL) { + TRACE("[LSM9DS1 MAG] init() : CTRL_REG1_M readback failed\n"); return false; } if (spi.read(regMapM::CTRL_REG2_M) != CTRL_REG2_M_VAL) { + TRACE("[LSM9DS1 MAG] init() : CTRL_REG2_M readback failed\n"); return false; } if (spi.read(regMapM::CTRL_REG3_M) != CTRL_REG3_M_VAL) { + TRACE("[LSM9DS1 MAG] init() : CTRL_REG3_M readback failed\n"); return false; } if (spi.read(regMapM::CTRL_REG4_M) != CTRL_REG4_M_VAL) { + TRACE("[LSM9DS1 MAG] init() : CTRL_REG4_M readback failed\n"); return false; } if (spi.read(regMapM::INT_CFG_M) != INT_CFG_M_VAL) { + TRACE("[LSM9DS1 MAG] init() : INT_CFG_M readback failed\n"); return false; } - // select Sensitivity - switch (magFSR) - { - case MagFSR::FS_4: - magSensitivity = 0.14f; - break; - case MagFSR::FS_8: - magSensitivity = 0.29f; - break; - case MagFSR::FS_12: - magSensitivity = 0.43f; - break; - case MagFSR::FS_16: - magSensitivity = 0.58f; - break; - default: - magSensitivity = 0.14f; - break; - } + // wait 20ms for stable output + miosix::Thread::sleep(20); - TRACE("[LSM9DS1 XLG] init() : done\n"); + TRACE("[LSM9DS1 MAG] init() : done\n"); sensor_initialized = true; return true; } -bool LSM9DS1_M::selfTest() { return true; } +bool LSM9DS1_M::selfTest() +{ + TRACE("[LSM9DS1 MAG] selfTest() : starting self-test\n"); + + Stats nostX, nostY, nostZ; + Stats stX, stY, stZ; + bool selfTestResult = false; + + selfTest_mode = true; + + { + SPITransaction spi(spislave); + + // Reset all registers (if called after init()) + spi.write(regMapM::CTRL_REG2_M, SOFT_RESET); + miosix::Thread::sleep(20); + + // self-test ROUTINE of LIS3MDL - seems to be same magnetometer as + // LSM9DS1 init sensor for self-test: FSR = +/-12 Gauss, ODR = 80Hz + uint8_t CTRL_REG1_M_VAL = (ODR::ODR_80 << 2); + spi.write(regMapM::CTRL_REG1_M, CTRL_REG1_M_VAL); + + uint8_t CTRL_REG2_M_VAL = MagFSR::FS_12 << 5; + spi.write(regMapM::CTRL_REG2_M, CTRL_REG2_M_VAL); + + // wait 20ms for stable output + miosix::Thread::sleep(20); + + // out enable - continuous mode + spi.write(regMapM::CTRL_REG3_M, 0x00); + + // wait data-ready bit on STATUS REG - read first sample and discard + while ((spi.read(regMapM::STATUS_REG_M) & DRDY_MASK) == 0) + { + } + + spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, 6); + } + + // wait data-ready bit on STATUS REG - read NOST sample at least 5 times + LSM9DS1_M::getSelfTestData(nostX, nostY, nostZ); + + // enable self-test mode and wait 60ms + { + SPITransaction spi(spislave); + + uint8_t CTRL_REG1_M_VAL = (ODR::ODR_80 << 2) | SELFTEST_ENABLE; + spi.write(regMapM::CTRL_REG1_M, CTRL_REG1_M_VAL); + + miosix::Thread::sleep(60); + + // wait data-ready bit on STATUS REG - read first sample and discard + while ((spi.read(regMapM::STATUS_REG_M) & DRDY_MASK) == 0) + { + } + + spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, 6); + } + + LSM9DS1_M::getSelfTestData(stX, stY, stZ); + + float deltaX = fabsf(stX.getStats().mean - nostX.getStats().mean); + float deltaY = fabsf(stY.getStats().mean - nostY.getStats().mean); + float deltaZ = fabsf(stZ.getStats().mean - nostZ.getStats().mean); + +// verify if sensor is inside parameters + +// print stats +#ifdef DEBUG + std::cout << "[LSM9DS1 MAG] selfTest() :" << std::endl + << "X-AXIS stats : " << nostX.getStats() << std::endl + << "Y-AXIS stats : " << nostY.getStats() << std::endl + << "Z-AXIS stats : " << nostZ.getStats() << std::endl + << "deltaX : " << deltaX << std::endl + << "deltaY : " << deltaY << std::endl + << "deltaZ : " << deltaZ << std::endl; +#endif + + // clang-format off + + if(ST_XY_MIN < deltaX && deltaX < ST_XY_MAX && + ST_XY_MIN < deltaY && deltaY < ST_XY_MAX && + ST_Z_MIN < deltaZ && deltaZ < ST_Z_MAX) + { + TRACE("[LSM9DS1 MAG] selfTest() : self-test passed\n"); + selfTestResult = true; + } + else + { + TRACE("[LSM9DS1 MAG] selfTest() : self-test failed\n"); + selfTestResult = false; + } + + // clang-format on + + // re-init if necessary + if(sensor_initialized == true) + { + sensor_initialized = false; + LSM9DS1_M::init(); + } + + selfTest_mode = true; + + return selfTestResult; +} + +void LSM9DS1_M::getSelfTestData(Stats& outxStats, Stats& outyStats, + Stats& outzStats) +{ + for (int i = 0; i < SELFTEST_MAX_SAMPLES; i++) + { + { + SPITransaction spi(spislave); + while ((spi.read(regMapM::STATUS_REG_M) & DRDY_MASK) == 0) + { + } + } + LSM9DS1_M::onSimpleUpdate(); + + printf("%.3f,%.3f,%.3f\n", lastMagneto.magData.getX(), + lastMagneto.magData.getY(), + lastMagneto.magData.getZ()); + // compute statistics + outxStats.add(lastMagneto.magData.getX()); + outyStats.add(lastMagneto.magData.getY()); + outzStats.add(lastMagneto.magData.getZ()); + } + printf("\n\n\n"); +} bool LSM9DS1_M::onSimpleUpdate() { +#ifdef DEBUG + assert(sensor_initialized == true || selfTest_mode == true); +#endif uint8_t magData[6]; - // read output magneto raw data X,Y,Z + // read output magneto raw data X,Y,Z and timestamp { SPITransaction spi(spislave); - // bit 1 of SPI transaction = 1 means "auto-increment address" - spi.read(regMapM::OUT_X_L_M | 0x40, magData, 6); + spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, magData, 6); + lastMagneto.timestamp = miosix::getTick(); } // compose signed 16-bit raw data as 2 bytes from the sensor @@ -148,30 +273,10 @@ bool LSM9DS1_M::onSimpleUpdate() int16_t z = magData[4] | magData[5] << 8; //convert raw data - mLastCompass = Vec3(x * magSensitivity / 1000, - y * magSensitivity / 1000, - z * magSensitivity / 1000); + lastMagneto.magData = Vec3(x * magFSR_SMap.at(magFSR), + y * magFSR_SMap.at(magFSR), + z * magFSR_SMap.at(magFSR)); // clang-format on return true; -} - -bool LSM9DS1_M::setOffset(vector<uint16_t>& offVect) -{ - if (offVect.size() != 3) - return false; - - uint8_t toStore[6]; - - //separate each byte (MSB first) - for (int i = 6; i > 0; i = i - 2) - { - toStore[i - 1] = offVect.back() & 0x00FF; // LSB - toStore[i - 2] = offVect.back() >> 8; // MSB - offVect.pop_back(); - } - - SPITransaction spi(spislave); - // bit 1 of SPI transaction = 1 means "auto-increment address". - spi.write(regMapM::OFFSET_X_REG_L_M | 0x40, toStore, 6); } \ No newline at end of file diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h index b1396989f6f8d73f3521677537c4013097d83a75..f53638fa9593904da9805c7e7b66a52b5b9ddc79 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h @@ -29,6 +29,7 @@ #include "LSM9DS1_Data.h" #include "drivers/spi/SPIDriver.h" +#include "math/Stats.h" using miosix::GpioPin; using std::vector; @@ -41,7 +42,7 @@ using std::vector; class LSM9DS1_M : public CompassSensor { public: - enum MagFSR + enum MagFSR : uint8_t { FS_4 = 0x00, // +/- 4Gauss FS_8 = 0x01, // +/- 8Gauss @@ -49,7 +50,7 @@ public: FS_16 = 0x03 // +/- 16Gauss }; - enum ODR + enum ODR : uint8_t { ODR_0_625 = 0X00, // 0.625Hz ODR_1_25 = 0x01, // 1.25Hz @@ -61,6 +62,12 @@ public: ODR_80 = 0x07 // 80Hz }; + // Sesitivity Map (axelFSR) + const std::map<MagFSR, float> magFSR_SMap{{FS_4, 0.00014f}, + {FS_8, 0.00029f}, + {FS_12, 0.00043f}, + {FS_16, 0.00058f}}; + /** * @brief Creates an instance of an LSM9DS1 magnetometer sensor. * @@ -70,8 +77,8 @@ public: * @param odr Output Data Rate (See datasheet) */ - LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, MagFSR magRange = MagFSR::FS_8, - ODR odr = ODR::ODR_0_625); + LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, MagFSR magRange = MagFSR::FS_16, + ODR odr = ODR::ODR_80); /** * @brief Creates an instance of an LSM9DS1 magnetometer sensor. @@ -84,7 +91,7 @@ public: */ LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config, - MagFSR magRange = MagFSR::FS_8, ODR odr = ODR::ODR_0_625); + MagFSR magRange = MagFSR::FS_16, ODR odr = ODR::ODR_80); /** * @brief initializes the LSM9DS1 Sensor (Magnetometer). @@ -109,23 +116,18 @@ public: bool onSimpleUpdate() override; - /** - * @brief set offset of the compass sensor in case of offset errors. - * @param offVect offset values (X,Y,Z) - * @return true if register has been set - */ - - bool setOffset(vector<uint16_t>& offVect); - private: + void getSelfTestData(Stats& outx, Stats& outy, Stats& outz); + bool sensor_initialized = false; + bool selfTest_mode = false; SPISlave spislave; MagFSR magFSR; ODR odr; - float magSensitivity; + lsm9ds1MSample lastMagneto; /** * @brief Registers' addresses definition. @@ -161,4 +163,18 @@ private: static const uint8_t CTRL_REG3_M_VAL = 0x80; static const uint8_t CTRL_REG4_M_VAL = 0x0C; static const uint8_t INT_CFG_M_VAL = 0x00; + + static const uint8_t AUTO_INCREMENT_ADDR = 0x40; + static const uint8_t SOFT_RESET = 0x08; + + // For selfTEST - limit for FS = +/-12Gauss + // LIS3MDL limits - seems to be same magnetometer as LSM9DS1 + static constexpr float ST_XY_MIN = 1.0f; + static constexpr float ST_XY_MAX = 3.0f; + static constexpr float ST_Z_MIN = 0.1f; + static constexpr float ST_Z_MAX = 1.0f; + + static const uint8_t DRDY_MASK = 0x04; + static const uint8_t SELFTEST_MAX_SAMPLES = 10; + static const uint8_t SELFTEST_ENABLE = 0x01; }; \ No newline at end of file