From 1a6bb97f168a30c1910bbbb748335e7900670c58 Mon Sep 17 00:00:00 2001
From: Nivek-domo <123359286+Nivek-domo@users.noreply.github.com>
Date: Mon, 16 Jun 2025 13:54:55 +0200
Subject: [PATCH] Fix RCWL9620Sensor for rak11310 support (#6617)

* Update RCWL9620Sensor.cpp

test on rak11310, work very wel now

* Update RCWL9620Sensor.cpp

* Trunk

---------

Co-authored-by: Ben Meadors <benmmeadors@gmail.com>
---
 .../Telemetry/Sensor/RCWL9620Sensor.cpp       | 43 +++++++++++++------
 1 file changed, 29 insertions(+), 14 deletions(-)

diff --git a/src/modules/Telemetry/Sensor/RCWL9620Sensor.cpp b/src/modules/Telemetry/Sensor/RCWL9620Sensor.cpp
index e352dda8d..9f7a55cc5 100644
--- a/src/modules/Telemetry/Sensor/RCWL9620Sensor.cpp
+++ b/src/modules/Telemetry/Sensor/RCWL9620Sensor.cpp
@@ -41,21 +41,36 @@ void RCWL9620Sensor::begin(TwoWire *wire, uint8_t addr, uint8_t sda, uint8_t scl
 
 float RCWL9620Sensor::getDistance()
 {
-    uint32_t data;
-    _wire->beginTransmission(_addr); // Transfer data to addr.
-    _wire->write(0x01);
-    _wire->endTransmission(); // Stop data transmission with the Ultrasonic
-                              // Unit.
+    uint32_t data = 0;
+    uint8_t b1 = 0, b2 = 0, b3 = 0;
 
-    _wire->requestFrom(_addr,
-                       (uint8_t)3); // Request 3 bytes from Ultrasonic Unit.
+    LOG_DEBUG("[RCWL9620] Start measure command");
+
+    _wire->beginTransmission(_addr);
+    _wire->write(0x01); // À tester aussi sans cette ligne si besoin
+    uint8_t result = _wire->endTransmission();
+    LOG_DEBUG("[RCWL9620] endTransmission result = %d", result);
+    delay(100); // délai pour laisser le capteur répondre
+
+    LOG_DEBUG("[RCWL9620] Read i2c data:");
+    _wire->requestFrom(_addr, (uint8_t)3);
+
+    if (_wire->available() < 3) {
+        LOG_DEBUG("[RCWL9620] less than 3 octets !");
+        return 0.0;
+    }
+
+    b1 = _wire->read();
+    b2 = _wire->read();
+    b3 = _wire->read();
+
+    data = ((uint32_t)b1 << 16) | ((uint32_t)b2 << 8) | b3;
+
+    float Distance = float(data) / 1000.0;
+
+    LOG_DEBUG("[RCWL9620] Bytes readed = %02X %02X %02X", b1, b2, b3);
+    LOG_DEBUG("[RCWL9620] data=%.2f, level=%.2f", (double)data, (double)Distance);
 
-    data = _wire->read();
-    data <<= 8;
-    data |= _wire->read();
-    data <<= 8;
-    data |= _wire->read();
-    float Distance = float(data) / 1000;
     if (Distance > 4500.00) {
         return 4500.00;
     } else {
@@ -63,4 +78,4 @@ float RCWL9620Sensor::getDistance()
     }
 }
 
-#endif
\ No newline at end of file
+#endif