[LSM6DSRX] Refactored fifo threshold interrupt settings and fixed reading from...

[LSM6DSRX] Refactored fifo threshold interrupt settings and fixed reading from fifo (sensor data has to be casted into a signed number before float).

src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp

@@ -126,22 +126,7 @@ bool LSM6DSRX::init()
     }
 
     // set interrupt
-    {
-        SPITransaction spi{m_spiSlave};
-        spi.writeRegister(
-            LSM6DSRXDefs::REG_INT1_CTRL,
-            static_cast<uint8_t>(m_config.int1InterruptSelection));
-        spi.writeRegister(
-            LSM6DSRXDefs::REG_INT2_CTRL,
-            static_cast<uint8_t>(m_config.int2InterruptSelection));
-
-        // setto a caso watermark per la fifo
-        uint8_t numeroByte =
-            100;  // 100 campioni
-                  // //////////////////////////////////////////////////////////////////////
-        spi.writeRegister(LSM6DSRXDefs::REG_FIFO_CTRL1, numeroByte);
-        spi.writeRegister(LSM6DSRXDefs::REG_FIFO_CTRL2, 0);
-    }
+    initInterrupts();
 
     m_isInit = true;
     return true;
@@ -210,6 +195,25 @@ bool LSM6DSRX::initFifo()
     return true;
 }
 
+void LSM6DSRX::initInterrupts()
+{
+    uint8_t ui8Value = 0;
+    SPITransaction spi{m_spiSlave};
+
+    // set interrupt on pin INT1
+    spi.writeRegister(LSM6DSRXDefs::REG_INT1_CTRL,
+                      static_cast<uint8_t>(m_config.int1InterruptSelection));
+    // set interrupt on pin INT2
+    spi.writeRegister(LSM6DSRXDefs::REG_INT2_CTRL,
+                      static_cast<uint8_t>(m_config.int2InterruptSelection));
+
+    // set watermark level
+    ui8Value = static_cast<uint8_t>(m_config.fifoWatermark & 255);
+    spi.writeRegister(LSM6DSRXDefs::REG_FIFO_CTRL1, ui8Value);
+    ui8Value = static_cast<uint8_t>(m_config.fifoWatermark & 768);
+    spi.writeRegister(LSM6DSRXDefs::REG_FIFO_CTRL2, ui8Value);
+}
+
 bool LSM6DSRX::selfTestAcc()
 {
     bool returnValue        = false;

src/shared/sensors/LSM6DSRX/LSM6DSRX.h

@@ -146,6 +146,9 @@ public:
         return num;
     }
 
+    /**
+     * @brief Returns the number of unread data in the fifo.
+     */
     unsigned int unreadDataInFifo()
     {
         unsigned int ris = 0;
@@ -231,6 +234,11 @@ private:
      */
     bool initFifo();
 
+    /**
+     * @brief Initialize interrupts.
+     */
+    void initInterrupts();
+
     /**
      * @brief Performs self test for the accelerometer.
      * @return Return true if successful.

src/shared/sensors/LSM6DSRX/LSM6DSRXConfig.h

@@ -173,7 +173,7 @@ struct LSM6DSRXConfig
 
         FIFO_THRESHOLD = 8,  ///< FIFO threshold interrupt.
         // FIFO_OVERRUN = 16,  ///< FIFO overrun interrupt.
-        FIFO_FULL = 32,  ///< FIFO full interrupt.
+        // FIFO_FULL = 32,  ///< FIFO full interrupt.
     };
 
     BDU bdu;  ///< Data update mode.
@@ -199,6 +199,8 @@ struct LSM6DSRXConfig
     // interrupt
     INTERRUPT int1InterruptSelection;  ///< interrupts selected on INT1 pin.
     INTERRUPT int2InterruptSelection;  ///< interrupts selected on INT2 pin.
+    uint16_t fifoWatermark;  ///< fifo watermark level (expressed as number of
+                             ///< sample). Expressed as a 10 bits number.
 };
 
 }  // namespace Boardcore
\ No newline at end of file

src/tests/sensors/test-lsm6dsrx.cpp

@@ -26,6 +26,8 @@
 #include <sensors/LSM6DSRX/LSM6DSRX.h>
 #include <utils/Debug.h>
 
+#include <algorithm>
+
 using namespace Boardcore;
 using namespace miosix;
 
@@ -61,7 +63,7 @@ int main()
 
     // acc
     sensConfig.fsAcc     = LSM6DSRXConfig::ACC_FULLSCALE::G2;
-    sensConfig.odrAcc    = LSM6DSRXConfig::ACC_ODR::HZ_1_6;
+    sensConfig.odrAcc    = LSM6DSRXConfig::ACC_ODR::HZ_12_5;
     sensConfig.opModeAcc = LSM6DSRXConfig::OPERATING_MODE::NORMAL;
 
     // gyr
@@ -72,14 +74,15 @@ int main()
     // fifo
     sensConfig.fifoMode = LSM6DSRXConfig::FIFO_MODE::CONTINUOUS;
     sensConfig.fifoTimestampDecimation =
-        LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DISABLED;
+        LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1;
     sensConfig.fifoTemperatureBdr =
         LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED;
 
     // interrupt
-    sensConfig.int1InterruptSelection = LSM6DSRXConfig::INTERRUPT::NOTHING;
+    sensConfig.int1InterruptSelection = LSM6DSRXConfig::INTERRUPT::GYR_DRDY;
     sensConfig.int2InterruptSelection =
         LSM6DSRXConfig::INTERRUPT::FIFO_THRESHOLD;
+    sensConfig.fifoWatermark = 100;
 
     LSM6DSRX sens(bus, csPin, busConfiguration, sensConfig);
 
@@ -130,10 +133,14 @@ int main()
         //     dataReady = int1Pin.value();
         // }
         // sens.getGyroscopeData(data);
+        // if(std::max({data.x, data.y, data.z}) > 1000.0 || std::min({data.x,
+        // data.y, data.z}) < -1000.0)
+        // {
         //     TRACE("Gyroscope:\n");
         //     TRACE("x: %f\n", data.x);
         //     TRACE("y: %f\n", data.y);
         //     TRACE("z: %f\n\n\n", data.z);
+        // }
 
         // accelerometer, wait for data ready
         // dataReady = int2Pin.value();
@@ -179,7 +186,7 @@ int main()
                 case 0x02:
                     // accelerometer
                     TRACE("%d) ACCELEROMETER\n", i);
-                    sensitivity = 0.122;
+                    sensitivity = 0.061;
                     break;
                 case 0x04:
                     // timestamp
@@ -195,10 +202,19 @@ int main()
             if (tagSensor != 0x04)
             {
                 // print data
-                TRACE("x: %f\n", static_cast<float>(buf[i].x) * sensitivity);
-                TRACE("y: %f\n", static_cast<float>(buf[i].y) * sensitivity);
+
+                // note: data from sensors has to be casted as signed, before
+                // casting again as float.
+
+                TRACE("x: %f\n",
+                      static_cast<float>(static_cast<int16_t>(buf[i].x)) *
+                          sensitivity);
+                TRACE("y: %f\n",
+                      static_cast<float>(static_cast<int16_t>(buf[i].y)) *
+                          sensitivity);
                 TRACE("z: %f\n\n\n",
-                      static_cast<float>(buf[i].z) * sensitivity);
+                      static_cast<float>(static_cast<int16_t>(buf[i].z)) *
+                          sensitivity);
             }
             else
             {