From 402b0d7e0bb41c2437f94e346ab1f35b2d597345 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Thomas=20G=C3=B6ttgens?= <tgoettgens@gmail.com>
Date: Sun, 21 Apr 2024 14:39:55 +0200
Subject: [PATCH] ditch that no-good m5 dependancy and do it ourself

---
 platformio.ini                                |  1 -
 .../Telemetry/Sensor/RCWL9620Sensor.cpp       | 41 +++++++++++++++++--
 src/modules/Telemetry/Sensor/RCWL9620Sensor.h |  8 +++-
 3 files changed, 44 insertions(+), 6 deletions(-)

diff --git a/platformio.ini b/platformio.ini
index 01924e290..89c69013d 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -133,4 +133,3 @@ lib_deps =
   adafruit/Adafruit LIS3DH@^1.2.4
   https://github.com/lewisxhe/SensorLib#27fd0f721e20cd09e1f81383f0ba58a54fe84a17
   adafruit/Adafruit LSM6DS@^4.7.2
-  m5stack/M5Unit-Sonic@^0.0.2
diff --git a/src/modules/Telemetry/Sensor/RCWL9620Sensor.cpp b/src/modules/Telemetry/Sensor/RCWL9620Sensor.cpp
index 96e9a7445..84003188e 100644
--- a/src/modules/Telemetry/Sensor/RCWL9620Sensor.cpp
+++ b/src/modules/Telemetry/Sensor/RCWL9620Sensor.cpp
@@ -11,8 +11,7 @@ int32_t RCWL9620Sensor::runOnce()
     if (!hasSensor()) {
         return DEFAULT_SENSOR_MINIMUM_WAIT_TIME_BETWEEN_READS;
     }
-    status = 1;
-    rcwl9620.begin(nodeTelemetrySensorsMap[sensorType].second, nodeTelemetrySensorsMap[sensorType].first, -1, -1);
+    status = begin(nodeTelemetrySensorsMap[sensorType].first, nodeTelemetrySensorsMap[sensorType].second);
     return initI2CSensor();
 }
 
@@ -21,6 +20,42 @@ void RCWL9620Sensor::setup() {}
 bool RCWL9620Sensor::getMetrics(meshtastic_Telemetry *measurement)
 {
     LOG_DEBUG("RCWL9620Sensor::getMetrics\n");
-    measurement->variant.environment_metrics.distance = rcwl9620.getDistance();
+    measurement->variant.environment_metrics.distance = getDistance();
     return true;
+}
+
+bool RCWL9620Sensor::begin(uint8_t addr, TwoWire *wire)
+{
+    _wire = wire;
+    _addr = addr;
+    if (i2c_dev)
+        delete i2c_dev;
+    i2c_dev = new Adafruit_I2CDevice(_addr, _wire);
+    if (!i2c_dev->begin())
+        return false;
+    return true;
+}
+
+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.
+
+    _wire->requestFrom(_addr,
+                       (uint8_t)3); // Request 3 bytes from Ultrasonic Unit.
+
+    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 {
+        return Distance;
+    }
 }
\ No newline at end of file
diff --git a/src/modules/Telemetry/Sensor/RCWL9620Sensor.h b/src/modules/Telemetry/Sensor/RCWL9620Sensor.h
index d3efe0ef5..4120e19d9 100644
--- a/src/modules/Telemetry/Sensor/RCWL9620Sensor.h
+++ b/src/modules/Telemetry/Sensor/RCWL9620Sensor.h
@@ -1,14 +1,18 @@
 #include "../mesh/generated/meshtastic/telemetry.pb.h"
 #include "TelemetrySensor.h"
-#include <Unit_Sonic.h>
+#include <Adafruit_I2CDevice.h>
 
 class RCWL9620Sensor : public TelemetrySensor
 {
   private:
-    SONIC_I2C rcwl9620;
+    uint8_t _addr;
+    TwoWire *_wire;
 
   protected:
     virtual void setup() override;
+    bool begin(uint8_t addr = 0x57, TwoWire *wire = &Wire);
+    Adafruit_I2CDevice *i2c_dev = NULL;
+    float getDistance();
 
   public:
     RCWL9620Sensor();