From 2f168d42071b0857ba1b2d2bfcbcff840cc318f1 Mon Sep 17 00:00:00 2001
From: "Ettore L. Pane" <ettore.pane4@gmail.com>
Date: Tue, 14 Nov 2023 18:20:47 +0100
Subject: [PATCH] Driver nearly done
---
src/shared/drivers/ettore/SPIDriver.cpp | 5 +-
src/shared/sensors/LIS3DSH-ettore/LIS3DSH.cpp | 119 +++++++++++++++++-
src/shared/sensors/LIS3DSH-ettore/LIS3DSH.h | 32 ++++-
src/tests/sensors/test-ettore.cpp | 15 ++-
4 files changed, 159 insertions(+), 12 deletions(-)
diff --git a/src/shared/drivers/ettore/SPIDriver.cpp b/src/shared/drivers/ettore/SPIDriver.cpp
index b153ae5a2..291a80105 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 d4b3f6411..77eb9e744 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 f13d5f688..85c2d0732 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 c156c9b79..a5f45897f 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)
--
GitLab