From 98705ced2fc933dd20d204fbfff1a25f16b2ac5c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 17 Jan 2023 15:16:39 +0100 Subject: [PATCH] lightware_laser_i2c: fix unreliable startup detection In rare occasions asking for the protocol values after setting it returned [0, 0]. I did not see any documentation for having to wait, but adding a short wait period fixes it. --- .../lightware_laser_i2c.cpp | 25 +++++++++++++------ 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp index 38b3e8f163..baa2649d48 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp +++ b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp @@ -259,17 +259,26 @@ int LightwareLaser::enableI2CBinaryProtocol() return ret; } - // now read and check against the expected values - uint8_t value[2]; - ret = transfer(cmd, 1, value, sizeof(value)); + // Now read and check against the expected values + for (int i = 0; i < 2; ++i) { + uint8_t value[2]; + ret = transfer(cmd, 1, value, sizeof(value)); - if (ret != 0) { - return ret; + if (ret != 0) { + return ret; + } + + PX4_DEBUG("protocol values: 0x%" PRIx8 " 0x%" PRIx8, value[0], value[1]); + + if (value[0] == 0xcc && value[1] == 0x00) { + return 0; + } + + // Occasionally the previous transfer returns ret == value[0] == value[1] == 0. If so, wait a bit and retry + px4_usleep(1000); } - PX4_DEBUG("protocol values: 0x%" PRIx8 " 0x%" PRIx8, value[0], value[1]); - - return (value[0] == 0xcc && value[1] == 0x00) ? 0 : -1; + return -1; } int LightwareLaser::configure()