src/main/drivers/rangefinder/rangefinder_vl53l1x.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  *
  24  * =================================================================
  25  *
  26  * Most of the functionality of this library is based on the VL53L1X
  27  * API provided by ST (STSW-IMG009).
  28  *
  29  * The following applies to source code reproduced or derived from
  30  * the API:
  31  *
  32  * -----------------------------------------------------------------
  33  *
  34  * Copyright (c) 2017, STMicroelectronics - All Rights Reserved
  35  *
  36  *  This file : part of VL53L1 Core and : dual licensed,
  37  *  either 'STMicroelectronics
  38  *  Proprietary license'
  39  *  or 'BSD 3-clause "New" or "Revised" License' , at your option.
  40  *
  41  * *******************************************************************************
  42  *
  43  *  'STMicroelectronics Proprietary license'
  44  *
  45  * *******************************************************************************
  46  *
  47  *  License terms: STMicroelectronics Proprietary in accordance with licensing
  48  *  terms at www.st.com/sla0081
  49  *
  50  *  STMicroelectronics confidential
  51  *  Reproduction and Communication of this document : strictly prohibited unless
  52  *  specifically authorized in writing by STMicroelectronics.
  53  *
  54  *
  55  * *******************************************************************************
  56  *
  57  *  Alternatively, VL53L1 Core may be distributed under the terms of
  58  *  'BSD 3-clause "New" or "Revised" License', in which case the following
  59  *  provisions apply instead of the ones mentioned above :
  60  *
  61  * *******************************************************************************
  62  *
  63  *  License terms: BSD 3-clause "New" or "Revised" License.
  64  *
  65  *  Redistribution and use in source and binary forms, with or without
  66  *  modification, are permitted provided that the following conditions are met:
  67  *
  68  *  1. Redistributions of source code must retain the above copyright notice, this
  69  *  list of conditions and the following disclaimer.
  70  *
  71  *  2. Redistributions in binary form must reproduce the above copyright notice,
  72  *  this list of conditions and the following disclaimer in the documentation
  73  *  and/or other materials provided with the distribution.
  74  *
  75  *  3. Neither the name of the copyright holder nor the names of its contributors
  76  *  may be used to endorse or promote products derived from this software
  77  *  without specific prior written permission.
  78  *
  79  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  80  *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  81  *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  82  *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
  83  *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  84  *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  85  *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  86  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  87  *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  88  *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  89  */
  90
  91
  92 #include <stdbool.h>
  93 #include <stdint.h>
  94 #include <string.h>
  95
  96 #include "platform.h"
  97
  98 #if defined(USE_RANGEFINDER) && defined(USE_RANGEFINDER_VL53L1X)
  99
 100 #include "build/build_config.h"
 101
 102 #include "drivers/time.h"
 103 #include "drivers/bus_i2c.h"
 104
 105 #include "drivers/rangefinder/rangefinder.h"
 106 #include "drivers/rangefinder/rangefinder_vl53l1x.h"
 107
 108 #include "build/debug.h"
 109
 110 #define VL53L1X_MAX_RANGE_CM                                (300)
 111 #define VL53L1X_DETECTION_CONE_DECIDEGREES                  (270)
 112 #define VL53L1X_TIMING_BUDGET                               (33)
 113
 114 #define VL53L1X_IMPLEMENTATION_VER_MAJOR       3
 115 #define VL53L1X_IMPLEMENTATION_VER_MINOR       4
 116 #define VL53L1X_IMPLEMENTATION_VER_SUB         0
 117 #define VL53L1X_IMPLEMENTATION_VER_REVISION  0000
 118
 119 typedef int8_t VL53L1X_ERROR;
 120
 121 #define SOFT_RESET                                          0x0000
 122 #define VL53L1_I2C_SLAVE__DEVICE_ADDRESS                    0x0001
 123 #define VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND        0x0008
 124 #define ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS      0x0016
 125 #define ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS  0x0018
 126 #define ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS  0x001A
 127 #define ALGO__PART_TO_PART_RANGE_OFFSET_MM                  0x001E
 128 #define MM_CONFIG__INNER_OFFSET_MM                          0x0020
 129 #define MM_CONFIG__OUTER_OFFSET_MM                          0x0022
 130 #define GPIO_HV_MUX__CTRL                                   0x0030
 131 #define GPIO__TIO_HV_STATUS                                 0x0031
 132 #define SYSTEM__INTERRUPT_CONFIG_GPIO                       0x0046
 133 #define PHASECAL_CONFIG__TIMEOUT_MACROP                     0x004B
 134 #define RANGE_CONFIG__TIMEOUT_MACROP_A_HI                   0x005E
 135 #define RANGE_CONFIG__VCSEL_PERIOD_A                        0x0060
 136 #define RANGE_CONFIG__VCSEL_PERIOD_B                        0x0063
 137 #define RANGE_CONFIG__TIMEOUT_MACROP_B_HI                   0x0061
 138 #define RANGE_CONFIG__TIMEOUT_MACROP_B_LO                   0x0062
 139 #define RANGE_CONFIG__SIGMA_THRESH                          0x0064
 140 #define RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS         0x0066
 141 #define RANGE_CONFIG__VALID_PHASE_HIGH                      0x0069
 142 #define VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD              0x006C
 143 #define SYSTEM__THRESH_HIGH                                 0x0072
 144 #define SYSTEM__THRESH_LOW                                  0x0074
 145 #define SD_CONFIG__WOI_SD0                                  0x0078
 146 #define SD_CONFIG__INITIAL_PHASE_SD0                        0x007A
 147 #define ROI_CONFIG__USER_ROI_CENTRE_SPAD                    0x007F
 148 #define ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE       0x0080
 149 #define SYSTEM__SEQUENCE_CONFIG                             0x0081
 150 #define VL53L1_SYSTEM__GROUPED_PARAMETER_HOLD               0x0082
 151 #define SYSTEM__INTERRUPT_CLEAR                             0x0086
 152 #define SYSTEM__MODE_START                                  0x0087
 153 #define VL53L1_RESULT__RANGE_STATUS                         0x0089
 154 #define VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0       0x008C
 155 #define RESULT__AMBIENT_COUNT_RATE_MCPS_SD                  0x0090
 156 #define VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0               0x0096
 157 #define VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0  0x0098
 158 #define VL53L1_RESULT__OSC_CALIBRATE_VAL                    0x00DE
 159 #define VL53L1_FIRMWARE__SYSTEM_STATUS                      0x00E5
 160 #define VL53L1_IDENTIFICATION__MODEL_ID                     0x010F
 161 #define VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD             0x013E
 162
 163 #define ALGO__PART_TO_PART_RANGE_OFFSET_MM  0x001E
 164 #define MM_CONFIG__INNER_OFFSET_MM          0x0020
 165 #define MM_CONFIG__OUTER_OFFSET_MM          0x0022
 166
 167
 168
 169 #define VL53L1_ERROR_NONE                              ((VL53L1X_ERROR)  0)
 170 #define VL53L1_ERROR_CALIBRATION_WARNING               ((VL53L1X_ERROR) - 1)
 171     /*!< Warning invalid calibration data may be in used
 172         \a  VL53L1_InitData()
 173         \a VL53L1_GetOffsetCalibrationData
 174         \a VL53L1_SetOffsetCalibrationData */
 175 #define VL53L1_ERROR_MIN_CLIPPED                       ((VL53L1X_ERROR) - 2)
 176     /*!< Warning parameter passed was clipped to min before to be applied */
 177
 178 #define VL53L1_ERROR_UNDEFINED                         ((VL53L1X_ERROR) - 3)
 179     /*!< Unqualified error */
 180 #define VL53L1_ERROR_INVALID_PARAMS                    ((VL53L1X_ERROR) - 4)
 181     /*!< Parameter passed is invalid or out of range */
 182 #define VL53L1_ERROR_NOT_SUPPORTED                     ((VL53L1X_ERROR) - 5)
 183     /*!< Function is not supported in current mode or configuration */
 184 #define VL53L1_ERROR_RANGE_ERROR                       ((VL53L1X_ERROR) - 6)
 185     /*!< Device report a ranging error interrupt status */
 186 #define VL53L1_ERROR_TIME_OUT                          ((VL53L1X_ERROR) - 7)
 187     /*!< Aborted due to time out */
 188 #define VL53L1_ERROR_MODE_NOT_SUPPORTED                ((VL53L1X_ERROR) - 8)
 189     /*!< Asked mode is not supported by the device */
 190 #define VL53L1_ERROR_BUFFER_TOO_SMALL                  ((VL53L1X_ERROR) - 9)
 191     /*!< ... */
 192 #define VL53L1_ERROR_COMMS_BUFFER_TOO_SMALL            ((VL53L1X_ERROR) - 10)
 193     /*!< Supplied buffer is larger than I2C supports */
 194 #define VL53L1_ERROR_GPIO_NOT_EXISTING                 ((VL53L1X_ERROR) - 11)
 195     /*!< User tried to setup a non-existing GPIO pin */
 196 #define VL53L1_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED  ((VL53L1X_ERROR) - 12)
 197     /*!< unsupported GPIO functionality */
 198 #define VL53L1_ERROR_CONTROL_INTERFACE                 ((VL53L1X_ERROR) - 13)
 199     /*!< error reported from IO functions */
 200 #define VL53L1_ERROR_INVALID_COMMAND                   ((VL53L1X_ERROR) - 14)
 201     /*!< The command is not allowed in the current device state
 202      *  (power down) */
 203 #define VL53L1_ERROR_DIVISION_BY_ZERO                  ((VL53L1X_ERROR) - 15)
 204     /*!< In the function a division by zero occurs */
 205 #define VL53L1_ERROR_REF_SPAD_INIT                     ((VL53L1X_ERROR) - 16)
 206     /*!< Error during reference SPAD initialization */
 207 #define VL53L1_ERROR_GPH_SYNC_CHECK_FAIL               ((VL53L1X_ERROR) - 17)
 208     /*!<  GPH sync interrupt check fail - API out of sync with device*/
 209 #define VL53L1_ERROR_STREAM_COUNT_CHECK_FAIL           ((VL53L1X_ERROR) - 18)
 210     /*!<  Stream count check fail - API out of sync with device */
 211 #define VL53L1_ERROR_GPH_ID_CHECK_FAIL                 ((VL53L1X_ERROR) - 19)
 212     /*!<  GPH ID check fail - API out of sync with device */
 213 #define VL53L1_ERROR_ZONE_STREAM_COUNT_CHECK_FAIL      ((VL53L1X_ERROR) - 20)
 214     /*!<  Zone dynamic config stream count check failed - API out of sync */
 215 #define VL53L1_ERROR_ZONE_GPH_ID_CHECK_FAIL            ((VL53L1X_ERROR) - 21)
 216     /*!<  Zone dynamic config GPH ID check failed - API out of sync */
 217
 218 #define VL53L1_ERROR_XTALK_EXTRACTION_NO_SAMPLE_FAIL   ((VL53L1X_ERROR) - 22)
 219     /*!<  Thrown when run_xtalk_extraction fn has 0 succesful samples
 220      * when using the full array to sample the xtalk. In this case there is
 221      * not enough information to generate new Xtalk parm info. The function
 222      * will exit and leave the current xtalk parameters unaltered */
 223 #define VL53L1_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL ((VL53L1X_ERROR) - 23)
 224     /*!<  Thrown when run_xtalk_extraction fn has found that the
 225      * avg sigma estimate of the full array xtalk sample is > than the
 226      * maximal limit allowed. In this case the xtalk sample is too noisy for
 227      * measurement. The function will exit and leave the current xtalk parameters
 228      * unaltered. */
 229
 230
 231 #define VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL           ((VL53L1X_ERROR) - 24)
 232     /*!<  Thrown if there one of stages has no valid offset calibration
 233      *    samples. A fatal error calibration not valid */
 234 #define VL53L1_ERROR_OFFSET_CAL_NO_SPADS_ENABLED_FAIL    ((VL53L1X_ERROR) - 25)
 235     /*!<  Thrown if there one of stages has zero effective SPADS
 236      *    Traps the case when MM1 SPADs is zero.
 237      *    A fatal error calibration not valid */
 238 #define VL53L1_ERROR_ZONE_CAL_NO_SAMPLE_FAIL             ((VL53L1X_ERROR) - 26)
 239     /*!<  Thrown if then some of the zones have no valid samples
 240      *    A fatal error calibration not valid */
 241
 242 #define VL53L1_ERROR_TUNING_PARM_KEY_MISMATCH             ((VL53L1X_ERROR) - 27)
 243     /*!<  Thrown if the tuning file key table version does not match with
 244      * expected value. The driver expects the key table version to match
 245      * the compiled default version number in the define
 246      * #VL53L1_TUNINGPARM_KEY_TABLE_VERSION_DEFAULT
 247      * */
 248
 249 #define VL53L1_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS   ((VL53L1X_ERROR) - 28)
 250     /*!<  Thrown if there are less than 5 good SPADs are available. */
 251 #define VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH      ((VL53L1X_ERROR) - 29)
 252     /*!<  Thrown if the final reference rate is greater than
 253           the upper reference rate limit - default is 40 Mcps.
 254           Implies a minimum Q3 (x10) SPAD (5) selected */
 255 #define VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW       ((VL53L1X_ERROR) - 30)
 256     /*!<  Thrown if the final reference rate is less than
 257           the lower reference rate limit - default is 10 Mcps.
 258           Implies maximum Q1 (x1) SPADs selected */
 259
 260
 261 #define VL53L1_WARNING_OFFSET_CAL_MISSING_SAMPLES       ((VL53L1X_ERROR) - 31)
 262     /*!<  Thrown if there is less than the requested number of
 263      *    valid samples. */
 264 #define VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH        ((VL53L1X_ERROR) - 32)
 265     /*!<  Thrown if the offset calibration range sigma estimate is greater
 266      *    than 8.0 mm. This is the recommended min value to yield a stable
 267      *    offset measurement */
 268 #define VL53L1_WARNING_OFFSET_CAL_RATE_TOO_HIGH         ((VL53L1X_ERROR) - 33)
 269     /*!< Thrown when VL53L1_run_offset_calibration()  peak rate is greater
 270          than that 50.0Mcps. This is the recommended  max rate to avoid
 271          pile-up influencing the offset measurement */
 272 #define VL53L1_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW    ((VL53L1X_ERROR) - 34)
 273     /*!< Thrown when VL53L1_run_offset_calibration() when one of stages
 274          range has less that 5.0 effective SPADS. This is the recommended
 275          min value to yield a stable offset */
 276
 277
 278 #define VL53L1_WARNING_ZONE_CAL_MISSING_SAMPLES       ((VL53L1X_ERROR) - 35)
 279     /*!<  Thrown if one of more of the zones have less than
 280           the requested number of valid samples */
 281 #define VL53L1_WARNING_ZONE_CAL_SIGMA_TOO_HIGH        ((VL53L1X_ERROR) - 36)
 282     /*!<  Thrown if one or more zones have sigma estimate value greater
 283      *    than 8.0 mm. This is the recommended min value to yield a stable
 284      *    offset measurement */
 285 #define VL53L1_WARNING_ZONE_CAL_RATE_TOO_HIGH         ((VL53L1X_ERROR) - 37)
 286     /*!< Thrown if one of more zones have  peak rate higher than
 287           that 50.0Mcps. This is the recommended  max rate to avoid
 288          pile-up influencing the offset measurement */
 289
 290
 291 #define VL53L1_WARNING_XTALK_MISSING_SAMPLES             ((VL53L1X_ERROR) - 38)
 292     /*!< Thrown to notify that some of the xtalk samples did not yield
 293      * valid ranging pulse data while attempting to measure
 294      * the xtalk signal in vl53l1_run_xtalk_extract(). This can signify any of
 295      * the zones are missing samples, for further debug information the
 296      * xtalk_results struct should be referred to. This warning is for
 297      * notification only, the xtalk pulse and shape have still been generated
 298      */
 299 #define VL53L1_WARNING_XTALK_NO_SAMPLES_FOR_GRADIENT     ((VL53L1X_ERROR) - 39)
 300     /*!< Thrown to notify that some of teh xtalk samples used for gradient
 301      * generation did not yield valid ranging pulse data while attempting to
 302      * measure the xtalk signal in vl53l1_run_xtalk_extract(). This can signify
 303      * that any one of the zones 0-3 yielded no successful samples. The
 304      * xtalk_results struct should be referred to for further debug info.
 305      * This warning is for notification only, the xtalk pulse and shape
 306      * have still been generated.
 307      */
 308 #define VL53L1_WARNING_XTALK_SIGMA_LIMIT_FOR_GRADIENT    ((VL53L1X_ERROR) - 40)
 309 /*!< Thrown to notify that some of the xtalk samples used for gradient
 310      * generation did not pass the sigma limit check  while attempting to
 311      * measure the xtalk signal in vl53l1_run_xtalk_extract(). This can signify
 312      * that any one of the zones 0-3 yielded an avg sigma_mm value > the limit.
 313      * The xtalk_results struct should be referred to for further debug info.
 314      * This warning is for notification only, the xtalk pulse and shape
 315      * have still been generated.
 316      */
 317
 318 #define VL53L1_ERROR_NOT_IMPLEMENTED                   ((VL53L1X_ERROR) - 41)
 319     /*!< Tells requested functionality has not been implemented yet or
 320      * not compatible with the device */
 321 #define VL53L1_ERROR_PLATFORM_SPECIFIC_START           ((VL53L1X_ERROR) - 60)
 322     /*!< Tells the starting code for platform */
 323 /** @} VL53L1_define_Error_group */
 324
 325 /****************************************
 326  * PRIVATE define do not edit
 327  ****************************************/
 328
 329 /**
 330  *  @brief defines SW Version
 331  */
 332 typedef struct {
 333     uint8_t      major;    /*!< major number */
 334     uint8_t      minor;    /*!< minor number */
 335     uint8_t      build;    /*!< build number */
 336     uint32_t     revision; /*!< revision number */
 337 } VL53L1X_Version_t;
 338
 339 /**
 340  *  @brief defines packed reading results type
 341  */
 342 typedef struct {
 343     uint8_t Status;     /*!< ResultStatus */
 344     uint16_t Distance;  /*!< ResultDistance */
 345     uint16_t Ambient;   /*!< ResultAmbient */
 346     uint16_t SigPerSPAD;/*!< ResultSignalPerSPAD */
 347     uint16_t NumSPADs;  /*!< ResultNumSPADs */
 348 } VL53L1X_Result_t;
 349
 350 /**
 351  * @brief This function returns the SW driver version
 352  */
 353 // VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion);
 354
 355 /**
 356  * @brief This function sets the sensor I2C address used in case multiple devices application, default address 0x52
 357  */
 358 // // VL53L1X_ERROR VL53L1X_SetI2CAddress(busDevice_t * dev, uint8_t new_address);
 359
 360 // /**
 361 //  * @brief This function loads the 135 bytes default values to initialize the sensor.
 362 //  * @param dev Device address
 363 //  * @return 0:success, != 0:failed
 364 //  */
 365 VL53L1X_ERROR VL53L1X_SensorInit(busDevice_t * dev);
 366
 367 /**
 368  * @brief This function clears the interrupt, to be called after a ranging data reading
 369  * to arm the interrupt for the next data ready event.
 370  */
 371 static VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev);
 372
 373 /**
 374  * @brief This function programs the interrupt polarity\n
 375  * 1=active high (default), 0=active low
 376  */
 377 // VL53L1X_ERROR VL53L1X_SetInterruptPolarity(busDevice_t * dev, uint8_t IntPol);
 378
 379 /**
 380  * @brief This function returns the current interrupt polarity\n
 381  * 1=active high (default), 0=active low
 382  */
 383 static VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pIntPol);
 384
 385 /**
 386  * @brief This function starts the ranging distance operation\n
 387  * The ranging operation is continuous. The clear interrupt has to be done after each get data to allow the interrupt to raise when the next data is ready\n
 388  * 1=active high (default), 0=active low, use SetInterruptPolarity() to change the interrupt polarity if required.
 389  */
 390 static VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev);
 391
 392 /**
 393  * @brief This function stops the ranging.
 394  */
 395 static VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev);
 396
 397 /**
 398  * @brief This function checks if the new ranging data is available by polling the dedicated register.
 399  * @param : isDataReady==0 -> not ready; isDataReady==1 -> ready
 400  */
 401 static VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady);
 402
 403 /**
 404  * @brief This function programs the timing budget in ms.
 405  * Predefined values = 15, 20, 33, 50, 100(default), 200, 500.
 406  */
 407 static VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudgetInMs);
 408
 409 /**
 410  * @brief This function returns the current timing budget in ms.
 411  */
 412 static VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBudgetInMs);
 413
 414 /**
 415  * @brief This function programs the distance mode (1=short, 2=long(default)).
 416  * Short mode max distance is limited to 1.3 m but better ambient immunity.\n
 417  * Long mode can range up to 4 m in the dark with 200 ms timing budget.
 418  */
 419 static VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DistanceMode);
 420
 421 /**
 422  * @brief This function returns the current distance mode (1=short, 2=long).
 423  */
 424 static VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *pDistanceMode);
 425
 426 /**
 427  * @brief This function programs the Intermeasurement period in ms\n
 428  * Intermeasurement period must be >/= timing budget. This condition is not checked by the API,
 429  * the customer has the duty to check the condition. Default = 100 ms
 430  */
 431 static VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev,
 432                      uint32_t InterMeasurementInMs);
 433
 434 /**
 435  * @brief This function returns the Intermeasurement period in ms.
 436  */
 437 // VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(busDevice_t * dev, uint16_t * pIM);
 438
 439 /**
 440  * @brief This function returns the boot state of the device (1:booted, 0:not booted)
 441  */
 442 // VL53L1X_ERROR VL53L1X_BootState(busDevice_t * dev, uint8_t *state);
 443
 444 /**
 445  * @brief This function returns the sensor id, sensor Id must be 0xEEAC
 446  */
 447 // VL53L1X_ERROR VL53L1X_GetSensorId(busDevice_t * dev, uint16_t *id);
 448
 449 /**
 450  * @brief This function returns the distance measured by the sensor in mm
 451  */
 452 static VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance);
 453
 454 /**
 455  * @brief This function returns the returned signal per SPAD in kcps/SPAD.
 456  * With kcps stands for Kilo Count Per Second
 457  */
 458 // VL53L1X_ERROR VL53L1X_GetSignalPerSpad(busDevice_t * dev, uint16_t *signalPerSp);
 459
 460 /**
 461  * @brief This function returns the ambient per SPAD in kcps/SPAD
 462  */
 463 // VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(busDevice_t * dev, uint16_t *amb);
 464
 465 /**
 466  * @brief This function returns the returned signal in kcps.
 467  */
 468 // static VL53L1X_ERROR VL53L1X_GetSignalRate(busDevice_t * dev, uint16_t *signalRate);
 469
 470 /**
 471  * @brief This function returns the current number of enabled SPADs
 472  */
 473 // static VL53L1X_ERROR VL53L1X_GetSpadNb(busDevice_t * dev, uint16_t *spNb);
 474
 475 /**
 476  * @brief This function returns the ambient rate in kcps
 477  */
 478 // VL53L1X_ERROR VL53L1X_GetAmbientRate(busDevice_t * dev, uint16_t *ambRate);
 479
 480 /**
 481  * @brief This function returns the ranging status error \n
 482  * (0:no error, 1:sigma failed, 2:signal failed, ..., 7:wrap-around)
 483  */
 484 // VL53L1X_ERROR VL53L1X_GetRangeStatus(busDevice_t * dev, uint8_t *rangeStatus);
 485
 486 /**
 487  * @brief This function returns measurements and the range status in a single read access
 488  */
 489 // VL53L1X_ERROR VL53L1X_GetResult(busDevice_t * dev, VL53L1X_Result_t *pResult);
 490
 491 /**
 492  * @brief This function programs the offset correction in mm
 493  * @param OffsetValue:the offset correction value to program in mm
 494  */
 495 // VL53L1X_ERROR VL53L1X_SetOffset(busDevice_t * dev, int16_t OffsetValue);
 496
 497 /**
 498  * @brief This function returns the programmed offset correction value in mm
 499  */
 500 // VL53L1X_ERROR VL53L1X_GetOffset(busDevice_t * dev, int16_t *Offset);
 501
 502 /**
 503  * @brief This function programs the xtalk correction value in cps (Count Per Second).\n
 504  * This is the number of photons reflected back from the cover glass in cps.
 505  */
 506 // VL53L1X_ERROR VL53L1X_SetXtalk(busDevice_t * dev, uint16_t XtalkValue);
 507
 508 /**
 509  * @brief This function returns the current programmed xtalk correction value in cps
 510  */
 511 // VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *Xtalk);
 512
 513 /**
 514  * @brief This function programs the threshold detection mode\n
 515  * Example:\n
 516  * VL53L1X_SetDistanceThreshold(dev,100,300,0,1): Below 100 \n
 517  * VL53L1X_SetDistanceThreshold(dev,100,300,1,1): Above 300 \n
 518  * VL53L1X_SetDistanceThreshold(dev,100,300,2,1): Out of window \n
 519  * VL53L1X_SetDistanceThreshold(dev,100,300,3,1): In window \n
 520  * @param   dev : device address
 521  * @param   ThreshLow(in mm) : the threshold under which one the device raises an interrupt if Window = 0
 522  * @param   ThreshHigh(in mm) :  the threshold above which one the device raises an interrupt if Window = 1
 523  * @param   Window detection mode : 0=below, 1=above, 2=out, 3=in
 524  * @param   IntOnNoTarget = 0 (No longer used - just use 0)
 525  */
 526 // VL53L1X_ERROR VL53L1X_SetDistanceThreshold(busDevice_t * dev, uint16_t ThreshLow,
 527 //                   uint16_t ThreshHigh, uint8_t Window,
 528 //                   uint8_t IntOnNoTarget);
 529
 530 /**
 531  * @brief This function returns the window detection mode (0=below; 1=above; 2=out; 3=in)
 532  */
 533 // VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(busDevice_t * dev, uint16_t *window);
 534
 535 /**
 536  * @brief This function returns the low threshold in mm
 537  */
 538 // VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(busDevice_t * dev, uint16_t *low);
 539
 540 /**
 541  * @brief This function returns the high threshold in mm
 542  */
 543 // VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high);
 544
 545 /**
 546  * @brief This function programs the ROI (Region of Interest)\n
 547  * The ROI position is centered, only the ROI size can be reprogrammed.\n
 548  * The smallest acceptable ROI size = 4\n
 549  * @param X:ROI Width; Y=ROI Height
 550  */
 551 // VL53L1X_ERROR VL53L1X_SetROI(busDevice_t * dev, uint16_t X, uint16_t Y);
 552
 553 /**
 554  *@brief This function returns width X and height Y
 555  */
 556 // VL53L1X_ERROR VL53L1X_GetROI_XY(busDevice_t * dev, uint16_t *ROI_X, uint16_t *ROI_Y);
 557
 558 /**
 559  *@brief This function programs the new user ROI center, please to be aware that there is no check in this function.
 560  *if the ROI center vs ROI size is out of border the ranging function return error #13
 561  */
 562 // VL53L1X_ERROR VL53L1X_SetROICenter(busDevice_t * dev, uint8_t ROICenter);
 563
 564 /**
 565  *@brief This function returns the current user ROI center
 566  */
 567 // VL53L1X_ERROR VL53L1X_GetROICenter(busDevice_t * dev, uint8_t *ROICenter);
 568
 569 /**
 570  * @brief This function programs a new signal threshold in kcps (default=1024 kcps\n
 571  */
 572 // VL53L1X_ERROR VL53L1X_SetSignalThreshold(busDevice_t * dev, uint16_t signal);
 573
 574 /**
 575  * @brief This function returns the current signal threshold in kcps
 576  */
 577 // VL53L1X_ERROR VL53L1X_GetSignalThreshold(busDevice_t * dev, uint16_t *signal);
 578
 579 /**
 580  * @brief This function programs a new sigma threshold in mm (default=15 mm)
 581  */
 582 // VL53L1X_ERROR VL53L1X_SetSigmaThreshold(busDevice_t * dev, uint16_t sigma);
 583
 584 /**
 585  * @brief This function returns the current sigma threshold in mm
 586  */
 587 // VL53L1X_ERROR VL53L1X_GetSigmaThreshold(busDevice_t * dev, uint16_t *signal);
 588
 589 /**
 590  * @brief This function performs the temperature calibration.
 591  * It is recommended to call this function any time the temperature might have changed by more than 8 deg C
 592  * without sensor ranging activity for an extended period.
 593  */
 594 // VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev);
 595
 596 /**
 597  * @brief This function performs the offset calibration.\n
 598  * The function returns the offset value found and programs the offset compensation into the device.
 599  * @param TargetDistInMm target distance in mm, ST recommended 100 mm
 600  * Target reflectance = grey17%
 601  * @return 0:success, !=0: failed
 602  * @return offset pointer contains the offset found in mm
 603  */
 604 // int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16_t *offset);
 605
 606 /**
 607  * @brief This function performs the xtalk calibration.\n
 608  * The function returns the xtalk value found and programs the xtalk compensation to the device
 609  * @param TargetDistInMm target distance in mm\n
 610  * The target distance : the distance where the sensor start to "under range"\n
 611  * due to the influence of the photons reflected back from the cover glass becoming strong\n
 612  * It's also called inflection point\n
 613  * Target reflectance = grey 17%
 614  * @return 0: success, !=0: failed
 615  * @return xtalk pointer contains the xtalk value found in cps (number of photons in count per second)
 616  */
 617 // int8_t VL53L1X_CalibrateXtalk(busDevice_t * dev, uint16_t TargetDistInMm, uint16_t *xtalk);
 618
 619
 620
 621 const uint8_t VL51L1X_DEFAULT_CONFIGURATION[] = {
 622 0x00, /* 0x2d : set bit 2 and 5 to 1 for fast plus mode (1MHz I2C), else don't touch */
 623 0x00, /* 0x2e : bit 0 if I2C pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */
 624 0x00, /* 0x2f : bit 0 if GPIO pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */
 625 0x01, /* 0x30 : set bit 4 to 0 for active high interrupt and 1 for active low (bits 3:0 must be 0x1), use SetInterruptPolarity() */
 626 0x02, /* 0x31 : bit 1 = interrupt depending on the polarity, use CheckForDataReady() */
 627 0x00, /* 0x32 : not user-modifiable */
 628 0x02, /* 0x33 : not user-modifiable */
 629 0x08, /* 0x34 : not user-modifiable */
 630 0x00, /* 0x35 : not user-modifiable */
 631 0x08, /* 0x36 : not user-modifiable */
 632 0x10, /* 0x37 : not user-modifiable */
 633 0x01, /* 0x38 : not user-modifiable */
 634 0x01, /* 0x39 : not user-modifiable */
 635 0x00, /* 0x3a : not user-modifiable */
 636 0x00, /* 0x3b : not user-modifiable */
 637 0x00, /* 0x3c : not user-modifiable */
 638 0x00, /* 0x3d : not user-modifiable */
 639 0xff, /* 0x3e : not user-modifiable */
 640 0x00, /* 0x3f : not user-modifiable */
 641 0x0F, /* 0x40 : not user-modifiable */
 642 0x00, /* 0x41 : not user-modifiable */
 643 0x00, /* 0x42 : not user-modifiable */
 644 0x00, /* 0x43 : not user-modifiable */
 645 0x00, /* 0x44 : not user-modifiable */
 646 0x00, /* 0x45 : not user-modifiable */
 647 0x20, /* 0x46 : interrupt configuration 0->level low detection, 1-> level high, 2-> Out of window, 3->In window, 0x20-> New sample ready , TBC */
 648 0x0b, /* 0x47 : not user-modifiable */
 649 0x00, /* 0x48 : not user-modifiable */
 650 0x00, /* 0x49 : not user-modifiable */
 651 0x02, /* 0x4a : not user-modifiable */
 652 0x0a, /* 0x4b : not user-modifiable */
 653 0x21, /* 0x4c : not user-modifiable */
 654 0x00, /* 0x4d : not user-modifiable */
 655 0x00, /* 0x4e : not user-modifiable */
 656 0x05, /* 0x4f : not user-modifiable */
 657 0x00, /* 0x50 : not user-modifiable */
 658 0x00, /* 0x51 : not user-modifiable */
 659 0x00, /* 0x52 : not user-modifiable */
 660 0x00, /* 0x53 : not user-modifiable */
 661 0xc8, /* 0x54 : not user-modifiable */
 662 0x00, /* 0x55 : not user-modifiable */
 663 0x00, /* 0x56 : not user-modifiable */
 664 0x38, /* 0x57 : not user-modifiable */
 665 0xff, /* 0x58 : not user-modifiable */
 666 0x01, /* 0x59 : not user-modifiable */
 667 0x00, /* 0x5a : not user-modifiable */
 668 0x08, /* 0x5b : not user-modifiable */
 669 0x00, /* 0x5c : not user-modifiable */
 670 0x00, /* 0x5d : not user-modifiable */
 671 0x01, /* 0x5e : not user-modifiable */
 672 0xcc, /* 0x5f : not user-modifiable */
 673 0x0f, /* 0x60 : not user-modifiable */
 674 0x01, /* 0x61 : not user-modifiable */
 675 0xf1, /* 0x62 : not user-modifiable */
 676 0x0d, /* 0x63 : not user-modifiable */
 677 0x01, /* 0x64 : Sigma threshold MSB (mm in 14.2 format for MSB+LSB), use SetSigmaThreshold(), default value 90 mm  */
 678 0x68, /* 0x65 : Sigma threshold LSB */
 679 0x00, /* 0x66 : Min count Rate MSB (MCPS in 9.7 format for MSB+LSB), use SetSignalThreshold() */
 680 0x80, /* 0x67 : Min count Rate LSB */
 681 0x08, /* 0x68 : not user-modifiable */
 682 0xb8, /* 0x69 : not user-modifiable */
 683 0x00, /* 0x6a : not user-modifiable */
 684 0x00, /* 0x6b : not user-modifiable */
 685 0x00, /* 0x6c : Intermeasurement period MSB, 32 bits register, use SetIntermeasurementInMs() */
 686 0x00, /* 0x6d : Intermeasurement period */
 687 0x0f, /* 0x6e : Intermeasurement period */
 688 0x89, /* 0x6f : Intermeasurement period LSB */
 689 0x00, /* 0x70 : not user-modifiable */
 690 0x00, /* 0x71 : not user-modifiable */
 691 0x00, /* 0x72 : distance threshold high MSB (in mm, MSB+LSB), use SetD:tanceThreshold() */
 692 0x00, /* 0x73 : distance threshold high LSB */
 693 0x00, /* 0x74 : distance threshold low MSB ( in mm, MSB+LSB), use SetD:tanceThreshold() */
 694 0x00, /* 0x75 : distance threshold low LSB */
 695 0x00, /* 0x76 : not user-modifiable */
 696 0x01, /* 0x77 : not user-modifiable */
 697 0x0f, /* 0x78 : not user-modifiable */
 698 0x0d, /* 0x79 : not user-modifiable */
 699 0x0e, /* 0x7a : not user-modifiable */
 700 0x0e, /* 0x7b : not user-modifiable */
 701 0x00, /* 0x7c : not user-modifiable */
 702 0x00, /* 0x7d : not user-modifiable */
 703 0x02, /* 0x7e : not user-modifiable */
 704 0xc7, /* 0x7f : ROI center, use SetROI() */
 705 0xff, /* 0x80 : XY ROI (X=Width, Y=Height), use SetROI() */
 706 0x9B, /* 0x81 : not user-modifiable */
 707 0x00, /* 0x82 : not user-modifiable */
 708 0x00, /* 0x83 : not user-modifiable */
 709 0x00, /* 0x84 : not user-modifiable */
 710 0x01, /* 0x85 : not user-modifiable */
 711 0x00, /* 0x86 : clear interrupt, use ClearInterrupt() */
 712 0x00  /* 0x87 : start ranging, use StartRanging() or StopRanging(), If you want an automatic start after VL53L1X_init() call, put 0x40 in location 0x87 */
 713 };
 714
 715 // static const uint8_t status_rtn[24] = { 255, 255, 255, 5, 2, 4, 1, 7, 3, 0,
 716 //     255, 255, 9, 13, 255, 255, 255, 255, 10, 6,
 717 //     255, 255, 11, 12
 718 // };
 719
 720 static uint8_t _I2CBuffer[256];
 721
 722 static int32_t lastMeasurementCm = RANGEFINDER_OUT_OF_RANGE;
 723 static bool lastMeasurementIsNew = false;
 724 static bool isInitialized = false;
 725 static bool isResponding = true;
 726
 727 #define _I2CWrite(dev, data, size) \
 728     (busWriteBuf(dev, 0xFF, data, size) ? 0 : -1)
 729
 730 #define _I2CRead(dev, data, size) \
 731     (busReadBuf(dev, 0xFF, data, size) ? 0 : -1)
 732
 733 VL53L1X_ERROR VL53L1_WriteMulti(busDevice_t * Dev, uint16_t index, uint8_t *pdata, uint32_t count) {
 734     int status_int;
 735     VL53L1X_ERROR Status = VL53L1_ERROR_NONE;
 736     if (count > sizeof(_I2CBuffer) - 1) {
 737         return VL53L1_ERROR_INVALID_PARAMS;
 738     }
 739     _I2CBuffer[0] = index>>8;
 740     _I2CBuffer[1] = index&0xFF;
 741     memcpy(&_I2CBuffer[2], pdata, count);
 742     status_int = _I2CWrite(Dev, _I2CBuffer, count + 2);
 743     if (status_int != 0) {
 744         Status = VL53L1_ERROR_CONTROL_INTERFACE;
 745     }
 746     return Status;
 747 }
 748
 749 // the ranging_sensor_comms.dll will take care of the page selection
 750 VL53L1X_ERROR VL53L1_ReadMulti(busDevice_t * Dev, uint16_t index, uint8_t *pdata, uint32_t count) {
 751     VL53L1X_ERROR Status = VL53L1_ERROR_NONE;
 752     int32_t status_int;
 753
 754     _I2CBuffer[0] = index>>8;
 755     _I2CBuffer[1] = index&0xFF;
 756     status_int = _I2CWrite(Dev, _I2CBuffer, 2);
 757     if (status_int != 0) {
 758         Status = VL53L1_ERROR_CONTROL_INTERFACE;
 759         goto done;
 760     }
 761     status_int = _I2CRead(Dev, pdata, count);
 762     if (status_int != 0) {
 763         Status = VL53L1_ERROR_CONTROL_INTERFACE;
 764     }
 765 done:
 766     return Status;
 767 }
 768
 769 VL53L1X_ERROR VL53L1_WrByte(busDevice_t * Dev, uint16_t index, uint8_t data) {
 770     VL53L1X_ERROR Status = VL53L1_ERROR_NONE;
 771     int32_t status_int;
 772
 773     _I2CBuffer[0] = index>>8;
 774     _I2CBuffer[1] = index&0xFF;
 775     _I2CBuffer[2] = data;
 776
 777     status_int = _I2CWrite(Dev, _I2CBuffer, 3);
 778     if (status_int != 0) {
 779         Status = VL53L1_ERROR_CONTROL_INTERFACE;
 780     }
 781     return Status;
 782 }
 783
 784 VL53L1X_ERROR VL53L1_WrWord(busDevice_t * Dev, uint16_t index, uint16_t data) {
 785     VL53L1X_ERROR Status = VL53L1_ERROR_NONE;
 786     int32_t status_int;
 787
 788     _I2CBuffer[0] = index>>8;
 789     _I2CBuffer[1] = index&0xFF;
 790     _I2CBuffer[2] = data >> 8;
 791     _I2CBuffer[3] = data & 0x00FF;
 792
 793     status_int = _I2CWrite(Dev, _I2CBuffer, 4);
 794     if (status_int != 0) {
 795         Status = VL53L1_ERROR_CONTROL_INTERFACE;
 796     }
 797     return Status;
 798 }
 799
 800 VL53L1X_ERROR VL53L1_WrDWord(busDevice_t * Dev, uint16_t index, uint32_t data) {
 801     VL53L1X_ERROR Status = VL53L1_ERROR_NONE;
 802     int32_t status_int;
 803     _I2CBuffer[0] = index>>8;
 804     _I2CBuffer[1] = index&0xFF;
 805     _I2CBuffer[2] = (data >> 24) & 0xFF;
 806     _I2CBuffer[3] = (data >> 16) & 0xFF;
 807     _I2CBuffer[4] = (data >> 8)  & 0xFF;
 808     _I2CBuffer[5] = (data >> 0 ) & 0xFF;
 809     status_int = _I2CWrite(Dev, _I2CBuffer, 6);
 810     if (status_int != 0) {
 811         Status = VL53L1_ERROR_CONTROL_INTERFACE;
 812     }
 813     return Status;
 814 }
 815
 816 VL53L1X_ERROR VL53L1_RdByte(busDevice_t * Dev, uint16_t index, uint8_t *data) {
 817     VL53L1X_ERROR Status = VL53L1_ERROR_NONE;
 818     int32_t status_int;
 819
 820     _I2CBuffer[0] = index>>8;
 821     _I2CBuffer[1] = index&0xFF;
 822     status_int = _I2CWrite(Dev, _I2CBuffer, 2);
 823     if( status_int ){
 824         Status = VL53L1_ERROR_CONTROL_INTERFACE;
 825         goto done;
 826     }
 827     status_int = _I2CRead(Dev, data, 1);
 828     if (status_int != 0) {
 829         Status = VL53L1_ERROR_CONTROL_INTERFACE;
 830     }
 831 done:
 832     return Status;
 833 }
 834
 835 VL53L1X_ERROR VL53L1_UpdateByte(busDevice_t * Dev, uint16_t index, uint8_t AndData, uint8_t OrData) {
 836     VL53L1X_ERROR Status = VL53L1_ERROR_NONE;
 837     uint8_t data;
 838
 839     Status = VL53L1_RdByte(Dev, index, &data);
 840     if (Status) {
 841         goto done;
 842     }
 843     data = (data & AndData) | OrData;
 844     Status = VL53L1_WrByte(Dev, index, data);
 845 done:
 846     return Status;
 847 }
 848
 849 VL53L1X_ERROR VL53L1_RdWord(busDevice_t * Dev, uint16_t index, uint16_t *data) {
 850     VL53L1X_ERROR Status = VL53L1_ERROR_NONE;
 851     int32_t status_int;
 852
 853     _I2CBuffer[0] = index>>8;
 854     _I2CBuffer[1] = index&0xFF;
 855     status_int = _I2CWrite(Dev, _I2CBuffer, 2);
 856
 857     if( status_int ){
 858         Status = VL53L1_ERROR_CONTROL_INTERFACE;
 859         goto done;
 860     }
 861     status_int = _I2CRead(Dev, _I2CBuffer, 2);
 862     if (status_int != 0) {
 863         Status = VL53L1_ERROR_CONTROL_INTERFACE;
 864         goto done;
 865     }
 866
 867     *data = ((uint16_t)_I2CBuffer[0]<<8) + (uint16_t)_I2CBuffer[1];
 868 done:
 869     return Status;
 870 }
 871
 872 VL53L1X_ERROR VL53L1_RdDWord(busDevice_t * Dev, uint16_t index, uint32_t *data) {
 873     VL53L1X_ERROR Status = VL53L1_ERROR_NONE;
 874     int32_t status_int;
 875
 876     _I2CBuffer[0] = index>>8;
 877     _I2CBuffer[1] = index&0xFF;
 878     status_int = _I2CWrite(Dev, _I2CBuffer, 2);
 879     if (status_int != 0) {
 880         Status = VL53L1_ERROR_CONTROL_INTERFACE;
 881         goto done;
 882     }
 883     status_int = _I2CRead(Dev, _I2CBuffer, 4);
 884     if (status_int != 0) {
 885         Status = VL53L1_ERROR_CONTROL_INTERFACE;
 886         goto done;
 887     }
 888
 889     *data = ((uint32_t)_I2CBuffer[0]<<24) + ((uint32_t)_I2CBuffer[1]<<16) + ((uint32_t)_I2CBuffer[2]<<8) + (uint32_t)_I2CBuffer[3];
 890
 891 done:
 892     return Status;
 893 }
 894
 895 // VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion)
 896 // {
 897 //     VL53L1X_ERROR Status = 0;
 898
 899 //     pVersion->major = VL53L1X_IMPLEMENTATION_VER_MAJOR;
 900 //     pVersion->minor = VL53L1X_IMPLEMENTATION_VER_MINOR;
 901 //     pVersion->build = VL53L1X_IMPLEMENTATION_VER_SUB;
 902 //     pVersion->revision = VL53L1X_IMPLEMENTATION_VER_REVISION;
 903 //     return Status;
 904 // }
 905
 906 // VL53L1X_ERROR VL53L1X_SetI2CAddress(busDevice_t * dev, uint8_t new_address)
 907 // {
 908 //     VL53L1X_ERROR status = 0;
 909
 910 //     status = VL53L1_WrByte(dev, VL53L1_I2C_SLAVE__DEVICE_ADDRESS, new_address >> 1);
 911 //     return status;
 912 // }
 913
 914 VL53L1X_ERROR VL53L1X_SensorInit(busDevice_t * dev)
 915 {
 916     VL53L1X_ERROR status = 0;
 917     uint8_t Addr = 0x00, tmp;
 918
 919     for (Addr = 0x2D; Addr <= 0x87; Addr++){
 920         status = VL53L1_WrByte(dev, Addr, VL51L1X_DEFAULT_CONFIGURATION[Addr - 0x2D]);
 921     }
 922     status = VL53L1X_StartRanging(dev);
 923     tmp  = 0;
 924     while(tmp==0){
 925             status = VL53L1X_CheckForDataReady(dev, &tmp);
 926     }
 927     status = VL53L1X_ClearInterrupt(dev);
 928     status = VL53L1X_StopRanging(dev);
 929     status = VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */
 930     status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */
 931     return status;
 932 }
 933
 934 static VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev)
 935 {
 936     VL53L1X_ERROR status = 0;
 937
 938     status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CLEAR, 0x01);
 939     return status;
 940 }
 941
 942 // VL53L1X_ERROR VL53L1X_SetInterruptPolarity(busDevice_t * dev, uint8_t NewPolarity)
 943 // {
 944 //     uint8_t Temp;
 945 //     VL53L1X_ERROR status = 0;
 946
 947 //     status = VL53L1_RdByte(dev, GPIO_HV_MUX__CTRL, &Temp);
 948 //     Temp = Temp & 0xEF;
 949 //     status = VL53L1_WrByte(dev, GPIO_HV_MUX__CTRL, Temp | (!(NewPolarity & 1)) << 4);
 950 //     return status;
 951 // }
 952
 953 static VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pInterruptPolarity)
 954 {
 955     uint8_t Temp;
 956     VL53L1X_ERROR status = 0;
 957
 958     status = VL53L1_RdByte(dev, GPIO_HV_MUX__CTRL, &Temp);
 959     Temp = Temp & 0x10;
 960     *pInterruptPolarity = !(Temp>>4);
 961     return status;
 962 }
 963
 964 static VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev)
 965 {
 966     VL53L1X_ERROR status = 0;
 967
 968     status = VL53L1_WrByte(dev, SYSTEM__MODE_START, 0x40);  /* Enable VL53L1X */
 969     return status;
 970 }
 971
 972 static VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev)
 973 {
 974     VL53L1X_ERROR status = 0;
 975
 976     status = VL53L1_WrByte(dev, SYSTEM__MODE_START, 0x00);  /* Disable VL53L1X */
 977     return status;
 978 }
 979
 980 static VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady)
 981 {
 982     uint8_t Temp;
 983     uint8_t IntPol;
 984     VL53L1X_ERROR status = 0;
 985
 986     status = VL53L1X_GetInterruptPolarity(dev, &IntPol);
 987     status = VL53L1_RdByte(dev, GPIO__TIO_HV_STATUS, &Temp);
 988     /* Read in the register to check if a new value is available */
 989     if (status == 0){
 990         if ((Temp & 1) == IntPol)
 991             *isDataReady = 1;
 992         else
 993             *isDataReady = 0;
 994     }
 995     return status;
 996 }
 997
 998 static VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudgetInMs)
 999 {
1000     uint16_t DM;
1001     VL53L1X_ERROR  status=0;
1002
1003     status = VL53L1X_GetDistanceMode(dev, &DM);
1004     if (DM == 0)
1005         return 1;
1006     else if (DM == 1) { /* Short DistanceMode */
1007         switch (TimingBudgetInMs) {
1008         case 15: /* only available in short distance mode */
1009             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
1010                     0x01D);
1011             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
1012                     0x0027);
1013             break;
1014         case 20:
1015             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
1016                     0x0051);
1017             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
1018                     0x006E);
1019             break;
1020         case 33:
1021             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
1022                     0x00D6);
1023             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
1024                     0x006E);
1025             break;
1026         case 50:
1027             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
1028                     0x1AE);
1029             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
1030                     0x01E8);
1031             break;
1032         case 100:
1033             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
1034                     0x02E1);
1035             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
1036                     0x0388);
1037             break;
1038         case 200:
1039             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
1040                     0x03E1);
1041             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
1042                     0x0496);
1043             break;
1044         case 500:
1045             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
1046                     0x0591);
1047             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
1048                     0x05C1);
1049             break;
1050         default:
1051             status = 1;
1052             break;
1053         }
1054     } else {
1055         switch (TimingBudgetInMs) {
1056         case 20:
1057             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
1058                     0x001E);
1059             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
1060                     0x0022);
1061             break;
1062         case 33:
1063             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
1064                     0x0060);
1065             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
1066                     0x006E);
1067             break;
1068         case 50:
1069             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
1070                     0x00AD);
1071             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
1072                     0x00C6);
1073             break;
1074         case 100:
1075             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
1076                     0x01CC);
1077             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
1078                     0x01EA);
1079             break;
1080         case 200:
1081             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
1082                     0x02D9);
1083             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
1084                     0x02F8);
1085             break;
1086         case 500:
1087             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
1088                     0x048F);
1089             VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
1090                     0x04A4);
1091             break;
1092         default:
1093             status = 1;
1094             break;
1095         }
1096     }
1097     return status;
1098 }
1099
1100 static VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBudget)
1101 {
1102     uint16_t Temp;
1103     VL53L1X_ERROR status = 0;
1104
1105     status = VL53L1_RdWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, &Temp);
1106     switch (Temp) {
1107         case 0x001D :
1108             *pTimingBudget = 15;
1109             break;
1110         case 0x0051 :
1111         case 0x001E :
1112             *pTimingBudget = 20;
1113             break;
1114         case 0x00D6 :
1115         case 0x0060 :
1116             *pTimingBudget = 33;
1117             break;
1118         case 0x1AE :
1119         case 0x00AD :
1120             *pTimingBudget = 50;
1121             break;
1122         case 0x02E1 :
1123         case 0x01CC :
1124             *pTimingBudget = 100;
1125             break;
1126         case 0x03E1 :
1127         case 0x02D9 :
1128             *pTimingBudget = 200;
1129             break;
1130         case 0x0591 :
1131         case 0x048F :
1132             *pTimingBudget = 500;
1133             break;
1134         default:
1135             status = 1;
1136             *pTimingBudget = 0;
1137     }
1138     return status;
1139 }
1140
1141 static VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DM)
1142 {
1143     uint16_t TB;
1144     VL53L1X_ERROR status = 0;
1145
1146     status = VL53L1X_GetTimingBudgetInMs(dev, &TB);
1147     if (status != 0)
1148         return 1;
1149     switch (DM) {
1150     case 1:
1151         status = VL53L1_WrByte(dev, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x14);
1152         status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_A, 0x07);
1153         status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_B, 0x05);
1154         status = VL53L1_WrByte(dev, RANGE_CONFIG__VALID_PHASE_HIGH, 0x38);
1155         status = VL53L1_WrWord(dev, SD_CONFIG__WOI_SD0, 0x0705);
1156         status = VL53L1_WrWord(dev, SD_CONFIG__INITIAL_PHASE_SD0, 0x0606);
1157         break;
1158     case 2:
1159         status = VL53L1_WrByte(dev, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x0A);
1160         status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F);
1161         status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D);
1162         status = VL53L1_WrByte(dev, RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8);
1163         status = VL53L1_WrWord(dev, SD_CONFIG__WOI_SD0, 0x0F0D);
1164         status = VL53L1_WrWord(dev, SD_CONFIG__INITIAL_PHASE_SD0, 0x0E0E);
1165         break;
1166     default:
1167         status = 1;
1168         break;
1169     }
1170
1171     if (status == 0)
1172         status = VL53L1X_SetTimingBudgetInMs(dev, TB);
1173     return status;
1174 }
1175
1176 static VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *DM)
1177 {
1178     uint8_t TempDM, status=0;
1179
1180     status = VL53L1_RdByte(dev,PHASECAL_CONFIG__TIMEOUT_MACROP, &TempDM);
1181     if (TempDM == 0x14)
1182         *DM=1;
1183     if(TempDM == 0x0A)
1184         *DM=2;
1185     return status;
1186 }
1187
1188 static VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, uint32_t InterMeasMs)
1189 {
1190     uint16_t ClockPLL;
1191     VL53L1X_ERROR status = 0;
1192
1193     status = VL53L1_RdWord(dev, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL);
1194     ClockPLL = ClockPLL&0x3FF;
1195     VL53L1_WrDWord(dev, VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD,
1196             (uint32_t)(ClockPLL * InterMeasMs * 1.075));
1197     return status;
1198
1199 }
1200
1201 // VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(busDevice_t * dev, uint16_t *pIM)
1202 // {
1203 //     uint16_t ClockPLL;
1204 //     VL53L1X_ERROR status = 0;
1205 //     uint32_t tmp;
1206
1207 //     status = VL53L1_RdDWord(dev,VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, &tmp);
1208 //     *pIM = (uint16_t)tmp;
1209 //     status = VL53L1_RdWord(dev, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL);
1210 //     ClockPLL = ClockPLL&0x3FF;
1211 //     *pIM= (uint16_t)(*pIM/(ClockPLL*1.065));
1212 //     return status;
1213 // }
1214
1215 // VL53L1X_ERROR VL53L1X_BootState(busDevice_t * dev, uint8_t *state)
1216 // {
1217 //     VL53L1X_ERROR status = 0;
1218 //     uint8_t tmp = 0;
1219
1220 //     status = VL53L1_RdByte(dev,VL53L1_FIRMWARE__SYSTEM_STATUS, &tmp);
1221 //     *state = tmp;
1222 //     return status;
1223 // }
1224
1225 // VL53L1X_ERROR VL53L1X_GetSensorId(busDevice_t * dev, uint16_t *sensorId)
1226 // {
1227 //     VL53L1X_ERROR status = 0;
1228 //     uint16_t tmp = 0;
1229
1230 //     status = VL53L1_RdWord(dev, VL53L1_IDENTIFICATION__MODEL_ID, &tmp);
1231 //     *sensorId = tmp;
1232 //     return status;
1233 // }
1234
1235 static VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance)
1236 {
1237     VL53L1X_ERROR status = 0;
1238     uint16_t tmp;
1239
1240     status = (VL53L1_RdWord(dev,
1241             VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, &tmp));
1242     *distance = tmp;
1243     return status;
1244 }
1245
1246 // VL53L1X_ERROR VL53L1X_GetSignalPerSpad(busDevice_t * dev, uint16_t *signalRate)
1247 // {
1248 //     VL53L1X_ERROR status = 0;
1249 //     uint16_t SpNb=1, signal;
1250
1251 //     status = VL53L1_RdWord(dev,
1252 //         VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &signal);
1253 //     status = VL53L1_RdWord(dev,
1254 //         VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb);
1255 //     *signalRate = (uint16_t) (200.0*signal/SpNb);
1256 //     return status;
1257 // }
1258
1259 // VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(busDevice_t * dev, uint16_t *ambPerSp)
1260 // {
1261 //     VL53L1X_ERROR status = 0;
1262 //     uint16_t AmbientRate, SpNb = 1;
1263
1264 //     status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &AmbientRate);
1265 //     status = VL53L1_RdWord(dev, VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb);
1266 //     *ambPerSp=(uint16_t) (200.0 * AmbientRate / SpNb);
1267 //     return status;
1268 // }
1269
1270 // static VL53L1X_ERROR VL53L1X_GetSignalRate(busDevice_t * dev, uint16_t *signal)
1271 // {
1272 //     VL53L1X_ERROR status = 0;
1273 //     uint16_t tmp;
1274
1275 //     status = VL53L1_RdWord(dev,
1276 //         VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &tmp);
1277 //     *signal = tmp*8;
1278 //     return status;
1279 // }
1280
1281 // static VL53L1X_ERROR VL53L1X_GetSpadNb(busDevice_t * dev, uint16_t *spNb)
1282 // {
1283 //     VL53L1X_ERROR status = 0;
1284 //     uint16_t tmp;
1285
1286 //     status = VL53L1_RdWord(dev,
1287 //                   VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &tmp);
1288 //     *spNb = tmp >> 8;
1289 //     return status;
1290 // }
1291
1292 // VL53L1X_ERROR VL53L1X_GetAmbientRate(busDevice_t * dev, uint16_t *ambRate)
1293 // {
1294 //     VL53L1X_ERROR status = 0;
1295 //     uint16_t tmp;
1296
1297 //     status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &tmp);
1298 //     *ambRate = tmp*8;
1299 //     return status;
1300 // }
1301
1302 // VL53L1X_ERROR VL53L1X_GetRangeStatus(busDevice_t * dev, uint8_t *rangeStatus)
1303 // {
1304 //     VL53L1X_ERROR status = 0;
1305 //     uint8_t RgSt;
1306
1307 //     *rangeStatus = 255;
1308 //     status = VL53L1_RdByte(dev, VL53L1_RESULT__RANGE_STATUS, &RgSt);
1309 //     RgSt = RgSt & 0x1F;
1310 //     if (RgSt < 24)
1311 //         *rangeStatus = status_rtn[RgSt];
1312 //     return status;
1313 // }
1314
1315 // VL53L1X_ERROR VL53L1X_GetResult(busDevice_t * dev, VL53L1X_Result_t *pResult)
1316 // {
1317 //     VL53L1X_ERROR status = 0;
1318 //     uint8_t Temp[17];
1319 //     uint8_t RgSt = 255;
1320
1321 //     status = VL53L1_ReadMulti(dev, VL53L1_RESULT__RANGE_STATUS, Temp, 17);
1322 //     RgSt = Temp[0] & 0x1F;
1323 //     if (RgSt < 24)
1324 //         RgSt = status_rtn[RgSt];
1325 //     pResult->Status = RgSt;
1326 //     pResult->Ambient = (Temp[7] << 8 | Temp[8]) * 8;
1327 //     pResult->NumSPADs = Temp[3];
1328 //     pResult->SigPerSPAD = (Temp[15] << 8 | Temp[16]) * 8;
1329 //     pResult->Distance = Temp[13] << 8 | Temp[14];
1330
1331 //     return status;
1332 // }
1333
1334 // VL53L1X_ERROR VL53L1X_SetOffset(busDevice_t * dev, int16_t OffsetValue)
1335 // {
1336 //     VL53L1X_ERROR status = 0;
1337 //     int16_t Temp;
1338
1339 //     Temp = (OffsetValue*4);
1340 //     VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM,
1341 //             (uint16_t)Temp);
1342 //     VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0);
1343 //     VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0);
1344 //     return status;
1345 // }
1346
1347 // VL53L1X_ERROR  VL53L1X_GetOffset(busDevice_t * dev, int16_t *offset)
1348 // {
1349 //     VL53L1X_ERROR status = 0;
1350 //     uint16_t Temp;
1351
1352 //     status = VL53L1_RdWord(dev,ALGO__PART_TO_PART_RANGE_OFFSET_MM, &Temp);
1353 //     Temp = Temp<<3;
1354 //     Temp = Temp>>5;
1355 //     *offset = (int16_t)(Temp);
1356 //     return status;
1357 // }
1358
1359 // VL53L1X_ERROR VL53L1X_SetXtalk(busDevice_t * dev, uint16_t XtalkValue)
1360 // {
1361 // /* XTalkValue in count per second to avoid float type */
1362 //     VL53L1X_ERROR status = 0;
1363
1364 //     status = VL53L1_WrWord(dev,
1365 //             ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS,
1366 //             0x0000);
1367 //     status = VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS,
1368 //             0x0000);
1369 //     status = VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS,
1370 //             (XtalkValue<<9)/1000); /* * << 9 (7.9 format) and /1000 to convert cps to kpcs */
1371 //     return status;
1372 // }
1373
1374 // VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *xtalk )
1375 // {
1376 //     VL53L1X_ERROR status = 0;
1377
1378 //     status = VL53L1_RdWord(dev,ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, xtalk);
1379 //     *xtalk = (uint16_t)((*xtalk*1000)>>9); /* * 1000 to convert kcps to cps and >> 9 (7.9 format) */
1380 //     return status;
1381 // }
1382
1383 // VL53L1X_ERROR VL53L1X_SetDistanceThreshold(busDevice_t * dev, uint16_t ThreshLow,
1384 //                   uint16_t ThreshHigh, uint8_t Window,
1385 //                   uint8_t IntOnNoTarget)
1386 // {
1387 //     VL53L1X_ERROR status = 0;
1388 //     uint8_t Temp = 0;
1389
1390 //     status = VL53L1_RdByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, &Temp);
1391 //     Temp = Temp & 0x47;
1392 //     if (IntOnNoTarget == 0) {
1393 //         status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO,
1394 //                    (Temp | (Window & 0x07)));
1395 //     } else {
1396 //         status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO,
1397 //                    ((Temp | (Window & 0x07)) | 0x40));
1398 //     }
1399 //     status = VL53L1_WrWord(dev, SYSTEM__THRESH_HIGH, ThreshHigh);
1400 //     status = VL53L1_WrWord(dev, SYSTEM__THRESH_LOW, ThreshLow);
1401 //     return status;
1402 // }
1403
1404 // VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(busDevice_t * dev, uint16_t *window)
1405 // {
1406 //     VL53L1X_ERROR status = 0;
1407 //     uint8_t tmp;
1408 //     status = VL53L1_RdByte(dev,SYSTEM__INTERRUPT_CONFIG_GPIO, &tmp);
1409 //     *window = (uint16_t)(tmp & 0x7);
1410 //     return status;
1411 // }
1412
1413 // VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(busDevice_t * dev, uint16_t *low)
1414 // {
1415 //     VL53L1X_ERROR status = 0;
1416 //     uint16_t tmp;
1417
1418 //     status = VL53L1_RdWord(dev,SYSTEM__THRESH_LOW, &tmp);
1419 //     *low = tmp;
1420 //     return status;
1421 // }
1422
1423 // VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high)
1424 // {
1425 //     VL53L1X_ERROR status = 0;
1426 //     uint16_t tmp;
1427
1428 //     status = VL53L1_RdWord(dev,SYSTEM__THRESH_HIGH, &tmp);
1429 //     *high = tmp;
1430 //     return status;
1431 // }
1432
1433 // VL53L1X_ERROR VL53L1X_SetROICenter(busDevice_t * dev, uint8_t ROICenter)
1434 // {
1435 //     VL53L1X_ERROR status = 0;
1436 //     status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, ROICenter);
1437 //     return status;
1438 // }
1439
1440 // VL53L1X_ERROR VL53L1X_GetROICenter(busDevice_t * dev, uint8_t *ROICenter)
1441 // {
1442 //     VL53L1X_ERROR status = 0;
1443 //     uint8_t tmp;
1444 //     status = VL53L1_RdByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, &tmp);
1445 //     *ROICenter = tmp;
1446 //     return status;
1447 // }
1448
1449 // VL53L1X_ERROR VL53L1X_SetROI(busDevice_t * dev, uint16_t X, uint16_t Y)
1450 // {
1451 //     uint8_t OpticalCenter;
1452 //     VL53L1X_ERROR status = 0;
1453
1454 //     status =VL53L1_RdByte(dev, VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD, &OpticalCenter);
1455 //     if (X > 16)
1456 //         X = 16;
1457 //     if (Y > 16)
1458 //         Y = 16;
1459 //     if (X > 10 || Y > 10){
1460 //         OpticalCenter = 199;
1461 //     }
1462 //     status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, OpticalCenter);
1463 //     status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE,
1464 //                (Y - 1) << 4 | (X - 1));
1465 //     return status;
1466 // }
1467
1468 // VL53L1X_ERROR VL53L1X_GetROI_XY(busDevice_t * dev, uint16_t *ROI_X, uint16_t *ROI_Y)
1469 // {
1470 //     VL53L1X_ERROR status = 0;
1471 //     uint8_t tmp;
1472
1473 //     status = VL53L1_RdByte(dev,ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, &tmp);
1474 //     *ROI_X = ((uint16_t)tmp & 0x0F) + 1;
1475 //     *ROI_Y = (((uint16_t)tmp & 0xF0) >> 4) + 1;
1476 //     return status;
1477 // }
1478
1479 // VL53L1X_ERROR VL53L1X_SetSignalThreshold(busDevice_t * dev, uint16_t Signal)
1480 // {
1481 //     VL53L1X_ERROR status = 0;
1482
1483 //     VL53L1_WrWord(dev,RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,Signal>>3);
1484 //     return status;
1485 // }
1486
1487 // VL53L1X_ERROR VL53L1X_GetSignalThreshold(busDevice_t * dev, uint16_t *signal)
1488 // {
1489 //     VL53L1X_ERROR status = 0;
1490 //     uint16_t tmp;
1491
1492 //     status = VL53L1_RdWord(dev,
1493 //                 RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, &tmp);
1494 //     *signal = tmp <<3;
1495 //     return status;
1496 // }
1497
1498 // VL53L1X_ERROR VL53L1X_SetSigmaThreshold(busDevice_t * dev, uint16_t Sigma)
1499 // {
1500 //     VL53L1X_ERROR status = 0;
1501
1502 //     if(Sigma>(0xFFFF>>2)){
1503 //         return 1;
1504 //     }
1505 //     /* 16 bits register 14.2 format */
1506 //     status = VL53L1_WrWord(dev,RANGE_CONFIG__SIGMA_THRESH,Sigma<<2);
1507 //     return status;
1508 // }
1509
1510 // VL53L1X_ERROR VL53L1X_GetSigmaThreshold(busDevice_t * dev, uint16_t *sigma)
1511 // {
1512 //     VL53L1X_ERROR status = 0;
1513 //     uint16_t tmp;
1514
1515 //     status = VL53L1_RdWord(dev,RANGE_CONFIG__SIGMA_THRESH, &tmp);
1516 //     *sigma = tmp >> 2;
1517 //     return status;
1518
1519 // }
1520
1521 // VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev)
1522 // {
1523 //     VL53L1X_ERROR status = 0;
1524 //     uint8_t tmp=0;
1525
1526 //     status = VL53L1_WrByte(dev,VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,0x81); /* full VHV */
1527 //     status = VL53L1_WrByte(dev,0x0B,0x92);
1528 //     status = VL53L1X_StartRanging(dev);
1529 //     while(tmp==0){
1530 //         status = VL53L1X_CheckForDataReady(dev, &tmp);
1531 //     }
1532 //     tmp  = 0;
1533 //     status = VL53L1X_ClearInterrupt(dev);
1534 //     status = VL53L1X_StopRanging(dev);
1535 //     status = VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */
1536 //     status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */
1537 //     return status;
1538 // }
1539
1540
1541 // int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16_t *offset)
1542 // {
1543 //     uint8_t i, tmp;
1544 //     int16_t AverageDistance = 0;
1545 //     uint16_t distance;
1546 //     VL53L1X_ERROR status = 0;
1547
1548 //     status = VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, 0x0);
1549 //     status = VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0);
1550 //     status = VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0);
1551 //     status = VL53L1X_StartRanging(dev); /* Enable VL53L1X sensor */
1552 //     for (i = 0; i < 50; i++) {
1553 //         tmp = 0;
1554 //         while (tmp == 0){
1555 //             status = VL53L1X_CheckForDataReady(dev, &tmp);
1556 //         }
1557 //         status = VL53L1X_GetDistance(dev, &distance);
1558 //         status = VL53L1X_ClearInterrupt(dev);
1559 //         AverageDistance = AverageDistance + distance;
1560 //     }
1561 //     status = VL53L1X_StopRanging(dev);
1562 //     AverageDistance = AverageDistance / 50;
1563 //     *offset = TargetDistInMm - AverageDistance;
1564 //     status = VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, *offset*4);
1565 //     return status;
1566 // }
1567
1568 // int8_t VL53L1X_CalibrateXtalk(busDevice_t * dev, uint16_t TargetDistInMm, uint16_t *xtalk)
1569 // {
1570 //     uint8_t i, tmp;
1571 //     float AverageSignalRate = 0;
1572 //     float AverageDistance = 0;
1573 //     float AverageSpadNb = 0;
1574 //     uint16_t distance = 0, spadNum;
1575 //     uint16_t sr;
1576 //     VL53L1X_ERROR status = 0;
1577 //     uint32_t calXtalk;
1578
1579 //     status = VL53L1_WrWord(dev, 0x0016,0);
1580 //     status = VL53L1X_StartRanging(dev);
1581 //     for (i = 0; i < 50; i++) {
1582 //         tmp = 0;
1583 //         while (tmp == 0){
1584 //             status = VL53L1X_CheckForDataReady(dev, &tmp);
1585 //         }
1586 //         status= VL53L1X_GetSignalRate(dev, &sr);
1587 //         status= VL53L1X_GetDistance(dev, &distance);
1588 //         status = VL53L1X_ClearInterrupt(dev);
1589 //         AverageDistance = AverageDistance + distance;
1590 //         status = VL53L1X_GetSpadNb(dev, &spadNum);
1591 //         AverageSpadNb = AverageSpadNb + spadNum;
1592 //         AverageSignalRate =
1593 //             AverageSignalRate + sr;
1594 //     }
1595 //     status = VL53L1X_StopRanging(dev);
1596 //     AverageDistance = AverageDistance / 50;
1597 //     AverageSpadNb = AverageSpadNb / 50;
1598 //     AverageSignalRate = AverageSignalRate / 50;
1599 //     /* Calculate Xtalk value */
1600 //     calXtalk = (uint16_t)(512*(AverageSignalRate*(1-(AverageDistance/TargetDistInMm)))/AverageSpadNb);
1601 //     *xtalk = (uint16_t)((calXtalk*1000)>>9);
1602 //     status = VL53L1_WrWord(dev, 0x0016, (uint16_t)calXtalk);
1603 //     return status;
1604 // }
1605
1606 static void vl53l1x_Init(rangefinderDev_t * rangefinder)
1607 {
1608     VL53L1X_ERROR status = VL53L1_ERROR_NONE;
1609     isInitialized = false;
1610     status = VL53L1X_SensorInit(rangefinder->busDev);
1611     if (status == VL53L1_ERROR_NONE) {
1612         VL53L1X_SetDistanceMode(rangefinder->busDev, 2); /* 1=short, 2=long */
1613         VL53L1X_SetTimingBudgetInMs(rangefinder->busDev, 33); /* in ms possible values [20, 50, 100, 200, 500] */
1614         VL53L1X_SetInterMeasurementInMs(rangefinder->busDev, RANGEFINDER_VL53L1X_TASK_PERIOD_MS); /* in ms, IM must be > = TB */
1615         status = VL53L1X_StartRanging(rangefinder->busDev);
1616     }
1617     isInitialized = (status == VL53L1_ERROR_NONE);
1618 }
1619
1620 void vl53l1x_Update(rangefinderDev_t * rangefinder)
1621 {
1622     uint16_t Distance;
1623     uint8_t dataReady;
1624
1625     if (!isInitialized) {
1626         return;
1627     }
1628
1629     VL53L1X_CheckForDataReady(rangefinder->busDev, &dataReady);
1630     if (dataReady != 0) {
1631         VL53L1X_GetDistance(rangefinder->busDev, &Distance);
1632         lastMeasurementCm = Distance / 10;
1633         lastMeasurementIsNew = true;
1634     }
1635     VL53L1X_ClearInterrupt(rangefinder->busDev);
1636 }
1637
1638 int32_t vl53l1x_GetDistance(rangefinderDev_t *dev)
1639 {
1640     UNUSED(dev);
1641
1642     if (isResponding && isInitialized) {
1643         if (lastMeasurementIsNew) {
1644             lastMeasurementIsNew = false;
1645             return (lastMeasurementCm < VL53L1X_MAX_RANGE_CM) ? lastMeasurementCm : RANGEFINDER_OUT_OF_RANGE;
1646         }
1647         else {
1648             return RANGEFINDER_NO_NEW_DATA;
1649         }
1650     }
1651     else {
1652         return RANGEFINDER_HARDWARE_FAILURE;
1653     }
1654 }
1655
1656 static bool deviceDetect(busDevice_t * busDev)
1657 {
1658     for (int retry = 0; retry < 5; retry++) {
1659         uint8_t model_id;
1660
1661         delay(150);
1662
1663         VL53L1X_ERROR err = VL53L1_RdByte(busDev, 0x010F, &model_id);
1664         if (err == 0 && model_id == 0xEA) {
1665             return true;
1666         }
1667     };
1668
1669     return false;
1670 }
1671
1672 bool vl53l1xDetect(rangefinderDev_t * rangefinder)
1673 {
1674     rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_VL53L1X, 0, OWNER_RANGEFINDER);
1675     if (rangefinder->busDev == NULL) {
1676         return false;
1677     }
1678
1679     if (!deviceDetect(rangefinder->busDev)) {
1680         busDeviceDeInit(rangefinder->busDev);
1681         return false;
1682     }
1683
1684     rangefinder->delayMs = RANGEFINDER_VL53L1X_TASK_PERIOD_MS;
1685     rangefinder->maxRangeCm = VL53L1X_MAX_RANGE_CM;
1686     rangefinder->detectionConeDeciDegrees = VL53L1X_DETECTION_CONE_DECIDEGREES;
1687     rangefinder->detectionConeExtendedDeciDegrees = VL53L1X_DETECTION_CONE_DECIDEGREES;
1688
1689     rangefinder->init = &vl53l1x_Init;
1690     rangefinder->update = &vl53l1x_Update;
1691     rangefinder->read = &vl53l1x_GetDistance;
1692
1693     return true;
1694 }
1695
1696 #endif