ST Expansion SW Team
/
Vl6180
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: VL6180_Board
Diff: src/vl6180_api.c
- Revision:
- 2:bc1d979ae392
- Parent:
- 0:1da5e4bcb8e5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/src/vl6180_api.c Tue Nov 10 10:15:46 2020 +0000
@@ -0,0 +1,2451 @@
+/*******************************************************************************
+Copyright � 2019, STMicroelectronics International N.V.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+ * Neither the name of STMicroelectronics nor the
+ names of its contributors may be used to endorse or promote products
+ derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
+NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
+IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************/
+#include "stdio.h"
+#include "vl6180_api.h"
+
+#define VL6180_9to7Conv(x) (x)
+
+/* TODO when set all "cached" value with "default init" are updated after init from register read back */
+#define REFRESH_CACHED_DATA_AFTER_INIT 1
+
+
+#define IsValidGPIOFunction(x) ((x) == GPIOx_SELECT_GPIO_INTERRUPT_OUTPUT || (x) == GPIOx_SELECT_OFF)
+
+
+/** default value ECE factor Molecular */
+#define DEF_ECE_FACTOR_M 85
+/** default value ECE factor Denominator */
+#define DEF_ECE_FACTOR_D 100
+/** default value for DMAX Enable */
+#define DEF_DMAX_ENABLE 1
+/** default ambient tuning factor %x1000 */
+#define DEF_AMBIENT_TUNING 80
+
+#define DEF_CROSS_TALK_VALID_HEIGHT_VALUE 20
+
+
+#if VL6180_SINGLE_DEVICE_DRIVER
+extern struct VL6180DevData_t SingleVL6180DevData;
+#define VL6180DevDataGet(dev, field) (SingleVL6180DevData.field)
+#define VL6180DevDataSet(dev, field, data) SingleVL6180DevData.field = (data)
+#endif
+
+#define LUXRES_FIX_PREC 8
+#define GAIN_FIX_PREC 8 /* ! if not sme as LUX_PREC then :( adjust GetLux */
+#define AN_GAIN_MULT (1 << GAIN_FIX_PREC)
+
+
+static int32_t _GetAveTotalTime(VL6180Dev_t dev);
+static int VL6180_RangeSetEarlyConvergenceEestimateThreshold(VL6180Dev_t dev);
+
+/**
+ * ScalerLookUP scaling factor-1 to register #RANGE_SCALER lookup
+ */
+static const uint16_t ScalerLookUP[] ROMABLE_DATA = {253, 127, 84}; /* lookup table for scaling->scalar 1x2x 3x */
+/**
+ * scaling factor to Upper limit look up
+ */
+static const uint16_t UpperLimitLookUP[] ROMABLE_DATA = {185, 370, 580}; /* lookup table for scaling->limit 1x2x3x */
+
+
+#if VL6180_RANGE_STATUS_ERRSTRING
+const char *ROMABLE_DATA VL6180_RangeStatusErrString[] = {
+ "No Error",
+ "VCSEL Continuity Test",
+ "VCSEL Watchdog Test",
+ "VCSEL Watchdog",
+ "PLL1 Lock",
+ "PLL2 Lock",
+ "Early Convergence Estimate",
+ "Max Convergence",
+ "No Target Ignore",
+ "Not used 9",
+ "Not used 10",
+ "Max Signal To Noise Ratio",
+ "Raw Ranging Algo Underflow",
+ "Raw Ranging Algo Overflow",
+ "Ranging Algo Underflow",
+ "Ranging Algo Overflow",
+
+ "Filtered by post processing (WAF)",
+ "Ranging filtering (WAF) on-going",
+ "Data not ready",
+};
+
+const char *VL6180_RangeGetStatusErrString(uint8_t RangeErrCode)
+{
+ if (RangeErrCode > sizeof(VL6180_RangeStatusErrString) / sizeof(VL6180_RangeStatusErrString[0]))
+ return NULL;
+ return VL6180_RangeStatusErrString[RangeErrCode];
+}
+#endif
+
+#if VL6180_UPSCALE_SUPPORT == 1
+ #define _GetUpscale(dev, ...) 1
+ #define _SetUpscale(...) -1
+ #define DEF_UPSCALE 1
+#elif VL6180_UPSCALE_SUPPORT == 2
+ #define _GetUpscale(dev, ...) 2
+ #define _SetUpscale(...)
+ #define DEF_UPSCALE 2
+#elif VL6180_UPSCALE_SUPPORT == 3
+ #define _GetUpscale(dev, ...) 3
+ #define _SetUpscale(...)
+ #define DEF_UPSCALE 3
+#else
+ #define DEF_UPSCALE (-(VL6180_UPSCALE_SUPPORT))
+ #define _GetUpscale(dev, ...) VL6180DevDataGet(dev, UpscaleFactor)
+ #define _SetUpscale(dev, Scaling) VL6180DevDataSet(dev, UpscaleFactor, Scaling)
+#endif
+
+
+#if VL6180_SINGLE_DEVICE_DRIVER
+/**
+ * the unique driver data When single device driver is active
+ */
+struct VL6180DevData_t VL6180_DEV_DATA_ATTR SingleVL6180DevData = {
+ .EceFactorM = DEF_ECE_FACTOR_M,
+ .EceFactorD = DEF_ECE_FACTOR_D,
+#ifdef VL6180_HAVE_UPSCALE_DATA
+ .UpscaleFactor = DEF_UPSCALE,
+#endif
+#ifdef VL6180_HAVE_DMAX_RANGING
+ .DMaxEnable = DEF_DMAX_ENABLE,
+#endif
+};
+#endif /* VL6180_SINGLE_DEVICE_DRIVER */
+
+
+
+#define Fix7_2_KCPs(x) ((((uint32_t)(x))*1000)>>7)
+
+
+#if VL6180_WRAP_AROUND_FILTER_SUPPORT || VL6180_HAVE_DMAX_RANGING
+static int _GetRateResult(VL6180Dev_t dev, VL6180_RangeData_t *pRangeData);
+#endif
+
+#if VL6180_WRAP_AROUND_FILTER_SUPPORT
+static int _filter_Init(VL6180Dev_t dev);
+static int _filter_GetResult(VL6180Dev_t dev, VL6180_RangeData_t *pData);
+ #define _IsWrapArroundActive(dev) VL6180DevDataGet(dev, WrapAroundFilterActive)
+#else
+ #define _IsWrapArroundActive(dev) 0
+#endif
+
+
+#if VL6180_HAVE_DMAX_RANGING
+ void _DMax_OneTimeInit(VL6180Dev_t dev);
+ static int _DMax_InitData(VL6180Dev_t dev);
+ static int _DMax_Compute(VL6180Dev_t dev, VL6180_RangeData_t *pRange);
+ #define _IsDMaxActive(dev) VL6180DevDataGet(dev, DMaxEnable)
+#else
+ #define _DMax_InitData(...) 0 /* success */
+ #define _DMax_OneTimeInit(...) (void)0
+ #define _IsDMaxActive(...) 0
+#endif
+
+static int VL6180_RangeStaticInit(VL6180Dev_t dev);
+static int VL6180_UpscaleStaticInit(VL6180Dev_t dev);
+
+int VL6180_WaitDeviceBooted(VL6180Dev_t dev)
+{
+ uint8_t FreshOutReset;
+ int status;
+ LOG_FUNCTION_START("");
+ do {
+ status = VL6180_RdByte(dev, SYSTEM_FRESH_OUT_OF_RESET, &FreshOutReset);
+ } while (FreshOutReset != 1 && status == 0);
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+int VL6180_InitData(VL6180Dev_t dev)
+{
+ int status, dmax_status ;
+ int8_t offset;
+ uint8_t FreshOutReset;
+ uint32_t CalValue;
+ uint16_t u16;
+ uint32_t XTalkCompRate_KCps;
+
+ LOG_FUNCTION_START("");
+
+ VL6180DevDataSet(dev, EceFactorM, DEF_ECE_FACTOR_M);
+ VL6180DevDataSet(dev, EceFactorD, DEF_ECE_FACTOR_D);
+
+ VL6180DevDataSet(dev, RangeIgnore.Enabled, 0);
+
+#ifdef VL6180_HAVE_UPSCALE_DATA
+ VL6180DevDataSet(dev, UpscaleFactor, DEF_UPSCALE);
+#endif
+
+#ifdef VL6180_HAVE_WRAP_AROUND_DATA
+ VL6180DevDataSet(dev, WrapAroundFilterActive, (VL6180_WRAP_AROUND_FILTER_SUPPORT > 0));
+ VL6180DevDataSet(dev, DMaxEnable, DEF_DMAX_ENABLE);
+#endif
+
+ _DMax_OneTimeInit(dev);
+ do {
+
+ /* backup offset initial value from nvm these must be done prior any over call that use offset */
+ status = VL6180_RdByte(dev, SYSRANGE_PART_TO_PART_RANGE_OFFSET, (uint8_t *)&offset);
+ if (status) {
+ VL6180_ErrLog("SYSRANGE_PART_TO_PART_RANGE_OFFSET rd fail");
+ break;
+ }
+ VL6180DevDataSet(dev, Part2PartOffsetNVM, offset);
+
+ status = VL6180_RdDWord(dev, SYSRANGE_RANGE_IGNORE_THRESHOLD, &CalValue);
+ if (status) {
+ VL6180_ErrLog("Part2PartAmbNVM rd fail");
+ break;
+ }
+ if ((CalValue&0xFFFF0000) == 0) {
+ CalValue = 0x00CE03F8;
+ }
+ VL6180DevDataSet(dev, Part2PartAmbNVM, CalValue);
+
+ status = VL6180_RdWord(dev, SYSRANGE_CROSSTALK_COMPENSATION_RATE , &u16);
+ if (status) {
+ VL6180_ErrLog("SYSRANGE_CROSSTALK_COMPENSATION_RATE rd fail ");
+ break;
+ }
+ XTalkCompRate_KCps = Fix7_2_KCPs(u16);
+ VL6180DevDataSet(dev, XTalkCompRate_KCps, XTalkCompRate_KCps);
+
+ dmax_status = _DMax_InitData(dev);
+ if (dmax_status < 0) {
+ VL6180_ErrLog("DMax init failure");
+ break;
+ }
+
+ /* Read or wait for fresh out of reset */
+ status = VL6180_RdByte(dev, SYSTEM_FRESH_OUT_OF_RESET, &FreshOutReset);
+ if (status) {
+ VL6180_ErrLog("SYSTEM_FRESH_OUT_OF_RESET rd fail");
+ break;
+ }
+ if (FreshOutReset != 1 || dmax_status)
+ status = CALIBRATION_WARNING;
+
+ } while (0);
+
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+int8_t VL6180_GetOffsetCalibrationData(VL6180Dev_t dev)
+{
+ int8_t offset;
+ LOG_FUNCTION_START("");
+ offset = VL6180DevDataGet(dev, Part2PartOffsetNVM);
+ LOG_FUNCTION_END(offset);
+ return offset;
+}
+
+int VL6180_SetOffsetCalibrationData(VL6180Dev_t dev, int8_t offset)
+{
+ int status;
+ LOG_FUNCTION_START("%d", offset);
+ VL6180DevDataSet(dev, Part2PartOffsetNVM, offset);
+ offset /= _GetUpscale(dev);
+ status = VL6180_WrByte(dev, SYSRANGE_PART_TO_PART_RANGE_OFFSET, offset);
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+int VL6180_SetXTalkCompensationRate(VL6180Dev_t dev, FixPoint97_t Rate)
+{
+ int status;
+ LOG_FUNCTION_START("%d", Rate);
+ status = VL6180_WrWord(dev, SYSRANGE_CROSSTALK_COMPENSATION_RATE, Rate);
+ if (status == 0) {
+ uint32_t XTalkCompRate_KCps;
+ XTalkCompRate_KCps = Fix7_2_KCPs(Rate);
+ VL6180DevDataSet(dev, XTalkCompRate_KCps, XTalkCompRate_KCps);
+ /* update dmax whenever xtalk rate changes */
+ status = _DMax_InitData(dev);
+ }
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+int VL6180_SetI2CAddress(VL6180Dev_t dev, uint8_t NewAddress)
+{
+ int status;
+ LOG_FUNCTION_START("");
+
+ status = VL6180_WrByte(dev, I2C_SLAVE_DEVICE_ADDRESS, NewAddress / 2);
+ if (status) {
+ VL6180_ErrLog("new i2c addr Wr fail");
+ }
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+uint16_t VL6180_GetUpperLimit(VL6180Dev_t dev)
+{
+ uint16_t limit;
+ int scaling;
+
+ LOG_FUNCTION_START("");
+
+ scaling = _GetUpscale(dev);
+ /* FIXME we do assume here _GetUpscale is valid if user call us prior to init we may overflow the LUT mem area */
+ limit = UpperLimitLookUP[scaling - 1];
+
+ LOG_FUNCTION_END((int)limit);
+ return limit;
+}
+
+
+
+int VL6180_StaticInit(VL6180Dev_t dev)
+{
+ int status = 0, init_status;
+ LOG_FUNCTION_START("");
+
+ /* TODO doc When using configurable scaling but using 1x as start condition
+ * load tunning upscale or not ??? */
+ if (_GetUpscale(dev) == 1 && !(VL6180_UPSCALE_SUPPORT < 0))
+ {
+ init_status = VL6180_RangeStaticInit(dev);
+ }
+ else
+ {
+ init_status = VL6180_UpscaleStaticInit(dev);
+ }
+
+ if (init_status < 0) {
+ VL6180_ErrLog("StaticInit fail");
+ goto error;
+ } else if (init_status > 0) {
+ VL6180_ErrLog("StaticInit warning");
+ }
+
+ if (status < 0) {
+ VL6180_ErrLog("StaticInit fail");
+ }
+ if (!status && init_status) {
+ status = init_status;
+ }
+error:
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+
+int VL6180_SetGroupParamHold(VL6180Dev_t dev, int Hold)
+{
+ int status;
+ uint8_t value;
+
+ LOG_FUNCTION_START("%d", Hold);
+ if (Hold)
+ value = 1;
+ else
+ value = 0;
+ status = VL6180_WrByte(dev, SYSTEM_GROUPED_PARAMETER_HOLD, value);
+
+ LOG_FUNCTION_END(status);
+ return status;
+
+}
+
+int VL6180_Prepare(VL6180Dev_t dev)
+{
+ int status;
+ LOG_FUNCTION_START("");
+
+ do {
+ status = VL6180_StaticInit(dev);
+ if (status < 0)
+ break;
+
+ /* set range InterruptMode to new sample */
+ status = VL6180_RangeConfigInterrupt(dev, CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY);
+ if (status)
+ break;
+
+ /* set default threshold */
+ status = VL6180_RangeSetRawThresholds(dev, 10, 200);
+ if (status) {
+ VL6180_ErrLog("VL6180_RangeSetRawThresholds fail");
+ break;
+ }
+ #if VL6180_WRAP_AROUND_FILTER_SUPPORT
+ _filter_Init(dev);
+ #endif
+ /* make sure to reset any left previous condition that can hangs first poll */
+
+ status = VL6180_ClearAllInterrupt(dev);
+ } while (0);
+ LOG_FUNCTION_END(status);
+
+ return status;
+}
+
+
+int VL6180_RangePollMeasurement(VL6180Dev_t dev, VL6180_RangeData_t *pRangeData)
+{
+ int status;
+ int ClrStatus;
+ IntrStatus_t IntStatus;
+
+ LOG_FUNCTION_START("");
+ /* start single range measurement */
+
+
+ #if VL6180_SAFE_POLLING_ENTER
+ /* if device get stopped with left interrupt uncleared , it is required to clear them now or poll for new condition will never occur*/
+ status = VL6180_RangeClearInterrupt(dev);
+ if (status) {
+ VL6180_ErrLog("VL6180_RangeClearInterrupt fail");
+ goto done;
+ }
+ #endif
+ /* //![single_shot_snipet] */
+ status = VL6180_RangeSetSystemMode(dev, MODE_START_STOP | MODE_SINGLESHOT);
+ if (status) {
+ VL6180_ErrLog("VL6180_RangeSetSystemMode fail");
+ goto done;
+ }
+
+
+ /* poll for new sample ready */
+ while (1) {
+ status = VL6180_RangeGetInterruptStatus(dev, &IntStatus.val);
+ if (status) {
+ break;
+ }
+ if (IntStatus.status.Range == RES_INT_STAT_GPIO_NEW_SAMPLE_READY || IntStatus.status.Error != 0) {
+ break;
+ }
+
+ VL6180_PollDelay(dev);
+ }
+ /* //![single_shot_snipet] */
+
+ if (!status) {
+ status = VL6180_RangeGetMeasurement(dev, pRangeData);
+ }
+
+ /* clear range interrupt source */
+ ClrStatus = VL6180_RangeClearInterrupt(dev);
+
+ if (ClrStatus) {
+ VL6180_ErrLog("VL6180_RangeClearInterrupt fail");
+ /* leave initial status if already in error */
+ if (!status) {
+ status = ClrStatus;
+ }
+ }
+done:
+ VL6180_ErrLog("VL6180_RangeClearInterrupt end");
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+
+#if VL6180_CACHED_REG
+
+int VL6180_GetCachedDWord(VL6180Dev_t dev, uint16_t index, uint32_t *pValue)
+{
+ int status;
+ uint32_t Value;
+ if (VL6180DevDataGet(dev, CacheFilled) != 0 &&
+ index >= VL6180_FIRST_CACHED_INDEX &&
+ index <= (VL6180_LAST_CACHED_INDEX - 3)) {
+ uint8_t *pBytes = &VL6180DevDataGet(dev, CachedRegs[index - VL6180_FIRST_CACHED_INDEX]);
+ Value = ((uint32_t)pBytes[0] << 24) |
+ ((uint32_t)pBytes[1] << 16) |
+ ((uint32_t)pBytes[2] << 8) |
+ (uint32_t)pBytes[3];
+ *pValue = Value;
+ status = 0;
+ } else {
+ status = VL6180_RdDWord(dev, index, pValue);
+ }
+ return status;
+}
+
+int VL6180_GetCachedWord(VL6180Dev_t dev, uint16_t index, uint16_t *pValue)
+{
+ int status;
+ uint32_t Value;
+ if (VL6180DevDataGet(dev, CacheFilled) != 0 &&
+ index >= VL6180_FIRST_CACHED_INDEX &&
+ index <= (VL6180_LAST_CACHED_INDEX - 1)) {
+ uint8_t *pBytes = &VL6180DevDataGet(dev, CachedRegs[index - VL6180_FIRST_CACHED_INDEX]);
+ Value = ((uint32_t)pBytes[0] << 8) | (uint32_t)pBytes[1];
+ *pValue = Value;
+ status = 0;
+ } else {
+ status = VL6180_RdWord(dev, index, pValue);
+ }
+ return status;
+}
+
+int VL6180_GetCachedByte(VL6180Dev_t dev, uint16_t index, uint8_t *pValue)
+{
+ int status;
+ uint8_t Value;
+ if (VL6180DevDataGet(dev, CacheFilled) != 0 &&
+ index >= VL6180_FIRST_CACHED_INDEX &&
+ index <= VL6180_LAST_CACHED_INDEX) {
+ Value = VL6180DevDataGet(dev, CachedRegs[index - VL6180_FIRST_CACHED_INDEX]);
+ *pValue = Value;
+ status = 0;
+ } else {
+ status = VL6180_RdByte(dev, index, pValue);
+ }
+ return status;
+}
+
+
+int _CachedRegs_Fetch(VL6180Dev_t dev)
+{
+ int status;
+ uint8_t *Buffer;
+ if (VL6180DevDataGet(dev, CacheFilled) == 0) {
+ VL6180DevDataSet(dev, CacheFilled, 1);
+ Buffer = &VL6180DevDataGet(dev, CachedRegs[0]);
+ status = VL6180_RdMulti(dev, VL6180_FIRST_CACHED_INDEX, Buffer, VL6180_CACHED_REG_CNT);
+ } else {
+ status = 0 ;
+ }
+ return status;
+}
+
+void _CachedRegs_Flush(VL6180Dev_t dev)
+{
+ VL6180DevDataSet(dev, CacheFilled, 0);
+}
+
+#else
+# define _CachedRegs_Fetch(...) 0
+# define _CachedRegs_Flush(...) (void)0
+# define _Fetch_CachedRegs(...) 0
+# define VL6180_GetCachedByte(dev, index, pValue) VL6180_RdByte(dev, index, pValue)
+# define VL6180_GetCachedWord(dev, index, pValue) VL6180_RdWord(dev, index, pValue)
+# define VL6180_GetCachedDWord(dev, index, pValue) VL6180_RdDWord(dev, index, pValue)
+#endif /* VL6180_CACHED_REG */
+
+
+
+int VL6180_RangeGetMeasurement(VL6180Dev_t dev, VL6180_RangeData_t *pRangeData)
+{
+ int status;
+ uint16_t RawRate;
+ uint8_t RawStatus;
+
+ LOG_FUNCTION_START("");
+
+ status = _CachedRegs_Fetch(dev);
+ if (status) {
+ VL6180_ErrLog("Cache register read fail");
+ goto error;
+ }
+ status = VL6180_RangeGetResult(dev, &pRangeData->range_mm);
+ if (!status) {
+ status = VL6180_GetCachedWord(dev, RESULT_RANGE_SIGNAL_RATE, &RawRate);
+ if (!status) {
+ pRangeData->signalRate_mcps = VL6180_9to7Conv(RawRate);
+ status = VL6180_GetCachedByte(dev, RESULT_RANGE_STATUS, &RawStatus);
+ if (!status) {
+ pRangeData->errorStatus = RawStatus >> 4;
+ } else {
+ VL6180_ErrLog("Rd RESULT_RANGE_STATUS fail");
+ }
+ #if VL6180_WRAP_AROUND_FILTER_SUPPORT || VL6180_HAVE_DMAX_RANGING
+ status = _GetRateResult(dev, pRangeData);
+ if (status)
+ goto error;
+ #endif
+ #if VL6180_WRAP_AROUND_FILTER_SUPPORT
+ /* if enabled run filter */
+ if (_IsWrapArroundActive(dev)) {
+ status = _filter_GetResult(dev, pRangeData);
+ if (!status) {
+ /* patch the range status and measure if it is filtered */
+ if(pRangeData->FilteredData.filterError != NoError) {
+ pRangeData->errorStatus = pRangeData->FilteredData.filterError;
+ pRangeData->range_mm = pRangeData->FilteredData.range_mm;
+ }
+ }
+ }
+ #endif
+
+ #if VL6180_HAVE_DMAX_RANGING
+ if (_IsDMaxActive(dev)) {
+ _DMax_Compute(dev, pRangeData);
+ }
+ #endif
+ } else {
+ VL6180_ErrLog("Rd RESULT_RANGE_SIGNAL_RATE fail");
+ }
+ } else {
+ VL6180_ErrLog("VL6180_GetRangeResult fail");
+ }
+error:
+ _CachedRegs_Flush(dev);
+ LOG_FUNCTION_END_FMT(status, "%d %d %d", (int)pRangeData->range_mm, (int)pRangeData->signalRate_mcps, (int)pRangeData->errorStatus) ;
+ return status;
+}
+
+
+int VL6180_RangeGetMeasurementIfReady(VL6180Dev_t dev, VL6180_RangeData_t *pRangeData)
+{
+ int status;
+ IntrStatus_t IntStatus;
+
+ LOG_FUNCTION_START();
+ status = VL6180_RangeGetInterruptStatus(dev, &IntStatus.val);
+ if (status == 0) {
+ if (IntStatus.status.Range == RES_INT_STAT_GPIO_NEW_SAMPLE_READY ||
+ IntStatus.status.Error != 0) {
+ status = VL6180_RangeGetMeasurement(dev, pRangeData);
+ if (status == 0) {
+ /* clear range interrupt source */
+ status = VL6180_RangeClearInterrupt(dev);
+ if (status) {
+ VL6180_ErrLog("VL6180_RangeClearInterrupt fail");
+ }
+ }
+ } else {
+ pRangeData->errorStatus = DataNotReady;
+ }
+ } else {
+ VL6180_ErrLog("fail to get interrupt status");
+ }
+ LOG_FUNCTION_END(status) ;
+ return status;
+}
+
+int VL6180_FilterSetState(VL6180Dev_t dev, int state)
+{
+ int status;
+ LOG_FUNCTION_START("%d", state);
+ #if VL6180_WRAP_AROUND_FILTER_SUPPORT
+ VL6180DevDataSet(dev, WrapAroundFilterActive, state);
+ status = 0;
+ #else
+ status = NOT_SUPPORTED;
+ #endif
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+int VL6180_FilterGetState(VL6180Dev_t dev)
+{
+ int status;
+ LOG_FUNCTION_START("");
+ #if VL6180_WRAP_AROUND_FILTER_SUPPORT
+ status = VL6180DevDataGet(dev, WrapAroundFilterActive);
+ #else
+ status = 0;
+ #endif
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+int VL6180_RangeGetResult(VL6180Dev_t dev, int32_t *pRange_mm)
+{
+ int status;
+ uint8_t RawRange;
+ int32_t Upscale;
+
+ LOG_FUNCTION_START("%p", pRange_mm);
+
+ status = VL6180_GetCachedByte(dev, RESULT_RANGE_VAL, &RawRange);
+ if (!status) {
+ Upscale = _GetUpscale(dev);
+ *pRange_mm = Upscale * (int32_t)RawRange;
+ }
+ LOG_FUNCTION_END_FMT(status, "%d", (int)*pRange_mm);
+ return status;
+}
+
+int VL6180_RangeSetRawThresholds(VL6180Dev_t dev, uint8_t low, uint8_t high)
+{
+ int status;
+ LOG_FUNCTION_START("%d %d", (int) low, (int)high);
+ /* TODO we can optimize here grouping high/low in a word but that's cpu endianness dependent */
+ status = VL6180_WrByte(dev, SYSRANGE_THRESH_HIGH, high);
+ if (!status) {
+ status = VL6180_WrByte(dev, SYSRANGE_THRESH_LOW, low);
+ }
+
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+int VL6180_RangeSetThresholds(VL6180Dev_t dev, uint16_t low, uint16_t high, int UseSafeParamHold)
+{
+ int status;
+ int scale;
+ LOG_FUNCTION_START("%d %d", (int) low, (int)high);
+ scale = _GetUpscale(dev, UpscaleFactor);
+ if (low > scale * 255 || high > scale * 255) {
+ status = INVALID_PARAMS;
+ } else {
+ do {
+ if (UseSafeParamHold) {
+ status = VL6180_SetGroupParamHold(dev, 1);
+ if (status)
+ break;
+ }
+ status = VL6180_RangeSetRawThresholds(dev, (uint8_t)(low / scale), (uint8_t)(high / scale));
+ if (status) {
+ VL6180_ErrLog("VL6180_RangeSetRawThresholds fail");
+ }
+ if (UseSafeParamHold) {
+ int HoldStatus;
+ /* tryt to unset param hold vene if previous fail */
+ HoldStatus = VL6180_SetGroupParamHold(dev, 0);
+ if (!status)
+ status = HoldStatus;
+ }
+ } while (0);
+ }
+
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+
+int VL6180_RangeGetThresholds(VL6180Dev_t dev, uint16_t *low, uint16_t *high)
+{
+ int status = 0;
+ uint8_t RawLow, RawHigh;
+ int scale;
+
+ LOG_FUNCTION_START("%p %p", low , high);
+
+ scale = _GetUpscale(dev, UpscaleFactor);
+ do {
+ if (high != NULL) {
+ status = VL6180_RdByte(dev, SYSRANGE_THRESH_HIGH, &RawHigh);
+ if (status) {
+ VL6180_ErrLog("rd SYSRANGE_THRESH_HIGH fail");
+ break;
+ }
+ *high = (uint16_t)RawHigh * scale;
+ }
+ if (low != NULL) {
+ status = VL6180_RdByte(dev, SYSRANGE_THRESH_LOW, &RawLow);
+ if (status) {
+ VL6180_ErrLog("rd SYSRANGE_THRESH_LOW fail");
+ break;
+ }
+ *low = (uint16_t)RawLow * scale;
+ }
+ } while (0);
+ LOG_FUNCTION_END_FMT(status, "%d %d", (int)*low , (int)*high);
+ return status;
+}
+
+
+int VL6180_RangeGetInterruptStatus(VL6180Dev_t dev, uint8_t *pIntStatus)
+{
+ int status;
+ uint8_t IntStatus;
+ LOG_FUNCTION_START("%p", pIntStatus);
+ /* FIXME we are grouping "error" with over status the user must check implicitly for it
+ * not just new sample or over status , that will nevr show up in case of error*/
+ status = VL6180_GetCachedByte(dev, RESULT_INTERRUPT_STATUS_GPIO, &IntStatus);
+ *pIntStatus = IntStatus & 0xC7;
+
+ LOG_FUNCTION_END_FMT(status, "%d", (int)*pIntStatus);
+ return status;
+}
+
+
+int VL6180_GetInterruptStatus(VL6180Dev_t dev, uint8_t *IntStatus)
+{
+ int status;
+ LOG_FUNCTION_START("%p" , IntStatus);
+ status = VL6180_RdByte(dev, RESULT_INTERRUPT_STATUS_GPIO, IntStatus);
+ LOG_FUNCTION_END_FMT(status, "%d", (int)*IntStatus);
+ return status;
+}
+
+int VL6180_ClearInterrupt(VL6180Dev_t dev, uint8_t IntClear)
+{
+ int status;
+ LOG_FUNCTION_START("%d", (int)IntClear);
+ if (IntClear <= 7) {
+ status = VL6180_WrByte(dev, SYSTEM_INTERRUPT_CLEAR, IntClear);
+ } else {
+ status = INVALID_PARAMS;
+ }
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+
+static int VL6180_RangeStaticInit(VL6180Dev_t dev)
+{
+ int status;
+ LOG_FUNCTION_START("");
+printf("VL6180_RangeStaticInit start \n");
+ /* REGISTER_TUNING_SR03_270514_CustomerView.txt */
+ VL6180_WrByte(dev, 0x0207, 0x01);
+ VL6180_WrByte(dev, 0x0208, 0x01);
+ VL6180_WrByte(dev, 0x0096, 0x00);
+ VL6180_WrByte(dev, 0x0097, 0xfd);
+ VL6180_WrByte(dev, 0x00e3, 0x00);
+ VL6180_WrByte(dev, 0x00e4, 0x04);
+ VL6180_WrByte(dev, 0x00e5, 0x02);
+ VL6180_WrByte(dev, 0x00e6, 0x01);
+ VL6180_WrByte(dev, 0x00e7, 0x03);
+ VL6180_WrByte(dev, 0x00f5, 0x02);
+ VL6180_WrByte(dev, 0x00d9, 0x05);
+ VL6180_WrByte(dev, 0x00db, 0xce);
+ VL6180_WrByte(dev, 0x00dc, 0x03);
+ VL6180_WrByte(dev, 0x00dd, 0xf8);
+ VL6180_WrByte(dev, 0x009f, 0x00);
+ VL6180_WrByte(dev, 0x00a3, 0x3c);
+ VL6180_WrByte(dev, 0x00b7, 0x00);
+ VL6180_WrByte(dev, 0x00bb, 0x3c);
+ VL6180_WrByte(dev, 0x00b2, 0x09);
+ VL6180_WrByte(dev, 0x00ca, 0x09);
+ VL6180_WrByte(dev, 0x0198, 0x01);
+ VL6180_WrByte(dev, 0x01b0, 0x17);
+ VL6180_WrByte(dev, 0x01ad, 0x00);
+ VL6180_WrByte(dev, 0x00ff, 0x05);
+ VL6180_WrByte(dev, 0x0100, 0x05);
+ VL6180_WrByte(dev, 0x0199, 0x05);
+ VL6180_WrByte(dev, 0x01a6, 0x1b);
+ VL6180_WrByte(dev, 0x01ac, 0x3e);
+ VL6180_WrByte(dev, 0x01a7, 0x1f);
+ VL6180_WrByte(dev, 0x0030, 0x00);
+
+ /* Recommended : Public registers - See data sheet for more detail */
+ VL6180_WrByte(dev, 0x0011, 0x10); /* Enables polling for New Sample ready when measurement completes */
+ VL6180_WrByte(dev, 0x010a, 0x30); /* Set the averaging sample period (compromise between lower noise and increased execution time) */
+ VL6180_WrByte(dev, 0x003f, 0x46); /* Sets the light and dark gain (upper nibble). Dark gain should not be changed.*/
+ VL6180_WrByte(dev, 0x0031, 0xFF); /* sets the # of range measurements after which auto calibration of system is performed */
+ VL6180_WrByte(dev, 0x002e, 0x01); /* perform a single temperature calibration of the ranging sensor */
+
+ /* Optional: Public registers - See data sheet for more detail */
+ VL6180_WrByte(dev, 0x001b, 0x09); /* Set default ranging inter-measurement period to 100ms */
+ VL6180_WrByte(dev, 0x0014, 0x24); /* Configures interrupt on New sample ready */
+
+printf("VL6180_RangeSetMaxConvergenceTime \n");
+ status = VL6180_RangeSetMaxConvergenceTime(dev, 50); /* Calculate ece value on initialization (use max conv) */
+ LOG_FUNCTION_END(status);
+
+ return status;
+}
+
+#if VL6180_UPSCALE_SUPPORT != 1
+
+static int _UpscaleInitPatch0(VL6180Dev_t dev)
+{
+ int status;
+ uint32_t CalValue = 0;
+ CalValue = VL6180DevDataGet(dev, Part2PartAmbNVM);
+ status = VL6180_WrDWord(dev, 0xDA, CalValue);
+ return status;
+}
+
+/* only include up-scaling register setting when up-scale support is configured in */
+int VL6180_UpscaleRegInit(VL6180Dev_t dev)
+{
+
+ /* apply REGISTER_TUNING_ER02_100614_CustomerView.txt */
+ VL6180_WrByte(dev, 0x0207, 0x01);
+ VL6180_WrByte(dev, 0x0208, 0x01);
+ VL6180_WrByte(dev, 0x0096, 0x00);
+ VL6180_WrByte(dev, 0x0097, 0x54);
+ VL6180_WrByte(dev, 0x00e3, 0x00);
+ VL6180_WrByte(dev, 0x00e4, 0x04);
+ VL6180_WrByte(dev, 0x00e5, 0x02);
+ VL6180_WrByte(dev, 0x00e6, 0x01);
+ VL6180_WrByte(dev, 0x00e7, 0x03);
+ VL6180_WrByte(dev, 0x00f5, 0x02);
+ VL6180_WrByte(dev, 0x00d9, 0x05);
+
+ _UpscaleInitPatch0(dev);
+
+
+ VL6180_WrByte(dev, 0x009f, 0x00);
+ VL6180_WrByte(dev, 0x00a3, 0x28);
+ VL6180_WrByte(dev, 0x00b7, 0x00);
+ VL6180_WrByte(dev, 0x00bb, 0x28);
+ VL6180_WrByte(dev, 0x00b2, 0x09);
+ VL6180_WrByte(dev, 0x00ca, 0x09);
+ VL6180_WrByte(dev, 0x0198, 0x01);
+ VL6180_WrByte(dev, 0x01b0, 0x17);
+ VL6180_WrByte(dev, 0x01ad, 0x00);
+ VL6180_WrByte(dev, 0x00ff, 0x05);
+ VL6180_WrByte(dev, 0x0100, 0x05);
+ VL6180_WrByte(dev, 0x0199, 0x05);
+ VL6180_WrByte(dev, 0x01a6, 0x1b);
+ VL6180_WrByte(dev, 0x01ac, 0x3e);
+ VL6180_WrByte(dev, 0x01a7, 0x1f);
+ VL6180_WrByte(dev, 0x0030, 0x00);
+ VL6180_WrByte(dev, 0x0011, 0x10);
+ VL6180_WrByte(dev, 0x010a, 0x30);
+ VL6180_WrByte(dev, 0x003f, 0x46);
+ VL6180_WrByte(dev, 0x0031, 0xFF);
+ VL6180_WrByte(dev, 0x0040, 0x63);
+ VL6180_WrByte(dev, 0x002e, 0x01);
+ VL6180_WrByte(dev, 0x002c, 0xff);
+ VL6180_WrByte(dev, 0x001b, 0x09);
+ VL6180_WrByte(dev, 0x003e, 0x31);
+ VL6180_WrByte(dev, 0x0014, 0x24);
+#if VL6180_EXTENDED_RANGE
+ VL6180_RangeSetMaxConvergenceTime(dev, 63);
+#else
+ VL6180_RangeSetMaxConvergenceTime(dev, 50);
+#endif
+
+ return 0;
+}
+#else
+#define VL6180_UpscaleRegInit(...) -1
+#endif
+
+int VL6180_UpscaleSetScaling(VL6180Dev_t dev, uint8_t scaling)
+{
+ int status;
+ uint16_t Scaler;
+ uint16_t ValidHeight;
+ int8_t Offset;
+
+ LOG_FUNCTION_START("%d", (int)scaling);
+
+#ifdef VL6180_HAVE_UPSCALE_DATA
+ #define min_scaling 1
+ #define max_scaling (sizeof(ScalerLookUP) / sizeof(ScalerLookUP[0]))
+#else
+ /* we are in fixed config so only allow configured factor */
+ #define min_scaling VL6180_UPSCALE_SUPPORT
+ #define max_scaling VL6180_UPSCALE_SUPPORT
+#endif
+
+ if (scaling >= min_scaling && scaling <= max_scaling) {
+
+ Scaler = ScalerLookUP[scaling - 1];
+ status = VL6180_WrWord(dev, RANGE_SCALER, Scaler);
+ _SetUpscale(dev, scaling);
+
+ /* Apply scaling on part-2-part offset */
+ Offset = VL6180DevDataGet(dev, Part2PartOffsetNVM) / scaling;
+ status = VL6180_WrByte(dev, SYSRANGE_PART_TO_PART_RANGE_OFFSET, Offset);
+
+ /* Apply scaling on CrossTalkValidHeight */
+ if (status == 0) {
+ status = VL6180_WrByte(dev, SYSRANGE_CROSSTALK_VALID_HEIGHT,
+ DEF_CROSS_TALK_VALID_HEIGHT_VALUE / scaling);
+ }
+ /* Apply scaling on RangeIgnore ValidHeight if enabled */
+ if( status == 0){
+ if( VL6180DevDataGet(dev, RangeIgnore.Enabled) !=0 ){
+ ValidHeight = VL6180DevDataGet(dev, RangeIgnore.ValidHeight);
+ ValidHeight /= _GetUpscale(dev);
+ if( ValidHeight > 255 )
+ ValidHeight = 255;
+
+ status = VL6180_WrByte(dev, SYSRANGE_RANGE_IGNORE_VALID_HEIGHT,
+ (uint8_t)(ValidHeight & 0xFF) );
+ }
+ }
+
+#if !VL6180_EXTENDED_RANGE
+ if (status == 0) {
+ status = VL6180_RangeSetEceState(dev, scaling == 1); /* enable ece only at 1x scaling */
+ }
+ if (status == 0 && !VL6180_EXTENDED_RANGE && scaling != 1) {
+ status = NOT_GUARANTEED ;
+ }
+#endif
+ } else {
+ status = INVALID_PARAMS;
+ }
+#undef min_scaling
+#undef max_scaling
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+
+int VL6180_UpscaleGetScaling(VL6180Dev_t dev)
+{
+ int status;
+ LOG_FUNCTION_START("");
+ status = _GetUpscale(dev);
+ LOG_FUNCTION_END(status);
+
+ return status;
+}
+
+
+static int VL6180_UpscaleStaticInit(VL6180Dev_t dev)
+{
+ /* todo make these a fail macro in case only 1x is suppoted */
+ int status;
+
+ LOG_FUNCTION_START("");
+ do {
+ status = VL6180_UpscaleRegInit(dev);
+ if (status) {
+ VL6180_ErrLog("regInit fail");
+ break;
+ }
+#if VL6180_EXTENDED_RANGE
+ status = VL6180_RangeSetEceState(dev, 0);
+ if (status) {
+ VL6180_ErrLog("VL6180_RangeSetEceState fail");
+ break;
+ }
+#endif
+ } while (0);
+ if (!status) {
+ /* must write the scaler at least once to the device to ensure the scaler is in a known state. */
+ status = VL6180_UpscaleSetScaling(dev, _GetUpscale(dev));
+ VL6180_WrByte(dev, 0x016, 0x00); /* change fresh out of set status to 0 */
+ }
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+
+int VL6180_SetGPIOxPolarity(VL6180Dev_t dev, int pin, int active_high)
+{
+ int status;
+ LOG_FUNCTION_START("%d %d", (int) pin, (int)active_high);
+
+ if (pin == 0 || pin == 1) {
+ uint16_t RegIndex;
+ uint8_t DataSet;
+ if (pin == 0)
+ RegIndex = SYSTEM_MODE_GPIO0;
+ else
+ RegIndex = SYSTEM_MODE_GPIO1;
+
+ if (active_high)
+ DataSet = GPIOx_POLARITY_SELECT_MASK;
+ else
+ DataSet = 0;
+
+ status = VL6180_UpdateByte(dev, RegIndex, (uint8_t)~GPIOx_POLARITY_SELECT_MASK, DataSet);
+ } else {
+ VL6180_ErrLog("Invalid pin param %d", (int)pin);
+ status = INVALID_PARAMS;
+ }
+
+ LOG_FUNCTION_END(status);
+
+ return status;
+}
+
+int VL6180_SetGPIOxFunctionality(VL6180Dev_t dev, int pin, uint8_t functionality)
+{
+ int status;
+
+ LOG_FUNCTION_START("%d %d", (int) pin, (int)functionality);
+
+ if (((pin == 0) || (pin == 1)) && IsValidGPIOFunction(functionality)) {
+ uint16_t RegIndex;
+
+ if (pin == 0)
+ RegIndex = SYSTEM_MODE_GPIO0;
+ else
+ RegIndex = SYSTEM_MODE_GPIO1;
+
+ status = VL6180_UpdateByte(dev, RegIndex, (uint8_t)~GPIOx_FUNCTIONALITY_SELECT_MASK,
+ functionality << GPIOx_FUNCTIONALITY_SELECT_SHIFT);
+ if (status) {
+ VL6180_ErrLog("Update SYSTEM_MODE_GPIO%d fail", (int)pin);
+ }
+ } else {
+ VL6180_ErrLog("Invalid pin %d or function %d", (int)pin, (int)functionality);
+ status = INVALID_PARAMS;
+ }
+
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+
+int VL6180_SetupGPIOx(VL6180Dev_t dev, int pin, uint8_t IntFunction, int ActiveHigh)
+{
+ int status;
+
+ LOG_FUNCTION_START("%d %d", (int) pin, (int)IntFunction);
+
+ if (((pin == 0) || (pin == 1)) && IsValidGPIOFunction(IntFunction)) {
+ uint16_t RegIndex;
+ uint8_t value = 0;
+
+ if (pin == 0)
+ RegIndex = SYSTEM_MODE_GPIO0;
+ else
+ RegIndex = SYSTEM_MODE_GPIO1;
+
+ if (ActiveHigh)
+ value |= GPIOx_POLARITY_SELECT_MASK;
+
+ value |= IntFunction << GPIOx_FUNCTIONALITY_SELECT_SHIFT;
+ status = VL6180_WrByte(dev, RegIndex, value);
+ if (status) {
+ VL6180_ErrLog("SYSTEM_MODE_GPIO%d wr fail", (int)pin-SYSTEM_MODE_GPIO0);
+ }
+ } else {
+ VL6180_ErrLog("Invalid pin %d or function %d", (int)pin, (int) IntFunction);
+ status = INVALID_PARAMS;
+ }
+
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+
+int VL6180_DisableGPIOxOut(VL6180Dev_t dev, int pin)
+{
+ int status;
+
+ LOG_FUNCTION_START("%d", (int)pin);
+
+ status = VL6180_SetGPIOxFunctionality(dev, pin, GPIOx_SELECT_OFF);
+
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+
+int VL6180_SetupGPIO1(VL6180Dev_t dev, uint8_t IntFunction, int ActiveHigh)
+{
+ int status;
+ LOG_FUNCTION_START("%d %d", (int)IntFunction, (int)ActiveHigh);
+ status = VL6180_SetupGPIOx(dev, 1, IntFunction, ActiveHigh);
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+int VL6180_RangeConfigInterrupt(VL6180Dev_t dev, uint8_t ConfigGpioInt)
+{
+ int status;
+
+ if (ConfigGpioInt <= CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY) {
+ status = VL6180_UpdateByte(dev, SYSTEM_INTERRUPT_CONFIG_GPIO,
+ (uint8_t)(~CONFIG_GPIO_RANGE_MASK),
+ ConfigGpioInt);
+ } else {
+ VL6180_ErrLog("Invalid config mode param %d", (int)ConfigGpioInt);
+ status = INVALID_PARAMS;
+ }
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+
+int VL6180_RangeSetEceFactor(VL6180Dev_t dev, uint16_t FactorM, uint16_t FactorD)
+{
+ int status;
+ uint8_t u8;
+
+ LOG_FUNCTION_START("%d %d", (int)FactorM, (int)FactorD);
+ do {
+ /* D cannot be 0 M must be <=D and >= 0 */
+ if (FactorM <= FactorD && FactorD > 0) {
+ VL6180DevDataSet(dev, EceFactorM, FactorM);
+ VL6180DevDataSet(dev, EceFactorD, FactorD);
+ /* read and re-apply max conv time to get new ece factor set */
+ status = VL6180_RdByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, &u8);
+ if (status) {
+ VL6180_ErrLog("SYSRANGE_MAX_CONVERGENCE_TIME rd fail ");
+ break;
+ }
+ status = VL6180_RangeSetMaxConvergenceTime(dev, u8);
+ if (status < 0) {
+ VL6180_ErrLog("fail to apply time after ece m/d change");
+ break;
+ }
+ } else {
+ VL6180_ErrLog("invalid factor %d/%d", (int)FactorM, (int)FactorD);
+ status = INVALID_PARAMS;
+ }
+ } while (0);
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+int VL6180_RangeSetEceState(VL6180Dev_t dev, int enable)
+{
+ int status;
+ uint8_t or_mask;
+
+ LOG_FUNCTION_START("%d", (int)enable);
+ if (enable)
+ or_mask = RANGE_CHECK_ECE_ENABLE_MASK;
+ else
+ or_mask = 0;
+
+ status = VL6180_UpdateByte(dev, SYSRANGE_RANGE_CHECK_ENABLES, ~RANGE_CHECK_ECE_ENABLE_MASK, or_mask);
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+
+int VL6180_RangeSetMaxConvergenceTime(VL6180Dev_t dev, uint8_t MaxConTime_msec)
+{
+ int status = 0;
+ LOG_FUNCTION_START("%d", (int)MaxConTime_msec);
+ do {
+ status = VL6180_WrByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, MaxConTime_msec);
+ if (status) {
+ break;
+ }
+ status = VL6180_RangeSetEarlyConvergenceEestimateThreshold(dev);
+ if (status) {
+ break;
+ }
+ status = _DMax_InitData(dev);
+ } while (0);
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+int VL6180_RangeSetInterMeasPeriod(VL6180Dev_t dev, uint32_t InterMeasTime_msec)
+{
+ uint8_t SetTime;
+ int status;
+
+ LOG_FUNCTION_START("%d", (int)InterMeasTime_msec);
+ do {
+ if (InterMeasTime_msec > 2550) {
+ status = INVALID_PARAMS;
+ break;
+ }
+ /* doc in not 100% clear and confusing about the limit practically all value are OK but 0
+ * that can hang device in continuous mode */
+ if (InterMeasTime_msec < 10) {
+ InterMeasTime_msec = 10;
+ }
+ SetTime = (uint8_t)(InterMeasTime_msec / 10);
+ status = VL6180_WrByte(dev, SYSRANGE_INTERMEASUREMENT_PERIOD, SetTime);
+ if (status) {
+ VL6180_ErrLog("SYSRANGE_INTERMEASUREMENT_PERIOD wr fail");
+ } else if (SetTime != InterMeasTime_msec / 10) {
+ status = MIN_CLIPED; /* on success change status to clip if it did */
+ }
+ } while (0);
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+
+int VL6180_RangeGetDeviceReady(VL6180Dev_t dev, int *Ready)
+{
+ int status;
+ uint8_t u8;
+ LOG_FUNCTION_START("%p", (int)Ready);
+ status = VL6180_RdByte(dev, RESULT_RANGE_STATUS, &u8);
+ if (!status)
+ *Ready = u8&RANGE_DEVICE_READY_MASK;
+ LOG_FUNCTION_END_FMT(status, "%d", *Ready);
+ return status;
+}
+
+
+int VL6180_RangeWaitDeviceReady(VL6180Dev_t dev, int MaxLoop)
+{
+ int status = 0; /* if user specify an invalid <=0 loop count we'll return error */
+ int n;
+ uint8_t u8;
+ LOG_FUNCTION_START("%d", (int)MaxLoop);
+ if (MaxLoop < 1) {
+ status = INVALID_PARAMS;
+ } else {
+ for (n = 0; n < MaxLoop ; n++) {
+ status = VL6180_RdByte(dev, RESULT_RANGE_STATUS, &u8);
+ if (status)
+ break;
+ u8 = u8 & RANGE_DEVICE_READY_MASK;
+ if (u8)
+ break;
+
+ }
+ if (!status && !u8) {
+ status = TIME_OUT;
+ }
+ }
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+int VL6180_RangeSetSystemMode(VL6180Dev_t dev, uint8_t mode)
+{
+ int status;
+ LOG_FUNCTION_START("%d", (int)mode);
+ /* FIXME we are not checking device is ready via @a VL6180_RangeWaitDeviceReady
+ * so if called back to back real fast we are not checking
+ * if previous mode "set" got absorbed => bit 0 must be 0 so that it work
+ */
+ if (mode <= 3) {
+ status = VL6180_WrByte(dev, SYSRANGE_START, mode);
+ if (status) {
+ VL6180_ErrLog("SYSRANGE_START wr fail");
+ }
+ } else {
+ status = INVALID_PARAMS;
+ }
+
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+
+int VL6180_RangeStartContinuousMode(VL6180Dev_t dev)
+{
+ int status;
+ LOG_FUNCTION_START("");
+ status = VL6180_RangeSetSystemMode(dev, MODE_START_STOP | MODE_CONTINUOUS);
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+int VL6180_RangeStartSingleShot(VL6180Dev_t dev)
+{
+ int status;
+ LOG_FUNCTION_START("");
+ status = VL6180_RangeSetSystemMode(dev, MODE_START_STOP | MODE_SINGLESHOT);
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+
+static int VL6180_RangeSetEarlyConvergenceEestimateThreshold(VL6180Dev_t dev)
+{
+ int status;
+
+ const uint32_t cMicroSecPerMilliSec = 1000;
+ const uint32_t cEceSampleTime_us = 500;
+ uint32_t ece_factor_m = VL6180DevDataGet(dev, EceFactorM);
+ uint32_t ece_factor_d = VL6180DevDataGet(dev, EceFactorD);
+ uint32_t convergTime_us;
+ uint32_t fineThresh;
+ uint32_t eceThresh;
+ uint8_t u8;
+ uint32_t maxConv_ms;
+ int32_t AveTime;
+
+ LOG_FUNCTION_START("");
+
+ do {
+ status = VL6180_RdByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, &u8);
+ if (status) {
+ VL6180_ErrLog("SYSRANGE_MAX_CONVERGENCE_TIME rd fail");
+ break;
+ }
+ maxConv_ms = u8;
+ AveTime = _GetAveTotalTime(dev);
+ if (AveTime < 0) {
+ status = -1;
+ break;
+ }
+
+ convergTime_us = maxConv_ms * cMicroSecPerMilliSec - AveTime;
+ status = VL6180_RdDWord(dev, 0xB8, &fineThresh);
+ if (status) {
+ VL6180_ErrLog("reg 0xB8 rd fail");
+ break;
+ }
+ fineThresh *= 256;
+ eceThresh = ece_factor_m * cEceSampleTime_us * fineThresh / (convergTime_us * ece_factor_d);
+
+ status = VL6180_WrWord(dev, SYSRANGE_EARLY_CONVERGENCE_ESTIMATE, (uint16_t)eceThresh);
+ } while (0);
+
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+
+static int _RangeIgnore_UpdateDevice(VL6180Dev_t dev){
+ int status;
+ int enable;
+ int threshold;
+ int range;
+ int or_mask;
+ enable= VL6180DevDataGet(dev, RangeIgnore.Enabled);
+ if( enable ){
+ // if to be nabled program first range value and threshold
+ range = VL6180DevDataGet(dev, RangeIgnore.ValidHeight);
+ range /= _GetUpscale(dev);
+ if( range > 255 )
+ range = 255;
+
+ status = VL6180_WrByte(dev, SYSRANGE_RANGE_IGNORE_VALID_HEIGHT, range);
+ if( status ){
+ goto done;
+ }
+
+ threshold = VL6180DevDataGet(dev, RangeIgnore.IgnoreThreshold);
+ status = VL6180_WrWord(dev, SYSRANGE_RANGE_IGNORE_THRESHOLD, threshold);
+ if( status ){
+ goto done;
+ }
+ or_mask = RANGE_CHECK_RANGE_ENABLE_MASK;
+ }
+ else{
+ or_mask = 0;
+ }
+ status = VL6180_UpdateByte(dev, SYSRANGE_RANGE_CHECK_ENABLES, ~RANGE_CHECK_RANGE_ENABLE_MASK, or_mask);
+ _DMax_InitData(dev);
+done:
+ return status;
+}
+
+int VL6180_RangeIgnoreSetEnable(VL6180Dev_t dev, int EnableState){
+ int CurEnable;
+ int status=0;
+ LOG_FUNCTION_START("enable %d", EnableState);
+
+ if( EnableState )
+ EnableState = 1;
+
+ CurEnable = VL6180DevDataGet(dev, RangeIgnore.Enabled);
+ if( EnableState != CurEnable ){
+ VL6180DevDataSet(dev, RangeIgnore.Enabled, EnableState);
+ status = _RangeIgnore_UpdateDevice(dev);
+ }
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+int VL6180_RangeIgnoreConfigure(VL6180Dev_t dev, uint16_t ValidHeight_mm, uint16_t IgnoreThreshold){
+ int status;
+ int enabled;
+
+ LOG_FUNCTION_START("height= %d Threshold=%d", (int)ValidHeight_mm, (int)Threshold);
+
+ enabled = VL6180DevDataGet(dev, RangeIgnore.Enabled);
+ VL6180DevDataSet(dev, RangeIgnore.ValidHeight, ValidHeight_mm);
+ VL6180DevDataSet(dev, RangeIgnore.IgnoreThreshold, IgnoreThreshold);
+ if( enabled ){
+ status = _RangeIgnore_UpdateDevice(dev);
+ }
+ else{
+ status = 0;
+ }
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+/*
+ * Return >0 = time
+ * <0 1 if fail to get read data from device to compute time
+ */
+static int32_t _GetAveTotalTime(VL6180Dev_t dev)
+{
+ uint32_t cFwOverhead_us = 24;
+ uint32_t cVcpSetupTime_us = 70;
+ uint32_t cPLL2_StartupDelay_us = 200;
+ uint8_t cMeasMask = 0x07;
+ uint32_t Samples;
+ uint32_t SamplePeriod;
+ uint32_t SingleTime_us;
+ int32_t TotalAveTime_us;
+ uint8_t u8;
+ int status;
+
+ LOG_FUNCTION_START("");
+
+ status = VL6180_RdByte(dev, 0x109, &u8);
+ if (status) {
+ VL6180_ErrLog("rd 0x109 fail");
+ return -1;
+ }
+ Samples = u8 & cMeasMask;
+ status = VL6180_RdByte(dev, READOUT_AVERAGING_SAMPLE_PERIOD, &u8);
+ if (status) {
+ VL6180_ErrLog("i2c READOUT_AVERAGING_SAMPLE_PERIOD fail");
+ return -1;
+ }
+ SamplePeriod = u8;
+ SingleTime_us = cFwOverhead_us + cVcpSetupTime_us + (SamplePeriod * 10);
+ TotalAveTime_us = (Samples + 1) * SingleTime_us + cPLL2_StartupDelay_us;
+
+ LOG_FUNCTION_END(TotalAveTime_us);
+ return TotalAveTime_us;
+}
+
+#if VL6180_HAVE_DMAX_RANGING
+#define _GetDMaxDataRetSignalAt400mm(dev) VL6180DevDataGet(dev, DMaxData.retSignalAt400mm)
+#else
+#define _GetDMaxDataRetSignalAt400mm(dev) 375 /* Use a default high value */
+#endif
+
+
+#if VL6180_WRAP_AROUND_FILTER_SUPPORT
+
+#define PRESERVE_DEVICE_ERROR_CODE /* If uncommented, device error code will be preserved on top of wraparound error code, but this may lead to some error code instability like overflow error <==> RangingFilteringOnGoing error oscillations */
+#define SENSITIVE_FILTERING_ON_GOING /* If uncommented, filter will go back to RangingFilteringOnGoing if it must go through the std dev testing */
+
+#define FILTER_STDDEV_SAMPLES 6
+#define MIN_FILTER_STDDEV_SAMPLES 3
+#define MIN_FILTER_STDDEV_SAMPLES_AFTER_FLUSH_OR_BYPASS 5
+#define STDDEV_BASE_VALUE 150
+
+#define FILTER_INVALID_DISTANCE 65535
+
+#define _FilterData(field) VL6180DevDataGet(dev, FilterData.field)
+/*
+ * One time init
+ */
+int _filter_Init(VL6180Dev_t dev)
+{
+ int i;
+ _FilterData(MeasurementIndex) = 0;
+
+ _FilterData(Default_ZeroVal) = 0;
+ _FilterData(Default_VAVGVal) = 0;
+ _FilterData(NoDelay_ZeroVal) = 0;
+ _FilterData(NoDelay_VAVGVal) = 0;
+ _FilterData(Previous_VAVGDiff) = 0;
+
+ _FilterData(StdFilteredReads) = 0;
+ _FilterData(FilteringOnGoingConsecutiveStates) = 0;
+
+ for (i = 0; i < FILTER_NBOF_SAMPLES; i++) {
+ _FilterData(LastTrueRange)[i] = FILTER_INVALID_DISTANCE;
+ _FilterData(LastReturnRates)[i] = 0;
+ }
+ _FilterData(MeasurementsSinceLastFlush)=0;
+ return 0;
+}
+
+
+static uint32_t _filter_StdDevDamper(uint32_t AmbientRate,
+ uint32_t SignalRate,
+ const uint32_t StdDevLimitLowLight,
+ const uint32_t StdDevLimitLowLightSNR,
+ const uint32_t StdDevLimitHighLight,
+ const uint32_t StdDevLimitHighLightSNR)
+{
+ uint32_t newStdDev;
+ uint16_t SNR;
+
+ if (AmbientRate > 0)
+ SNR = (uint16_t) ((100 * SignalRate) / AmbientRate);
+ else
+ SNR = 9999;
+
+ if (SNR >= StdDevLimitLowLightSNR) {
+ newStdDev = StdDevLimitLowLight;
+ } else {
+ if (SNR <= StdDevLimitHighLightSNR)
+ newStdDev = StdDevLimitHighLight;
+ else {
+ newStdDev = (uint32_t)(StdDevLimitHighLight -
+ (SNR - StdDevLimitHighLightSNR) *
+ (StdDevLimitHighLight - StdDevLimitLowLight) /
+ (StdDevLimitLowLightSNR - StdDevLimitHighLightSNR));
+ }
+ }
+
+ return newStdDev;
+}
+
+
+/*
+ * Return <0 on error
+ */
+static int32_t _filter_Start(VL6180Dev_t dev,
+ uint16_t m_trueRange_mm,
+ uint16_t m_rawRange_mm,
+ uint32_t m_rtnSignalRate,
+ uint32_t m_rtnAmbientRate,
+ uint16_t errorCode)
+{
+ int status;
+ uint16_t m_newTrueRange_mm = 0;
+ #if VL6180_HAVE_MULTI_READ
+ uint8_t MultiReadBuf[8];
+ #endif
+ uint16_t i;
+ uint16_t bypassFilter = 0;
+ uint16_t resetVAVGData = 1;
+
+ uint16_t filterErrorCode = NoError;
+ uint16_t filterErrorCodeOnRangingErrorCode = NoError;
+
+ uint16_t registerValue;
+
+ uint32_t register32BitsValue1;
+ uint32_t register32BitsValue2;
+
+ uint16_t ValidDistance = 0;
+ uint16_t SuspicuousRangingZone = 0;
+
+ uint16_t WrapAroundFlag = 0;
+ uint16_t NoWrapAroundFlag = 0;
+ uint16_t NoWrapAroundHighConfidenceFlag = 0;
+
+ uint16_t FlushFilter = 0;
+ uint32_t RateChange = 0;
+
+ uint16_t StdDevSamplesMinNeeded = 0;
+ uint16_t StdDevSamples = 0;
+ uint32_t StdDevDistanceSum = 0;
+ uint32_t StdDevDistanceMean = 0;
+ uint32_t StdDevDistance = 0;
+ uint32_t StdDevRateSum = 0;
+ uint32_t StdDevRateMean = 0;
+ uint32_t StdDevRate = 0;
+ uint32_t StdDevLimitWithTargetMove = 0;
+
+ uint32_t VAVGDiff;
+ uint32_t IdealVAVGDiff;
+ uint32_t MinVAVGDiff;
+ uint32_t MaxVAVGDiff;
+
+ /* Filter Parameters */
+ static const uint16_t ROMABLE_DATA WrapAroundLowRawRangeLimit = 60;
+ static const uint32_t ROMABLE_DATA WrapAroundLowReturnRateLimit_ROM = 800;
+ /* Shall be adapted depending on crossTalk */
+ static const uint16_t ROMABLE_DATA WrapAroundLowRawRangeLimit2 = 165;
+ static const uint32_t ROMABLE_DATA WrapAroundLowReturnRateLimit2_ROM = 180;
+ /* Shall be adapted depending on crossTalk and device sensitivity*/
+ static const uint32_t ROMABLE_DATA WrapAroundLowRawRangeLimit2SuspicuousAddedSignalRate = 150;
+
+
+ static const uint32_t ROMABLE_DATA WrapAroundLowReturnRateFilterLimit_ROM = 850;
+ /* Shall be adapted depending on crossTalk and device sensitivity*/
+ static const uint16_t ROMABLE_DATA WrapAroundHighRawRangeFilterLimit = 350;
+ static const uint32_t ROMABLE_DATA WrapAroundHighReturnRateFilterLimit_ROM = 1400;
+ /* Shall be adapted depending on crossTalk and device sensitivity*/
+
+ static const uint32_t ROMABLE_DATA WrapAroundMaximumAmbientRateFilterLimit = 15000;
+
+ /* Temporal filter data and flush values */
+ static const uint32_t ROMABLE_DATA MinReturnRateFilterFlush = 75;
+ static const uint32_t ROMABLE_DATA MaxReturnRateChangeFilterFlush = 50;
+
+ /* STDDEV values and damper values */
+ static const uint32_t ROMABLE_DATA StdDevLimitLowLight = STDDEV_BASE_VALUE;
+ static const uint32_t ROMABLE_DATA StdDevLimitLowLightSNR = 30; /* 0.3 */
+ static const uint32_t ROMABLE_DATA StdDevLimitHighLight = STDDEV_BASE_VALUE*6;
+ static const uint32_t ROMABLE_DATA StdDevLimitHighLightSNR = 5; /* 0.05 */
+
+ static const uint32_t ROMABLE_DATA StdDevHighConfidenceSNRLimit = 8;
+ static const uint32_t ROMABLE_DATA StdDevNoWrapDetectedMultiplier = 4;
+
+ static const uint32_t ROMABLE_DATA StdDevMovingTargetStdDevLimit = 90000;
+
+ static const uint32_t ROMABLE_DATA StdDevMovingTargetReturnRateLimit = 3500;
+ static const uint32_t ROMABLE_DATA StdDevMovingTargetStdDevForReturnRateLimit = STDDEV_BASE_VALUE*25;
+
+ static const uint32_t ROMABLE_DATA MAX_VAVGDiff_ROM = 1800;
+ static const uint32_t ROMABLE_DATA SuspicuousMAX_VAVGDiffRatio = 2;
+
+ /* WrapAroundDetection variables */
+ static const uint16_t ROMABLE_DATA WrapAroundNoDelayCheckPeriod = 2;
+ static const uint16_t ROMABLE_DATA StdFilteredReadsIncrement = 2;
+ static const uint16_t ROMABLE_DATA StdFilteredReadsDecrement = 1;
+ static const uint16_t ROMABLE_DATA StdMaxFilteredReads = 4;
+
+ uint32_t SignalRateDMax;
+ uint32_t WrapAroundLowReturnRateLimit;
+ uint32_t WrapAroundLowReturnRateLimit2;
+ uint32_t WrapAroundLowReturnRateFilterLimit;
+ uint32_t WrapAroundHighReturnRateFilterLimit;
+
+ uint32_t MAX_VAVGDiff = 1800;
+
+ uint8_t u8;//, u8_2;
+ uint32_t XTalkCompRate_KCps;
+ uint32_t StdDevLimit = 300;
+ uint32_t MaxOrInvalidDistance = 255*_GetUpscale(dev);
+ /* #define MaxOrInvalidDistance (uint16_t) (255 * 3) */
+
+ /* Check if distance is Valid or not */
+ switch (errorCode) {
+ case Raw_Ranging_Algo_Underflow:
+ case Ranging_Algo_Underflow:
+ filterErrorCodeOnRangingErrorCode = RangingFiltered; /* If we have to go through filter, mean we have here a wraparound case */
+ ValidDistance = 0;
+ break;
+ case Raw_Ranging_Algo_Overflow:
+ case Ranging_Algo_Overflow:
+ filterErrorCodeOnRangingErrorCode = RangingFiltered; /* If we have to go through filter, mean we have here a wraparound case */
+ //m_trueRange_mm = MaxOrInvalidDistance;
+ m_trueRange_mm = 200*_GetUpscale(dev);
+ ValidDistance = 1;
+ break;
+ default:
+ if (m_rawRange_mm >= MaxOrInvalidDistance) {
+ ValidDistance = 0;
+ bypassFilter = 1; /* Bypass the filter in this case as produced distance is not usable (and also the VAVGVal and ZeroVal values) */
+ } else {
+ ValidDistance = 1;
+ }
+ break;
+ }
+ m_newTrueRange_mm = m_trueRange_mm;
+
+ XTalkCompRate_KCps = VL6180DevDataGet(dev, XTalkCompRate_KCps);
+
+ /* Update signal rate limits depending on crosstalk */
+ SignalRateDMax = (uint32_t)_GetDMaxDataRetSignalAt400mm(dev) ;
+ WrapAroundLowReturnRateLimit = WrapAroundLowReturnRateLimit_ROM + XTalkCompRate_KCps;
+ WrapAroundLowReturnRateLimit2 = ((WrapAroundLowReturnRateLimit2_ROM *
+ SignalRateDMax) / 312) +
+ XTalkCompRate_KCps;
+ WrapAroundLowReturnRateFilterLimit = ((WrapAroundLowReturnRateFilterLimit_ROM *
+ SignalRateDMax) / 312) + XTalkCompRate_KCps;
+ WrapAroundHighReturnRateFilterLimit = ((WrapAroundHighReturnRateFilterLimit_ROM *
+ SignalRateDMax) / 312) + XTalkCompRate_KCps;
+
+
+ /* Checks on low range data */
+ if ((m_rawRange_mm < WrapAroundLowRawRangeLimit) && (m_rtnSignalRate < WrapAroundLowReturnRateLimit)) {
+ filterErrorCode = RangingFiltered; /* On this condition, wraparound case is ensured */
+ bypassFilter = 1;
+ }
+ if ((m_rawRange_mm < WrapAroundLowRawRangeLimit2) && (m_rtnSignalRate < WrapAroundLowReturnRateLimit2)) {
+ filterErrorCode = RangingFiltered; /* On this condition, wraparound case is ensured */
+ bypassFilter = 1;
+ }
+ if ((m_rawRange_mm < WrapAroundLowRawRangeLimit2) && (m_rtnSignalRate < (WrapAroundLowReturnRateLimit2 + WrapAroundLowRawRangeLimit2SuspicuousAddedSignalRate))) {
+ SuspicuousRangingZone = 1; /* On this area, we are in an highly suspicuous wraparound ares, filter parameter will be stengthen */
+ }
+
+
+ /* Checks on Ambient rate level */
+ if (m_rtnAmbientRate > WrapAroundMaximumAmbientRateFilterLimit) {
+ /* Too high ambient rate */
+ FlushFilter = 1;
+ bypassFilter = 1;
+ }
+
+ /* Checks on Filter flush */
+ if (m_rtnSignalRate < MinReturnRateFilterFlush) {
+ /* Completely lost target, so flush the filter */
+ FlushFilter = 1;
+ bypassFilter = 1;
+ }
+ if (_FilterData(LastReturnRates)[0] != 0) {
+ if (m_rtnSignalRate > _FilterData(LastReturnRates)[0])
+ RateChange = (100 *
+ (m_rtnSignalRate - _FilterData(LastReturnRates)[0])) /
+ _FilterData(LastReturnRates)[0];
+ else
+ RateChange = (100 *
+ (_FilterData(LastReturnRates)[0] - m_rtnSignalRate)) /
+ _FilterData(LastReturnRates)[0];
+ } else
+ RateChange = 0;
+ if (RateChange > MaxReturnRateChangeFilterFlush) {
+ FlushFilter = 1;
+ }
+ /* TODO optimize filter using circular buffer */
+ if (FlushFilter == 1) {
+ _FilterData(MeasurementIndex) = 0;
+ for (i = 0; i < FILTER_NBOF_SAMPLES; i++) {
+ _FilterData(LastTrueRange)[i] = FILTER_INVALID_DISTANCE;
+ _FilterData(LastReturnRates)[i] = 0;
+ }
+ _FilterData(MeasurementsSinceLastFlush)=0;
+ } else {
+ for (i = (uint16_t) (FILTER_NBOF_SAMPLES - 1); i > 0; i--) {
+ _FilterData(LastTrueRange)[i] = _FilterData(LastTrueRange)[i - 1];
+ _FilterData(LastReturnRates)[i] = _FilterData(LastReturnRates)[i - 1];
+ }
+ }
+
+ if (ValidDistance == 1)
+ _FilterData(LastTrueRange)[0] = m_trueRange_mm;
+ else
+ _FilterData(LastTrueRange)[0] = FILTER_INVALID_DISTANCE;
+ _FilterData(LastReturnRates)[0] = m_rtnSignalRate;
+ _FilterData(MeasurementsSinceLastFlush)++;
+
+ /* Check if we need to go through the filter or not */
+ if (!(((m_rawRange_mm < WrapAroundHighRawRangeFilterLimit) &&
+ (m_rtnSignalRate < WrapAroundLowReturnRateFilterLimit)) ||
+ ((m_rawRange_mm >= WrapAroundHighRawRangeFilterLimit) &&
+ (m_rtnSignalRate < WrapAroundHighReturnRateFilterLimit))))
+ bypassFilter = 1;
+ else {
+ /* if some wraparound filtering due to some ranging error code has been detected, update the filter status and bypass the filter */
+ if(filterErrorCodeOnRangingErrorCode!=NoError){
+#ifndef PRESERVE_DEVICE_ERROR_CODE
+ filterErrorCode = filterErrorCodeOnRangingErrorCode;
+#else
+ if((errorCode==Raw_Ranging_Algo_Underflow) || (errorCode==Ranging_Algo_Underflow)) {
+ /* Preserves the error codes except for Raw_Ranging_Algo_Underflow and Ranging_Algo_Underflow */
+ filterErrorCode = filterErrorCodeOnRangingErrorCode;
+ }
+#endif
+ bypassFilter = 1;
+ resetVAVGData = 0;
+ }
+ }
+
+ /* Check which kind of measurement has been made */
+ status = VL6180_RdByte(dev, 0x01AC, &u8);
+ if (status) {
+ VL6180_ErrLog("0x01AC rd fail");
+ goto done_err;
+ }
+ registerValue = u8;
+
+ /* Read data for filtering */
+#if VL6180_HAVE_MULTI_READ
+ status = VL6180_RdMulti(dev, 0x10C, MultiReadBuf, 8); /* read only 8 lsb bits */
+ if (status) {
+ VL6180_ErrLog("0x10C multi rd fail");
+ goto done_err;
+ }
+ register32BitsValue1 = ((uint32_t) MultiReadBuf[0] << 24)
+ + ((uint32_t) MultiReadBuf[1] << 16)
+ + ((uint32_t) MultiReadBuf[2] << 8)
+ + ((uint32_t) MultiReadBuf[3] << 0);
+ register32BitsValue2 = ((uint32_t) MultiReadBuf[4] << 24)
+ + ((uint32_t) MultiReadBuf[5] << 16)
+ + ((uint32_t) MultiReadBuf[6] << 8)
+ + ((uint32_t) MultiReadBuf[7] << 0);
+#else
+ status = VL6180_RdDWord(dev, 0x10C, ®ister32BitsValue1); /* read 32 bits, lower 17 bits are the one useful */
+ if (status) {
+ VL6180_ErrLog("0x010C rd fail");
+ goto done_err;
+ }
+ status = VL6180_RdDWord(dev, 0x0110, & register32BitsValue2); /* read 32 bits, lower 17 bits are the one useful */
+ if (status) {
+ VL6180_ErrLog("0x0110 rd fail");
+ goto done_err;
+ }
+#endif
+
+
+ if ((FlushFilter == 1) || ((bypassFilter == 1) && (resetVAVGData == 1))) {
+ if (registerValue != 0x3E) {
+ status = VL6180_WrByte(dev, 0x1AC, 0x3E);
+ if (status) {
+ VL6180_ErrLog("0x01AC bypass wr fail");
+ goto done_err;
+ }
+ //status = VL6180_WrByte(dev, 0x0F2, 0x01);
+ //if (status) {
+ // VL6180_ErrLog("0x0F2 bypass wr fail");
+ // goto done_err;
+ //}
+ }
+ /* Set both Default and NoDelay To same value */
+ _FilterData(Default_ZeroVal) = register32BitsValue1;
+ _FilterData(Default_VAVGVal) = register32BitsValue2;
+ _FilterData(NoDelay_ZeroVal) = register32BitsValue1;
+ _FilterData(NoDelay_VAVGVal) = register32BitsValue2;
+
+ _FilterData(MeasurementIndex) = 0;
+ } else {
+ if (registerValue == 0x3E) {
+ _FilterData(Default_ZeroVal) = register32BitsValue1;
+ _FilterData(Default_VAVGVal) = register32BitsValue2;
+ } else {
+ _FilterData(NoDelay_ZeroVal) = register32BitsValue1;
+ _FilterData(NoDelay_VAVGVal) = register32BitsValue2;
+ }
+
+ if (_FilterData(MeasurementIndex) % WrapAroundNoDelayCheckPeriod == 0) {
+ u8 = 0x3C;
+ //u8_2 = 0x05;
+ } else {
+ u8 = 0x3E;
+ //u8_2 = 0x01;
+ }
+ status = VL6180_WrByte(dev, 0x01AC, u8);
+ if (status) {
+ VL6180_ErrLog("0x01AC wr fail");
+ goto done_err;
+ }
+ //status = VL6180_WrByte(dev, 0x0F2, u8_2);
+ //if (status) {
+ // VL6180_ErrLog("0x0F2 wr fail");
+ // goto done_err;
+ //}
+ _FilterData(MeasurementIndex)++;
+ }
+
+ if (bypassFilter == 1) {
+ /* Do not go through the filter */
+
+ /* Update filter error code */
+ _FilterData(filterError) = filterErrorCode;
+
+ /* Update reported range */
+ if(filterErrorCode==RangingFiltered)
+ m_newTrueRange_mm = MaxOrInvalidDistance; /* Set to invalid distance */
+
+ return m_newTrueRange_mm;
+ }
+
+ /* Computes current VAVGDiff */
+ if (_FilterData(Default_VAVGVal) > _FilterData(NoDelay_VAVGVal))
+ VAVGDiff = _FilterData(Default_VAVGVal) - _FilterData(NoDelay_VAVGVal);
+ else
+ VAVGDiff = 0;
+ _FilterData(Previous_VAVGDiff) = VAVGDiff;
+
+ if(SuspicuousRangingZone==0)
+ MAX_VAVGDiff = MAX_VAVGDiff_ROM;
+ else
+ /* In suspicuous area, strengthen the filter */
+ MAX_VAVGDiff = MAX_VAVGDiff_ROM / SuspicuousMAX_VAVGDiffRatio;
+
+ /* Check the VAVGDiff */
+ if (_FilterData(Default_ZeroVal) > _FilterData(NoDelay_ZeroVal))
+ IdealVAVGDiff = _FilterData(Default_ZeroVal) - _FilterData(NoDelay_ZeroVal);
+ else
+ IdealVAVGDiff = _FilterData(NoDelay_ZeroVal) - _FilterData(Default_ZeroVal);
+ if (IdealVAVGDiff > MAX_VAVGDiff)
+ MinVAVGDiff = IdealVAVGDiff - MAX_VAVGDiff;
+ else
+ MinVAVGDiff = 0;
+ MaxVAVGDiff = IdealVAVGDiff + MAX_VAVGDiff;
+ if (VAVGDiff < MinVAVGDiff || VAVGDiff > MaxVAVGDiff) {
+ WrapAroundFlag = 1;
+ filterErrorCode = RangingFiltered;
+ } else {
+ /* Go through filtering check */
+
+ if(_FilterData(MeasurementIndex)<=1)
+ /* On measurement after a bypass, uses an increase number of samples */
+ StdDevSamplesMinNeeded = MIN_FILTER_STDDEV_SAMPLES_AFTER_FLUSH_OR_BYPASS;
+ else
+ StdDevSamplesMinNeeded = MIN_FILTER_STDDEV_SAMPLES;
+
+ /* StdDevLimit Damper on SNR */
+ StdDevLimit = _filter_StdDevDamper(m_rtnAmbientRate, m_rtnSignalRate, StdDevLimitLowLight, StdDevLimitLowLightSNR, StdDevLimitHighLight, StdDevLimitHighLightSNR);
+
+ /* Standard deviations computations */
+ StdDevSamples = 0;
+ StdDevDistanceSum = 0;
+ StdDevDistanceMean = 0;
+ StdDevDistance = 0;
+ StdDevRateSum = 0;
+ StdDevRateMean = 0;
+ StdDevRate = 0;
+ for (i = 0; (i < FILTER_NBOF_SAMPLES) && (StdDevSamples < FILTER_STDDEV_SAMPLES); i++) {
+ if (_FilterData(LastTrueRange)[i] != FILTER_INVALID_DISTANCE) {
+ StdDevSamples = (uint16_t) (StdDevSamples + 1);
+ StdDevDistanceSum = (uint32_t) (StdDevDistanceSum + _FilterData(LastTrueRange)[i]);
+ StdDevRateSum = (uint32_t) (StdDevRateSum + _FilterData(LastReturnRates)[i]);
+ }
+ }
+ if (StdDevSamples > 0) {
+ StdDevDistanceMean = (uint32_t) (StdDevDistanceSum / StdDevSamples);
+ StdDevRateMean = (uint32_t) (StdDevRateSum / StdDevSamples);
+ }
+ /* TODO optimize shorten Std dev in aisngle loop computation using sum of x2 - (sum of x)2 */
+ StdDevSamples = 0;
+ StdDevDistanceSum = 0;
+ StdDevRateSum = 0;
+ for (i = 0; (i < FILTER_NBOF_SAMPLES) && (StdDevSamples < FILTER_STDDEV_SAMPLES); i++) {
+ if (_FilterData(LastTrueRange)[i] != FILTER_INVALID_DISTANCE) {
+ StdDevSamples = (uint16_t) (StdDevSamples + 1);
+ StdDevDistanceSum = (uint32_t) (StdDevDistanceSum +
+ (int)(_FilterData(LastTrueRange)[i] -
+ StdDevDistanceMean) *
+ (int) (_FilterData(LastTrueRange)[i] -
+ StdDevDistanceMean));
+ StdDevRateSum = (uint32_t) (StdDevRateSum +
+ (int) (_FilterData(LastReturnRates)[i] -
+ StdDevRateMean) *
+ (int) (_FilterData(LastReturnRates)[i] -
+ StdDevRateMean));
+ }
+ }
+ if (StdDevSamples >= StdDevSamplesMinNeeded) {
+ StdDevDistance = (uint16_t) (StdDevDistanceSum / StdDevSamples);
+ StdDevRate = (uint16_t) (StdDevRateSum / StdDevSamples);
+ } else {
+ StdDevDistance = 0;
+ StdDevRate = 0;
+ }
+
+ /* Check Return rate standard deviation */
+ if (StdDevRate < StdDevMovingTargetStdDevLimit) {
+ if (StdDevSamples < StdDevSamplesMinNeeded) {
+ //m_newTrueRange_mm = MaxOrInvalidDistance;
+ filterErrorCode = RangingFiltered;
+ } else {
+ /* Check distance standard deviation */
+ if (StdDevRate < StdDevMovingTargetReturnRateLimit)
+ StdDevLimitWithTargetMove = StdDevLimit +
+ (((StdDevMovingTargetStdDevForReturnRateLimit -
+ StdDevLimit) * StdDevRate) /
+ StdDevMovingTargetReturnRateLimit);
+ else
+ StdDevLimitWithTargetMove = StdDevMovingTargetStdDevForReturnRateLimit;
+
+ if(_FilterData(filterError)==NoError){
+ /* No wrapAround detected yet, so relax constraints on the std dev */
+ StdDevLimitWithTargetMove = StdDevLimitWithTargetMove * StdDevNoWrapDetectedMultiplier;
+ }
+
+ if (((StdDevDistance * StdDevHighConfidenceSNRLimit) < StdDevLimit) && (StdDevSamples>=FILTER_STDDEV_SAMPLES)) {
+ NoWrapAroundHighConfidenceFlag = 1;
+ } else {
+ if (StdDevDistance < StdDevLimitWithTargetMove) {
+ NoWrapAroundFlag = 1;
+ } else {
+ WrapAroundFlag = 1;
+ filterErrorCode = RangingFiltered;
+ }
+ }
+ }
+ } else {
+ /* Target moving too fast */
+ WrapAroundFlag = 1;
+ filterErrorCode = RangingFiltered;
+ }
+ }
+
+ if (ValidDistance == 0) {
+ /* In case of invalid distance */
+ if (_FilterData(StdFilteredReads) > 0)
+ _FilterData(StdFilteredReads) = (uint16_t) (_FilterData(StdFilteredReads) - 1);
+ } else {
+ if (WrapAroundFlag == 1) {
+ _FilterData(StdFilteredReads) = (uint16_t) (_FilterData(StdFilteredReads) +
+ StdFilteredReadsIncrement);
+ if (_FilterData(StdFilteredReads) > StdMaxFilteredReads)
+ _FilterData(StdFilteredReads) = StdMaxFilteredReads;
+ } else {
+ if (NoWrapAroundFlag == 1) {
+ if (_FilterData(StdFilteredReads) > 0) {
+ filterErrorCode = RangingFiltered;
+ if (_FilterData(StdFilteredReads) > StdFilteredReadsDecrement)
+ _FilterData(StdFilteredReads) = (uint16_t) (_FilterData(StdFilteredReads) -
+ StdFilteredReadsDecrement);
+ else
+ _FilterData(StdFilteredReads) = 0;
+ }
+ } else {
+ if (NoWrapAroundHighConfidenceFlag == 1) {
+ _FilterData(StdFilteredReads) = 0;
+ }
+ }
+ }
+ }
+
+ /* If we detect a change from no Error to RangingFilteringOnGoing, then it means that
+ * the filter detected a change in te scene, so discard all previous measurements.
+ */
+ if((_FilterData(filterError) == NoError) && (filterErrorCode!=NoError)) {
+ for (i = 1; i < FILTER_NBOF_SAMPLES; i++) {
+ _FilterData(LastTrueRange)[i] = FILTER_INVALID_DISTANCE;
+ _FilterData(LastReturnRates)[i] = 0;
+ }
+ }
+
+ /* Update filter error code */
+ _FilterData(filterError) = filterErrorCode;
+
+ /* Update reported range */
+ if(filterErrorCode==RangingFiltered)
+ m_newTrueRange_mm = MaxOrInvalidDistance; /* Set to invalid distance */
+
+ return m_newTrueRange_mm;
+done_err:
+ return -1;
+
+#undef MaxOrInvalidDistance
+}
+
+
+static int _filter_GetResult(VL6180Dev_t dev, VL6180_RangeData_t *pRangeData)
+{
+ uint32_t m_rawRange_mm = 0;
+ int32_t FilteredRange;
+ const uint8_t scaler = _GetUpscale(dev);
+ uint8_t u8;
+ int status;
+
+ do {
+ status = VL6180_GetCachedByte(dev, RESULT_RANGE_RAW, &u8);
+ if (status) {
+ VL6180_ErrLog("RESULT_RANGE_RAW rd fail");
+ break;
+ }
+ m_rawRange_mm = u8;
+
+ FilteredRange = _filter_Start(dev, pRangeData->range_mm, (m_rawRange_mm * scaler), pRangeData->rtnRate, pRangeData->rtnAmbRate, pRangeData->errorStatus);
+ if (FilteredRange < 0) {
+ status = -1;
+ break;
+ }
+ pRangeData->FilteredData.range_mm = FilteredRange;
+ pRangeData->FilteredData.rawRange_mm = m_rawRange_mm * scaler;
+ pRangeData->FilteredData.filterError= _FilterData(filterError);
+ } while (0);
+ return status;
+}
+
+#undef _FilterData
+#ifdef PRESERVE_DEVICE_ERROR_CODE
+#undef PRESERVE_DEVICE_ERROR_CODE
+#endif
+#ifdef SENSITIVE_FILTERING_ON_GOING
+#undef SENSITIVE_FILTERING_ON_GOING
+#endif
+#undef FILTER_STDDEV_SAMPLES
+#undef MIN_FILTER_STDDEV_SAMPLES
+#undef MIN_FILTER_STDDEV_SAMPLES_AFTER_FLUSH_OR_BYPASS
+#undef STDDEV_BASE_VALUE
+#undef FILTER_INVALID_DISTANCE
+
+#endif /* VL6180_WRAP_AROUND_FILTER_SUPPORT */
+
+#ifdef VL6180_HAVE_RATE_DATA
+
+static int _GetRateResult(VL6180Dev_t dev, VL6180_RangeData_t *pRangeData)
+{
+ uint32_t m_rtnConvTime = 0;
+ uint32_t m_rtnSignalRate = 0;
+ uint32_t m_rtnAmbientRate = 0;
+ uint32_t m_rtnSignalCount = 0;
+ uint32_t m_rtnAmbientCount = 0;
+ uint32_t m_refConvTime = 0;
+ uint32_t cRtnSignalCountMax = 0x7FFFFFFF;
+ uint32_t cDllPeriods = 6;
+ uint32_t calcConvTime = 0;
+
+ int status;
+
+ do {
+ status = VL6180_GetCachedDWord(dev, RESULT_RANGE_RETURN_SIGNAL_COUNT, &m_rtnSignalCount);
+ if (status) {
+ VL6180_ErrLog("RESULT_RANGE_RETURN_SIGNAL_COUNT rd fail");
+ break;
+ }
+ if (m_rtnSignalCount > cRtnSignalCountMax) {
+ m_rtnSignalCount = 0;
+ }
+
+ status = VL6180_GetCachedDWord(dev, RESULT_RANGE_RETURN_AMB_COUNT, &m_rtnAmbientCount);
+ if (status) {
+ VL6180_ErrLog("RESULT_RANGE_RETURN_AMB_COUNTrd fail");
+ break;
+ }
+
+
+ status = VL6180_GetCachedDWord(dev, RESULT_RANGE_RETURN_CONV_TIME, &m_rtnConvTime);
+ if (status) {
+ VL6180_ErrLog("RESULT_RANGE_RETURN_CONV_TIME rd fail");
+ break;
+ }
+
+ status = VL6180_GetCachedDWord(dev, RESULT_RANGE_REFERENCE_CONV_TIME, &m_refConvTime);
+ if (status) {
+ VL6180_ErrLog("RESULT_RANGE_REFERENCE_CONV_TIME rd fail");
+ break;
+ }
+
+ pRangeData->rtnConvTime = m_rtnConvTime;
+ pRangeData->refConvTime = m_refConvTime;
+
+ calcConvTime = m_refConvTime;
+ if (m_rtnConvTime > m_refConvTime) {
+ calcConvTime = m_rtnConvTime;
+ }
+ if (calcConvTime == 0)
+ calcConvTime = 63000;
+
+ m_rtnSignalRate = (m_rtnSignalCount * 1000) / calcConvTime;
+ m_rtnAmbientRate = (m_rtnAmbientCount * cDllPeriods * 1000) / calcConvTime;
+
+ pRangeData->rtnRate = m_rtnSignalRate;
+ pRangeData->rtnAmbRate = m_rtnAmbientRate;
+
+
+ } while (0);
+ return status;
+}
+#endif /* VL6180_HAVE_RATE_DATA */
+
+
+int VL6180_DMaxSetState(VL6180Dev_t dev, int state)
+{
+ int status;
+ LOG_FUNCTION_START("%d", state);
+#if VL6180_HAVE_DMAX_RANGING
+ VL6180DevDataSet(dev, DMaxEnable, state);
+ if (state) {
+ status = _DMax_InitData(dev);
+ } else {
+ status = 0;
+ }
+#else
+ status = NOT_SUPPORTED;
+#endif
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+int VL6180_DMaxGetState(VL6180Dev_t dev)
+{
+ int status;
+ LOG_FUNCTION_START("");
+#if VL6180_HAVE_DMAX_RANGING
+ status = VL6180DevDataGet(dev, DMaxEnable);
+#else
+ status = 0;
+#endif
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+
+#if VL6180_HAVE_DMAX_RANGING
+
+#define _DMaxData(field) VL6180DevDataGet(dev, DMaxData.field)
+/*
+ * Convert fix point x.7 to KCpount per sec
+ */
+
+#ifndef VL6180_PLATFORM_PROVIDE_SQRT
+
+/*
+ * 32 bit integer square root with not so bad precision (integer result) and is quite fast
+ * see http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
+ */
+uint32_t VL6180_SqrtUint32(uint32_t num)
+{
+ uint32_t res = 0;
+ uint32_t bit = 1 << 30; /* The second-to-top bit is set: 1 << 30 for 32 bits */
+
+ /* "bit" starts at the highest power of four <= the argument. */
+ while (bit > num)
+ bit >>= 2;
+
+ while (bit != 0) {
+ if (num >= res + bit) {
+ num -= res + bit;
+ res = (res >> 1) + bit;
+ } else
+ res >>= 1;
+ bit >>= 2;
+ }
+ return res;
+}
+#endif
+
+
+/* DMax one time init */
+void _DMax_OneTimeInit(VL6180Dev_t dev)
+{
+ _DMaxData(ambTuningWindowFactor_K) = DEF_AMBIENT_TUNING;
+}
+
+
+static uint32_t _DMax_RawValueAtRateKCps(VL6180Dev_t dev, int32_t rate)
+{
+ uint32_t snrLimit_K;
+ int32_t DMaxSq;
+ uint32_t RawDMax;
+ DMaxFix_t retSignalAt400mm;
+ uint32_t ambTuningWindowFactor_K;
+
+
+ ambTuningWindowFactor_K = _DMaxData(ambTuningWindowFactor_K);
+ snrLimit_K = _DMaxData(snrLimit_K);
+ retSignalAt400mm = _DMaxData(retSignalAt400mm);
+ /* 12 to 18 bits Kcps */
+ if (rate > 0) {
+ DMaxSq = 400 * 400 * 1000 / rate - (400 * 400 / 330);
+ /* K of (1/RtnAmb -1/330 )=> 30bit- (12-18)bit => 12-18 bits*/
+ if (DMaxSq <= 0) {
+ RawDMax = 0;
+ } else {
+ /* value can be more 32 bit so base on raneg apply
+ * retSignalAt400mm before or after division to presevr accuracy */
+ if (DMaxSq < (2 << 12)) {
+ DMaxSq = DMaxSq * retSignalAt400mm /
+ (snrLimit_K + ambTuningWindowFactor_K);
+ /* max 12 + 12 to 18 -10 => 12-26 bit */
+ } else {
+ DMaxSq = DMaxSq / (snrLimit_K + ambTuningWindowFactor_K) * retSignalAt400mm;
+ /* 12 to 18 -10 + 12 to 18 *=> 12-26 bit */
+ }
+ RawDMax = VL6180_SqrtUint32(DMaxSq);
+ }
+ } else {
+ RawDMax = 0x7FFFFFFF; /* bigest possibmle 32bit signed value */
+ }
+ return RawDMax;
+}
+
+/*
+ * fetch static data from register to avoid re-read
+ * precompute all intermediate constant and cliipings
+ *
+ * to be re-used/call on changes of :
+ * 0x2A
+ * SYSRANGE_MAX_AMBIENT_LEVEL_MULT
+ * Dev Data XtalkComRate_KCPs
+ * SYSRANGE_MAX_CONVERGENCE_TIME
+ * SYSRANGE_RANGE_CHECK_ENABLES mask RANGE_CHECK_RANGE_ENABLE_MASK
+ * range 0xb8-0xbb (0xbb)
+ */
+static int _DMax_InitData(VL6180Dev_t dev)
+{
+ int status, warning;
+ uint8_t u8;
+ uint16_t u16;
+ uint32_t u32;
+ uint32_t Reg2A_KCps;
+ uint32_t RegB8;
+ uint8_t MaxConvTime;
+ uint32_t XTalkCompRate_KCps;
+ uint32_t RangeIgnoreThreshold;
+ int32_t minSignalNeeded;
+ uint8_t SysRangeCheckEn;
+ uint8_t snrLimit;
+ static const int ROMABLE_DATA MaxConvTimeAdjust = -4;
+
+ warning = 0;
+
+ LOG_FUNCTION_START("");
+ do {
+ status = VL6180_RdByte(dev, 0x02A, &u8);
+ if (status) {
+ VL6180_ErrLog("Reg 0x02A rd fail");
+ break;
+ }
+
+ if (u8 == 0) {
+ warning = CALIBRATION_WARNING;
+ u8 = 40; /* use a default average value */
+ }
+ Reg2A_KCps = Fix7_2_KCPs(u8); /* convert to KCPs */
+
+ status = VL6180_RdByte(dev, SYSRANGE_RANGE_CHECK_ENABLES, &SysRangeCheckEn);
+ if (status) {
+ VL6180_ErrLog("SYSRANGE_RANGE_CHECK_ENABLES rd fail ");
+ break;
+ }
+
+ status = VL6180_RdByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, &MaxConvTime);
+ if (status) {
+ VL6180_ErrLog("SYSRANGE_MAX_CONVERGENCE_TIME rd fail ");
+ break;
+ }
+
+ status = VL6180_RdDWord(dev, 0x0B8, &RegB8);
+ if (status) {
+ VL6180_ErrLog("reg 0x0B8 rd fail ");
+ break;
+ }
+
+ status = VL6180_RdByte(dev, SYSRANGE_MAX_AMBIENT_LEVEL_MULT, &snrLimit);
+ if (status) {
+ VL6180_ErrLog("SYSRANGE_MAX_AMBIENT_LEVEL_MULT rd fail ");
+ break;
+ }
+ _DMaxData(snrLimit_K) = (int32_t)16 * 1000 / snrLimit;
+ XTalkCompRate_KCps = VL6180DevDataGet(dev, XTalkCompRate_KCps);
+
+ if (Reg2A_KCps >= XTalkCompRate_KCps) {
+ _DMaxData(retSignalAt400mm) = Reg2A_KCps;
+ } else{
+ _DMaxData(retSignalAt400mm) = 0;
+ /* Reg2A_K - XTalkCompRate_KCp <0 is invalid */
+ }
+
+ /* if xtalk range check is off omit it in snr clipping */
+ if (SysRangeCheckEn&RANGE_CHECK_RANGE_ENABLE_MASK) {
+ status = VL6180_RdWord(dev, SYSRANGE_RANGE_IGNORE_THRESHOLD, &u16);
+ if (status) {
+ VL6180_ErrLog("SYSRANGE_RANGE_IGNORE_THRESHOLD rd fail ");
+ break;
+ }
+ RangeIgnoreThreshold = Fix7_2_KCPs(u16);
+ } else{
+ RangeIgnoreThreshold = 0;
+ }
+
+ minSignalNeeded = (RegB8 * 256) / ((int32_t)MaxConvTime + (int32_t)MaxConvTimeAdjust);
+ /* KCps 8+8 bit -(1 to 6 bit) => 15-10 bit */
+ /* minSignalNeeded = max ( minSignalNeeded, RangeIgnoreThreshold - XTalkCompRate_KCps) */
+ if (minSignalNeeded <= (int32_t)RangeIgnoreThreshold - (int32_t)XTalkCompRate_KCps)
+ minSignalNeeded = RangeIgnoreThreshold - XTalkCompRate_KCps;
+
+ u32 = (minSignalNeeded*(uint32_t)snrLimit) / 16;
+ _DMaxData(ClipSnrLimit) = _DMax_RawValueAtRateKCps(dev, u32);
+ /* clip to dmax to min signal snr limit rate*/
+ } while (0);
+ if (!status)
+ status = warning;
+ LOG_FUNCTION_END(status);
+ return status;
+}
+
+static int _DMax_Compute(VL6180Dev_t dev, VL6180_RangeData_t *pRange)
+{
+ uint32_t rtnAmbRate;
+ int32_t DMax;
+ int scaling;
+ uint16_t HwLimitAtScale;
+ static const int ROMABLE_DATA rtnAmbLowLimit_KCps = 330 * 1000;
+
+ rtnAmbRate = pRange->rtnAmbRate;
+ if (rtnAmbRate < rtnAmbLowLimit_KCps) {
+ DMax = _DMax_RawValueAtRateKCps(dev, rtnAmbRate);
+ scaling = _GetUpscale(dev);
+ HwLimitAtScale = UpperLimitLookUP[scaling - 1];
+
+ if (DMax > _DMaxData(ClipSnrLimit)) {
+ DMax = _DMaxData(ClipSnrLimit);
+ }
+ if (DMax > HwLimitAtScale) {
+ DMax = HwLimitAtScale;
+ }
+ pRange->DMax = DMax;
+ } else {
+ pRange->DMax = 0;
+ }
+ return 0;
+}
+
+#undef _DMaxData
+#undef Fix7_2_KCPs
+
+#endif /* VL6180_HAVE_DMAX_RANGING */
+
+
+
+