diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp index 1b9df36344..c3aa6d4842 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp @@ -84,6 +84,11 @@ bool AP_RangeFinder_VL53L1X::check_id(void) } bool AP_RangeFinder_VL53L1X::reset(void) { + if (dev->get_bus_id()!=0x29) { + // if sensor is on a different port than the default do not reset sensor otherwise we will lose the addess. + // we assume it is already confirgured. + return true; + } if (!write_register(SOFT_RESET, 0x00)) { return false; }