AP_Rangefinder: Reset the VL53L1X before setup

A number of the config steps make the assumption they are running from
the sensors default power on state. If you do a soft reset of the
vehicle without killing power to the sensor this means the second time
you configure the sensor you will start from a state which didn't match
the base assumptions and can lead to the sensor preforming signficantly
differently then it was intended to. This issues a soft reset, and waits
for it to reboot before proceeding with configuration. If it's a
watchdog reset we don't want to spend long enough to have ensured the
sensor reset, so we say the sensor setup completely failed and don't use
it, as not using it will be less erroneous then using a misconfigured
sensor.

libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp

@@ -86,14 +86,32 @@ bool AP_RangeFinder_VL53L1X::check_id(void)
     return true;
 }
 
+bool AP_RangeFinder_VL53L1X::reset(void) {
+    if (!write_register(SOFT_RESET, 0x00)) {
+        return false;
+    }
+    hal.scheduler->delay_microseconds(100);
+    if (!write_register(SOFT_RESET, 0x01)) {
+        return false;
+    }
+    hal.scheduler->delay(1000);
+    return true;
+}
+
 /*
   initialise sensor
  */
 bool AP_RangeFinder_VL53L1X::init()
 {
+    // we need to do resets and delays in order to configure the sensor, don't do this if we are trying to fast boot
+    if (hal.util->was_watchdog_armed()) {
+        return false;
+    }
+
     uint8_t pad_i2c_hv_extsup_config = 0;
     uint16_t mm_config_outer_offset_mm = 0;
-    if (!(// setup for 2.8V operation
+    if (!(reset() && // reset the chip, we make assumptions later on that we are on a clean power on of the sensor
+          // setup for 2.8V operation
           read_register(PAD_I2C_HV__EXTSUP_CONFIG, pad_i2c_hv_extsup_config) &&
           write_register(PAD_I2C_HV__EXTSUP_CONFIG,
                          pad_i2c_hv_extsup_config | 0x01) &&

libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h

@@ -1280,6 +1280,7 @@ private:
     bool write_register16(uint16_t reg, uint16_t value) WARN_IF_UNUSED;
     bool write_register32(uint16_t reg, uint32_t value) WARN_IF_UNUSED;
     bool dataReady(void);
+    bool reset(void) WARN_IF_UNUSED;
     bool setDistanceMode(DistanceMode distance_mode) WARN_IF_UNUSED;
     bool setMeasurementTimingBudget(uint32_t budget_us) WARN_IF_UNUSED;
     bool getMeasurementTimingBudget(uint32_t &budget) WARN_IF_UNUSED;