diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.cpp index 6ef5aa2283fd023d068adf9692518975b4b5efb4..d9a9218471d9de68537741fafb964eb3585d9a4a 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 } @@ -218,11 +218,9 @@ bool LSM9DS1_XLG::onSimpleUpdate() // 32(FIFO DEPTH MAX) = 384 samples uint8_t buf[384]; 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; @@ -232,7 +230,6 @@ bool LSM9DS1_XLG::onSimpleUpdate() 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 @@ -253,25 +250,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), - y_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].fifo_num = fifo_num; + fifo[i].fifo_num = fifo_num; } + //store Stats fifodebug.fifo_num = fifo_num; fifodebug.overrun = (bool)overrun; fifodebug.unread = fifo_samples; - fifodebug.transaction_time = delta_transaction; - last_fifo_level = fifo_samples; // unused + last_fifo_level = fifo_samples; } // temperature update if temp_count = temp_div_freq diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h index 9feda81c4ee489317e111f389bab20feb9058543..8629533c488decc1029c88f70b06229c2036082d 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h @@ -167,7 +167,8 @@ public: bool onSimpleUpdate() override; /** - * @brief set timestamp on last FIFO + * @brief set timestamp on last FIFO (if FIFO enabled) or set timestamp for + * sample * @warning remember to update FIFO data calling onSimpleUpdate */ @@ -195,7 +196,7 @@ public: const array<lsm9ds1XLGSample, 32>& getFIFO() const; - /** + /** * @brief get FIFO stats after calling onSimpleUpdate() - Just on FIFO * mode. * @return stats @@ -208,7 +209,7 @@ public: * @return number of samples */ - const uint8_t& getFIFOdepth() const; + const uint8_t& getFIFOdepth() const; private: /** @@ -244,7 +245,7 @@ private: uint32_t delta = 0; array<lsm9ds1XLGSample, 32> fifo; - lsm9ds1debug fifodebug; + lsm9ds1debug fifodebug; lsm9ds1TSample lastTemp; diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h b/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h index bc0e87659866abb3f57b317159b8d5a866d7477c..af5187a2ce3da8d5b1cccf75bf1a9828524a5a90 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Data.h @@ -37,41 +37,39 @@ using std::ofstream; // ACCELEROMETER + GYROSCOPE struct lsm9ds1XLGSample { - - //uint16_t fifo_num; + uint16_t fifo_num; uint64_t timestamp; - //Vec3 axelData; - //Vec3 gyroData; + Vec3 axelData; + Vec3 gyroData; - /*static std::string header() + 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 + 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 +// FIFO debug struct lsm9ds1debug { uint16_t fifo_num; uint16_t unread; bool overrun; - uint64_t transaction_time; static std::string header() { - return "fifo_num,unread,overrun,transaction_time\n"; + return "fifo_num,unread,overrun\n"; } void print(std::ostream& os) const { - os << fifo_num << "," << unread << "," << overrun << "," << transaction_time << "\n"; + os << fifo_num << "," << unread << "," << overrun << "," << "\n"; } }; diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp index 828963d182a0d8d3e06269f38b9f42f5d7c7a44a..233eada4f4083871dcd1e3c2ab6012c176b4e1dd 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.cpp @@ -277,7 +277,7 @@ bool LSM9DS1_M::onSimpleUpdate() spi.read(regMapM::OUT_X_L_M | AUTO_INCREMENT_ADDR, magData, 6); } - lastMagneto.timestamp = miosix::getTick(); + lastMagneto.timestamp = lastTimestamp; // compose signed 16-bit raw data as 2 bytes from the sensor // clang-format off @@ -294,4 +294,9 @@ bool LSM9DS1_M::onSimpleUpdate() return true; } +void LSM9DS1_M::updateTimestamp(uint64_t timestamp) +{ + lastTimestamp = timestamp; +} + 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 8dd422f6be2a789704526664901b93157a43f008..bad903562df5b7d5cf69fe407d5f4b4a6fc9c26d 100644 --- a/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h +++ b/src/shared/sensors/LSM9DS1/LSM9DS1_Magneto.h @@ -116,16 +116,22 @@ public: */ bool onSimpleUpdate() override; - + + /** + * @brief set timestamp for sample + * @warning remember to update FIFO data calling onSimpleUpdate + */ + + void updateTimestamp(uint64_t timestamp); + /** * @brief get last valid sample * @return sample */ - - const lsm9ds1MSample& getSample() const; -private: + const lsm9ds1MSample& getSample() const; +private: /** * @brief get data from sensor and compute stats. */ @@ -133,15 +139,15 @@ private: void getSelfTestData(Stats& outx, Stats& outy, Stats& outz); bool sensor_initialized = false; - bool selfTest_mode = false; + bool selfTest_mode = false; SPISlave spislave; MagFSR magFSR; ODR odr; - lsm9ds1MSample lastMagneto; - + lsm9ds1MSample lastMagneto; + uint64_t lastTimestamp = 0; /** * @brief Registers' addresses definition. */ diff --git a/src/tests/drivers/test-lsm9ds1-fifo.cpp b/src/tests/drivers/test-lsm9ds1-fifo.cpp index 3457b84e3807daaedf371440408a8d558ac65569..aada23eec9b90a9bb7206464203c80c1ffbc8d53 100644 --- a/src/tests/drivers/test-lsm9ds1-fifo.cpp +++ b/src/tests/drivers/test-lsm9ds1-fifo.cpp @@ -58,11 +58,10 @@ GpioPin PUSHBUTTON(GPIOA_BASE, 0); volatile bool flagSPIReadRequest = false; // IMU obj variables -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 = 84; +static const uint8_t FIFO_WATERMARK = 12; +static const uint8_t TEMP_DIV_FREQ = 20; +static const uint16_t MAG_SAMPLING_PERIOD = 10; // 100Hz +static const uint16_t FIFO_SAMPLING_PERIOD = 84; // 20 samples/ODR, ODR = 284Hz // High Resolution hardware timer using TIM5 HardwareTimer<uint32_t> hrclock( @@ -133,8 +132,10 @@ int main() lsm9ds1_xlg->enable_fifo(FIFO_WATERMARK); // perform self-tests - lsm9ds1_xlg->selfTest(); - lsm9ds1_m->selfTest(); + while (!lsm9ds1_xlg->selfTest()) + ; + while (!lsm9ds1_m->selfTest()) + ; // initialize sensor while (!lsm9ds1_xlg->init()) @@ -142,38 +143,41 @@ int main() while (!lsm9ds1_m->init()) ; - // sampling until you push the button + // sampling until you push the button, then restart the microcontroller while (!PUSHBUTTON.value()) { // ACCELEROMETER + GYROSCOPE + UPDATE (FIFO) // TEMPERATURE UPDATE // an interrupt is set: time to dump the FIFO - - //if (miosix::getTick() - lastFifotick >= FIFO_SAMPLING_PERIOD) - //if (flagSPIReadRequest) - if ((miosix::getTick() - lastFifotick >= FIFO_SAMPLING_PERIOD) && flagSPIReadRequest == true) + + if ((miosix::getTick() - lastFifotick >= FIFO_SAMPLING_PERIOD) && + flagSPIReadRequest == true) { flagSPIReadRequest = false; - lastFifotick = miosix::getTick(); + lastFifotick = miosix::getTick(); // dump the fifo lsm9ds1_xlg->onSimpleUpdate(); // log each sample - for (int i = 0; i < lsm9ds1_xlg->getFIFOdepth() ; i++) + 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); +#ifdef DEBUG + lsm9ds1debug debugstats = lsm9ds1_xlg->getFIFOStats(); + logger.log(debugstats); +#endif + + // update Temp if (lastTempcount == TEMP_DIV_FREQ) { lastTempcount = 0; lsm9ds1TSample Tsample = lsm9ds1_xlg->getTSample(); - //logger.log(Tsample); + logger.log(Tsample); } lastTempcount++; @@ -186,15 +190,15 @@ int main() lastMagtick = miosix::getTick(); // get sample from the sensor + lsm9ds1_m->updateTimestamp(miosix::getTick()); lsm9ds1_m->onSimpleUpdate(); // log the sample lsm9ds1MSample MAGsample = lsm9ds1_m->getSample(); - //logger.log(MAGsample); + logger.log(MAGsample); } - //Thread::sleep(FIFO_SAMPLING_PERIOD); - + Thread::sleep(FIFO_SAMPLING_PERIOD); } // stop log diff --git a/src/tests/drivers/test-lsm9ds1.cpp b/src/tests/drivers/test-lsm9ds1.cpp index ad7e59812370c890b0a5f0d4131e1d7e59563e06..da3a2f90fe40a2f652e80e619eb189590c3affc5 100644 --- a/src/tests/drivers/test-lsm9ds1.cpp +++ b/src/tests/drivers/test-lsm9ds1.cpp @@ -104,12 +104,13 @@ int main() { // get axel+gyro+temp data + lsm9ds1X.updateTimestamp(miosix::getTick()); lsm9ds1X.onSimpleUpdate(); agdata = lsm9ds1X.getXLGSample(); tdata = lsm9ds1X.getTSample(); - lsm9ds1X.updateTimestamp(miosix::getTick()); // get magneto data + lsm9ds1M.updateTimestamp(miosix::getTick()); lsm9ds1M.onSimpleUpdate(); mdata = lsm9ds1M.getSample();