From 224ea744e3f9017df5b39d4e6d1f1328e0b42954 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Tue, 10 Dec 2019 18:30:02 -0700
Subject: [PATCH] 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.
---
 .../AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp | 20 ++++++++++++++++++-
 .../AP_RangeFinder/AP_RangeFinder_VL53L1X.h   |  1 +
 2 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp
index c3263fc1df..8c5ced2d2d 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp
+++ b/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) &&
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h
index 8953570a3a..d047ff1639 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h
+++ b/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;