diff --git a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
index d1b0ffab235c4b9f6371f87e20392e2c31310221..d0b1d3416f989db4b4d5b8fb905ae9b1ea0031de 100644
--- a/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
+++ b/src/shared/sensors/LSM9DS1/LSM9DS1_AxelGyro.h
@@ -22,419 +22,438 @@
* THE SOFTWARE.
*/
-
#pragma once
#include <miosix.h>
#include <array>
-#include "drivers/spi/SPIDriver.h"
#include "../Sensor.h"
+#include "drivers/spi/SPIDriver.h"
using miosix::GpioPin;
using std::array;
-class LSM9DS1_XLG : public GyroSensor, public AccelSensor, public TemperatureSensor
+class LSM9DS1_XLG : public GyroSensor,
+ public AccelSensor,
+ public TemperatureSensor
{
- public:
-
- enum AxelFSR
+public:
+ enum AxelFSR
+ {
+ FS_2 = 0x00,
+ FS_16 = 0x01,
+ FS_4 = 0x02,
+ FS_8 = 0x03
+ };
+
+ enum GyroFSR
+ {
+ FS_245 = 0x00,
+ FS_500 = 0x01,
+ FS_2000 = 0x03 // 1 -> 3 jump is ok
+ };
+
+ enum ODR
+ {
+ PWR_DW = 0X00,
+ ODR_15 = 0X01,
+ ODR_60 = 0X02,
+ ODR_119 = 0X03,
+ ODR_238 = 0X04,
+ ODR_476 = 0X05,
+ ODR_952 = 0X06
+ };
+
+ /**
+ * @brief Creates an instance of an LSM9DS1 accelerometer + gyroscope sensor
+ *
+ * @param bus SPI bus the sensor is connected to
+ * @param cs Chip Select GPIO
+ * @param config (OPTIONAL) custom SPIBusConfig
+ * @param axelRange accelerometer Full Scale Range (See datasheet)
+ * @param gyroRange gyroscope Full Scale Range (See datasheet)
+ * @param odr Output Data Rate (See datasheet)
+ * @param fifo_enabled Fifo enabled
+ * @param fifo_watermark FIFO watermark level in range [1,32] (used for
+ * interrupt generation, see datasheet).
+ */
+
+ LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs,
+ AxelFSR axelRange = AxelFSR::FS_2,
+ GyroFSR gyroRange = GyroFSR::FS_245, ODR odr = ODR::ODR_15,
+ bool fifo_enabled = false, unsigned int fifo_watermark = 24)
+ : fifo_enabled(fifo_enabled), fifo_watermark(fifo_watermark),
+ spislave(bus, cs), axelFSR(axelRange), gyroFSR(gyroRange), odr(odr)
+ {
+ // SPI config
+ spislave.config.clock_div = SPIClockDivider::DIV64;
+ }
+
+ LSM9DS1_XLG(SPIBusInterface& bus, GpioPin cs, SPIBusConfig config,
+ AxelFSR axelRange = AxelFSR::FS_2,
+ GyroFSR gyroRange = GyroFSR::FS_245, ODR odr = ODR::ODR_15,
+ bool fifo_enabled = false, unsigned int fifo_watermark = 24)
+ : fifo_enabled(fifo_enabled), fifo_watermark(fifo_watermark),
+ spislave(bus, cs, config), axelFSR(axelRange), gyroFSR(gyroRange),
+ odr(odr)
+ {
+ }
+
+ bool init() override
+ {
+ // Set FSR
+ switch (axelFSR) // DA RIGUARDARE I COEFFICIENTI: SU DS C'E'
+ // SENSITIVITY IN mg/LSB
{
- FS_2 = 0x00,
- FS_16 = 0x01,
- FS_4 = 0x02,
- FS_8 = 0x03
- };
+ case AxelFSR::FS_2:
+ axelSensitivity = 0.598f;
+ break;
+ case AxelFSR::FS_4:
+ axelSensitivity = 1.196f;
+ break;
+ case AxelFSR::FS_8:
+ axelSensitivity = 2.393f;
+ break;
+ case AxelFSR::FS_16:
+ axelSensitivity = 7.178f;
+ break;
+ default:
+ axelSensitivity = 0.598f;
+ break;
+ }
- enum GyroFSR
+ switch (gyroFSR)
{
- FS_245 = 0x00,
- FS_500 = 0x01,
- FS_2000 = 0x03 //1 -> 3 jump is ok
- };
+ case GyroFSR::FS_245:
+ gyroSensitivity = 8.75f;
+ break;
+ case GyroFSR::FS_500:
+ gyroSensitivity = 17.50f;
+ break;
+ case GyroFSR::FS_2000:
+ gyroSensitivity = 70.0f;
+ break;
+ default:
+ gyroSensitivity = 8.75f;
+ break;
+ }
- enum ODR
+ switch (odr)
{
- PWR_DW = 0X00,
- ODR_15 = 0X01,
- ODR_60 = 0X02,
- ODR_119 = 0X03,
- ODR_238 = 0X04,
- ODR_476 = 0X05,
- ODR_952 = 0X06
- };
-
- /**
- * @brief Creates an instance of an LSM9DS1 accelerometer + gyroscope sensor
- *
- * @param bus SPI bus the sensor is connected to
- * @param cs Chip Select GPIO
- * @param config (OPTIONAL) custom SPIBusConfig
- * @param axelRange accelerometer Full Scale Range (See datasheet)
- * @param gyroRange gyroscope Full Scale Range (See datasheet)
- * @param odr Output Data Rate (See datasheet)
- * @param fifo_enabled Fifo enabled
- * @param fifo_watermark FIFO watermark level in range [1,32] (used for
- * interrupt generation, see datasheet).
- */
-
- LSM9DS1_XLG(
- SPIBusInterface& bus,
- GpioPin cs,
- AxelFSR axelRange = AxelFSR::FS_2,
- GyroFSR gyroRange = GyroFSR::FS_245,
- ODR odr = ODR::ODR_15,
- bool fifo_enabled = false,
- unsigned int fifo_watermark = 24
- ):fifo_enabled(fifo_enabled), fifo_watermark(fifo_watermark),
- spislave(bus, cs), axelFSR(axelRange), gyroFSR(gyroRange), odr(odr){
- //SPI config
- spislave.config.clock_div = SPIClockDivider::DIV64;
+ case ODR::PWR_DW:
+ odrHz = 0.0f;
+ break;
+ case ODR::ODR_15:
+ odrHz = 14.9f;
+ break;
+ case ODR::ODR_60:
+ odrHz = 59.5f;
+ break;
+ case ODR::ODR_119:
+ odrHz = 119.0f;
+ break;
+ case ODR::ODR_238:
+ odrHz = 238.0f;
+ break;
+ case ODR::ODR_476:
+ odrHz = 476.0f;
+ break;
+ case ODR::ODR_952:
+ odrHz = 952.0f;
+ break;
+ default:
+ odrHz = 14.9f;
+ break;
}
- LSM9DS1_XLG(
- SPIBusInterface& bus,
- GpioPin cs,
- SPIBusConfig config,
- AxelFSR axelRange = AxelFSR::FS_2,
- GyroFSR gyroRange = GyroFSR::FS_245,
- ODR odr = ODR::ODR_15,
- bool fifo_enabled = false,
- unsigned int fifo_watermark = 24
- ):fifo_enabled(fifo_enabled), fifo_watermark(fifo_watermark),
- spislave(bus, cs, config), axelFSR(axelRange), gyroFSR(gyroRange), odr(odr){
-
- }
+ SPITransaction spi(spislave);
- bool init() override
+ // Who Am I check:
+ uint8_t whoami = spi.read(regMapXLG::WHO_AM_I);
+ if (whoami != WHO_AM_I_XLG_VAL)
{
- //Set FSR
- switch(axelFSR) //DA RIGUARDARE I COEFFICIENTI: SU DS C'E' SENSITIVITY IN mg/LSB
- {
- case AxelFSR::FS_2:
- axelSensitivity = 0.598f;
- break;
- case AxelFSR::FS_4:
- axelSensitivity = 1.196f;
- break;
- case AxelFSR::FS_8:
- axelSensitivity = 2.393f;
- break;
- case AxelFSR::FS_16:
- axelSensitivity = 7.178f;
- break;
- default:
- axelSensitivity = 0.598f;
- break;
- }
-
- switch(gyroFSR)
- {
- case GyroFSR::FS_245:
- gyroSensitivity = 8.75f;
- break;
- case GyroFSR::FS_500:
- gyroSensitivity = 17.50f;
- break;
- case GyroFSR::FS_2000:
- gyroSensitivity = 70.0f;
- break;
- default:
- gyroSensitivity = 8.75f;
- break;
- }
-
- switch(odr)
- {
- case ODR::PWR_DW:
- odrHz = 0.0f;
- break;
- case ODR::ODR_15:
- odrHz = 14.9f;
- break;
- case ODR::ODR_60:
- odrHz = 59.5f;
- break;
- case ODR::ODR_119:
- odrHz = 119.0f;
- break;
- case ODR::ODR_238:
- odrHz = 238.0f;
- break;
- case ODR::ODR_476:
- odrHz = 476.0f;
- break;
- case ODR::ODR_952:
- odrHz = 952.0f;
- break;
- default:
- odrHz = 14.9f;
- break;
- }
-
- SPITransaction spi(spislave);
+ TRACE("LSM9DS1 AXEL+GYRO WAMI: %02X\n", whoami);
+ last_error = ERR_NOT_ME;
+ return false;
+ }
- //Who Am I check:
- uint8_t whoami = spi.read(regMapXLG::WHO_AM_I);
- if(whoami != WHO_AM_I_XLG_VAL){
- TRACE("LSM9DS1 AXEL+GYRO WAMI: %02X\n", whoami);
- last_error = ERR_NOT_ME;
- return false;
- }
+ // common setup
+ spi.write(regMapXLG::CTRL_REG8,
+ CTRL_REG8_VAL); // addr auto-increment while reading/writing
- //common setup
- spi.write(regMapXLG::CTRL_REG8, CTRL_REG8_VAL); //addr auto-increment while reading/writing
+ // FIFO setup: FIFO enabled in continous mode, decimation OFF,
+ // temperature on FIFO ON.
+ if (fifo_enabled)
+ {
+ spi.write(regMapXLG::FIFO_CTRL,
+ (FIFO_CTRL_VAL |
+ fifo_watermark)); // FIFO continous mode + fifo
+ // watermark threshold setup
+ spi.write(regMapXLG::INT1_CTRL,
+ INT1_CTRL_VAL); // interrupt on FIFO treshold
+ spi.write(
+ regMapXLG::CTRL_REG9,
+ (CTRL_REG9_VAL | 0x02)); // DRDY_mask_bit OFF, I2C OFF, FIFO ON
+ }
+ else
+ {
+ spi.write(regMapXLG::CTRL_REG9,
+ CTRL_REG9_VAL); // DRDY_mask_bit OFF, I2C OFF, FIFO OFF
+ }
- //FIFO setup: FIFO enabled in continous mode, decimation OFF, temperature on FIFO ON.
- if(fifo_enabled)
+ // Axel Setup: ODR, FSR defined by constructor, auto anti-aliasing BW
+ // (max), LPF2/HPF bypassed and disabled, axel output enabled by default
+ // @ startup
+ uint8_t CTRL_REG6_XL_VAL = (int)odr << 5 | (int)axelFSR << 3;
+ spi.write(regMapXLG::CTRL_REG6_XL,
+ CTRL_REG6_XL_VAL); // ODR, FSR, auto BW (max) function of ODR
+
+ // Gyro Setup : ODR, FSR defined by constructor, LPF2/HPF bypassed and
+ // disabled, gyro output enabled by default @ startup
+ uint8_t CTRL_REG1_G_VAL = (int)odr << 5 | (int)gyroFSR << 3;
+ spi.write(regMapXLG::CTRL_REG1_G, CTRL_REG1_G_VAL); // ODR,FSR
+ // spi.write(regMapXLG::ORIENT_CFG_G, ORIENT_CFG_VAL); //angular rate
+ // sign and orientation Setup <--- BOARD DEPENDENT
+
+ // Check all the registers have been written correctly
+ if (spi.read(regMapXLG::CTRL_REG8) != CTRL_REG8_VAL)
+ {
+ return false;
+ }
+ if (fifo_enabled)
+ {
+ if (spi.read(regMapXLG::FIFO_CTRL) !=
+ (FIFO_CTRL_VAL | fifo_watermark))
{
- spi.write(regMapXLG::FIFO_CTRL, (FIFO_CTRL_VAL|fifo_watermark)); //FIFO continous mode + fifo watermark threshold setup
- spi.write(regMapXLG::INT1_CTRL, INT1_CTRL_VAL); //interrupt on FIFO treshold
- spi.write(regMapXLG::CTRL_REG9, (CTRL_REG9_VAL|0x02)); //DRDY_mask_bit OFF, I2C OFF, FIFO ON
+ return false;
}
- else
+ if (spi.read(regMapXLG::INT1_CTRL) != INT1_CTRL_VAL)
{
- spi.write(regMapXLG::CTRL_REG9, CTRL_REG9_VAL); //DRDY_mask_bit OFF, I2C OFF, FIFO OFF
+ return false;
}
-
-
- //Axel Setup: ODR, FSR defined by constructor, auto anti-aliasing BW (max), LPF2/HPF bypassed and disabled, axel output enabled by default @ startup
- uint8_t CTRL_REG6_XL_VAL = (int)odr << 5 | (int)axelFSR << 3;
- spi.write(regMapXLG::CTRL_REG6_XL, CTRL_REG6_XL_VAL); //ODR, FSR, auto BW (max) function of ODR
-
-
-
- //Gyro Setup : ODR, FSR defined by constructor, LPF2/HPF bypassed and disabled, gyro output enabled by default @ startup
- uint8_t CTRL_REG1_G_VAL = (int)odr<<5 | (int)gyroFSR<<3;
- spi.write(regMapXLG::CTRL_REG1_G, CTRL_REG1_G_VAL); //ODR,FSR
- //spi.write(regMapXLG::ORIENT_CFG_G, ORIENT_CFG_VAL); //angular rate sign and orientation Setup <--- BOARD DEPENDENT
-
- //Check all the registers have been written correctly
- if(spi.read(regMapXLG::CTRL_REG8)!=CTRL_REG8_VAL) {return false;}
- if(fifo_enabled)
+ if (spi.read(regMapXLG::CTRL_REG9) != (CTRL_REG9_VAL | 0x02))
{
- if(spi.read(regMapXLG::FIFO_CTRL) != (FIFO_CTRL_VAL|fifo_watermark)) {return false;}
- if(spi.read(regMapXLG::INT1_CTRL) != INT1_CTRL_VAL) {return false;}
- if(spi.read(regMapXLG::CTRL_REG9) != (CTRL_REG9_VAL| 0x02)) {return false;}
+ return false;
}
- else
+ }
+ else
+ {
+ if (spi.read(regMapXLG::CTRL_REG9) != CTRL_REG9_VAL)
{
- if(spi.read(regMapXLG::CTRL_REG9) != CTRL_REG9_VAL) {return false;}
+ return false;
}
- if(spi.read(regMapXLG::CTRL_REG6_XL) != CTRL_REG6_XL_VAL) {return false;}
- if(spi.read(regMapXLG::CTRL_REG1_G) != CTRL_REG1_G_VAL) {return false;}
-
- discardSamples();
-
- return true;
}
-
- bool selfTest() override
- {
- return true;
+ if (spi.read(regMapXLG::CTRL_REG6_XL) != CTRL_REG6_XL_VAL)
+ {
+ return false;
}
-
- bool onSimpleUpdate() override
+ if (spi.read(regMapXLG::CTRL_REG1_G) != CTRL_REG1_G_VAL)
{
-
- if(!fifo_enabled){ //if FIFO disabled
- uint8_t data[12], tempData[2];
- // Read output axel+gyro data X,Y,Z
- {
- SPITransaction spi(spislave);
- spi.read(regMapXLG::OUT_X_L_G, data, 12);
- spi.read(regMapXLG::OUT_TEMP_L, tempData, 2);
- }
-
- int16_t x_gy = data[0] | data[1] << 8;
- int16_t y_gy = data[2] | data[3] << 8;
- int16_t z_gy = data[4] | data[5] << 8;
-
- int16_t x_xl = data[6] | data[7] << 8;
- int16_t y_xl = data[8] | data[9] << 8;
- int16_t z_xl = data[10] | data[11] << 8;
-
- int16_t temp = tempData[0] | tempData[1] << 8;
-
- mLastAccel =
- Vec3(x_xl * axelSensitivity / 1000,
- y_xl * axelSensitivity / 1000,
- z_xl * axelSensitivity / 1000);
-
- mLastGyro =
- Vec3(x_gy * gyroSensitivity / 1000,
- y_gy * gyroSensitivity / 1000,
- z_gy * gyroSensitivity / 1000);
-
- mLastTemp = tempZero + (temp / tempSensistivity); //25°C + TEMP*S devo castare a float "temp"?
- }
- else{ //if FIFO enabled: do not store temperature, it can be read using "temperatureUpdate()" function at low sampling frequency
- uint8_t buf[384]; //2 bytes per data * 3 axes * 2 (axel+gyro) * 32(FIFO DEPTH MAX) = 384 samples
- {
- SPITransaction spi(spislave);
- //read FIFO status and dump all the samples inside the FIFO
- //uint8_t fifo_src = spi.read(FIFO_SRC);
- //fifo_samples = fifo_src & 0x3F;
- //sanity check
- /*if(fifo_samples > 32)
- {
- fifo_samples = 32;
- }*/
- spi.read(OUT_X_L_G, buf, fifo_watermark*12); //format: gxl,gxh,gyl,gyh,gzl,gzh,axl,axh,ayl,ayh,azl,azh for each sample
- }
- //convert & store
- for(int i=0; i<fifo_samples; i++ )
- {
- uint16_t x_gy = buf[i*12] | buf[i*12 + 1] << 8;
- uint16_t y_gy = buf[i*12 + 2] | buf[i*12 + 3] << 8;
- uint16_t z_gy = buf[i*12 + 4] | buf[i*12 + 5] << 8;
-
- uint16_t x_xl = buf[i*12 + 6] | buf[i*12 + 7] << 8;
- uint16_t y_xl = buf[i*12 + 8] | buf[i*12 + 9] << 8;
- uint16_t z_xl = buf[i*12 + 10] | buf[i*12 + 11] << 8;
-
- gyro_fifo[i] =
- Vec3(x_gy * gyroSensitivity / 1000,
- y_gy * gyroSensitivity / 1000,
- z_gy * gyroSensitivity / 1000);
- axel_fifo[i] =
- Vec3(x_xl * axelSensitivity / 1000,
- y_xl * axelSensitivity / 1000,
- z_xl * axelSensitivity / 1000);
- }
- }
- return true;
+ return false;
}
- bool temperatureUpdate()
- {
- uint8_t tempData[2];
+ discardSamples();
+
+ return true;
+ }
+
+ bool selfTest() override { return true; }
+
+ bool onSimpleUpdate() override
+ {
+
+ if (!fifo_enabled)
+ { // if FIFO disabled
+ uint8_t data[12], tempData[2];
+ // Read output axel+gyro data X,Y,Z
{
SPITransaction spi(spislave);
+ spi.read(regMapXLG::OUT_X_L_G, data, 12);
spi.read(regMapXLG::OUT_TEMP_L, tempData, 2);
}
+ int16_t x_gy = data[0] | data[1] << 8;
+ int16_t y_gy = data[2] | data[3] << 8;
+ int16_t z_gy = data[4] | data[5] << 8;
+
+ int16_t x_xl = data[6] | data[7] << 8;
+ int16_t y_xl = data[8] | data[9] << 8;
+ int16_t z_xl = data[10] | data[11] << 8;
+
int16_t temp = tempData[0] | tempData[1] << 8;
- mLastTemp = tempZero + temp / tempSensistivity; //25°C + TEMP/S devo castare a float "temp"?
- return true;
+
+ mLastAccel = Vec3(x_xl * axelSensitivity / 1000,
+ y_xl * axelSensitivity / 1000,
+ z_xl * axelSensitivity / 1000);
+
+ mLastGyro = Vec3(x_gy * gyroSensitivity / 1000,
+ y_gy * gyroSensitivity / 1000,
+ z_gy * gyroSensitivity / 1000);
+
+ mLastTemp =
+ tempZero + (temp / tempSensistivity); // 25°C + TEMP*S devo
+ // castare a float "temp"?
}
+ else
+ { // if FIFO enabled: do not store temperature, it can be read using
+ // "temperatureUpdate()" function at low sampling frequency
+ uint8_t buf[384]; // 2 bytes per data * 3 axes * 2 (axel+gyro) *
+ // 32(FIFO DEPTH MAX) = 384 samples
+ {
+ SPITransaction spi(spislave);
- void clearFIFO() //FIFO clearing routine (see datasheet)
+ spi.read(OUT_X_L_G, buf,
+ fifo_watermark * 12); // format:
+ // gxl,gxh,gyl,gyh,gzl,gzh,axl,axh,ayl,ayh,azl,azh
+ // for each sample
+ }
+ // convert & store
+ for (int i = 0; i < fifo_watermark; i++)
+ {
+ int16_t x_gy = buf[i * 12] | buf[i * 12 + 1] << 8;
+ int16_t y_gy = buf[i * 12 + 2] | buf[i * 12 + 3] << 8;
+ int16_t z_gy = buf[i * 12 + 4] | buf[i * 12 + 5] << 8;
+
+ int16_t x_xl = buf[i * 12 + 6] | buf[i * 12 + 7] << 8;
+ int16_t y_xl = buf[i * 12 + 8] | buf[i * 12 + 9] << 8;
+ int16_t z_xl = buf[i * 12 + 10] | buf[i * 12 + 11] << 8;
+
+ gyro_fifo[i] = Vec3(x_gy * gyroSensitivity / 1000,
+ y_gy * gyroSensitivity / 1000,
+ z_gy * gyroSensitivity / 1000);
+ axel_fifo[i] = Vec3(x_xl * axelSensitivity / 1000,
+ y_xl * axelSensitivity / 1000,
+ z_xl * axelSensitivity / 1000);
+ }
+ }
+ return true;
+ }
+
+ bool temperatureUpdate()
+ {
+ uint8_t tempData[2];
{
SPITransaction spi(spislave);
- spi.write(FIFO_CTRL, 0); //Bypass Mode
- miosix::Thread::sleep(20); //Wait
- spi.write(FIFO_CTRL, FIFO_CTRL_VAL | fifo_watermark); //re-enable FIFO
- discardSamples();
-
+ spi.read(regMapXLG::OUT_TEMP_L, tempData, 2);
}
- const array<Vec3, 32>& getGyroFIFO() const { return gyro_fifo; }
- const array<Vec3, 32>& getAxelFIFO() const { return axel_fifo; }
- uint8_t getFIFOSamples() const { return fifo_samples;} //fifo_samples is the same for both axel & gyro FIFOs
-
- private:
-
- bool fifo_enabled;
- uint8_t fifo_watermark;
- uint8_t fifo_samples = 0;
- array<Vec3, 32> gyro_fifo, axel_fifo;
-
- SPISlave spislave;
-
- AxelFSR axelFSR;
- GyroFSR gyroFSR;
- ODR odr;
-
- float axelSensitivity;
- float gyroSensitivity;
- float odrHz;
- float tempZero = 25.0f;
- float tempSensistivity = 16.0f;
- static const uint8_t samplesToDiscard = 8; //max possible val
-
- void discardSamples()
+ int16_t temp = tempData[0] | tempData[1] << 8;
+ mLastTemp =
+ tempZero +
+ temp /
+ tempSensistivity; // 25°C + TEMP/S devo castare a float "temp"?
+ return true;
+ }
+
+ void clearFIFO() // FIFO clearing routine (see datasheet)
+ {
+ SPITransaction spi(spislave);
+ spi.write(FIFO_CTRL, 0); // Bypass Mode
+ miosix::Thread::sleep(20); // Wait
+ spi.write(FIFO_CTRL, FIFO_CTRL_VAL | fifo_watermark); // re-enable FIFO
+ discardSamples();
+ }
+
+ const array<Vec3, 32>& getGyroFIFO() const { return gyro_fifo; }
+ const array<Vec3, 32>& getAxelFIFO() const { return axel_fifo; }
+
+private:
+ bool fifo_enabled;
+ uint8_t fifo_watermark;
+ array<Vec3, 32> gyro_fifo, axel_fifo;
+
+ SPISlave spislave;
+
+ AxelFSR axelFSR;
+ GyroFSR gyroFSR;
+ ODR odr;
+
+ float axelSensitivity;
+ float gyroSensitivity;
+ float odrHz;
+ float tempZero = 25.0f;
+ float tempSensistivity = 16.0f;
+ static const uint8_t samplesToDiscard = 8; // max possible val
+
+ void discardSamples()
+ {
+
+ //@ startup, some samples have to be discarded (datasheet)
+ if (odr != ODR::PWR_DW)
{
-
- //@ startup, some samples have to be discarded (datasheet)
- if(odr != ODR::PWR_DW)
- {
- uint16_t toWait_ms = samplesToDiscard * 1000 / odrHz;
- //TRACE("toWait_ms: %d", toWait_ms);
- miosix::Thread::sleep(toWait_ms); //if FIFO is disabled, just wait
- if(fifo_enabled)
+ uint16_t toWait_ms = samplesToDiscard * 1000 / odrHz;
+ // TRACE("toWait_ms: %d", toWait_ms);
+ miosix::Thread::sleep(toWait_ms); // if FIFO is disabled, just wait
+ if (fifo_enabled)
+ {
+ // if FIFO is enabled, read first <samplesToDiscard> samples and
+ // discard them
+ SPITransaction spi(spislave);
+ for (int i = 0; i < samplesToDiscard; i++)
{
- //if FIFO is enabled, read first <samplesToDiscard> samples and discard them
- SPITransaction spi(spislave);
- for(int i=0; i < samplesToDiscard; i++)
- {
- spi.read(regMapXLG::OUT_X_L_XL, 6);
- spi.read(regMapXLG::OUT_X_L_G, 6);
- }
+ spi.read(regMapXLG::OUT_X_L_XL, 6);
+ spi.read(regMapXLG::OUT_X_L_G, 6);
}
}
-
}
-
- enum regMapXLG
- {
- ACT_THS = 0x04,
- ACT_DUR = 0x05,
- INT_GEN_CFG_XL = 0x06,
- INT_GEN_THS_X_XL = 0x07,
- INT_GEN_THS_Y_XL = 0x08,
- INT_GEN_THS_Z_XL = 0x09,
- INT_GEN_DUR_XL = 0x0A,
- REFERENCE_G = 0x0B,
- INT1_CTRL = 0x0C,
- INT2_CTRL = 0x0D,
- WHO_AM_I = 0x0F,
- CTRL_REG1_G = 0x10,
- CTRL_REG2_G = 0x11,
- CTRL_REG3_G = 0x12,
- ORIENT_CFG_G = 0x13,
- INT_GEN_SRC_G = 0x14,
- OUT_TEMP_L = 0x15,
- OUT_TEMP_H = 0x16,
- STATUS_REG_G = 0x17, // per check stato
- OUT_X_L_G = 0x18,
- OUT_X_H_G = 0x19,
- OUT_Y_L_G = 0x1A,
- OUT_Y_H_G = 0x1B,
- OUT_Z_L_G = 0x1C,
- OUT_Z_H_G = 0x1D,
- CTRL_REG4 = 0x1E,
- CTRL_REG5_XL = 0x1F,
- CTRL_REG6_XL = 0x20,
- CTRL_REG7_XL = 0x21,
- CTRL_REG8 = 0x22,
- CTRL_REG9 = 0x23,
- CTRL_REG10 = 0x24, //per self-test ma n.u.
- INT_GEN_SRC_XL = 0x26,
- STATUS_REG_XL = 0x27, // per check stato
- OUT_X_L_XL = 0x28,
- OUT_X_H_XL = 0x29,
- OUT_Y_L_XL = 0x2A,
- OUT_Y_H_XL = 0x2B,
- OUT_Z_L_XL = 0x2C,
- OUT_Z_H_XL = 0x2D,
- FIFO_CTRL = 0x2E,
- FIFO_SRC = 0x2F, //FIFO status register
- INT_GEN_CFG_G = 0x30,
- INT_GEN_THS_XH_G = 0x31,
- INT_GEN_THS_XL_G = 0x32,
- INT_GEN_THS_YH_G = 0x33,
- INT_GEN_THS_YL_G = 0x34,
- INT_GEN_THS_ZH_G = 0x35,
- INT_GEN_THS_ZL_G = 0x36,
- INT_GEN_DUR_G = 0x37
- };
-
- static const uint8_t INT1_CTRL_VAL = 0x08;
- static const uint8_t WHO_AM_I_XLG_VAL = 0x68;
- static const uint8_t CTRL_REG8_VAL = 0x04;
- static const uint8_t CTRL_REG9_VAL = 0x04;
- static const uint8_t FIFO_CTRL_VAL = 0xC0;
-
-
+ }
+
+ enum regMapXLG
+ {
+ ACT_THS = 0x04,
+ ACT_DUR = 0x05,
+ INT_GEN_CFG_XL = 0x06,
+ INT_GEN_THS_X_XL = 0x07,
+ INT_GEN_THS_Y_XL = 0x08,
+ INT_GEN_THS_Z_XL = 0x09,
+ INT_GEN_DUR_XL = 0x0A,
+ REFERENCE_G = 0x0B,
+ INT1_CTRL = 0x0C,
+ INT2_CTRL = 0x0D,
+ WHO_AM_I = 0x0F,
+ CTRL_REG1_G = 0x10,
+ CTRL_REG2_G = 0x11,
+ CTRL_REG3_G = 0x12,
+ ORIENT_CFG_G = 0x13,
+ INT_GEN_SRC_G = 0x14,
+ OUT_TEMP_L = 0x15,
+ OUT_TEMP_H = 0x16,
+ STATUS_REG_G = 0x17, // per check stato
+ OUT_X_L_G = 0x18,
+ OUT_X_H_G = 0x19,
+ OUT_Y_L_G = 0x1A,
+ OUT_Y_H_G = 0x1B,
+ OUT_Z_L_G = 0x1C,
+ OUT_Z_H_G = 0x1D,
+ CTRL_REG4 = 0x1E,
+ CTRL_REG5_XL = 0x1F,
+ CTRL_REG6_XL = 0x20,
+ CTRL_REG7_XL = 0x21,
+ CTRL_REG8 = 0x22,
+ CTRL_REG9 = 0x23,
+ CTRL_REG10 = 0x24, // per self-test ma n.u.
+ INT_GEN_SRC_XL = 0x26,
+ STATUS_REG_XL = 0x27, // per check stato
+ OUT_X_L_XL = 0x28,
+ OUT_X_H_XL = 0x29,
+ OUT_Y_L_XL = 0x2A,
+ OUT_Y_H_XL = 0x2B,
+ OUT_Z_L_XL = 0x2C,
+ OUT_Z_H_XL = 0x2D,
+ FIFO_CTRL = 0x2E,
+ FIFO_SRC = 0x2F, // FIFO status register
+ INT_GEN_CFG_G = 0x30,
+ INT_GEN_THS_XH_G = 0x31,
+ INT_GEN_THS_XL_G = 0x32,
+ INT_GEN_THS_YH_G = 0x33,
+ INT_GEN_THS_YL_G = 0x34,
+ INT_GEN_THS_ZH_G = 0x35,
+ INT_GEN_THS_ZL_G = 0x36,
+ INT_GEN_DUR_G = 0x37
+ };
+
+ static const uint8_t INT1_CTRL_VAL = 0x08;
+ static const uint8_t WHO_AM_I_XLG_VAL = 0x68;
+ static const uint8_t CTRL_REG8_VAL = 0x04;
+ static const uint8_t CTRL_REG9_VAL = 0x04;
+ static const uint8_t FIFO_CTRL_VAL = 0xC0;
};
-
diff --git a/src/tests/drivers/test-lsm9ds1-fifo.cpp b/src/tests/drivers/test-lsm9ds1-fifo.cpp
index 5ed77b7f9877b91d5cf1c5bd2f285028a48a915f..174a78af6d9454a132a3c8c544633aba8245d377 100644
--- a/src/tests/drivers/test-lsm9ds1-fifo.cpp
+++ b/src/tests/drivers/test-lsm9ds1-fifo.cpp
@@ -39,7 +39,7 @@ typedef Gpio<GPIOA_BASE, 7> GpioMosi;
typedef Gpio<GPIOA_BASE, 1> GpioINT1;
static const bool FIFO_ENABLED = true;
-static const uint8_t FIFO_WATERMARK = 12;
+static const uint8_t FIFO_WATERMARK = 20;
static const uint8_t FIFO_SAMPLES = 5;
// SPI
@@ -53,15 +53,18 @@ GpioPin LED1(GPIOD_BASE, 15);
volatile bool flagSPIReadRequest = false;
// High Resolution hardware timer using TIM5
-HardwareTimer<uint32_t> hrclock{
- TIM5, TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)};
+HardwareTimer<uint32_t> hrclock(
+ TIM5, TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1));
// Last interrupt tick & delta
volatile uint32_t last_tick;
volatile uint32_t delta;
-//LSM9DS1 obj
-LSM9DS1_XLG* lsm9ds1 = nullptr;
+// Arrays for samples
+array<Vec3, 32> axelData[FIFO_SAMPLES], gyroData[FIFO_SAMPLES];
+
+// LSM9DS1 obj
+LSM9DS1_XLG* lsm9ds1 = nullptr;
// Interrupt handlers
void __attribute__((naked)) EXTI1_IRQHandler()
@@ -97,26 +100,21 @@ int main()
uint8_t fifo_counter = 0;
uint32_t dt[FIFO_SAMPLES];
- //array<Vec3, 32> axelData[FIFO_SAMPLES], gyroData[FIFO_SAMPLES];
gpioConfig();
-
- Thread::sleep(4000);
+ Thread::sleep(1000);
LED1.low();
timer5Config();
EXTI1Config();
- std::cout << "Before allocation" << std::endl; //FA ANCORA STACK OVERFLOW DOPO QUI GRRRR
-
- lsm9ds1 = new LSM9DS1_XLG(bus, cs_XLG, LSM9DS1_XLG::AxelFSR::FS_8,
- LSM9DS1_XLG::GyroFSR::FS_245, LSM9DS1_XLG::ODR::ODR_15,
- FIFO_ENABLED, FIFO_WATERMARK);
-
- std::cout << "After allocation" << std::endl;
+ lsm9ds1 = new LSM9DS1_XLG(
+ bus, cs_XLG, LSM9DS1_XLG::AxelFSR::FS_8, LSM9DS1_XLG::GyroFSR::FS_245,
+ LSM9DS1_XLG::ODR::ODR_952, FIFO_ENABLED, FIFO_WATERMARK);
- while (!lsm9ds1->init());
+ while (!lsm9ds1->init())
+ ;
lsm9ds1->clearFIFO();
@@ -125,10 +123,10 @@ int main()
if (flagSPIReadRequest && fifo_counter < FIFO_SAMPLES)
{
flagSPIReadRequest = false;
- dt[fifo_counter] = delta;
+ dt[fifo_counter] = hrclock.toMicroSeconds(delta);
lsm9ds1->onSimpleUpdate();
- //axelData[fifo_counter] = lsm9ds1->getAxelFIFO();
- //gyroData[fifo_counter] = lsm9ds1->getGyroFIFO();
+ axelData[fifo_counter] = lsm9ds1->getAxelFIFO();
+ gyroData[fifo_counter] = lsm9ds1->getGyroFIFO();
LED1.low();
fifo_counter++;
}
@@ -139,9 +137,20 @@ int main()
}
}
+ // print data
+ uint32_t timestamp = 0;
+
for (uint8_t i = 0; i < FIFO_SAMPLES; i++)
{
- std::cout << dt[i] << std::endl;
+ std::cout << "FIFO " << (int)i + 1 << std::endl;
+ for (uint8_t j = 0; j < FIFO_WATERMARK; j++)
+ {
+ timestamp += dt[i] / FIFO_WATERMARK;
+ printf("%d>>%ld>>%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\n", j,timestamp,
+ axelData[i][j].getX(), axelData[i][j].getY(),
+ axelData[i][j].getZ(), gyroData[i][j].getX(),
+ gyroData[i][j].getY(), gyroData[i][j].getZ());
+ }
}
return 0;
@@ -188,9 +197,11 @@ void timer5Config()
// Enable high resolution TIM5
RCC->APB1ENR |= RCC_APB1ENR_TIM5EN;
}
+ hrclock.setPrescaler(382);
+ hrclock.start();
}
-void EXTI1Config() //PC13
+void EXTI1Config() // PC13
{
// Enable SYSCFG for setting interrupts
{
diff --git a/src/tests/drivers/test-lsm9ds1.cpp b/src/tests/drivers/test-lsm9ds1.cpp
index 68b30977551376c46b0e0f78fa9a03d71075c042..f595cd209d6d03566c12c9ed30932a02b701d485 100644
--- a/src/tests/drivers/test-lsm9ds1.cpp
+++ b/src/tests/drivers/test-lsm9ds1.cpp
@@ -135,6 +135,7 @@ int main(){
gdata.getX(), gdata.getY(), gdata.getZ(),
mdata.getX(), mdata.getY(), mdata.getZ(),
tdata);
+ //printf("%.3f,%.3f,%.3f\n", mdata.getX(), mdata.getY(), mdata.getZ());
}