Driver nearly done

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;
 }
 

src/shared/sensors/LIS3DSH-ettore/LIS3DSH.cpp

 #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);
+}
+
 }
 
 

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

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)