From e514052f72dfd7a6f3ea55247d627ac548ca8bb2 Mon Sep 17 00:00:00 2001
From: oscgonfer <oscgonfer@gmail.com>
Date: Mon, 21 Jul 2025 14:01:09 +0200
Subject: [PATCH] Changes on bus speed for ADS1X15

This fixes daisy chaining of various sensors together, with a "dirty" I2C.
I2C buses with step-ups and with level-shifters with mosfets can have issues at high speeds.
---
 src/detect/ScanI2CTwoWire.cpp                 |  2 +-
 src/modules/Telemetry/PowerTelemetry.cpp      |  5 +++
 .../Telemetry/Sensor/ADS1X15Sensor.cpp        | 37 ++++++++++++++++++-
 src/modules/Telemetry/Sensor/ADS1X15Sensor.h  |  4 ++
 4 files changed, 46 insertions(+), 2 deletions(-)

diff --git a/src/detect/ScanI2CTwoWire.cpp b/src/detect/ScanI2CTwoWire.cpp
index 441743a90..63f50b9a1 100644
--- a/src/detect/ScanI2CTwoWire.cpp
+++ b/src/detect/ScanI2CTwoWire.cpp
@@ -600,7 +600,7 @@ void ScanI2CTwoWire::scanPort(I2CPort port, uint8_t *address, uint8_t asize)
                 registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x01), 2);
                 if (registerValue == 0x8583) {
                     type = ADS1X15_ALT;
-                    logFoundDevice("ADS1X15", (uint8_t)addr.address);
+                    logFoundDevice("ADS1X15_ALT", (uint8_t)addr.address);
                     break;
                 }
 
diff --git a/src/modules/Telemetry/PowerTelemetry.cpp b/src/modules/Telemetry/PowerTelemetry.cpp
index e007a6115..9200cf2c7 100644
--- a/src/modules/Telemetry/PowerTelemetry.cpp
+++ b/src/modules/Telemetry/PowerTelemetry.cpp
@@ -276,6 +276,11 @@ bool PowerTelemetryModule::sendTelemetry(NodeNum dest, bool phoneOnly)
                  m.variant.power_metrics.ch1_voltage, m.variant.power_metrics.ch1_current, m.variant.power_metrics.ch2_voltage,
                  m.variant.power_metrics.ch2_current, m.variant.power_metrics.ch3_voltage, m.variant.power_metrics.ch3_current,
                  m.variant.power_metrics.ch4_voltage);
+        LOG_INFO("Send: ch5_voltage=%f, ch5_current=%f, ch6_voltage=%f, ch6_current=%f, "
+                 "ch7_voltage=%f, ch7_current=%f, ch8_voltage=%f",
+                 m.variant.power_metrics.ch5_voltage, m.variant.power_metrics.ch5_current, m.variant.power_metrics.ch6_voltage,
+                 m.variant.power_metrics.ch6_current, m.variant.power_metrics.ch7_voltage, m.variant.power_metrics.ch7_current,
+                 m.variant.power_metrics.ch8_voltage, m.variant.power_metrics.ch8_current);
 
         sensor_read_error_count = 0;
 
diff --git a/src/modules/Telemetry/Sensor/ADS1X15Sensor.cpp b/src/modules/Telemetry/Sensor/ADS1X15Sensor.cpp
index d196e7256..3715b2f9b 100644
--- a/src/modules/Telemetry/Sensor/ADS1X15Sensor.cpp
+++ b/src/modules/Telemetry/Sensor/ADS1X15Sensor.cpp
@@ -16,7 +16,26 @@ int32_t ADS1X15Sensor::runOnce()
         return DEFAULT_SENSOR_MINIMUM_WAIT_TIME_BETWEEN_READS;
     }
 
-    status = ads1x15.begin(nodeTelemetrySensorsMap[sensorType].first);
+    bus = nodeTelemetrySensorsMap[sensorType].second;
+    address = (uint8_t)nodeTelemetrySensorsMap[sensorType].first;
+
+#ifdef ADS1X15_I2C_CLOCK_SPEED
+    uint32_t currentClock;
+    currentClock = bus->getClock();
+    if (currentClock != ADS1X15_I2C_CLOCK_SPEED){
+        // LOG_DEBUG("Changing I2C clock to %u", ADS1X15_I2C_CLOCK_SPEED);
+        bus->setClock(ADS1X15_I2C_CLOCK_SPEED);
+    }
+#endif
+
+    status = ads1x15.begin(address);
+
+#ifdef ADS1X15_I2C_CLOCK_SPEED
+    if (currentClock != ADS1X15_I2C_CLOCK_SPEED){
+        // LOG_DEBUG("Restoring I2C clock to %uHz", currentClock);
+        bus->setClock(currentClock);
+    }
+#endif
 
     return initI2CSensor();
 }
@@ -27,6 +46,15 @@ struct _ADS1X15Measurement ADS1X15Sensor::getMeasurement(uint8_t ch)
 {
     struct _ADS1X15Measurement measurement;
 
+#ifdef ADS1X15_I2C_CLOCK_SPEED
+    uint32_t currentClock;
+    currentClock = bus->getClock();
+    if (currentClock != ADS1X15_I2C_CLOCK_SPEED){
+        // LOG_DEBUG("Changing I2C clock to %u", ADS1X15_I2C_CLOCK_SPEED);
+        bus->setClock(ADS1X15_I2C_CLOCK_SPEED);
+    }
+#endif
+
     // Reset gain
     ads1x15.setGain(GAIN_TWOTHIRDS);
     double voltage_range = 6.144;
@@ -72,6 +100,13 @@ struct _ADS1X15Measurement ADS1X15Sensor::getMeasurement(uint8_t ch)
         value = ads1x15.readADC_SingleEnded(ch);
     }
 
+#ifdef ADS1X15_I2C_CLOCK_SPEED
+    if (currentClock != ADS1X15_I2C_CLOCK_SPEED){
+        // LOG_DEBUG("Restoring I2C clock to %uHz", currentClock);
+        bus->setClock(currentClock);
+    }
+#endif
+
     measurement.voltage = (float)value / 32768 * voltage_range;
 
     return measurement;
diff --git a/src/modules/Telemetry/Sensor/ADS1X15Sensor.h b/src/modules/Telemetry/Sensor/ADS1X15Sensor.h
index 7ddd3d073..309beb922 100644
--- a/src/modules/Telemetry/Sensor/ADS1X15Sensor.h
+++ b/src/modules/Telemetry/Sensor/ADS1X15Sensor.h
@@ -6,10 +6,14 @@
 #include "TelemetrySensor.h"
 #include <Adafruit_ADS1X15.h>
 
+#define ADS1X15_I2C_CLOCK_SPEED 100000
+
 class ADS1X15Sensor : public TelemetrySensor
 {
   private:
     Adafruit_ADS1X15 ads1x15;
+    TwoWire * bus;
+    uint8_t address;
 
     // get a single measurement for a channel
     struct _ADS1X15Measurement getMeasurement(uint8_t ch);