From 0257e51cad5c24d706989132cf487d404b5586c0 Mon Sep 17 00:00:00 2001
From: Fabrizio Monti <fabrizio.monti@skywarder.eu>
Date: Mon, 25 Mar 2024 09:04:23 +0100
Subject: [PATCH] [VN100-SPI] Added write register function.

---
 src/shared/sensors/VN100/VN100Spi.cpp | 47 +++++++++++++++++++++++++++
 src/shared/sensors/VN100/VN100Spi.h   | 14 ++++++++
 2 files changed, 61 insertions(+)

diff --git a/src/shared/sensors/VN100/VN100Spi.cpp b/src/shared/sensors/VN100/VN100Spi.cpp
index 73b37a900..a2089312b 100644
--- a/src/shared/sensors/VN100/VN100Spi.cpp
+++ b/src/shared/sensors/VN100/VN100Spi.cpp
@@ -266,4 +266,51 @@ uint8_t VN100Spi::readRegister(const uint32_t REG_ID, uint8_t* payloadBuf,
     return 0;
 }
 
+uint8_t VN100Spi::writeRegister(const uint32_t REG_ID, uint8_t* payloadBuf,
+                                const uint32_t PAYLOAD_SIZE)
+{
+    /**
+     * When writing to a sensor's register 2 spi transactions are needed.
+     *
+     * First I have to send the request packet with the value to be written,
+     * then wait at least 100 microseconds to let the sensor process the
+     * request.
+     *
+     * After this period of time we proceed with reading the outcome of the
+     * operation. We receive a 4 bytes header, whit the first byte always 0, the
+     * second being the write register command, the third being the register we
+     * asked for and the fourth the error value. If the error value is 0 no
+     * error occurred and the operation is successful.
+     *
+     * Low level spi is needed in order to issue multiple readings and writings
+     * without raising the chip select.
+     */
+
+    const uint32_t requestPacket =
+        (VN100SpiDefs::WRITE_REG << 24) |  // Read register command
+        (REG_ID << 16);                    // Id of the register
+
+    // Send request packet
+    spiSlave.bus.select(spiSlave.cs);
+    spiSlave.bus.write32(requestPacket);
+    spiSlave.bus.write(payloadBuf, PAYLOAD_SIZE);
+    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();
+
+    spiSlave.bus.deselect(spiSlave.cs);
+
+    return err;
+}
+
 }  // namespace Boardcore
diff --git a/src/shared/sensors/VN100/VN100Spi.h b/src/shared/sensors/VN100/VN100Spi.h
index 1a0ddcf8b..0e4291085 100644
--- a/src/shared/sensors/VN100/VN100Spi.h
+++ b/src/shared/sensors/VN100/VN100Spi.h
@@ -77,6 +77,7 @@ private:
      * sensor.
      */
     void sendDummyPacket();
+    // TODO: check if really needed and if there is a better solution
 
     /**
      * @brief Get accelerometer, gyroscope, magnetometer, pressure and
@@ -121,6 +122,19 @@ private:
     uint8_t readRegister(const uint32_t REG_ID, uint8_t* payloadBuf,
                          const uint32_t PAYLOAD_SIZE);
 
+    /**
+     * @brief Utility function used to write data to a register of the sensor.
+     *
+     * @param REG_ID The id of the register to be written.
+     * @param payloadBuf The buffer containing the data to be written.
+     * @param PAYLOAD_SIZE The amount of data (in bytes) to be written.
+     *
+     * @return Zero if the operation is successful, the error code otherwise.
+     * See the datasheet for error codes details.
+     */
+    uint8_t writeRegister(const uint32_t REG_ID, uint8_t* payloadBuf,
+                          const uint32_t PAYLOAD_SIZE);
+
     bool isInit = false;
 
     SPISlave spiSlave;
-- 
GitLab