From f43fbe006848b3765410a1a9b8c43b0c28337c22 Mon Sep 17 00:00:00 2001
From: oscgonfer <oscgonfer@gmail.com>
Date: Tue, 8 Jul 2025 12:07:25 +0200
Subject: [PATCH] Further (non-working) progress in SEN5X

---
 src/detect/ScanI2CTwoWire.cpp                |  2 +-
 src/modules/Telemetry/Sensor/SEN5XSensor.cpp | 19 ++++++++++++++++++-
 src/modules/Telemetry/Sensor/SEN5XSensor.h   |  1 +
 3 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/src/detect/ScanI2CTwoWire.cpp b/src/detect/ScanI2CTwoWire.cpp
index 0cbb364a6..dff8969ca 100644
--- a/src/detect/ScanI2CTwoWire.cpp
+++ b/src/detect/ScanI2CTwoWire.cpp
@@ -522,12 +522,12 @@ void ScanI2CTwoWire::scanPort(I2CPort port, uint8_t *address, uint8_t asize)
             case ICM20948_ADDR_ALT: // same as MPU6050_ADDR
                 // ICM20948 Register check
                 registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x00), 1);
-                prod = readSEN5xProductName(i2cBus, addr.address);
                 if (registerValue == 0xEA) {
                     type = ICM20948;
                     logFoundDevice("ICM20948", (uint8_t)addr.address);
                     break;
                 } else {
+                    prod = readSEN5xProductName(i2cBus, addr.address);
                     if (prod.startsWith("SEN55")) {
                         type = SEN5X;
                         logFoundDevice("Sensirion SEN55", addr.address);
diff --git a/src/modules/Telemetry/Sensor/SEN5XSensor.cpp b/src/modules/Telemetry/Sensor/SEN5XSensor.cpp
index a352b6097..795d3bae7 100644
--- a/src/modules/Telemetry/Sensor/SEN5XSensor.cpp
+++ b/src/modules/Telemetry/Sensor/SEN5XSensor.cpp
@@ -101,6 +101,7 @@ bool SEN5XSensor::sendCommand(uint16_t command, uint8_t* buffer, uint8_t byteNum
     }
 
     // Transmit the data
+    LOG_INFO("Beginning connection to SEN5X: 0x%x", address);
     bus->beginTransmission(address);
     size_t writtenBytes = bus->write(toSend, bufferSize);
     uint8_t i2c_error = bus->endTransmission();
@@ -164,17 +165,33 @@ uint8_t SEN5XSensor::CRC(uint8_t* buffer)
     return crc;
 }
 
+bool SEN5XSensor::I2Cdetect(TwoWire *_Wire, uint8_t address)
+{
+    _Wire->beginTransmission(address);
+    byte error = _Wire->endTransmission();
+
+    if (error == 0) return true;
+    else return false;
+}
+
 int32_t SEN5XSensor::runOnce()
 {
+    state = SEN5X_NOT_DETECTED;
     LOG_INFO("Init sensor: %s", sensorName);
     if (!hasSensor()) {
         return DEFAULT_SENSOR_MINIMUM_WAIT_TIME_BETWEEN_READS;
     }
 
     bus = nodeTelemetrySensorsMap[sensorType].second;
+    address = (uint8_t)nodeTelemetrySensorsMap[sensorType].first;
     // sen5x.begin(*bus);
 
-    delay(50); // without this there is an error on the deviceReset function
+    if (!I2Cdetect(bus, address)) {
+        LOG_INFO("SEN5X ERROR no device found on adress");
+        return DEFAULT_SENSOR_MINIMUM_WAIT_TIME_BETWEEN_READS;
+    }
+
+    delay(25);
 
     if (!sendCommand(SEN5X_RESET)) {
         LOG_ERROR("SEN5X: Error reseting device");
diff --git a/src/modules/Telemetry/Sensor/SEN5XSensor.h b/src/modules/Telemetry/Sensor/SEN5XSensor.h
index 14168f587..9b80e3224 100644
--- a/src/modules/Telemetry/Sensor/SEN5XSensor.h
+++ b/src/modules/Telemetry/Sensor/SEN5XSensor.h
@@ -49,6 +49,7 @@ class SEN5XSensor : public TelemetrySensor
     bool sendCommand(uint16_t wichCommand, uint8_t* buffer, uint8_t byteNumber=0);
     uint8_t readBuffer(uint8_t* buffer, uint8_t byteNumber); // Return number of bytes received
     uint8_t CRC(uint8_t* buffer);
+    bool I2Cdetect(TwoWire *_Wire, uint8_t address);
 
   protected:
     virtual void setup() override;