diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp index 5c829e85abe54a39af95bbb68b4d72385652232a..c5a9066c18cffda06048ea205d69b59ddfc3edfc 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp @@ -329,18 +329,26 @@ int LidarLiteI2C::measure() } /* - reset the sensor to power on defaults + reset the sensor to power on defaults plus additional configurations */ int LidarLiteI2C::reset_sensor() { - int ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET); - + int ret; + ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET); if (ret != OK) { return ret; } // wait for sensor reset to complete usleep(1000); + ret = write_reg(LL40LS_SIG_COUNT_VAL_REG, LL40LS_SIG_COUNT_VAL_MAX); + + if (ret != OK) { + return ret; + } + + // wait for register write to complete + usleep(1000); return OK; } diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h index e99042897f6ca607cb41292645f562665e31e345..f2252ab19ad473a2796109002e6b2dc81b194b30 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h @@ -68,6 +68,9 @@ #define LL40LS_SW_VERSION 0x4f #define LL40LS_SIGNAL_STRENGTH_REG 0x5b +#define LL40LS_SIG_COUNT_VAL_REG 0x02 /* Maximum acquisition count register */ +#define LL40LS_SIG_COUNT_VAL_MAX 0xFF /* Maximum acquisition count max value */ + class LidarLiteI2C : public LidarLite, public device::I2C { public: