From 13ad307639e37ba24473e7de79b9abd7cff9ceb8 Mon Sep 17 00:00:00 2001
From: Fabrizio Monti <fabrizio.monti@skywarder.eu>
Date: Mon, 18 Mar 2024 11:05:18 +0100
Subject: [PATCH] [VN100-SPI] Fixed sensor initialization. The first read from
 the sensor could have failed because of junk, that is now removed thanks to
 sendDummyPacket().

---
 src/shared/sensors/VN100/VN100Spi.cpp | 22 +++++++++++++++++++---
 src/shared/sensors/VN100/VN100Spi.h   |  7 +++++++
 2 files changed, 26 insertions(+), 3 deletions(-)

diff --git a/src/shared/sensors/VN100/VN100Spi.cpp b/src/shared/sensors/VN100/VN100Spi.cpp
index d4a01e4f6..931881be0 100644
--- a/src/shared/sensors/VN100/VN100Spi.cpp
+++ b/src/shared/sensors/VN100/VN100Spi.cpp
@@ -46,6 +46,10 @@ bool VN100Spi::init()
         return false;
     }
 
+    // First communication after startup might fail
+    // Send dummy data to clean up
+    sendDummyPacket();
+
     if (checkModelNumber() == false)
     {
         LOG_ERR(logger, "Got bad CHIPID");
@@ -103,13 +107,25 @@ bool VN100Spi::checkModelNumber()
     if (err != 0)
     {
         // An error occurred while attempting to service the request
-        LOG_ERR(logger, "Error, cannot get CHIPID");
+        LOG_ERR(logger, "Error while reading CHIPID");
         return false;
     }
 
     // Check the model number
-    return strncmp(VN100SpiDefs::MODEL_NUMBER, buf,
-                   strlen(VN100SpiDefs::MODEL_NUMBER)) == 0;
+    if (strncmp(VN100SpiDefs::MODEL_NUMBER, buf,
+                strlen(VN100SpiDefs::MODEL_NUMBER)) != 0)
+    {
+        LOG_ERR(logger, "Error, invalid CHIPID");
+        return false;
+    }
+
+    return true;
+}
+
+void VN100Spi::sendDummyPacket()
+{
+    SPITransaction transaction(spiSlave);
+    transaction.write32(0);
 }
 
 bool VN100Spi::selfTest()
diff --git a/src/shared/sensors/VN100/VN100Spi.h b/src/shared/sensors/VN100/VN100Spi.h
index 2fb076c8f..cd105e0b5 100644
--- a/src/shared/sensors/VN100/VN100Spi.h
+++ b/src/shared/sensors/VN100/VN100Spi.h
@@ -74,6 +74,13 @@ private:
      */
     bool checkModelNumber();
 
+    /**
+     * @brief Utility function used to clean the junk before starting to
+     * communicate with the sensor. It send a 4 bytes packet of zeros to the
+     * sensor.
+     */
+    void sendDummyPacket();
+
     /**
      * @brief Extracts floating point measurement from the data received from
      * the sensor.
-- 
GitLab