diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp index 74d4ec8ac032f4b1c5cb213d88a98a251fb3b854..6ff0b898f76246d64f2880398c9886970cc9f224 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp @@ -196,7 +196,7 @@ bool LSM9DS1_XLG::onSimpleUpdate() int16_t z_xl = data[10] | data[11] << 8; //convert raw data - fifo[0].axelData = Vec3(x_xl * axelFSR_SMap.at(axelFSR), + /*fifo[0].axelData = Vec3(x_xl * axelFSR_SMap.at(axelFSR), y_xl * axelFSR_SMap.at(axelFSR), z_xl * axelFSR_SMap.at(axelFSR)); @@ -204,7 +204,7 @@ bool LSM9DS1_XLG::onSimpleUpdate() y_gy * gyroFSR_SMap.at(gyroFSR), z_gy * gyroFSR_SMap.at(gyroFSR)); - fifo[0].timestamp = IRQ_timestamp; + fifo[0].timestamp = IRQ_timestamp;*/ // clang-format on } @@ -217,23 +217,26 @@ bool LSM9DS1_XLG::onSimpleUpdate() // 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; + uint8_t overrun = 0; + uint32_t delta_transaction = 0; // Read output axel+gyro FIFO raw data X,Y,Z { + delta_transaction = miosix::getTick(); SPITransaction spi(spislave); 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; + overrun = (fifo_src_reg & FIFO_OVERRUN_MASK) >> 6; spi.read(OUT_X_L_G, buf, fifo_samples * 12); + delta_transaction = miosix::getTick() - delta_transaction; } // compute delta time for each sample - uint64_t dt = delta / last_fifo_level; + uint64_t dt = delta / fifo_watermark; fifo_num++; // convert & store @@ -250,20 +253,25 @@ 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 * gyroFSR_SMap.at(gyroFSR), + /*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)); + 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; + fifo[i].timestamp = + IRQ_timestamp - ((int)fifo_watermark - (int)i - 1) * dt; + //fifo[i].fifo_num = fifo_num; } - last_fifo_level = fifo_samples; + + fifodebug.fifo_num = fifo_num; + fifodebug.overrun = (bool)overrun; + fifodebug.unread = fifo_samples; + fifodebug.transaction_time = delta_transaction; + + last_fifo_level = fifo_samples; // unused } // temperature update if temp_count = temp_div_freq @@ -307,6 +315,8 @@ const lsm9ds1XLGSample& LSM9DS1_XLG::getXLGSample() const { return fifo[0]; } const lsm9ds1TSample& LSM9DS1_XLG::getTSample() const { return lastTemp; } +const lsm9ds1debug& LSM9DS1_XLG::getFIFOStats() const { return fifodebug; } + void LSM9DS1_XLG::clearFIFO(SPITransaction& spi) { spi.write(FIFO_CTRL, 0); // Bypass Mode diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h index 43bed351236de6c8d83e7cd863dea6c1813c5227..9feda81c4ee489317e111f389bab20feb9058543 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h @@ -195,6 +195,14 @@ public: const array<lsm9ds1XLGSample, 32>& getFIFO() const; + /** + * @brief get FIFO stats after calling onSimpleUpdate() - Just on FIFO + * mode. + * @return stats + */ + + const lsm9ds1debug& getFIFOStats() const; + /** * @brief get number of samples inside the last FIFO * @return number of samples @@ -236,6 +244,8 @@ private: uint32_t delta = 0; array<lsm9ds1XLGSample, 32> fifo; + lsm9ds1debug fifodebug; + lsm9ds1TSample lastTemp; SPISlave spislave; diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h b/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h index e07edc11fc3d05bbb05706cbd69351131722cd68..bc0e87659866abb3f57b317159b8d5a866d7477c 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h @@ -37,25 +37,41 @@ using std::ofstream; // ACCELEROMETER + GYROSCOPE struct lsm9ds1XLGSample { + + //uint16_t fifo_num; uint64_t timestamp; - Vec3 axelData; - Vec3 gyroData; - uint16_t unread; + //Vec3 axelData; + //Vec3 gyroData; + + /*static std::string header() + { + return "fifo_num,timestamp,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z\n"; + }*/ + + /*void print(std::ostream& os) const + { + os << fifo_num << "," << timestamp << "," << axelData.getX() << "," << axelData.getY() + << "," << axelData.getZ() << "," << gyroData.getX() << "," + << gyroData.getY() << "," << gyroData.getZ() << "\n"; + }*/ +}; + +// XLG debug +struct lsm9ds1debug +{ uint16_t fifo_num; + uint16_t unread; bool overrun; + uint64_t transaction_time; static std::string header() { - return "timestamp,unread,overrun,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_" - "z\n"; + return "fifo_num,unread,overrun,transaction_time\n"; } void print(std::ostream& os) const { - os << timestamp << "," << unread << "," << overrun << "," << fifo_num - << "," << axelData.getX() << "," << axelData.getY() << "," - << axelData.getZ() << "," << gyroData.getX() << "," - << gyroData.getY() << "," << gyroData.getZ() << "\n"; + os << fifo_num << "," << unread << "," << overrun << "," << transaction_time << "\n"; } }; diff --git a/src/tests/drivers/test-lsm9ds1-fifo.cpp b/src/tests/drivers/test-lsm9ds1-fifo.cpp index 8cc6f6e3b7c29a406a50f1256faaabdcec0e2b02..26e2f0979ddadb0c8e0beed807d01865b71f16ad 100644 --- a/src/tests/drivers/test-lsm9ds1-fifo.cpp +++ b/src/tests/drivers/test-lsm9ds1-fifo.cpp @@ -55,7 +55,7 @@ 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 uint8_t FIFO_WATERMARK = 12; @@ -90,7 +90,7 @@ void __attribute__((used)) EXTI13_IRQHandlerImpl() lsm9ds1_xlg->updateTimestamp(hrclock.toMicroSeconds(hrclock.tick())); // Set read flag - //flagSPIReadRequest = true; + flagSPIReadRequest = true; // Built-in LED on LED1.high(); @@ -124,7 +124,7 @@ int main() 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_XLG::ODR::ODR_952, TEMP_DIV_FREQ); lsm9ds1_m = new LSM9DS1_M(bus, cs_M, LSM9DS1_M::MagFSR::FS_8, LSM9DS1_M::ODR::ODR_80); @@ -141,7 +141,6 @@ int main() ; while (!lsm9ds1_m->init()) ; - LED2.high(); // init OK // sampling until you push the button while (!PUSHBUTTON.value()) @@ -149,11 +148,12 @@ int main() // ACCELEROMETER + GYROSCOPE + UPDATE (FIFO) // TEMPERATURE UPDATE // an interrupt is set: time to dump the FIFO - //if (flagSPIReadRequest) - if (miosix::getTick() - lastFifotick >= FIFO_SAMPLING_PERIOD) + + //if (miosix::getTick() - lastFifotick >= FIFO_SAMPLING_PERIOD) + if (flagSPIReadRequest) { - //flagSPIReadRequest = false; - lastFifotick = miosix::getTick(); + flagSPIReadRequest = false; + //lastFifotick = miosix::getTick(); // dump the fifo lsm9ds1_xlg->onSimpleUpdate(); @@ -161,14 +161,18 @@ int main() for (int i = 0; i < lsm9ds1_xlg->getFIFOdepth() ; i++) { lsm9ds1XLGSample XLGsample = lsm9ds1_xlg->getFIFO()[i]; + LED2.high(); logger.log(XLGsample); + LED2.low(); } + //lsm9ds1debug debugstats = lsm9ds1_xlg->getFIFOStats(); + //logger.log(debugstats); if (lastTempcount == TEMP_DIV_FREQ) { lastTempcount = 0; lsm9ds1TSample Tsample = lsm9ds1_xlg->getTSample(); - logger.log(Tsample); + //logger.log(Tsample); } lastTempcount++; @@ -185,10 +189,10 @@ int main() // log the sample lsm9ds1MSample MAGsample = lsm9ds1_m->getSample(); - logger.log(MAGsample); + //logger.log(MAGsample); } - Thread::sleep(FIFO_SAMPLING_PERIOD); + //Thread::sleep(FIFO_SAMPLING_PERIOD); }