diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp index 4980e5f80e02063f2baf4f14de730ef927d9d2b9..74d4ec8ac032f4b1c5cb213d88a98a251fb3b854 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp @@ -28,10 +28,9 @@ using miosix::GpioPin; using std::array; 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) + GyroFSR gyroRange, ODR odr, uint8_t temp_div_freq) + : spislave(bus, cs, {}), axelFSR(axelRange), gyroFSR(gyroRange), odr(odr), + temp_div_freq(temp_div_freq) { // SPI config spislave.config.clock_div = SPIClockDivider::DIV64; @@ -39,20 +38,23 @@ LSM9DS1_XLG::LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, AxelFSR axelRange, LSM9DS1_XLG::LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config, 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) + uint8_t temp_div_freq) + : spislave(bus, cs, config), axelFSR(axelRange), gyroFSR(gyroRange), + odr(odr), temp_div_freq(temp_div_freq) { } +void LSM9DS1_XLG::enable_fifo(uint8_t watermark) +{ + fifo_enabled = true; + fifo_watermark = watermark; +} + bool LSM9DS1_XLG::init() { - if (sensor_initialized) - { - TRACE("[LSM9DS1 XLG] init() : already initialized\n"); - return false; - } +#ifdef DEBUG + assert(sensor_initialized == false); +#endif SPITransaction spi(spislave); @@ -71,6 +73,13 @@ bool LSM9DS1_XLG::init() // FIFO setup: if (fifo_enabled) { + // check fifo-watermark <= 32 + if (fifo_watermark > 32) + { + TRACE("[LSM9DS1 XLG] init() : fifo watermark > 32, set to 32\n"); + fifo_watermark = 32; + } + // FIFO continous mode + fifo watermark threshold setup spi.write(regMapXLG::FIFO_CTRL, (FIFO_CTRL_VAL | fifo_watermark)); @@ -91,13 +100,13 @@ bool LSM9DS1_XLG::init() * (max), LPF2/HPF bypassed and disabled, axel output enabled by default * @ startup */ - uint8_t CTRL_REG6_XL_VAL = (int)odr << 5 | (int)axelFSR << 3; + uint8_t CTRL_REG6_XL_VAL = odr << 5 | 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; + uint8_t CTRL_REG1_G_VAL = odr << 5 | gyroFSR << 3; spi.write(regMapXLG::CTRL_REG1_G, CTRL_REG1_G_VAL); // sign and orientation Setup <--- BOARD DEPENDENT @@ -106,20 +115,24 @@ bool LSM9DS1_XLG::init() // Check all the registers have been written correctly if (spi.read(regMapXLG::CTRL_REG8) != CTRL_REG8_VAL) { + TRACE("[LSM9DS1 XLG] init() : CTRL_REG8 readback failed\n"); return false; } if (fifo_enabled) { if (spi.read(regMapXLG::FIFO_CTRL) != (FIFO_CTRL_VAL | fifo_watermark)) { + TRACE("[LSM9DS1 XLG] init() : FIFO_CTRL readback failed\n"); return false; } if (spi.read(regMapXLG::INT1_CTRL) != INT1_CTRL_VAL) { + TRACE("[LSM9DS1 XLG] init() : INT1_CTRL readback failed\n"); return false; } if (spi.read(regMapXLG::CTRL_REG9) != (CTRL_REG9_VAL | 0x02)) { + TRACE("[LSM9DS1 XLG] init() : CTRL_REG9 readback failed\n"); return false; } } @@ -127,85 +140,23 @@ bool LSM9DS1_XLG::init() { if (spi.read(regMapXLG::CTRL_REG9) != CTRL_REG9_VAL) { + TRACE("[LSM9DS1 XLG] init() : CTRL_REG9 readback failed\n"); return false; } } if (spi.read(regMapXLG::CTRL_REG6_XL) != CTRL_REG6_XL_VAL) { + TRACE("[LSM9DS1 XLG] init() : CTRL_REG6_XL readback failed\n"); return false; } if (spi.read(regMapXLG::CTRL_REG1_G) != CTRL_REG1_G_VAL) { + TRACE("[LSM9DS1 XLG] init() : CTRL_REG1_G readback failed\n"); return false; } - // select Sensitivity and ODR - switch (axelFSR) - - { - case AxelFSR::FS_2: - axelSensitivity = 0.598f; - break; - case AxelFSR::FS_4: - axelSensitivity = 1.196f; - break; - case AxelFSR::FS_8: - axelSensitivity = 2.393f; - break; - case AxelFSR::FS_16: - axelSensitivity = 7.178f; - break; - default: - axelSensitivity = 0.598f; - break; - } - - switch (gyroFSR) - { - case GyroFSR::FS_245: - gyroSensitivity = 8.75f; - break; - case GyroFSR::FS_500: - gyroSensitivity = 17.50f; - break; - case GyroFSR::FS_2000: - gyroSensitivity = 70.0f; - break; - default: - gyroSensitivity = 8.75f; - break; - } - - switch (odr) - { - case ODR::PWR_DW: - odrHz = 0.0f; - break; - case ODR::ODR_15: - odrHz = 14.9f; - break; - case ODR::ODR_60: - odrHz = 59.5f; - break; - case ODR::ODR_119: - odrHz = 119.0f; - break; - case ODR::ODR_238: - odrHz = 238.0f; - break; - case ODR::ODR_476: - odrHz = 476.0f; - break; - case ODR::ODR_952: - odrHz = 952.0f; - break; - default: - odrHz = 14.9f; - break; - } - - // discard first samples (see datasheet) - LSM9DS1_XLG::discardSamples(spi); + // clear FIFO and discard first samples + LSM9DS1_XLG::clearFIFO(spi); TRACE("[LSM9DS1 XLG] init() : done\n"); @@ -218,16 +169,20 @@ bool LSM9DS1_XLG::selfTest() { return true; } bool LSM9DS1_XLG::onSimpleUpdate() { +// you have to call init() before +#ifdef DEBUG + assert(sensor_initialized == true); +#endif + // if FIFO disabled if (!fifo_enabled) { - uint8_t data[12], tempData[2]; + uint8_t data[12]; // 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); } // compose signed 16-bit raw data as 2 bytes from the sensor @@ -240,41 +195,49 @@ bool LSM9DS1_XLG::onSimpleUpdate() 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; - //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); - - mLastTemp = tempZero + (temp / tempSensistivity); - + fifo[0].axelData = Vec3(x_xl * axelFSR_SMap.at(axelFSR), + y_xl * axelFSR_SMap.at(axelFSR), + z_xl * axelFSR_SMap.at(axelFSR)); + + fifo[0].gyroData = Vec3(x_gy * gyroFSR_SMap.at(gyroFSR), + y_gy * gyroFSR_SMap.at(gyroFSR), + z_gy * gyroFSR_SMap.at(gyroFSR)); + + fifo[0].timestamp = IRQ_timestamp; // 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 + * time. */ else { // 2 bytes per data * 3 axes per type * 2 types(axel+gyro) * // 32(FIFO DEPTH MAX) = 384 samples uint8_t buf[384]; + uint8_t overrun = 0; // Read output axel+gyro FIFO raw data X,Y,Z { SPITransaction spi(spislave); - spi.read(OUT_X_L_G, buf, fifo_watermark * 12); + + uint8_t fifo_src_reg; + spi.read(FIFO_SRC, &fifo_src_reg, 1); + + fifo_samples = fifo_src_reg & FIFO_UNREAD_MASK; + overrun = (fifo_src_reg & FIFO_OVERRUN_MASK) >> 6; + + spi.read(OUT_X_L_G, buf, fifo_samples * 12); } + // compute delta time for each sample + uint64_t dt = delta / last_fifo_level; + fifo_num++; + // convert & store - for (int i = 0; i < fifo_watermark; i++) + for (uint8_t i = 0; i < fifo_samples; ++i) { // compose signed 16-bit raw data as 2 bytes from the sensor // clang-format off @@ -287,18 +250,39 @@ bool LSM9DS1_XLG::onSimpleUpdate() 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); + fifo[i].gyroData = Vec3(x_gy * gyroFSR_SMap.at(gyroFSR), + y_gy * gyroFSR_SMap.at(gyroFSR), + z_gy * gyroFSR_SMap.at(gyroFSR)); + fifo[i].axelData = Vec3(x_xl * axelFSR_SMap.at(axelFSR), + y_xl * axelFSR_SMap.at(axelFSR), + z_xl * axelFSR_SMap.at(axelFSR)); // clang-format on + + fifo[i].timestamp = IRQ_timestamp - ((int)fifo_watermark - (int)i - 1) * dt; + fifo[i].unread = fifo_samples; + fifo[i].overrun = (bool)overrun; + fifo[i].fifo_num = (uint16_t)i; } + last_fifo_level = fifo_samples; } + + // temperature update if temp_count = temp_div_freq + temp_count++; + if (temp_count == temp_div_freq) + { + temp_count = 0; + LSM9DS1_XLG::temperatureUpdate(); + } + return true; } +void LSM9DS1_XLG::updateTimestamp(uint64_t timestamp) +{ + delta = timestamp - IRQ_timestamp; + IRQ_timestamp = timestamp; +} + bool LSM9DS1_XLG::temperatureUpdate() { // Read output temp raw data @@ -310,22 +294,28 @@ bool LSM9DS1_XLG::temperatureUpdate() // compose signed 16-bit raw data as 2 bytes from the sensor int16_t temp = tempData[0] | tempData[1] << 8; // convert raw data - mLastTemp = tempZero + temp / tempSensistivity; + lastTemp.tempData = tempZero + temp / tempSensistivity; + lastTemp.timestamp = IRQ_timestamp; return true; } -void LSM9DS1_XLG::clearFIFO() +const array<lsm9ds1XLGSample, 32>& LSM9DS1_XLG::getFIFO() const { return fifo; } + +const uint8_t& LSM9DS1_XLG::getFIFOdepth() const { return fifo_samples; } + +const lsm9ds1XLGSample& LSM9DS1_XLG::getXLGSample() const { return fifo[0]; } + +const lsm9ds1TSample& LSM9DS1_XLG::getTSample() const { return lastTemp; } + +void LSM9DS1_XLG::clearFIFO(SPITransaction& spi) { - SPITransaction spi(spislave); spi.write(FIFO_CTRL, 0); // Bypass Mode - miosix::Thread::sleep(20); // Wait spi.write(FIFO_CTRL, FIFO_CTRL_VAL | fifo_watermark); // re-enable FIFO - LSM9DS1_XLG::discardSamples(spi); -} -const array<lsm9ds1XLGSample, 32>& LSM9DS1_XLG::getLsm9ds1FIFO() const -{ - return fifo; + // sleep 20ms for stable output + miosix::Thread::sleep(20); + + LSM9DS1_XLG::discardSamples(spi); } void LSM9DS1_XLG::discardSamples(SPITransaction& spi) @@ -334,14 +324,14 @@ void LSM9DS1_XLG::discardSamples(SPITransaction& spi) if (odr != ODR::PWR_DW) { // wait samples to be overwritten or stored (FIFO on) - uint16_t toWait_ms = samplesToDiscard * 1000 / odrHz; + uint16_t toWait_ms = SAMPLES_TO_DISCARD * 1000 / odr_Map.at(odr); miosix::Thread::sleep(toWait_ms); - // if FIFO is enabled, read first "samplesToDiscard" samples and + // if FIFO is enabled, read first "SAMPLES_TO_DISCARD" samples and // discard them. if (fifo_enabled) { - spi.read(OUT_X_L_G, samplesToDiscard * 12); + spi.read(OUT_X_L_G, SAMPLES_TO_DISCARD * 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 2be91afcc0b020e9d0fc4aec99af2aa1a5f13174..43bed351236de6c8d83e7cd863dea6c1813c5227 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h @@ -45,7 +45,7 @@ class LSM9DS1_XLG : public GyroSensor, public TemperatureSensor { public: - enum AxelFSR + enum AxelFSR : uint8_t { FS_2 = 0x00, // +/- 2g FS_16 = 0x01, // +/- 16g @@ -53,14 +53,14 @@ public: FS_8 = 0x03 // +/- 8g }; - enum GyroFSR + enum GyroFSR : uint8_t { FS_245 = 0x00, // +/- 245dps FS_500 = 0x01, // +/- 500dps FS_2000 = 0x03 // +/- 2000dps }; - enum ODR + enum ODR : uint8_t { PWR_DW = 0X00, // power down ODR_15 = 0X01, // 15Hz @@ -71,6 +71,30 @@ public: ODR_952 = 0X06 // 952Hz }; + // clang-format off + + //Sesitivity Map (axelFSR) + const std::map<AxelFSR, float> axelFSR_SMap{{FS_2, 0.000598f}, + {FS_4, 0.001196f}, + {FS_8, 0.002393f}, + {FS_16, 0.007178f}}; + + //Sesitivity Map (gyroFSR) + const std::map<GyroFSR, float> gyroFSR_SMap{{FS_245, 0.0001527f}, + {FS_500, 0.0003054f}, + {FS_2000, 0.0012217f}}; + + //ODR Map + const std::map<ODR, float> odr_Map{{PWR_DW, 0.0f }, + {ODR_15, 14.9f }, + {ODR_60, 59.5f }, + {ODR_119, 119.0f}, + {ODR_238, 238.0f}, + {ODR_476, 476.0f}, + {ODR_952, 952.0f}}; + + // clang-format on + /** * @brief Creates an instance of an LSM9DS1 accelerometer + gyroscope * sensor. @@ -80,15 +104,13 @@ public: * @param axelRange accelerometer Full Scale Range (See datasheet) * @param gyroRange gyroscope Full Scale Range (See datasheet) * @param odr Output Data Rate (See datasheet) - * @param fifo_enabled Fifo enabled - * @param fifo_watermark FIFO watermark level in range [1,32] (used for - * interrupt generation, see datasheet). + * @param temp_div_freq Temperature update frequency division */ 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); + AxelFSR axelRange = AxelFSR::FS_16, + GyroFSR gyroRange = GyroFSR::FS_2000, ODR odr = ODR::ODR_952, + uint8_t temp_div_freq = 10); /** * @brief Creates an instance of an LSM9DS1 accelerometer + gyroscope @@ -100,15 +122,22 @@ public: * @param axelRange accelerometer Full Scale Range (See datasheet) * @param gyroRange gyroscope Full Scale Range (See datasheet) * @param odr Output Data Rate (See datasheet) - * @param fifo_enabled Fifo enabled - * @param fifo_watermark FIFO watermark level in range [1,32] (used for - * interrupt generation, see datasheet). + * @param temp_div_freq Temperature update frequency division */ 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 = AxelFSR::FS_16, + GyroFSR gyroRange = GyroFSR::FS_2000, ODR odr = ODR::ODR_952, + uint8_t temp_div_freq = 10); + + /** + * @brief enables LSM9DS1 embedded FIFO. + * @param fifo_watermark FIFO watermark level in range [1,32] (used for + * interrupt generation, see datasheet). + * @warning call before init() member + */ + + void enable_fifo(uint8_t watermark); /** * @brief initializes the LSM9DS1 Sensor (Accelerometer + Gyroscope). @@ -127,7 +156,9 @@ public: bool selfTest() override; /** - * @brief Dump single reading of Axel+Gyro+Temp from the sensor through SPI. + * @brief Dump single reading of Axel+Gyro+Temp from the sensor + * (if FIFO disabled) or dump fifo_watermark samples from the FIFO (if FIFO + * enabled) through SPI. * @return true if sensor sends data * @warning if FIFO is enabled, call only after interrupt flag from the * sensor has been set @@ -136,19 +167,25 @@ public: bool onSimpleUpdate() override; /** - * @brief Dump single read of Temperature from the sensor through SPI. - * @return true if sensor sends data + * @brief set timestamp on last FIFO + * @warning remember to update FIFO data calling onSimpleUpdate */ - bool temperatureUpdate(); + void updateTimestamp(uint64_t timestamp); /** - * @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. + * @brief get last valid sample + * @return sample + */ + + const lsm9ds1XLGSample& getXLGSample() const; + + /** + * @brief get last valid sample + * @return sample */ - void clearFIFO(); + const lsm9ds1TSample& getTSample() const; /** * @brief get FIFO dumped after calling onSimpleUpdate() - Just on FIFO @@ -156,9 +193,30 @@ public: * @return array containing the whole FIFO */ - const array<lsm9ds1XLGSample, 32>& getLsm9ds1FIFO() const; + const array<lsm9ds1XLGSample, 32>& getFIFO() const; + + /** + * @brief get number of samples inside the last FIFO + * @return number of samples + */ + + const uint8_t& getFIFOdepth() const; private: + /** + * @brief Dump single read of Temperature from the sensor through SPI. + * @return true if sensor sends data + */ + + bool temperatureUpdate(); + + /** + * @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(SPITransaction& spi); /** * @brief discard some samples from the sensor. Must be used when switching. * from FIFO mode to CONTINUOUS mode (or viceversa) and during power on @@ -167,9 +225,18 @@ private: void discardSamples(SPITransaction& spi); bool sensor_initialized = false; - bool fifo_enabled; + + bool fifo_enabled = false; uint8_t fifo_watermark; + uint8_t fifo_samples; + uint8_t last_fifo_level = 20; + + // + uint64_t IRQ_timestamp = 0; + uint32_t delta = 0; + array<lsm9ds1XLGSample, 32> fifo; + lsm9ds1TSample lastTemp; SPISlave spislave; @@ -177,12 +244,10 @@ private: GyroFSR gyroFSR; ODR odr; - float axelSensitivity; - float gyroSensitivity; - float odrHz; - float tempZero = 25.0f; - float tempSensistivity = 16.0f; - static const uint8_t samplesToDiscard = 8; // max possible val + float tempZero = 25.0f; + float tempSensistivity = 16.0f; + uint8_t temp_div_freq; + uint8_t temp_count = 0; /** * @brief Registers' addresses definition. @@ -241,9 +306,14 @@ private: INT_GEN_DUR_G = 0x37 }; - static const uint8_t INT1_CTRL_VAL = 0x08; - static const uint8_t WHO_AM_I_XLG_VAL = 0x68; - static const uint8_t CTRL_REG8_VAL = 0x04; - static const uint8_t CTRL_REG9_VAL = 0x04; - static const uint8_t FIFO_CTRL_VAL = 0xC0; + static const uint8_t INT1_CTRL_VAL = 0x08; + static const uint8_t WHO_AM_I_XLG_VAL = 0x68; + static const uint8_t CTRL_REG8_VAL = 0x04; + static const uint8_t CTRL_REG9_VAL = 0x04; + static const uint8_t FIFO_CTRL_VAL = 0xC0; + static const uint8_t FIFO_UNREAD_MASK = 0x3F; + static const uint8_t FIFO_OVERRUN_MASK = 0x40; + static const uint8_t SAMPLES_TO_DISCARD = 8; + + uint16_t fifo_num = 0; }; diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h b/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h index 0d4f83aaa890065309013809136f18f5dd4fd0af..e07edc11fc3d05bbb05706cbd69351131722cd68 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h @@ -40,16 +40,21 @@ struct lsm9ds1XLGSample uint64_t timestamp; Vec3 axelData; Vec3 gyroData; + uint16_t unread; + uint16_t fifo_num; + bool overrun; static std::string header() { - return "timestamp,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z\n"; + return "timestamp,unread,overrun,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_" + "z\n"; } void print(std::ostream& os) const { - os << timestamp << "," << axelData.getX() << "," << axelData.getY() - << "," << axelData.getZ() << "," << gyroData.getX() << "," + os << timestamp << "," << unread << "," << overrun << "," << fifo_num + << "," << axelData.getX() << "," << axelData.getY() << "," + << axelData.getZ() << "," << gyroData.getX() << "," << gyroData.getY() << "," << gyroData.getZ() << "\n"; } }; @@ -81,4 +86,4 @@ struct lsm9ds1MSample os << timestamp << "," << magData.getX() << "," << magData.getY() << "," << magData.getZ() << "\n"; } -}; +}; \ No newline at end of file diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp index 743a26acc4044015518ab7d559e2926d6f0279d6..828963d182a0d8d3e06269f38b9f42f5d7c7a44a 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp @@ -128,17 +128,31 @@ bool LSM9DS1_M::selfTest() Stats stX, stY, stZ; bool selfTestResult = false; + nostX.reset(); + nostY.reset(); + nostZ.reset(); + selfTest_mode = true; + // if already initialized + if (sensor_initialized == true) + { + TRACE("[LSM9DS1 MAG] selfTest() : sensor already initialized!\n"); +#ifdef DEBUG + assert(sensor_initialized == false); +#endif + return false; + } + { SPITransaction spi(spislave); - // Reset all registers (if called after init()) + // Reset all registers 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 + // 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); @@ -156,7 +170,9 @@ bool LSM9DS1_M::selfTest() { } - spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, 6); + uint8_t data[6]; + spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, data, 6); + UNUSED(data); } // wait data-ready bit on STATUS REG - read NOST sample at least 5 times @@ -176,7 +192,9 @@ bool LSM9DS1_M::selfTest() { } + uint8_t data[6]; spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, 6); + UNUSED(data); } LSM9DS1_M::getSelfTestData(stX, stY, stZ); @@ -185,17 +203,18 @@ bool LSM9DS1_M::selfTest() float deltaY = fabsf(stY.getStats().mean - nostY.getStats().mean); float deltaZ = fabsf(stZ.getStats().mean - nostZ.getStats().mean); -// verify if sensor is inside parameters + // verify if sensor is inside parameters -// print stats + // 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; + std::cout << "[LSM9DS1 MAG] selfTest() : statistics" + << "\n" + << "X-AXIS stats : " << nostX.getStats() << "\n" + << "Y-AXIS stats : " << nostY.getStats() << "\n" + << "Z-AXIS stats : " << nostZ.getStats() << "\n" + << "deltaX : " << deltaX << "\n" + << "deltaY : " << deltaY << "\n" + << "deltaZ : " << deltaZ << "\n"; #endif // clang-format off @@ -214,15 +233,12 @@ bool LSM9DS1_M::selfTest() } // clang-format on - - // re-init if necessary - if(sensor_initialized == true) { - sensor_initialized = false; - LSM9DS1_M::init(); + SPITransaction spi(spislave); + spi.write(regMapM::CTRL_REG2_M, SOFT_RESET); + miosix::Thread::sleep(20); } - - selfTest_mode = true; + selfTest_mode = false; return selfTestResult; } @@ -240,15 +256,11 @@ void LSM9DS1_M::getSelfTestData(Stats& outxStats, Stats& outyStats, } 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() @@ -263,9 +275,10 @@ bool LSM9DS1_M::onSimpleUpdate() { SPITransaction spi(spislave); spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, magData, 6); - lastMagneto.timestamp = miosix::getTick(); } + lastMagneto.timestamp = miosix::getTick(); + // compose signed 16-bit raw data as 2 bytes from the sensor // clang-format off int16_t x = magData[0] | magData[1] << 8; @@ -279,4 +292,6 @@ bool LSM9DS1_M::onSimpleUpdate() // clang-format on return true; -} \ No newline at end of file +} + +const lsm9ds1MSample& LSM9DS1_M::getSample() const { return lastMagneto; } \ 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 f53638fa9593904da9805c7e7b66a52b5b9ddc79..8dd422f6be2a789704526664901b93157a43f008 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h @@ -105,6 +105,7 @@ public: /** * @brief Run a self-test of the Sensor. * @return true if sensor behave correclty + * @warning blocking. */ bool selfTest() override; @@ -115,8 +116,20 @@ public: */ bool onSimpleUpdate() override; + + /** + * @brief get last valid sample + * @return sample + */ + + const lsm9ds1MSample& getSample() const; private: + + /** + * @brief get data from sensor and compute stats. + */ + void getSelfTestData(Stats& outx, Stats& outy, Stats& outz); bool sensor_initialized = false; diff --git a/src/tests/drivers/test-lsm9ds1-fifo.cpp b/src/tests/drivers/test-lsm9ds1-fifo.cpp index 25b84447edee848e7db21bcf73ee4f7d09b1f660..8cc6f6e3b7c29a406a50f1256faaabdcec0e2b02 100644 --- a/src/tests/drivers/test-lsm9ds1-fifo.cpp +++ b/src/tests/drivers/test-lsm9ds1-fifo.cpp @@ -55,14 +55,14 @@ GpioPin LED3(GPIOD_BASE, 14); GpioPin PUSHBUTTON(GPIOA_BASE, 0); // SPI read flag -volatile bool flagSPIReadRequest = false; +//volatile bool flagSPIReadRequest = false; // IMU obj variables -static const bool FIFO_ENABLED = true; -static const uint8_t FIFO_WATERMARK = 20; -static const uint16_t FIFO_SAMPLES = 1000; -static const uint16_t MAG_SAMPLING_PERIOD = 10; // 100Hz -static const uint16_t TEMP_SAMPLING_PERIOD = 100; // 10Hz +static const uint8_t FIFO_WATERMARK = 12; +static const uint16_t FIFO_SAMPLES = 1000; +static const uint8_t TEMP_DIV_FREQ = 20; +static const uint16_t MAG_SAMPLING_PERIOD = 10; // 100Hz +static const uint16_t FIFO_SAMPLING_PERIOD = 25.5 * 1000 / 476.0f; // High Resolution hardware timer using TIM5 HardwareTimer<uint32_t> hrclock( @@ -86,13 +86,11 @@ void __attribute__((naked)) EXTI13_IRQHandler() void __attribute__((used)) EXTI13_IRQHandlerImpl() { - // Computing delta beetween interrupts - uint32_t tick = hrclock.tick(); - delta = tick - last_tick; - last_tick = tick; + // Update timestamp + lsm9ds1_xlg->updateTimestamp(hrclock.toMicroSeconds(hrclock.tick())); // Set read flag - flagSPIReadRequest = true; + //flagSPIReadRequest = true; // Built-in LED on LED1.high(); @@ -109,10 +107,9 @@ void printStats(void*); int main() { - uint32_t dt; - uint64_t XLGtimestamp = 0; - uint64_t lastMagtick = 0; - uint64_t lastTemptick = 0; + uint64_t lastMagtick = 0; + uint64_t lastFifotick = 0; + uint16_t lastTempcount = 0; // Spawn thread for loggings logger stats Thread::create(printStats, 4096); @@ -125,88 +122,84 @@ int main() timer5Config(); EXTI1Config(); - lsm9ds1_xlg = new LSM9DS1_XLG( - bus, cs_XLG, LSM9DS1_XLG::AxelFSR::FS_8, LSM9DS1_XLG::GyroFSR::FS_245, - LSM9DS1_XLG::ODR::ODR_238, FIFO_ENABLED, FIFO_WATERMARK); + lsm9ds1_xlg = new LSM9DS1_XLG(bus, cs_XLG, LSM9DS1_XLG::AxelFSR::FS_8, + LSM9DS1_XLG::GyroFSR::FS_245, + LSM9DS1_XLG::ODR::ODR_476, TEMP_DIV_FREQ); lsm9ds1_m = new LSM9DS1_M(bus, cs_M, LSM9DS1_M::MagFSR::FS_8, - LSM9DS1_M::ODR::ODR_40); + LSM9DS1_M::ODR::ODR_80); + // enable FIFO + lsm9ds1_xlg->enable_fifo(FIFO_WATERMARK); + + // perform self-tests + lsm9ds1_xlg->selfTest(); + lsm9ds1_m->selfTest(); + + // initialize sensor while (!lsm9ds1_xlg->init()) ; while (!lsm9ds1_m->init()) ; LED2.high(); // init OK - // just to be sure to intercept the first interrupt rising edge - lsm9ds1_xlg->clearFIFO(); - // sampling until you push the button while (!PUSHBUTTON.value()) { - //ACCELEROMETER + GYROSCOPE UPDATE (FIFO) - //an interrupt is set: time to dump the FIFO - if (flagSPIReadRequest) + // ACCELEROMETER + GYROSCOPE + UPDATE (FIFO) + // TEMPERATURE UPDATE + // an interrupt is set: time to dump the FIFO + //if (flagSPIReadRequest) + if (miosix::getTick() - lastFifotick >= FIFO_SAMPLING_PERIOD) { - flagSPIReadRequest = false; - - // delta of each sample = delta beetween interrupts / #samples - dt = hrclock.toMicroSeconds(delta) / FIFO_WATERMARK; - - //dump the fifo + //flagSPIReadRequest = false; + lastFifotick = miosix::getTick(); + // dump the fifo lsm9ds1_xlg->onSimpleUpdate(); - //update each timestamp and log the sample (da integrare su driver?) - for (int i = 0; i < FIFO_WATERMARK; i++) + // log each sample + for (int i = 0; i < lsm9ds1_xlg->getFIFOdepth() ; i++) { - lsm9ds1XLGSample XLGsample = lsm9ds1_xlg->getLsm9ds1FIFO()[i]; - XLGtimestamp += dt; - XLGsample.timestamp = XLGtimestamp; + lsm9ds1XLGSample XLGsample = lsm9ds1_xlg->getFIFO()[i]; logger.log(XLGsample); } + if (lastTempcount == TEMP_DIV_FREQ) + { + lastTempcount = 0; + lsm9ds1TSample Tsample = lsm9ds1_xlg->getTSample(); + logger.log(Tsample); + } + lastTempcount++; + LED1.low(); } - //MAGNETOMETER UPDATE (SIMPLE) + // MAGNETOMETER UPDATE (SIMPLE) if (miosix::getTick() - lastMagtick >= MAG_SAMPLING_PERIOD) - { + { lastMagtick = miosix::getTick(); - - //get sample from the sensor + + // get sample from the sensor lsm9ds1_m->onSimpleUpdate(); - - //update timestamp and log the sample - lsm9ds1MSample MAGsample; - MAGsample.magData = *(lsm9ds1_m->compassDataPtr()); - MAGsample.timestamp = lastMagtick; + + // log the sample + lsm9ds1MSample MAGsample = lsm9ds1_m->getSample(); logger.log(MAGsample); } - //TEMPERATURE UPDATE (SIMPLE) - if (miosix::getTick() - lastTemptick >= TEMP_SAMPLING_PERIOD) - { - lastTemptick = miosix::getTick(); - - //get sample from the sensor - lsm9ds1_xlg->temperatureUpdate(); - - //update timestamp and log the sample - lsm9ds1TSample Tsample; - Tsample.tempData = *(lsm9ds1_xlg->tempDataPtr()); - Tsample.timestamp = lastTemptick; - logger.log(Tsample); - } + Thread::sleep(FIFO_SAMPLING_PERIOD); + } - //stop log + // stop log logger.stop(); LED1.low(); LED2.low(); Thread::sleep(10000); - reboot(); + miosix::reboot(); while (1) ; diff --git a/src/tests/drivers/test-lsm9ds1.cpp b/src/tests/drivers/test-lsm9ds1.cpp index c93b66ea01449ed8db8fb14178539b45486fc6a3..ad7e59812370c890b0a5f0d4131e1d7e59563e06 100644 --- a/src/tests/drivers/test-lsm9ds1.cpp +++ b/src/tests/drivers/test-lsm9ds1.cpp @@ -33,7 +33,8 @@ typedef Gpio<GPIOA_BASE, 5> GpioSck; typedef Gpio<GPIOA_BASE, 6> GpioMiso; typedef Gpio<GPIOA_BASE, 7> GpioMosi; -static const bool FIFO_ENABLED = false; +static const bool FIFO_ENABLED = false; +static const uint8_t TEMP_DIV_FREQ = 20; // SPI SPIBus bus(SPI1); @@ -46,9 +47,9 @@ GpioPin LED2(GPIOD_BASE, 13); int main() { - - Vec3 adata, gdata, mdata; - float tdata; + lsm9ds1XLGSample agdata; + lsm9ds1MSample mdata; + lsm9ds1TSample tdata; { FastInterruptDisableLock dLock; @@ -79,11 +80,13 @@ int main() LSM9DS1_XLG lsm9ds1X(bus, cs_XLG, LSM9DS1_XLG::AxelFSR::FS_8, LSM9DS1_XLG::GyroFSR::FS_245, - LSM9DS1_XLG::ODR::ODR_952); + LSM9DS1_XLG::ODR::ODR_952, TEMP_DIV_FREQ); - LSM9DS1_M lsm9ds1M(bus, cs_M, LSM9DS1_M::MagFSR::FS_8, - LSM9DS1_M::ODR::ODR_20); + LSM9DS1_M lsm9ds1M(bus, cs_M, LSM9DS1_M::MagFSR::FS_12, + LSM9DS1_M::ODR::ODR_80); + lsm9ds1M.selfTest(); + while (!lsm9ds1X.init()) { } @@ -94,33 +97,32 @@ int main() } LED2.high(); - Thread::sleep(500); + Thread::sleep(5000); printf("time,ax,ay,az,gx,gy,gz,mx,my,mz,t\n"); - long long first_tick = getTick(); for (;;) { - //get timestamp - long long last_tick = getTick(); - //get axel+gyro+temp data + // get axel+gyro+temp data lsm9ds1X.onSimpleUpdate(); - adata = *(lsm9ds1X.accelDataPtr()); - gdata = *(lsm9ds1X.gyroDataPtr()); - tdata = *(lsm9ds1X.tempDataPtr()); + agdata = lsm9ds1X.getXLGSample(); + tdata = lsm9ds1X.getTSample(); + lsm9ds1X.updateTimestamp(miosix::getTick()); - //get magneto data + // get magneto data lsm9ds1M.onSimpleUpdate(); - mdata = *(lsm9ds1M.compassDataPtr()); + mdata = lsm9ds1M.getSample(); // clang-format off - 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("%d,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%d,%.3f,%.3f,%.3f,%d,%.1f\n", + (int)agdata.timestamp, + agdata.axelData.getX(), agdata.axelData.getY(), agdata.axelData.getZ(), + agdata.gyroData.getX(), agdata.gyroData.getY(), agdata.gyroData.getZ(), + (int)mdata.timestamp, + mdata.magData.getX(), mdata.magData.getY(), mdata.magData.getZ(), + (int)tdata.timestamp, + tdata.tempData); // clang-format on