diff --git a/sbs.conf b/sbs.conf index 7a57bd1566ea29088fa093087a2f412e55410c9f..5dcb6c528cef36ab8b1d7768e65000215166a623 100644 --- a/sbs.conf +++ b/sbs.conf @@ -498,7 +498,7 @@ Main: drivers/test-l3gd20 Type: test BoardId: stm32f407vg_skyward_tortellino BinName: test-lsm9ds1 -Include: %shared %spi +Include: %shared %spi %lsm9ds1 Defines: -DDEBUG Main: drivers/test-lsm9ds1 diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp index 255b4134ebfa0f1f94d6d7ff5bf4a4b6953705f0..4980e5f80e02063f2baf4f14de730ef927d9d2b9 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp @@ -27,11 +27,9 @@ using miosix::GpioPin; using std::array; -LSM9DS1_XLG::LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, - AxelFSR axelRange = AxelFSR::FS_2, - GyroFSR gyroRange = GyroFSR::FS_245, - ODR odr = ODR::ODR_15, bool fifo_enabled = false, - unsigned int fifo_watermark = 24) +LSM9DS1_XLG::LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, AxelFSR axelRange, + GyroFSR gyroRange, ODR odr, bool fifo_enabled, + unsigned int fifo_watermark) : fifo_enabled(fifo_enabled), fifo_watermark(fifo_watermark), spislave(bus, cs, {}), axelFSR(axelRange), gyroFSR(gyroRange), odr(odr) { @@ -40,10 +38,8 @@ LSM9DS1_XLG::LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, } LSM9DS1_XLG::LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config, - AxelFSR axelRange = AxelFSR::FS_2, - GyroFSR gyroRange = GyroFSR::FS_245, - ODR odr = ODR::ODR_15, bool fifo_enabled = false, - unsigned int fifo_watermark = 24) + AxelFSR axelRange, GyroFSR gyroRange, ODR odr, + bool fifo_enabled, unsigned int fifo_watermark) : fifo_enabled(fifo_enabled), fifo_watermark(fifo_watermark), spislave(bus, cs, config), axelFSR(axelRange), gyroFSR(gyroRange), odr(odr) @@ -52,9 +48,100 @@ LSM9DS1_XLG::LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config, bool LSM9DS1_XLG::init() { - // Set FSR - switch (axelFSR) // DA RIGUARDARE I COEFFICIENTI: SU DS C'E' - // SENSITIVITY IN mg/LSB + if (sensor_initialized) + { + TRACE("[LSM9DS1 XLG] init() : already initialized\n"); + return false; + } + + SPITransaction spi(spislave); + + // Who Am I check: + uint8_t whoami = spi.read(regMapXLG::WHO_AM_I); + if (whoami != WHO_AM_I_XLG_VAL) + { + TRACE("[LSM9DS1 XLG] init() : unexpected WAMI -> %02X\n", whoami); + last_error = ERR_NOT_ME; + return false; + } + + // address auto-increment while reading/writing + spi.write(regMapXLG::CTRL_REG8, CTRL_REG8_VAL); + + // FIFO setup: + if (fifo_enabled) + { + // FIFO continous mode + fifo watermark threshold setup + spi.write(regMapXLG::FIFO_CTRL, (FIFO_CTRL_VAL | fifo_watermark)); + + // interrupt on FIFO treshold enabled + spi.write(regMapXLG::INT1_CTRL, INT1_CTRL_VAL); + + // DRDY_mask_bit OFF, I2C OFF, FIFO ON + spi.write(regMapXLG::CTRL_REG9, (CTRL_REG9_VAL | 0x02)); + } + else + { + // DRDY_mask_bit OFF, I2C OFF, FIFO OFF + spi.write(regMapXLG::CTRL_REG9, CTRL_REG9_VAL); + } + + /** Axel Setup: + * ODR, FSR defined by constructor, auto anti-aliasing BW + * (max), LPF2/HPF bypassed and disabled, axel output enabled by default + * @ startup + */ + uint8_t CTRL_REG6_XL_VAL = (int)odr << 5 | (int)axelFSR << 3; + spi.write(regMapXLG::CTRL_REG6_XL, CTRL_REG6_XL_VAL); + + /** Gyro Setup : ODR, FSR defined by constructor, LPF2/HPF bypassed and + * disabled, gyro output enabled by default @ startup + */ + uint8_t CTRL_REG1_G_VAL = (int)odr << 5 | (int)gyroFSR << 3; + spi.write(regMapXLG::CTRL_REG1_G, CTRL_REG1_G_VAL); + + // sign and orientation Setup <--- BOARD DEPENDENT + // spi.write(regMapXLG::ORIENT_CFG_G, ORIENT_CFG_VAL); + + // Check all the registers have been written correctly + if (spi.read(regMapXLG::CTRL_REG8) != CTRL_REG8_VAL) + { + return false; + } + if (fifo_enabled) + { + if (spi.read(regMapXLG::FIFO_CTRL) != (FIFO_CTRL_VAL | fifo_watermark)) + { + return false; + } + if (spi.read(regMapXLG::INT1_CTRL) != INT1_CTRL_VAL) + { + return false; + } + if (spi.read(regMapXLG::CTRL_REG9) != (CTRL_REG9_VAL | 0x02)) + { + return false; + } + } + else + { + if (spi.read(regMapXLG::CTRL_REG9) != CTRL_REG9_VAL) + { + return false; + } + } + if (spi.read(regMapXLG::CTRL_REG6_XL) != CTRL_REG6_XL_VAL) + { + return false; + } + if (spi.read(regMapXLG::CTRL_REG1_G) != CTRL_REG1_G_VAL) + { + return false; + } + + // select Sensitivity and ODR + switch (axelFSR) + { case AxelFSR::FS_2: axelSensitivity = 0.598f; @@ -117,93 +204,12 @@ bool LSM9DS1_XLG::init() break; } - SPITransaction spi(spislave); - - // Who Am I check: - uint8_t whoami = spi.read(regMapXLG::WHO_AM_I); - if (whoami != WHO_AM_I_XLG_VAL) - { - TRACE("LSM9DS1 AXEL+GYRO WAMI: %02X\n", whoami); - last_error = ERR_NOT_ME; - return false; - } - - // common setup - spi.write(regMapXLG::CTRL_REG8, - CTRL_REG8_VAL); // addr auto-increment while reading/writing - - // FIFO setup: FIFO enabled in continous mode, decimation OFF, - // temperature on FIFO ON. - if (fifo_enabled) - { - - spi.write( - regMapXLG::FIFO_CTRL, - (FIFO_CTRL_VAL | fifo_watermark)); // FIFO continous mode + fifo - // watermark threshold setup - spi.write(regMapXLG::INT1_CTRL, - INT1_CTRL_VAL); // interrupt on FIFO treshold - spi.write( - regMapXLG::CTRL_REG9, - (CTRL_REG9_VAL | 0x02)); // DRDY_mask_bit OFF, I2C OFF, FIFO ON - } - else - { - spi.write(regMapXLG::CTRL_REG9, - CTRL_REG9_VAL); // DRDY_mask_bit OFF, I2C OFF, FIFO OFF - } - - // Axel Setup: ODR, FSR defined by constructor, auto anti-aliasing BW - // (max), LPF2/HPF bypassed and disabled, axel output enabled by default - // @ startup - uint8_t CTRL_REG6_XL_VAL = (int)odr << 5 | (int)axelFSR << 3; - spi.write(regMapXLG::CTRL_REG6_XL, - CTRL_REG6_XL_VAL); // ODR, FSR, auto BW (max) function of ODR - - // Gyro Setup : ODR, FSR defined by constructor, LPF2/HPF bypassed and - // disabled, gyro output enabled by default @ startup - uint8_t CTRL_REG1_G_VAL = (int)odr << 5 | (int)gyroFSR << 3; - spi.write(regMapXLG::CTRL_REG1_G, CTRL_REG1_G_VAL); // ODR,FSR - // spi.write(regMapXLG::ORIENT_CFG_G, ORIENT_CFG_VAL); //angular rate - // sign and orientation Setup <--- BOARD DEPENDENT + // discard first samples (see datasheet) + LSM9DS1_XLG::discardSamples(spi); - // Check all the registers have been written correctly - if (spi.read(regMapXLG::CTRL_REG8) != CTRL_REG8_VAL) - { - return false; - } - if (fifo_enabled) - { - if (spi.read(regMapXLG::FIFO_CTRL) != (FIFO_CTRL_VAL | fifo_watermark)) - { - return false; - } - if (spi.read(regMapXLG::INT1_CTRL) != INT1_CTRL_VAL) - { - return false; - } - if (spi.read(regMapXLG::CTRL_REG9) != (CTRL_REG9_VAL | 0x02)) - { - return false; - } - } - else - { - if (spi.read(regMapXLG::CTRL_REG9) != CTRL_REG9_VAL) - { - return false; - } - } - if (spi.read(regMapXLG::CTRL_REG6_XL) != CTRL_REG6_XL_VAL) - { - return false; - } - if (spi.read(regMapXLG::CTRL_REG1_G) != CTRL_REG1_G_VAL) - { - return false; - } + TRACE("[LSM9DS1 XLG] init() : done\n"); - LSM9DS1_XLG::discardSamples(spi); // LUCA SCS MERDA (x2) <3 + sensor_initialized = true; return true; } @@ -212,70 +218,82 @@ bool LSM9DS1_XLG::selfTest() { return true; } bool LSM9DS1_XLG::onSimpleUpdate() { + // if FIFO disabled if (!fifo_enabled) - { // if FIFO disabled + { uint8_t data[12], tempData[2]; - // Read output axel+gyro data X,Y,Z + + // Read output axel+gyro raw data X,Y,Z and temp raw data { SPITransaction spi(spislave); spi.read(regMapXLG::OUT_X_L_G, data, 12); spi.read(regMapXLG::OUT_TEMP_L, tempData, 2); } - int16_t x_gy = data[0] | data[1] << 8; - int16_t y_gy = data[2] | data[3] << 8; - int16_t z_gy = data[4] | data[5] << 8; + // compose signed 16-bit raw data as 2 bytes from the sensor + // clang-format off + int16_t x_gy = data[0] | data[1] << 8; + int16_t y_gy = data[2] | data[3] << 8; + int16_t z_gy = data[4] | data[5] << 8; - int16_t x_xl = data[6] | data[7] << 8; - int16_t y_xl = data[8] | data[9] << 8; + int16_t x_xl = data[6] | data[7] << 8; + int16_t y_xl = data[8] | data[9] << 8; int16_t z_xl = data[10] | data[11] << 8; int16_t temp = tempData[0] | tempData[1] << 8; - mLastAccel = - Vec3(x_xl * axelSensitivity / 1000, y_xl * axelSensitivity / 1000, - z_xl * axelSensitivity / 1000); + //convert raw data + mLastAccel = Vec3(x_xl * axelSensitivity / 1000, + y_xl * axelSensitivity / 1000, + z_xl * axelSensitivity / 1000); + + mLastGyro = Vec3(x_gy * gyroSensitivity / 1000, + y_gy * gyroSensitivity / 1000, + z_gy * gyroSensitivity / 1000); - mLastGyro = - Vec3(x_gy * gyroSensitivity / 1000, y_gy * gyroSensitivity / 1000, - z_gy * gyroSensitivity / 1000); + mLastTemp = tempZero + (temp / tempSensistivity); - mLastTemp = - tempZero + (temp / tempSensistivity); // 25°C + TEMP*S devo - // castare a float "temp"? + // clang-format on } + + /** if FIFO enabled: + * dump fifo_watermark samples (axel+gyro only) from the sensor at one + * time. Temperature data can be read using temperatureUpdate() function + * at a lower frequency + */ else - { // if FIFO enabled: do not store temperature, it can be read using - // "temperatureUpdate()" function at low sampling frequency - uint8_t buf[384]; // 2 bytes per data * 3 axes * 2 (axel+gyro) * - // 32(FIFO DEPTH MAX) = 384 samples + { + // 2 bytes per data * 3 axes per type * 2 types(axel+gyro) * + // 32(FIFO DEPTH MAX) = 384 samples + uint8_t buf[384]; + + // Read output axel+gyro FIFO raw data X,Y,Z { SPITransaction spi(spislave); - - spi.read( - OUT_X_L_G, buf, - fifo_watermark * - 12); // format: - // gxl,gxh,gyl,gyh,gzl,gzh,axl,axh,ayl,ayh,azl,azh - // for each sample + spi.read(OUT_X_L_G, buf, fifo_watermark * 12); } + // convert & store for (int i = 0; i < fifo_watermark; i++) { - int16_t x_gy = buf[i * 12] | buf[i * 12 + 1] << 8; - int16_t y_gy = buf[i * 12 + 2] | buf[i * 12 + 3] << 8; - int16_t z_gy = buf[i * 12 + 4] | buf[i * 12 + 5] << 8; - - int16_t x_xl = buf[i * 12 + 6] | buf[i * 12 + 7] << 8; - int16_t y_xl = buf[i * 12 + 8] | buf[i * 12 + 9] << 8; + // compose signed 16-bit raw data as 2 bytes from the sensor + // clang-format off + int16_t x_gy = buf[i * 12] | buf[i * 12 + 1] << 8; + int16_t y_gy = buf[i * 12 + 2] | buf[i * 12 + 3] << 8; + int16_t z_gy = buf[i * 12 + 4] | buf[i * 12 + 5] << 8; + + int16_t x_xl = buf[i * 12 + 6] | buf[i * 12 + 7] << 8; + int16_t y_xl = buf[i * 12 + 8] | buf[i * 12 + 9] << 8; int16_t z_xl = buf[i * 12 + 10] | buf[i * 12 + 11] << 8; + //convert raw data fifo[i].gyroData = Vec3(x_gy * gyroSensitivity / 1000, y_gy * gyroSensitivity / 1000, z_gy * gyroSensitivity / 1000); fifo[i].axelData = Vec3(x_xl * axelSensitivity / 1000, y_xl * axelSensitivity / 1000, z_xl * axelSensitivity / 1000); + // clang-format on } } return true; @@ -283,16 +301,16 @@ bool LSM9DS1_XLG::onSimpleUpdate() bool LSM9DS1_XLG::temperatureUpdate() { + // Read output temp raw data uint8_t tempData[2]; { SPITransaction spi(spislave); spi.read(regMapXLG::OUT_TEMP_L, tempData, 2); } - + // compose signed 16-bit raw data as 2 bytes from the sensor int16_t temp = tempData[0] | tempData[1] << 8; - mLastTemp = - tempZero + - temp / tempSensistivity; // 25°C + TEMP/S devo castare a float "temp"? + // convert raw data + mLastTemp = tempZero + temp / tempSensistivity; return true; } @@ -312,21 +330,18 @@ const array<lsm9ds1XLGSample, 32>& LSM9DS1_XLG::getLsm9ds1FIFO() const void LSM9DS1_XLG::discardSamples(SPITransaction& spi) { - //@ startup, some samples have to be discarded (datasheet) + //@ startup, some samples must be discarded (datasheet) if (odr != ODR::PWR_DW) { + // wait samples to be overwritten or stored (FIFO on) uint16_t toWait_ms = samplesToDiscard * 1000 / odrHz; - // TRACE("toWait_ms: %d", toWait_ms); - miosix::Thread::sleep(toWait_ms); // if FIFO is disabled, just wait + miosix::Thread::sleep(toWait_ms); + + // if FIFO is enabled, read first "samplesToDiscard" samples and + // discard them. if (fifo_enabled) { - // if FIFO is enabled, read first <samplesToDiscard> samples and - // discard them - for (int i = 0; i < samplesToDiscard; i++) - { - spi.read(regMapXLG::OUT_X_L_XL, 6); - spi.read(regMapXLG::OUT_X_L_G, 6); - } + spi.read(OUT_X_L_G, samplesToDiscard * 12); } } } \ No newline at end of file diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h index 62801a5bf797107062f671c6e4fcd8771ba23667..2be91afcc0b020e9d0fc4aec99af2aa1a5f13174 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h @@ -34,7 +34,10 @@ using miosix::GpioPin; using std::array; /** - * @brief descrizione classe + * @brief LSM9DS1 gyro+axel+temp sensor driver. + * provides access to data generated by the sensor using SPI protocol. + * it is possible to set the sensor to have simple data samples or + * exploiting integrated FIFO and perform on-interrupt reading. */ class LSM9DS1_XLG : public GyroSensor, @@ -44,32 +47,33 @@ class LSM9DS1_XLG : public GyroSensor, public: enum AxelFSR { - FS_2 = 0x00, - FS_16 = 0x01, - FS_4 = 0x02, - FS_8 = 0x03 + FS_2 = 0x00, // +/- 2g + FS_16 = 0x01, // +/- 16g + FS_4 = 0x02, // +/- 4g + FS_8 = 0x03 // +/- 8g }; enum GyroFSR { - FS_245 = 0x00, - FS_500 = 0x01, - FS_2000 = 0x03 // 1 -> 3 jump is ok + FS_245 = 0x00, // +/- 245dps + FS_500 = 0x01, // +/- 500dps + FS_2000 = 0x03 // +/- 2000dps }; enum ODR { - PWR_DW = 0X00, - ODR_15 = 0X01, - ODR_60 = 0X02, - ODR_119 = 0X03, - ODR_238 = 0X04, - ODR_476 = 0X05, - ODR_952 = 0X06 + PWR_DW = 0X00, // power down + ODR_15 = 0X01, // 15Hz + ODR_60 = 0X02, // 60Hz + ODR_119 = 0X03, // 119Hz + ODR_238 = 0X04, // 238Hz + ODR_476 = 0X05, // 476Hz + ODR_952 = 0X06 // 952Hz }; /** - * @brief Creates an instance of an LSM9DS1 accelerometer + gyroscope sensor. + * @brief Creates an instance of an LSM9DS1 accelerometer + gyroscope + * sensor. * * @param bus SPI bus the sensor is connected to * @param cs Chip Select GPIO @@ -81,12 +85,14 @@ public: * interrupt generation, see datasheet). */ - LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, AxelFSR axelRange, - GyroFSR gyroRange, ODR odr, bool fifo_enabled, - unsigned int fifo_watermark); + LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, + AxelFSR axelRange = AxelFSR::FS_2, + GyroFSR gyroRange = GyroFSR::FS_245, ODR odr = ODR::ODR_15, + bool fifo_enabled = false, unsigned int fifo_watermark = 24); /** - * @brief Creates an instance of an LSM9DS1 accelerometer + gyroscope sensor. + * @brief Creates an instance of an LSM9DS1 accelerometer + gyroscope + * sensor. * * @param bus SPI bus the sensor is connected to * @param cs Chip Select GPIO @@ -100,13 +106,15 @@ public: */ LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config, - AxelFSR axelRange, GyroFSR gyroRange, ODR odr, - bool fifo_enabled, unsigned int fifo_watermark); + AxelFSR axelRange = AxelFSR::FS_2, + GyroFSR gyroRange = GyroFSR::FS_245, ODR odr = ODR::ODR_15, + bool fifo_enabled = false, unsigned int fifo_watermark = 24); /** * @brief initializes the LSM9DS1 Sensor (Accelerometer + Gyroscope). * @return true if all setup registers of the sensor have been written - * correctly + * correctly. false if already initialized, wrong who_am_i or uncorrect + * write. */ bool init() override; @@ -121,6 +129,8 @@ public: /** * @brief Dump single reading of Axel+Gyro+Temp from the sensor through SPI. * @return true if sensor sends data + * @warning if FIFO is enabled, call only after interrupt flag from the + * sensor has been set */ bool onSimpleUpdate() override; @@ -134,12 +144,15 @@ public: /** * @brief Clear the FIFO register inside the LSM9DS1 sensor. + * In order to perform a "clear", FIFO is disabled and then + * re-enabled; after that some samples are discarded according to datasheet. */ void clearFIFO(); /** - * @brief get FIFO dumped after calling onSimpleUpdate() - Just on FIFO mode. + * @brief get FIFO dumped after calling onSimpleUpdate() - Just on FIFO + * mode. * @return array containing the whole FIFO */ @@ -153,6 +166,7 @@ private: void discardSamples(SPITransaction& spi); + bool sensor_initialized = false; bool fifo_enabled; uint8_t fifo_watermark; array<lsm9ds1XLGSample, 32> fifo; @@ -170,6 +184,9 @@ private: float tempSensistivity = 16.0f; static const uint8_t samplesToDiscard = 8; // max possible val + /** + * @brief Registers' addresses definition. + */ enum regMapXLG { ACT_THS = 0x04, @@ -190,7 +207,7 @@ private: INT_GEN_SRC_G = 0x14, OUT_TEMP_L = 0x15, OUT_TEMP_H = 0x16, - STATUS_REG_G = 0x17, // per check stato + STATUS_REG_G = 0x17, OUT_X_L_G = 0x18, OUT_X_H_G = 0x19, OUT_Y_L_G = 0x1A, @@ -203,9 +220,9 @@ private: CTRL_REG7_XL = 0x21, CTRL_REG8 = 0x22, CTRL_REG9 = 0x23, - CTRL_REG10 = 0x24, // per self-test ma n.u. + CTRL_REG10 = 0x24, INT_GEN_SRC_XL = 0x26, - STATUS_REG_XL = 0x27, // per check stato + STATUS_REG_XL = 0x27, OUT_X_L_XL = 0x28, OUT_X_H_XL = 0x29, OUT_Y_L_XL = 0x2A, @@ -213,7 +230,7 @@ private: OUT_Z_L_XL = 0x2C, OUT_Z_H_XL = 0x2D, FIFO_CTRL = 0x2E, - FIFO_SRC = 0x2F, // FIFO status register + FIFO_SRC = 0x2F, INT_GEN_CFG_G = 0x30, INT_GEN_THS_XH_G = 0x31, INT_GEN_THS_XL_G = 0x32, diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h b/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h index 51e68aa98a9a689115599a3c6301caa78418642c..0d4f83aaa890065309013809136f18f5dd4fd0af 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h @@ -25,11 +25,16 @@ #pragma once #include <miosix.h> + +#include <fstream> + #include "../Sensor.h" +using std::ofstream; + // data Structs -//ACCELEROMETER + GYROSCOPE +// ACCELEROMETER + GYROSCOPE struct lsm9ds1XLGSample { uint64_t timestamp; @@ -43,22 +48,19 @@ struct lsm9ds1XLGSample void print(std::ostream& os) const { - os << timestamp << "," << - axelData.getX() << "," << axelData.getY() << "," << axelData.getZ() << "," << - gyroData.getX() << "," << gyroData.getY() << "," << gyroData.getZ() << "\n"; + os << timestamp << "," << axelData.getX() << "," << axelData.getY() + << "," << axelData.getZ() << "," << gyroData.getX() << "," + << gyroData.getY() << "," << gyroData.getZ() << "\n"; } }; -//TEMPERATURE +// TEMPERATURE struct lsm9ds1TSample { uint64_t timestamp; float tempData; - static std::string header() - { - return "timestamp,temp\n"; - } + static std::string header() { return "timestamp,temp\n"; } void print(std::ostream& os) const { @@ -66,20 +68,17 @@ struct lsm9ds1TSample } }; -//MAGNETOMETER +// MAGNETOMETER struct lsm9ds1MSample { uint64_t timestamp; Vec3 magData; - static std::string header() - { - return "timestamp,mag_x,mag_y,mag_z\n"; - } + static std::string header() { return "timestamp,mag_x,mag_y,mag_z\n"; } void print(std::ostream& os) const { - os << timestamp << "," << - magData.getX() << "," << magData.getY() << "," << magData.getZ() << "\n"; + os << timestamp << "," << magData.getX() << "," << magData.getY() << "," + << magData.getZ() << "\n"; } }; diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp index 7879bc7dfd10f0faa9234f3befbcdc752481632e..94a8db6bef5fe0b1137663c84afbf7858f83ca66 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp @@ -27,8 +27,7 @@ using miosix::GpioPin; using std::vector; -LSM9DS1_M::LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, - MagFSR magRange = MagFSR::FS_8, ODR odr = ODR::ODR_0_625) +LSM9DS1_M::LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, MagFSR magRange, ODR odr) : spislave(bus, cs, {}), magFSR(magRange), odr(odr) { // SPI config @@ -36,62 +35,47 @@ LSM9DS1_M::LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, } LSM9DS1_M::LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config, - MagFSR magRange = MagFSR::FS_8, ODR odr = ODR::ODR_0_625) + MagFSR magRange, ODR odr) : spislave(bus, cs, config), magFSR(magRange), odr(odr) { } bool LSM9DS1_M::init() { - // Set FSR - switch (magFSR) + if (sensor_initialized) { - 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; + TRACE("[LSM9DS1 MAG] init() : already initialized\n"); + return false; } SPITransaction spi(spislave); // Who Am I check: - uint8_t whoami = spi.read(WHO_AM_I_M); // regMapM::WHO_AM_I_M); + uint8_t whoami = spi.read(regMapM::WHO_AM_I_M); if (whoami != WHO_AM_I_M_VAL) { - TRACE("LSM9DS1 MAG WAMI: 0x%02X\n", whoami); + TRACE("[LSM9DS1 MAG] init() : unexpected WAMI -> %02X\n", whoami); last_error = ERR_NOT_ME; return false; } - // setup + // X,Y axes in ultra-high performance mode, ODR defined by constructor uint8_t CTRL_REG1_M_VAL = 0x60 | (int)odr << 2; - spi.write(regMapM::CTRL_REG1_M, - CTRL_REG1_M_VAL); // X,Y axes in ultra-high performance mode, - // NO auto-temp compensation and ODR - // defined by constructor + spi.write(regMapM::CTRL_REG1_M, CTRL_REG1_M_VAL); + // FSR defined by constructor uint8_t CTRL_REG2_M_VAL = (int)magFSR << 5; - spi.write(regMapM::CTRL_REG2_M, - CTRL_REG2_M_VAL); // FSR defined by constructor + spi.write(regMapM::CTRL_REG2_M, CTRL_REG2_M_VAL); - spi.write(regMapM::CTRL_REG4_M, - CTRL_REG4_M_VAL); // Z axis in ultra-high performance mode - spi.write(regMapM::CTRL_REG3_M, - CTRL_REG3_M_VAL); // I2C disabled, SPI mode: read/write + // Z axis in ultra-high performance mode + spi.write(regMapM::CTRL_REG4_M, CTRL_REG4_M_VAL); - spi.write(regMapM::INT_CFG_M, INT_CFG_M_VAL); // disable all interrupts + // I2C disabled, SPI mode: read/write + spi.write(regMapM::CTRL_REG3_M, CTRL_REG3_M_VAL); + + // disable all interrupts + spi.write(regMapM::INT_CFG_M, INT_CFG_M_VAL); // check that all registers have been written correctly @@ -116,6 +100,30 @@ bool LSM9DS1_M::init() 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; + } + + TRACE("[LSM9DS1 XLG] init() : done\n"); + + sensor_initialized = true; + return true; } @@ -125,31 +133,37 @@ bool LSM9DS1_M::onSimpleUpdate() { uint8_t magData[6]; - // read output data X,Y,Z + + // read output magneto raw data X,Y,Z { SPITransaction spi(spislave); - // bit 1 of SPI transaction = 1 means "auto-increment address" (see - // DS). - spi.read(regMapM::OUT_X_L_M | 0x40, magData, - 6); // so bit 6 of the address = 1 + // bit 1 of SPI transaction = 1 means "auto-increment address" + spi.read(regMapM::OUT_X_L_M | 0x40, magData, 6); } + // compose signed 16-bit raw data as 2 bytes from the sensor + // clang-format off int16_t x = magData[0] | magData[1] << 8; int16_t y = magData[2] | magData[3] << 8; int16_t z = magData[4] | magData[5] << 8; - // TRACE("LSM9DS1 mageto: %02X,%02X,%02X\n", x, y, z); - - mLastCompass = Vec3(x * magSensitivity / 1000, y * magSensitivity / 1000, + //convert raw data + mLastCompass = Vec3(x * magSensitivity / 1000, + y * magSensitivity / 1000, z * magSensitivity / 1000); + // clang-format on + return true; } -void LSM9DS1_M::setOffset(vector<uint16_t>& offVect) +bool LSM9DS1_M::setOffset(vector<uint16_t>& offVect) { - if (offVect.size() < 3) - return; + 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 @@ -159,6 +173,5 @@ void LSM9DS1_M::setOffset(vector<uint16_t>& offVect) SPITransaction spi(spislave); // bit 1 of SPI transaction = 1 means "auto-increment address". - spi.write(regMapM::OFFSET_X_REG_L_M | 0x40, toStore, - 6); // so bit 6 of the address = 1 + 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 6bfdad5520af21fe85fc4463bd67dbee00faf7e1..b1396989f6f8d73f3521677537c4013097d83a75 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h @@ -34,7 +34,8 @@ using miosix::GpioPin; using std::vector; /** - * @brief descrizione classe + * @brief LSM9DS1 magnetometer sensor driver. + * provides access to data generated by the sensor using SPI protocol. */ class LSM9DS1_M : public CompassSensor @@ -42,22 +43,22 @@ class LSM9DS1_M : public CompassSensor public: enum MagFSR { - FS_4 = 0x00, - FS_8 = 0x01, - FS_12 = 0x02, - FS_16 = 0x03 + FS_4 = 0x00, // +/- 4Gauss + FS_8 = 0x01, // +/- 8Gauss + FS_12 = 0x02, // +/- 12Gauss + FS_16 = 0x03 // +/- 16Gauss }; enum ODR { - ODR_0_625 = 0X00, - ODR_1_25 = 0x01, - ODR_2_5 = 0x02, - ODR_5 = 0x03, - ODR_10 = 0x04, - ODR_20 = 0x05, - ODR_40 = 0x06, - ODR_80 = 0x07 + ODR_0_625 = 0X00, // 0.625Hz + ODR_1_25 = 0x01, // 1.25Hz + ODR_2_5 = 0x02, // 2.5Hz + ODR_5 = 0x03, // 5Hz + ODR_10 = 0x04, // 10Hz + ODR_20 = 0x05, // 20Hz + ODR_40 = 0x06, // 40Hz + ODR_80 = 0x07 // 80Hz }; /** @@ -69,7 +70,8 @@ public: * @param odr Output Data Rate (See datasheet) */ - LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, MagFSR magRange, ODR odr); + LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, MagFSR magRange = MagFSR::FS_8, + ODR odr = ODR::ODR_0_625); /** * @brief Creates an instance of an LSM9DS1 magnetometer sensor. @@ -82,14 +84,15 @@ public: */ LSM9DS1_M(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config, - MagFSR magRange, ODR odr); - + MagFSR magRange = MagFSR::FS_8, ODR odr = ODR::ODR_0_625); + /** * @brief initializes the LSM9DS1 Sensor (Magnetometer). * @return true if all setup registers of the sensor have been written - * correctly + * correctly.false if already initialized, wrong who_am_i or uncorrect + * write. */ - + bool init() override; /** @@ -109,11 +112,14 @@ public: /** * @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 */ - void setOffset(vector<uint16_t>& offVect); + bool setOffset(vector<uint16_t>& offVect); private: + bool sensor_initialized = false; + SPISlave spislave; MagFSR magFSR; @@ -121,6 +127,9 @@ private: float magSensitivity; + /** + * @brief Registers' addresses definition. + */ enum regMapM { OFFSET_X_REG_L_M = 0x05, diff --git a/src/tests/drivers/test-lsm9ds1.cpp b/src/tests/drivers/test-lsm9ds1.cpp index 378b8b3218db7785a247a956525549b871fd5109..03079c1436fd9a9dcc16c9475041c141fd67b310 100644 --- a/src/tests/drivers/test-lsm9ds1.cpp +++ b/src/tests/drivers/test-lsm9ds1.cpp @@ -22,119 +22,107 @@ * THE SOFTWARE. */ - #include "drivers/spi/SPIDriver.h" #include "sensors/LSM9DS1/LSM9DS1_AxelGyro.h" #include "sensors/LSM9DS1/LSM9DS1_Magneto.h" using namespace miosix; -typedef Gpio<GPIOA_BASE, 5> GpioSck; //questi sono i pin SPI per f407_discovery +typedef Gpio<GPIOA_BASE, 5> GpioSck; // questi sono i pin SPI per + // f407_discovery typedef Gpio<GPIOA_BASE, 6> GpioMiso; typedef Gpio<GPIOA_BASE, 7> GpioMosi; static const bool FIFO_ENABLED = false; -//SPI +// SPI SPIBus bus(SPI1); -//SPIBusConfig cfg; +// SPIBusConfig cfg; GpioPin cs_XLG(GPIOE_BASE, 7); GpioPin cs_M(GPIOE_BASE, 9); -//LEDs +// LEDs GpioPin LED1(GPIOD_BASE, 15); GpioPin LED2(GPIOD_BASE, 13); - - -int main(){ +int main() +{ Vec3 adata, gdata, mdata; - float tdata; + float tdata; { FastInterruptDisableLock dLock; - RCC->APB2ENR |= RCC_APB2ENR_SPI1EN; //SPI1 ENABLE - + RCC->APB2ENR |= RCC_APB2ENR_SPI1EN; // SPI1 ENABLE + GpioSck::mode(Mode::ALTERNATE); GpioMiso::mode(Mode::ALTERNATE); GpioMosi::mode(Mode::ALTERNATE); - + cs_XLG.mode(Mode::OUTPUT); - cs_M.mode(Mode::OUTPUT); + cs_M.mode(Mode::OUTPUT); GpioSck::alternateFunction(5); GpioMiso::alternateFunction(5); GpioMosi::alternateFunction(5); - GpioSck::speed(Speed::_25MHz); + GpioSck::speed(Speed::_25MHz); LED1.mode(Mode::OUTPUT); LED2.mode(Mode::OUTPUT); - } cs_XLG.high(); - cs_M.high(); - -//dump regiters -/* - for(int reg=0; reg<=0x38; reg++) + cs_M.high(); + + // dump regiters + /* + for(int reg=0; reg<=0x38; reg++) + { + SPISlave spislave(bus, cs_XLG, cfg); + SPITransaction spi(spislave); + uint8_t data = spi.read(reg); + printf("0x%02X-->0x%02X\n", reg,data); + } + }*/ + + LSM9DS1_XLG lsm9ds1X(bus, cs_XLG, LSM9DS1_XLG::AxelFSR::FS_8, + LSM9DS1_XLG::GyroFSR::FS_245, + LSM9DS1_XLG::ODR::ODR_952); + + LSM9DS1_M lsm9ds1M(bus, cs_M, LSM9DS1_M::MagFSR::FS_8, + LSM9DS1_M::ODR::ODR_20); + + while (!lsm9ds1X.init()) { - SPISlave spislave(bus, cs_XLG, cfg); - SPITransaction spi(spislave); - uint8_t data = spi.read(reg); - printf("0x%02X-->0x%02X\n", reg,data); } -}*/ - - - - LSM9DS1_XLG lsm9ds1X( - bus, - cs_XLG, - //cfg, - LSM9DS1_XLG::AxelFSR::FS_8, - LSM9DS1_XLG::GyroFSR::FS_245, - LSM9DS1_XLG::ODR::ODR_952 - ); - - LSM9DS1_M lsm9ds1M( - bus, - cs_M, - LSM9DS1_M::MagFSR::FS_8, - LSM9DS1_M::ODR::ODR_20 - ); - - while(!lsm9ds1X.init()){} LED1.high(); - while(!lsm9ds1M.init()){} + while (!lsm9ds1M.init()) + { + } LED2.high(); Thread::sleep(500); - + printf("time,ax,ay,az,gx,gy,gz,mx,my,mz,t\n"); - long long first_tick = getTick(); - for(;;) - { - long long last_tick = getTick(); + long long first_tick = getTick(); + for (;;) + { + long long last_tick = getTick(); lsm9ds1X.onSimpleUpdate(); adata = *(lsm9ds1X.accelDataPtr()); gdata = *(lsm9ds1X.gyroDataPtr()); tdata = *(lsm9ds1X.tempDataPtr()); - lsm9ds1M.onSimpleUpdate(); + lsm9ds1M.onSimpleUpdate(); mdata = *(lsm9ds1M.compassDataPtr()); printf("%d,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.1f\n", - (int)(last_tick - first_tick), - adata.getX(), adata.getY(), adata.getZ(), - gdata.getX(), gdata.getY(), gdata.getZ(), - mdata.getX(), mdata.getY(), mdata.getZ(), - tdata); - //printf("%.3f,%.3f,%.3f\n", mdata.getX(), mdata.getY(), mdata.getZ()); + (int)(last_tick - first_tick), adata.getX(), adata.getY(), + adata.getZ(), gdata.getX(), gdata.getY(), gdata.getZ(), + mdata.getX(), mdata.getY(), mdata.getZ(), tdata); + // printf("%.3f,%.3f,%.3f\n", mdata.getX(), mdata.getY(), mdata.getZ()); } - - + return 0; } \ No newline at end of file