diff --git a/src/modules/Telemetry/Sensor/RCWL9620Sensor.cpp b/src/modules/Telemetry/Sensor/RCWL9620Sensor.cpp index e352dda8d..4b2a788e1 100644 --- a/src/modules/Telemetry/Sensor/RCWL9620Sensor.cpp +++ b/src/modules/Telemetry/Sensor/RCWL9620Sensor.cpp @@ -41,21 +41,38 @@ 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_INFO("[RCWL9620] Envoi de la commande de mesure"); + + _wire->beginTransmission(_addr); + _wire->write(0x01); // À tester aussi sans cette ligne si besoin + uint8_t result = _wire->endTransmission(); + LOG_INFO("[RCWL9620] endTransmission result = %d", result); + delay(100); // délai pour laisser le capteur répondre + + LOG_INFO("[RCWL9620] Lecture des données I2C..."); + _wire->requestFrom(_addr, (uint8_t)3); + + if (_wire->available() < 3) { + LOG_INFO("[RCWL9620] Moins de 3 octets reçus !"); + 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_INFO("[RCWL9620] Bytes lus = %02X %02X %02X", b1, b2, b3); + LOG_INFO("[RCWL9620] data=%.2f, mesure=%.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 +80,5 @@ float RCWL9620Sensor::getDistance() } } -#endif \ No newline at end of file + +#endif