diff --git a/src/shared/drivers/ettore/SPIDriver.cpp b/src/shared/drivers/ettore/SPIDriver.cpp index b153ae5a295447dee8df00f2d2e023b5d041cc1b..291a80105211b0491b80d740575b96995019c8d0 100644 --- a/src/shared/drivers/ettore/SPIDriver.cpp +++ b/src/shared/drivers/ettore/SPIDriver.cpp @@ -10,6 +10,7 @@ SpiDriver::SpiDriver(){ void SpiDriver::config() { + // CS has to be configured as output and put to high state. This because the // chip select, by design, is active low: when the master wants to communicate // with a slave, it has to pull the corresponding CS line to low @@ -101,13 +102,11 @@ void SpiDriver::write(uint8_t addr, uint8_t value) uint8_t SpiDriver::read(uint8_t addr) { cs::low(); - sendRecv(0x80 | addr); // <--deeper + sendRecv(0x80 | addr); uint8_t data = sendRecv(0x00); cs::high(); miosix::delayUs(10); // wait 10 microseconds - printf("returning\n"); - return data; } diff --git a/src/shared/sensors/LIS3DSH-ettore/LIS3DSH.cpp b/src/shared/sensors/LIS3DSH-ettore/LIS3DSH.cpp index d4b3f6411fe72cf666ec8af1de93fee6264c429b..77eb9e744576a2c132f838b8cd19301ea328f596 100644 --- a/src/shared/sensors/LIS3DSH-ettore/LIS3DSH.cpp +++ b/src/shared/sensors/LIS3DSH-ettore/LIS3DSH.cpp @@ -1,5 +1,4 @@ #include "LIS3DSH.h" - namespace Boardcore { LIS3DSHettore::LIS3DSHettore(uint8_t odr, uint8_t bdu) @@ -10,25 +9,23 @@ LIS3DSHettore::LIS3DSHettore(uint8_t odr, uint8_t bdu) bool LIS3DSHettore::checkWhoAmI() { uint8_t who_am_i = spi_driver.read(WHO_AM_I_REG); // <-- we need to go deeper - if (who_am_i == WHO_AM_I_VALUE) { - return true; } - return false; } bool LIS3DSHettore::init() { + spi_driver.config(); if (!checkWhoAmI()) // wrong who_am_i value { - printf("init not okay"); + printf("LIS3DSH: wrong who_am_i value \n"); return false; } // set the output data rate and the BDU in CTRL_REG4 - uint8_t ctrl_reg4_value = (odr << 4) | (bdu << 3); + uint8_t ctrl_reg4_value = (odr << 4) | (bdu << 3) | (7 << 0); spi_driver.write(CTRL_REG4, ctrl_reg4_value); return true; // correctly initialized } @@ -41,6 +38,116 @@ int8_t LIS3DSHettore::getTemperature() return spi_driver.read(OUT_T) + TEMPERATURE_REF; } +bool LIS3DSHettore::selfTest() +{ + //LIS3DSH + //CTRL_REG5 = 0x24 + //set data rate to 50 Hz + const int samples = 10; + uint8_t ctrlReg4Value = (OutputDataRate::ODR_100_HZ << 4) | + (BlockDataUpdate::UPDATE_AFTER_READ_MODE << 3) | + (7 << 0); + float X_ST[samples] = {0}; + float Y_ST[samples] = {0}; + float Z_ST[samples] = {0}; + float X_NO_ST[samples] = {0}; + float Y_NO_ST[samples] = {0}; + float Z_NO_ST[samples] = {0}; + float AVG_ST[3] = {0}; // one element per axis + float AVG_NO_ST[3] = {0}; // one element per axis + spi_driver.write(CTRL_REG4, ctrlReg4Value); + uint8_t ctrlReg5Value = (FullScale::FULL_SCALE_2G << 3) | (1 << 1); + spi_driver.write(CTRL_REG5, ctrlReg5Value); + for (uint8_t i = 0; i < samples; i++) { + //read using function + float x = getAccX(); + float y = getAccY(); + float z = getAccZ(); + //store the samples + X_ST[i] = x; + Y_ST[i] = y; + Z_ST[i] = z; + //wait 10 ms + miosix::Thread::sleep(10); + } + //reset the registers + ctrlReg5Value &= ~(3 << 1); + ctrlReg5Value |= (FULL_SCALE_2G << 3); + spi_driver.write(CTRL_REG5, ctrlReg5Value); + for (uint8_t i = 0; i < samples; i++) { + //read using function + float x = getAccX(); + float y = getAccY(); + float z = getAccZ(); + //store the samples + X_NO_ST[i] = x; + Y_NO_ST[i] = y; + Z_NO_ST[i] = z; + //wait 10 ms + miosix::Thread::sleep(10); + } + //calculate the average + for (uint8_t i = 0; i < samples; i++) { + AVG_ST[0] += X_ST[i]; + AVG_ST[1] += Y_ST[i]; + AVG_ST[2] += Z_ST[i]; + AVG_NO_ST[0] += X_NO_ST[i]; + AVG_NO_ST[1] += Y_NO_ST[i]; + AVG_NO_ST[2] += Z_NO_ST[i]; + } + for (uint8_t i = 0; i < 3; i++) { + AVG_ST[i] /= samples; + AVG_NO_ST[i] /= samples; + } + //calculate delta + float delta[3] = {0}; + for (uint8_t i = 0; i < 3; i++) { + delta[i] = fabs(AVG_NO_ST[i] - AVG_ST[i]); + printf("delta[%d] = %f\n", i, delta[i]); + } + if ((delta[0] > + SELF_TEST_DIFF_X_Y + SELF_TEST_DIFF_X_Y * SELF_TEST_TOLERANCE) || + (delta[1] > + SELF_TEST_DIFF_X_Y + SELF_TEST_DIFF_X_Y * SELF_TEST_TOLERANCE) || + (delta[2] > + SELF_TEST_DIFF_Z + SELF_TEST_DIFF_Z * SELF_TEST_TOLERANCE)) + { + printf("LIS3DSH: self test failed\n"); + return false; + } + return true; +} + +float LIS3DSHettore::getAccX() +{ + //LIS3DSH + //OUT_X_L = 0x28 + //OUT_X_H = 0x29 + int8_t accelL = spi_driver.read(OUT_X_L); + int8_t accelH = spi_driver.read(OUT_X_H); + return static_cast<float>(accelH << 8 | accelL); +} + +float LIS3DSHettore::getAccY() +{ + //LIS3DSH + //OUT_Y_L = 0x2A + //OUT_Y_H = 0x2B + int8_t accelL = spi_driver.read(OUT_Y_L); + int8_t accelH = spi_driver.read(OUT_Y_H); + return static_cast<float>(accelH << 8 | accelL); +} + +float LIS3DSHettore::getAccZ() +{ + //LIS3DSH + //OUT_Z_L = 0x2C + //OUT_Z_H = 0x2D + int8_t accelL = spi_driver.read(OUT_Z_L); + int8_t accelH = spi_driver.read(OUT_Z_H); + return static_cast<float>(accelH << 8 | accelL); +} + } diff --git a/src/shared/sensors/LIS3DSH-ettore/LIS3DSH.h b/src/shared/sensors/LIS3DSH-ettore/LIS3DSH.h index f13d5f688209b7ecb1d96d87fa85b157e59e06c8..85c2d0732046a45a1116f29eae4517195d52482c 100644 --- a/src/shared/sensors/LIS3DSH-ettore/LIS3DSH.h +++ b/src/shared/sensors/LIS3DSH-ettore/LIS3DSH.h @@ -3,6 +3,9 @@ #pragma once #include "drivers/ettore/SPIDriver.h" +#include <math.h> + +using namespace miosix; namespace Boardcore { @@ -19,6 +22,13 @@ public: int8_t getTemperature(); + float getAccX(); + float getAccY(); + float getAccZ(); + + + bool selfTest(); + enum OutputDataRate : uint8_t { ODR_POWER_DOWN = 0, // default value @@ -43,7 +53,23 @@ public: { WHO_AM_I_REG = 0x0F, CTRL_REG4 = 0x20, // control register to set ODR and BDU - OUT_T = 0x0C // temperature output register + CTRL_REG5 = 0x24, // control register to set self-test + OUT_T = 0x0C, // temperature output register + OUT_X_L = 0x28, + OUT_X_H = 0x29, + OUT_Y_L = 0x2A, + OUT_Y_H = 0x2B, + OUT_Z_L = 0x2C, + OUT_Z_H = 0x2D, + }; + + enum FullScale + { + FULL_SCALE_2G = 0, // 000, +/- 2g + FULL_SCALE_4G = 1, // 001, +/- 4g + FULL_SCALE_6G = 2, // 010, +/- 6g + FULL_SCALE_8G = 3, // 011, +/- 8g + FULL_SCALE_16G = 4, // 100 +/- 16g }; @@ -59,6 +85,10 @@ private: } // namespace Boardcore + const float SELF_TEST_DIFF_X_Y = 140.0f; // 140 mg + const float SELF_TEST_DIFF_Z = 590.0f; // 590 mg + const float SELF_TEST_TOLERANCE = 0.3f; + enum OutputDataRate : uint8_t { ODR_POWER_DOWN = 0, // default value diff --git a/src/tests/sensors/test-ettore.cpp b/src/tests/sensors/test-ettore.cpp index c156c9b7964b42db9132bf776fa196c87f7bfcfc..a5f45897f169f6cc1a505974d6b732a802a1c358 100644 --- a/src/tests/sensors/test-ettore.cpp +++ b/src/tests/sensors/test-ettore.cpp @@ -14,13 +14,24 @@ int main() printf("Ciao\n"); if (driver.init()) { + if (driver.selfTest()) + { + printf("Self test passed\n"); + } else { + while (true) + { + TRACE("Self test failed\n"); + Thread::sleep(2000); + } + } + while (true) { TRACE("Temp : %d °C \n", driver.getTemperature()); - printf("Ciao\n"); + TRACE("AccX : %f g \n", driver.getAccX()); - Thread::sleep(2000); + Thread::sleep(5000); } } else { while (true)