diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp index b9a929860f..1ff49b6c17 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp @@ -83,16 +83,9 @@ bool AP_RangeFinder_PulsedLightLRF::get_reading(uint16_t &reading_cm) return false; } - // read the high byte + // read the high and low byte distance registers if (hal.i2c->readRegisters(AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR, - AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG, 1, &buff[0]) != 0) { - i2c_sem->give(); - return false; - } - hal.scheduler->delay_microseconds(200); - // read the low byte - if (hal.i2c->readRegisters(AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR, - AP_RANGEFINDER_PULSEDLIGHTLRF_DISTLOW_REG, 1, &buff[1]) != 0) { + AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG, 2, &buff[0]) != 0) { i2c_sem->give(); return false; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h index 171e3d1bca..896882bc22 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h @@ -33,8 +33,7 @@ // registers #define AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG 0x00 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG 0x0f // high byte of distance measurement -#define AP_RANGEFINDER_PULSEDLIGHTLRF_DISTLOW_REG 0x10 // low byte of distance measurement +#define AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG 0x8f // high byte of distance measurement // command register values #define AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE 0x04 // Varies based on sensor revision, 0x04 is newest, 0x61 is older