diff --git a/src/shared/sensors/LIS3DSH-ettore/LIS3DSH.cpp b/src/shared/sensors/LIS3DSH-ettore/LIS3DSH.cpp
index 77eb9e744576a2c132f838b8cd19301ea328f596..a0b041da8584d798353303e7413382d1f3e9b457 100644
--- a/src/shared/sensors/LIS3DSH-ettore/LIS3DSH.cpp
+++ b/src/shared/sensors/LIS3DSH-ettore/LIS3DSH.cpp
@@ -1,8 +1,8 @@
#include "LIS3DSH.h"
namespace Boardcore {
-LIS3DSHettore::LIS3DSHettore(uint8_t odr, uint8_t bdu)
- : odr(odr), bdu(bdu)
+LIS3DSHettore::LIS3DSHettore(uint8_t odr, uint8_t bdu, uint8_t fullScale)
+ : odr(odr), bdu(bdu), fullScale(fullScale)
{
}
@@ -19,6 +19,8 @@ bool LIS3DSHettore::checkWhoAmI()
bool LIS3DSHettore::init()
{
spi_driver.config();
+ printf("LIS3DSH: init \n");
+ printf("Sensitive: %f\n", sensitivity);
if (!checkWhoAmI()) // wrong who_am_i value
{
printf("LIS3DSH: wrong who_am_i value \n");
@@ -27,9 +29,47 @@ bool LIS3DSHettore::init()
// set the output data rate and the BDU in CTRL_REG4
uint8_t ctrl_reg4_value = (odr << 4) | (bdu << 3) | (7 << 0);
spi_driver.write(CTRL_REG4, ctrl_reg4_value);
+
+ // set the full scale in CTRL_REG5
+ uint8_t ctrl_reg5_value = (fullScale << 3);
+ spi_driver.write(CTRL_REG5, ctrl_reg5_value);
+
+ sensitivity = 0.06;
+
return true; // correctly initialized
}
+void LIS3DSHettore::setFullScale(uint8_t fullScale)
+{
+ //LIS3DSH
+ //CTRL_REG5 = 0x24
+ //set full scale
+ uint8_t ctrlReg5Value = (fullScale << 3);
+ spi_driver.write(CTRL_REG5, ctrlReg5Value);
+ switch (fullScale)
+ {
+ case FULL_SCALE_2G:
+ sensitivity = sensitivityValues[0];
+ break;
+ case FULL_SCALE_4G:
+ sensitivity = sensitivityValues[1];
+ break;
+ case FULL_SCALE_6G:
+ sensitivity = sensitivityValues[2];
+ break;
+ case FULL_SCALE_8G:
+ sensitivity = sensitivityValues[3];
+ break;
+ case FULL_SCALE_16G:
+ sensitivity = sensitivityValues[4];
+ break;
+ default:
+ break;
+ }
+ printf("LIS3DSH: set full scale \n");
+ printf("Sensitive: %f\n", sensitivity);
+}
+
int8_t LIS3DSHettore::getTemperature()
{
// the temperature is given as a 8-bits integer (in 2-complement)
@@ -56,7 +96,7 @@ bool LIS3DSHettore::selfTest()
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);
+ uint8_t ctrlReg5Value = (FullScale::FULL_SCALE_2G << 3) ;
spi_driver.write(CTRL_REG5, ctrlReg5Value);
for (uint8_t i = 0; i < samples; i++) {
//read using function
@@ -71,8 +111,7 @@ bool LIS3DSHettore::selfTest()
miosix::Thread::sleep(10);
}
//reset the registers
- ctrlReg5Value &= ~(3 << 1);
- ctrlReg5Value |= (FULL_SCALE_2G << 3);
+ ctrlReg5Value = (fullScale << 3);
spi_driver.write(CTRL_REG5, ctrlReg5Value);
for (uint8_t i = 0; i < samples; i++) {
//read using function
@@ -118,6 +157,13 @@ bool LIS3DSHettore::selfTest()
return true;
}
+uint64_t LIS3DSHettore::getAccelerationTimestamp()
+{
+ return TimestampTimer::getTimestamp();
+}
+
+int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8) | lsb; }
+
float LIS3DSHettore::getAccX()
{
//LIS3DSH
@@ -125,7 +171,7 @@ float LIS3DSHettore::getAccX()
//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);
+ return static_cast<float>(combine(accelH, accelL)) * sensitivity;
}
float LIS3DSHettore::getAccY()
@@ -135,7 +181,7 @@ float LIS3DSHettore::getAccY()
//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);
+ return static_cast<float>(combine(accelH, accelL)) * sensitivity;
}
float LIS3DSHettore::getAccZ()
@@ -145,7 +191,7 @@ float LIS3DSHettore::getAccZ()
//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);
+ return static_cast<float>(combine(accelH, accelL)) * sensitivity;
}
}
diff --git a/src/shared/sensors/LIS3DSH-ettore/LIS3DSH.h b/src/shared/sensors/LIS3DSH-ettore/LIS3DSH.h
index 85c2d0732046a45a1116f29eae4517195d52482c..8d890b313e48a769c75944f8054a9b24cb6873d9 100644
--- a/src/shared/sensors/LIS3DSH-ettore/LIS3DSH.h
+++ b/src/shared/sensors/LIS3DSH-ettore/LIS3DSH.h
@@ -3,6 +3,7 @@
#pragma once
#include "drivers/ettore/SPIDriver.h"
+#include <drivers/timer/TimestampTimer.h>
#include <math.h>
using namespace miosix;
@@ -14,7 +15,8 @@ class LIS3DSHettore
{
public:
- LIS3DSHettore(uint8_t odr, uint8_t bdu);
+ LIS3DSHettore(uint8_t odr, uint8_t bdu,
+ uint8_t fullScale = FullScale::FULL_SCALE_2G);
bool init();
@@ -22,11 +24,17 @@ public:
int8_t getTemperature();
+ void setFullScale(uint8_t fullScale);
+
+ uint64_t getAccelerationTimestamp();
+
float getAccX();
float getAccY();
float getAccZ();
+
+
bool selfTest();
enum OutputDataRate : uint8_t
@@ -73,11 +81,19 @@ public:
};
+
+
+
private:
SpiDriver spi_driver;
uint8_t odr;
uint8_t bdu;
+ uint8_t fullScale; // full scale value, default +/- 2g
+
+ const float sensitivityValues[5] = {0.06, 0.12, 0.18, 0.24, 0.73};
+ float sensitivity = 0.06;
+
const uint8_t WHO_AM_I_VALUE = 63;
const uint8_t TEMPERATURE_REF = 25;
diff --git a/src/tests/sensors/test-ettore.cpp b/src/tests/sensors/test-ettore.cpp
index a5f45897f169f6cc1a505974d6b732a802a1c358..a33eb5c56267e84e6b828561b824e0260f4d1145 100644
--- a/src/tests/sensors/test-ettore.cpp
+++ b/src/tests/sensors/test-ettore.cpp
@@ -8,7 +8,8 @@ using namespace Boardcore;
int main()
{
LIS3DSHettore driver(LIS3DSHettore::OutputDataRate::ODR_100_HZ,
- LIS3DSHettore::BlockDataUpdate::UPDATE_AFTER_READ_MODE);
+ LIS3DSHettore::BlockDataUpdate::CONTINUOUS_UPDATE_MODE,
+ LIS3DSHettore::FullScale::FULL_SCALE_2G);
printf("Ciao\n");
@@ -24,14 +25,23 @@ int main()
Thread::sleep(2000);
}
}
+
+ driver.setFullScale(LIS3DSHettore::FullScale::FULL_SCALE_4G);
while (true)
{
TRACE("Temp : %d °C \n", driver.getTemperature());
- TRACE("AccX : %f g \n", driver.getAccX());
+ TRACE("Timestamp : %llu \n", driver.getAccelerationTimestamp());
+ float accX = driver.getAccX();
+ float accY = driver.getAccY();
+ float accZ = driver.getAccZ();
+ TRACE("AccX RAW : %f\n", accX );
+ TRACE("AccY RAW: %f\n", accY);
+ TRACE("AccZ RAW: %f\n", accZ);
+
- Thread::sleep(5000);
+ Thread::sleep(200);
}
} else {
while (true)