[VN100-SPI] Correctly reads accelerometer data.

src/shared/sensors/VN100/VN100Spi.cpp

@@ -22,6 +22,10 @@
 
 #include "VN100Spi.h"
 
+#include <drivers/timer/TimestampTimer.h>
+#include <interfaces/endianness.h>
+#include <utils/Debug.h>
+
 #include "VN100SpiDefs.h"
 
 namespace Boardcore
@@ -86,7 +90,8 @@ bool VN100Spi::checkModelNumber()
         spiSlave.bus.select(spiSlave.cs);
 
         // Discard the first 3 bytes of the response
-        spiSlave.bus.read24();
+        spiSlave.bus
+            .read24();  // TODO: should I verify also the command and register?
 
         err = spiSlave.bus.read();
 
@@ -121,4 +126,78 @@ VN100Data VN100Spi::sampleImpl()
     return VN100Data();
 }
 
+AccelerometerData VN100Spi::readAcc()
+{
+    // Low level spi is needed in order to read multiple data
+    // without raising the chip select pin
+
+    AccelerometerData data;
+
+    // Variables used to store raw data extracted from the sensor
+    uint32_t rawDataX = 0;
+    uint32_t rawDataY = 0;
+    uint32_t rawDataZ = 0;
+
+    constexpr uint32_t requestPacket =
+        (VN100SpiDefs::READ_REG << 24) |  // Read register command
+        (18 << 16);                       // Id of the register
+
+    data.accelerationTimestamp = TimestampTimer::getTimestamp();
+
+    // Send request packet
+    spiSlave.bus.select(spiSlave.cs);
+    spiSlave.bus.write32(requestPacket);
+    spiSlave.bus.deselect(spiSlave.cs);
+
+    // Wait at least 100us
+    miosix::delayUs(100);
+
+    // Read response
+    spiSlave.bus.select(spiSlave.cs);
+
+    // Discard the first 3 bytes of the response
+    spiSlave.bus
+        .read24();  // TODO: should I verify also the command and register?
+
+    uint8_t err = spiSlave.bus.read();
+
+    if (err != 0)
+    {
+        // An error occurred while attempting to service the request
+        spiSlave.bus.deselect(spiSlave.cs);
+        lastError = COMMAND_FAILED;
+        return AccelerometerData();
+    }
+
+    rawDataX = spiSlave.bus.read32();
+    rawDataY = spiSlave.bus.read32();
+    rawDataZ = spiSlave.bus.read32();
+
+    spiSlave.bus.deselect(spiSlave.cs);
+
+    // Get measurements from raw data
+    data.accelerationX = extractMeasurement(rawDataX);
+    data.accelerationY = extractMeasurement(rawDataY);
+    data.accelerationZ = extractMeasurement(rawDataZ);
+
+    return data;
+}
+
+float VN100Spi::extractMeasurement(uint32_t rawData)
+{
+    // The floating point values received are stored as 32-bit IEEE
+    // floating point numbers in little endian byte order.
+
+    // Ensure that the operation is legal
+    static_assert(sizeof(uint32_t) == sizeof(float) &&
+                  "Error, data size mismatch");
+
+    rawData = swapBytes32(rawData);
+
+    float f;
+    std::memcpy(&f, &rawData, sizeof(uint32_t));
+
+    return f;
+}
+
 }  // namespace Boardcore

src/shared/sensors/VN100/VN100Spi.h

@@ -63,6 +63,9 @@ public:
      */
     VN100Data sampleImpl() override;
 
+    // TODO: remove this, only for testing.
+    AccelerometerData readAcc();
+
 private:
     /**
      * @brief Check the model number register.
@@ -71,6 +74,16 @@ private:
      */
     bool checkModelNumber();
 
+    /**
+     * @brief Extracts floating point measurement from the data received from
+     * the sensor.
+     *
+     * @param rawData The data received from the sensor.
+     *
+     * @return The floating point data.
+     */
+    float extractMeasurement(uint32_t rawData);
+
     bool isInit = false;
 
     SPISlave spiSlave;

src/shared/sensors/VN100/VN100SpiDefs.h

@@ -33,7 +33,7 @@ namespace VN100SpiDefs
  */
 enum Registers
 {
-    REG_MODEL_NUMBER = 0x01,
+    REG_MODEL_NUMBER = 1,
 };
 
 /**
@@ -41,8 +41,8 @@ enum Registers
  */
 enum Commands
 {
-    READ_REG  = 0x01,
-    WRITE_REG = 0x02,
+    READ_REG  = 1,
+    WRITE_REG = 2,
 };
 
 /**

src/tests/sensors/test-vn100-spi.cpp

@@ -50,6 +50,9 @@ int main()
 
     VN100Spi sensor(bus, csPin, busConfiguration);
 
+    // Let the sensor start up
+    Thread::sleep(1000);
+
     if (!sensor.init())
     {
         printf("Error, cannot initialize the sensor\n\n");
@@ -57,5 +60,17 @@ int main()
     }
     printf("Sensor initialized\n");
 
+    for (int i = 0; i < 100; ++i)
+    {
+        AccelerometerData data = sensor.readAcc();
+
+        printf("sample %d:\n", i + 1);
+        printf("AccX: %f\n", data.accelerationX);
+        printf("AccY: %f\n", data.accelerationY);
+        printf("AccZ: %f\n\n", data.accelerationZ);
+
+        Thread::sleep(500);
+    }
+
     return 0;
 }