2762 lines
83 KiB
C
2762 lines
83 KiB
C
/*******************************************************************************
|
||
Copyright <20> 2015, 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.
|
||
********************************************************************************/
|
||
|
||
/*
|
||
* $Date: 2018-07-04 16:49:57 +0200 (Wed, 04 Jul 2018) $
|
||
* $Revision: 2768 $
|
||
*/
|
||
#include "vl6180x_api.h"
|
||
|
||
#define VL6180x_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 ALS integration time */
|
||
#define DEF_INT_PEFRIOD 100
|
||
/** default value ALS gain */
|
||
#define DEF_ALS_GAIN 1
|
||
/** default value ALS scaler */
|
||
#define DEF_ALS_SCALER 1
|
||
/** 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 VL6180x_SINGLE_DEVICE_DRIVER
|
||
extern struct VL6180xDevData_t SingleVL6180xDevData;
|
||
#define VL6180xDevDataGet(dev, field) (SingleVL6180xDevData.field)
|
||
#define VL6180xDevDataSet(dev, field, data) SingleVL6180xDevData.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(VL6180xDev_t dev);
|
||
static int VL6180x_RangeSetEarlyConvergenceEestimateThreshold(VL6180xDev_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 */
|
||
/**
|
||
* Als Code gain to fix point gain lookup
|
||
*/
|
||
static const uint16_t AlsGainLookUp[8] ROMABLE_DATA = {
|
||
(uint16_t)(20.0f * AN_GAIN_MULT), (uint16_t)(10.0f * AN_GAIN_MULT),
|
||
(uint16_t)(5.0f * AN_GAIN_MULT), (uint16_t)(2.5f * AN_GAIN_MULT),
|
||
(uint16_t)(1.67f * AN_GAIN_MULT), (uint16_t)(1.25f * AN_GAIN_MULT),
|
||
(uint16_t)(1.0f * AN_GAIN_MULT), (uint16_t)(40.0f * AN_GAIN_MULT),
|
||
};
|
||
|
||
#if VL6180x_RANGE_STATUS_ERRSTRING
|
||
const char *ROMABLE_DATA VL6180x_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 *VL6180x_RangeGetStatusErrString(uint8_t RangeErrCode) {
|
||
if (RangeErrCode > sizeof(VL6180x_RangeStatusErrString) /
|
||
sizeof(VL6180x_RangeStatusErrString[0]))
|
||
return NULL;
|
||
return VL6180x_RangeStatusErrString[RangeErrCode];
|
||
}
|
||
#endif
|
||
|
||
#if VL6180x_UPSCALE_SUPPORT == 1
|
||
#define _GetUpscale(dev, ...) 1
|
||
#define _SetUpscale(...) -1
|
||
#define DEF_UPSCALE 1
|
||
#elif VL6180x_UPSCALE_SUPPORT == 2
|
||
#define _GetUpscale(dev, ...) 2
|
||
#define _SetUpscale(...)
|
||
#define DEF_UPSCALE 2
|
||
#elif VL6180x_UPSCALE_SUPPORT == 3
|
||
#define _GetUpscale(dev, ...) 3
|
||
#define _SetUpscale(...)
|
||
#define DEF_UPSCALE 3
|
||
#else
|
||
#define DEF_UPSCALE (-(VL6180x_UPSCALE_SUPPORT))
|
||
#define _GetUpscale(dev, ...) VL6180xDevDataGet(dev, UpscaleFactor)
|
||
#define _SetUpscale(dev, Scaling) VL6180xDevDataSet(dev, UpscaleFactor, Scaling)
|
||
#endif
|
||
|
||
#if VL6180x_SINGLE_DEVICE_DRIVER
|
||
/**
|
||
* the unique driver data When single device driver is active
|
||
*/
|
||
struct VL6180xDevData_t VL6180x_DEV_DATA_ATTR SingleVL6180xDevData = {
|
||
.EceFactorM = DEF_ECE_FACTOR_M,
|
||
.EceFactorD = DEF_ECE_FACTOR_D,
|
||
#ifdef VL6180x_HAVE_UPSCALE_DATA
|
||
.UpscaleFactor = DEF_UPSCALE,
|
||
#endif
|
||
#ifdef VL6180x_HAVE_ALS_DATA
|
||
.IntegrationPeriod = DEF_INT_PEFRIOD,
|
||
.AlsGainCode = DEF_ALS_GAIN,
|
||
.AlsScaler = DEF_ALS_SCALER,
|
||
#endif
|
||
#ifdef VL6180x_HAVE_DMAX_RANGING
|
||
.DMaxEnable = DEF_DMAX_ENABLE,
|
||
#endif
|
||
};
|
||
#endif /* VL6180x_SINGLE_DEVICE_DRIVER */
|
||
|
||
#define Fix7_2_KCPs(x) ((((uint32_t)(x)) * 1000) >> 7)
|
||
|
||
#if VL6180x_WRAP_AROUND_FILTER_SUPPORT || VL6180x_HAVE_DMAX_RANGING
|
||
static int _GetRateResult(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData);
|
||
#endif
|
||
|
||
#if VL6180x_WRAP_AROUND_FILTER_SUPPORT
|
||
static int _filter_Init(VL6180xDev_t dev);
|
||
static int _filter_GetResult(VL6180xDev_t dev, VL6180x_RangeData_t *pData);
|
||
#define _IsWrapArroundActive(dev) VL6180xDevDataGet(dev, WrapAroundFilterActive)
|
||
#else
|
||
#define _IsWrapArroundActive(dev) 0
|
||
#endif
|
||
|
||
#if VL6180x_HAVE_DMAX_RANGING
|
||
void _DMax_OneTimeInit(VL6180xDev_t dev);
|
||
static int _DMax_InitData(VL6180xDev_t dev);
|
||
static int _DMax_Compute(VL6180xDev_t dev, VL6180x_RangeData_t *pRange);
|
||
#define _IsDMaxActive(dev) VL6180xDevDataGet(dev, DMaxEnable)
|
||
#else
|
||
#define _DMax_InitData(...) 0 /* success */
|
||
#define _DMax_OneTimeInit(...) (void)0
|
||
#define _IsDMaxActive(...) 0
|
||
#endif
|
||
|
||
static int VL6180x_RangeStaticInit(VL6180xDev_t dev);
|
||
static int VL6180x_UpscaleStaticInit(VL6180xDev_t dev);
|
||
|
||
int VL6180x_WaitDeviceBooted(VL6180xDev_t dev) {
|
||
uint8_t FreshOutReset;
|
||
int status;
|
||
LOG_FUNCTION_START("");
|
||
do {
|
||
status = VL6180x_RdByte(dev, SYSTEM_FRESH_OUT_OF_RESET, &FreshOutReset);
|
||
} while (FreshOutReset != 1 && status == 0);
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_InitData(VL6180xDev_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("");
|
||
|
||
VL6180xDevDataSet(dev, EceFactorM, DEF_ECE_FACTOR_M);
|
||
VL6180xDevDataSet(dev, EceFactorD, DEF_ECE_FACTOR_D);
|
||
|
||
VL6180xDevDataSet(dev, RangeIgnore.Enabled, 0);
|
||
|
||
#ifdef VL6180x_HAVE_UPSCALE_DATA
|
||
VL6180xDevDataSet(dev, UpscaleFactor, DEF_UPSCALE);
|
||
#endif
|
||
|
||
#ifdef VL6180x_HAVE_ALS_DATA
|
||
VL6180xDevDataSet(dev, IntegrationPeriod, DEF_INT_PEFRIOD);
|
||
VL6180xDevDataSet(dev, AlsGainCode, DEF_ALS_GAIN);
|
||
VL6180xDevDataSet(dev, AlsScaler, DEF_ALS_SCALER);
|
||
#endif
|
||
|
||
#ifdef VL6180x_HAVE_WRAP_AROUND_DATA
|
||
VL6180xDevDataSet(dev, WrapAroundFilterActive,
|
||
(VL6180x_WRAP_AROUND_FILTER_SUPPORT > 0));
|
||
VL6180xDevDataSet(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 = VL6180x_RdByte(dev, SYSRANGE_PART_TO_PART_RANGE_OFFSET,
|
||
(uint8_t *)&offset);
|
||
if (status) {
|
||
VL6180x_ErrLog("SYSRANGE_PART_TO_PART_RANGE_OFFSET rd fail");
|
||
break;
|
||
}
|
||
VL6180xDevDataSet(dev, Part2PartOffsetNVM, offset);
|
||
|
||
status = VL6180x_RdDWord(dev, SYSRANGE_RANGE_IGNORE_THRESHOLD, &CalValue);
|
||
if (status) {
|
||
VL6180x_ErrLog("Part2PartAmbNVM rd fail");
|
||
break;
|
||
}
|
||
if ((CalValue & 0xFFFF0000) == 0) {
|
||
CalValue = 0x00CE03F8;
|
||
}
|
||
VL6180xDevDataSet(dev, Part2PartAmbNVM, CalValue);
|
||
|
||
status = VL6180x_RdWord(dev, SYSRANGE_CROSSTALK_COMPENSATION_RATE, &u16);
|
||
if (status) {
|
||
VL6180x_ErrLog("SYSRANGE_CROSSTALK_COMPENSATION_RATE rd fail ");
|
||
break;
|
||
}
|
||
XTalkCompRate_KCps = Fix7_2_KCPs(u16);
|
||
VL6180xDevDataSet(dev, XTalkCompRate_KCps, XTalkCompRate_KCps);
|
||
|
||
dmax_status = _DMax_InitData(dev);
|
||
if (dmax_status < 0) {
|
||
VL6180x_ErrLog("DMax init failure");
|
||
break;
|
||
}
|
||
|
||
/* Read or wait for fresh out of reset */
|
||
status = VL6180x_RdByte(dev, SYSTEM_FRESH_OUT_OF_RESET, &FreshOutReset);
|
||
if (status) {
|
||
VL6180x_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 VL6180x_GetOffsetCalibrationData(VL6180xDev_t dev) {
|
||
int8_t offset;
|
||
LOG_FUNCTION_START("");
|
||
offset = VL6180xDevDataGet(dev, Part2PartOffsetNVM);
|
||
LOG_FUNCTION_END(offset);
|
||
return offset;
|
||
}
|
||
|
||
int VL6180x_SetOffsetCalibrationData(VL6180xDev_t dev, int8_t offset) {
|
||
int status;
|
||
LOG_FUNCTION_START("%d", offset);
|
||
VL6180xDevDataSet(dev, Part2PartOffsetNVM, offset);
|
||
offset /= _GetUpscale(dev);
|
||
status = VL6180x_WrByte(dev, SYSRANGE_PART_TO_PART_RANGE_OFFSET, offset);
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_SetXTalkCompensationRate(VL6180xDev_t dev, FixPoint97_t Rate) {
|
||
int status;
|
||
LOG_FUNCTION_START("%d", Rate);
|
||
status = VL6180x_WrWord(dev, SYSRANGE_CROSSTALK_COMPENSATION_RATE, Rate);
|
||
if (status == 0) {
|
||
uint32_t XTalkCompRate_KCps;
|
||
XTalkCompRate_KCps = Fix7_2_KCPs(Rate);
|
||
VL6180xDevDataSet(dev, XTalkCompRate_KCps, XTalkCompRate_KCps);
|
||
/* update dmax whenever xtalk rate changes */
|
||
status = _DMax_InitData(dev);
|
||
}
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_SetI2CAddress(VL6180xDev_t dev, uint8_t NewAddress) {
|
||
int status;
|
||
LOG_FUNCTION_START("");
|
||
|
||
status = VL6180x_WrByte(dev, I2C_SLAVE_DEVICE_ADDRESS, NewAddress / 2);
|
||
if (status) {
|
||
VL6180x_ErrLog("new i2c addr Wr fail");
|
||
}
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
uint16_t VL6180x_GetUpperLimit(VL6180xDev_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 VL6180x_StaticInit(VL6180xDev_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 && !(VL6180x_UPSCALE_SUPPORT < 0))
|
||
init_status = VL6180x_RangeStaticInit(dev);
|
||
else
|
||
init_status = VL6180x_UpscaleStaticInit(dev);
|
||
|
||
if (init_status < 0) {
|
||
VL6180x_ErrLog("StaticInit fail");
|
||
goto error;
|
||
} else if (init_status > 0) {
|
||
VL6180x_ErrLog("StaticInit warning");
|
||
}
|
||
|
||
#if REFRESH_CACHED_DATA_AFTER_INIT
|
||
#ifdef VL6180x_HAVE_ALS_DATA
|
||
/* update cached value after tuning applied */
|
||
do {
|
||
uint8_t data;
|
||
status = VL6180x_RdByte(dev, FW_ALS_RESULT_SCALER, &data);
|
||
if (status)
|
||
break;
|
||
VL6180xDevDataSet(dev, AlsScaler, data);
|
||
|
||
status = VL6180x_RdByte(dev, SYSALS_ANALOGUE_GAIN, &data);
|
||
if (status)
|
||
break;
|
||
VL6180x_AlsSetAnalogueGain(dev, data);
|
||
} while (0);
|
||
#endif
|
||
#endif /* REFRESH_CACHED_DATA_AFTER_INIT */
|
||
if (status < 0) {
|
||
VL6180x_ErrLog("StaticInit fail");
|
||
}
|
||
if (!status && init_status) {
|
||
status = init_status;
|
||
}
|
||
error:
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_SetGroupParamHold(VL6180xDev_t dev, int Hold) {
|
||
int status;
|
||
uint8_t value;
|
||
|
||
LOG_FUNCTION_START("%d", Hold);
|
||
if (Hold)
|
||
value = 1;
|
||
else
|
||
value = 0;
|
||
status = VL6180x_WrByte(dev, SYSTEM_GROUPED_PARAMETER_HOLD, value);
|
||
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_Prepare(VL6180xDev_t dev) {
|
||
int status;
|
||
LOG_FUNCTION_START("");
|
||
|
||
do {
|
||
status = VL6180x_StaticInit(dev);
|
||
if (status < 0)
|
||
break;
|
||
|
||
/* set range InterruptMode to new sample */
|
||
status = VL6180x_RangeConfigInterrupt(
|
||
dev, CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY);
|
||
if (status)
|
||
break;
|
||
|
||
/* set default threshold */
|
||
status = VL6180x_RangeSetRawThresholds(dev, 10, 200);
|
||
if (status) {
|
||
VL6180x_ErrLog("VL6180x_RangeSetRawThresholds fail");
|
||
break;
|
||
}
|
||
#if VL6180x_ALS_SUPPORT
|
||
status = VL6180x_AlsSetIntegrationPeriod(dev, 100);
|
||
if (status)
|
||
break;
|
||
status = VL6180x_AlsSetInterMeasurementPeriod(dev, 200);
|
||
if (status)
|
||
break;
|
||
status = VL6180x_AlsSetAnalogueGain(dev, 0);
|
||
if (status)
|
||
break;
|
||
status = VL6180x_AlsSetThresholds(dev, 0, 0xFF);
|
||
if (status)
|
||
break;
|
||
/* set Als InterruptMode to new sample */
|
||
status =
|
||
VL6180x_AlsConfigInterrupt(dev, CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY);
|
||
if (status) {
|
||
VL6180x_ErrLog("VL6180x_AlsConfigInterrupt fail");
|
||
break;
|
||
}
|
||
#endif
|
||
#if VL6180x_WRAP_AROUND_FILTER_SUPPORT
|
||
_filter_Init(dev);
|
||
#endif
|
||
/* make sure to reset any left previous condition that can hangs first poll
|
||
*/
|
||
status = VL6180x_ClearAllInterrupt(dev);
|
||
} while (0);
|
||
LOG_FUNCTION_END(status);
|
||
|
||
return status;
|
||
}
|
||
|
||
#if VL6180x_ALS_SUPPORT
|
||
int VL6180x_AlsGetLux(VL6180xDev_t dev, lux_t *pLux) {
|
||
int status;
|
||
uint16_t RawAls;
|
||
uint32_t luxValue = 0;
|
||
uint32_t IntPeriod;
|
||
uint32_t AlsAnGain;
|
||
uint32_t GainFix;
|
||
uint32_t AlsScaler;
|
||
|
||
#if LUXRES_FIX_PREC != GAIN_FIX_PREC
|
||
#error "LUXRES_FIX_PREC != GAIN_FIX_PREC review these code to be correct"
|
||
#endif
|
||
const uint32_t LuxResxIntIme =
|
||
(uint32_t)(0.56f * DEF_INT_PEFRIOD * (1 << LUXRES_FIX_PREC));
|
||
|
||
LOG_FUNCTION_START("%p", pLux);
|
||
|
||
status = VL6180x_RdWord(dev, RESULT_ALS_VAL, &RawAls);
|
||
if (!status) {
|
||
/* wer are yet here at no fix point */
|
||
IntPeriod = VL6180xDevDataGet(dev, IntegrationPeriod);
|
||
AlsScaler = VL6180xDevDataGet(dev, AlsScaler);
|
||
IntPeriod++; /* what stored is real time ms -1 and it can be 0 for or 0 or
|
||
1ms */
|
||
luxValue =
|
||
(uint32_t)RawAls * LuxResxIntIme; /* max # 16+8bits + 6bit (0.56*100) */
|
||
luxValue /= IntPeriod; /* max # 16+8bits + 6bit 16+8+1 to 9 bit */
|
||
/* between 29 - 21 bit */
|
||
AlsAnGain = VL6180xDevDataGet(dev, AlsGainCode);
|
||
GainFix = AlsGainLookUp[AlsAnGain];
|
||
luxValue = luxValue / (AlsScaler * GainFix);
|
||
*pLux = luxValue;
|
||
}
|
||
|
||
LOG_FUNCTION_END_FMT(status, "%x", (int)*pLux);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_AlsGetMeasurement(VL6180xDev_t dev, VL6180x_AlsData_t *pAlsData) {
|
||
int status;
|
||
uint8_t ErrStatus;
|
||
|
||
LOG_FUNCTION_START("%p", pAlsData);
|
||
|
||
status = VL6180x_AlsGetLux(dev, &pAlsData->lux);
|
||
if (!status) {
|
||
status = VL6180x_RdByte(dev, RESULT_ALS_STATUS, &ErrStatus);
|
||
pAlsData->errorStatus = ErrStatus >> 4;
|
||
}
|
||
LOG_FUNCTION_END_FMT(status, "%d %d", (int)pAlsData->lux,
|
||
(int)pAlsData->errorStatus);
|
||
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_AlsPollMeasurement(VL6180xDev_t dev, VL6180x_AlsData_t *pAlsData) {
|
||
int status;
|
||
int ClrStatus;
|
||
uint8_t IntStatus;
|
||
|
||
LOG_FUNCTION_START("%p", pAlsData);
|
||
#if VL6180X_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 = VL6180x_AlsClearInterrupt(dev);
|
||
if (status) {
|
||
VL6180x_ErrLog("VL6180x_AlsClearInterrupt fail");
|
||
goto over;
|
||
}
|
||
#endif
|
||
|
||
status = VL6180x_AlsSetSystemMode(dev, MODE_START_STOP | MODE_SINGLESHOT);
|
||
if (status) {
|
||
VL6180x_ErrLog("VL6180x_AlsSetSystemMode fail");
|
||
goto over;
|
||
}
|
||
|
||
/* poll for new sample ready */
|
||
while (1) {
|
||
status = VL6180x_AlsGetInterruptStatus(dev, &IntStatus);
|
||
if (status) {
|
||
break;
|
||
}
|
||
if (IntStatus == RES_INT_STAT_GPIO_NEW_SAMPLE_READY) {
|
||
break; /* break on new data (status is 0) */
|
||
}
|
||
|
||
VL6180x_PollDelay(dev);
|
||
};
|
||
|
||
if (!status) {
|
||
status = VL6180x_AlsGetMeasurement(dev, pAlsData);
|
||
}
|
||
|
||
ClrStatus = VL6180x_AlsClearInterrupt(dev);
|
||
if (ClrStatus) {
|
||
VL6180x_ErrLog("VL6180x_AlsClearInterrupt fail");
|
||
if (!status) {
|
||
status = ClrStatus; /* leave previous if already on error */
|
||
}
|
||
}
|
||
over:
|
||
LOG_FUNCTION_END(status);
|
||
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_AlsGetInterruptStatus(VL6180xDev_t dev, uint8_t *pIntStatus) {
|
||
int status;
|
||
uint8_t IntStatus;
|
||
LOG_FUNCTION_START("%p", pIntStatus);
|
||
|
||
status = VL6180x_RdByte(dev, RESULT_INTERRUPT_STATUS_GPIO, &IntStatus);
|
||
*pIntStatus = (IntStatus >> 3) & 0x07;
|
||
|
||
LOG_FUNCTION_END_FMT(status, "%d", (int)*pIntStatus);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_AlsWaitDeviceReady(VL6180xDev_t dev, int MaxLoop) {
|
||
int status;
|
||
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 = VL6180x_RdByte(dev, RESULT_ALS_STATUS, &u8);
|
||
if (status)
|
||
break;
|
||
u8 = u8 & ALS_DEVICE_READY_MASK;
|
||
if (u8)
|
||
break;
|
||
}
|
||
if (!status && !u8) {
|
||
status = TIME_OUT;
|
||
}
|
||
}
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_AlsSetSystemMode(VL6180xDev_t dev, uint8_t mode) {
|
||
int status;
|
||
LOG_FUNCTION_START("%d", (int)mode);
|
||
/* FIXME if we are called back to back real fast we are not checking
|
||
* if previous mode "set" got absorbed => bit 0 must be 0 so that wr 1 work */
|
||
if (mode <= 3) {
|
||
status = VL6180x_WrByte(dev, SYSALS_START, mode);
|
||
} else {
|
||
status = INVALID_PARAMS;
|
||
}
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_AlsConfigInterrupt(VL6180xDev_t dev, uint8_t ConfigGpioInt) {
|
||
int status;
|
||
|
||
if (ConfigGpioInt <= CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY) {
|
||
status = VL6180x_UpdateByte(dev, SYSTEM_INTERRUPT_CONFIG_GPIO,
|
||
(uint8_t)(~CONFIG_GPIO_ALS_MASK),
|
||
(ConfigGpioInt << 3));
|
||
} else {
|
||
VL6180x_ErrLog("Invalid config mode param %d", (int)ConfigGpioInt);
|
||
status = INVALID_PARAMS;
|
||
}
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_AlsSetThresholds(VL6180xDev_t dev, uint8_t low, uint8_t high) {
|
||
int status;
|
||
|
||
LOG_FUNCTION_START("%d %d", (int)low, (int)high);
|
||
|
||
status = VL6180x_WrByte(dev, SYSALS_THRESH_LOW, low);
|
||
if (!status) {
|
||
status = VL6180x_WrByte(dev, SYSALS_THRESH_HIGH, high);
|
||
}
|
||
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_AlsSetAnalogueGain(VL6180xDev_t dev, uint8_t gain) {
|
||
int status;
|
||
uint8_t GainTotal;
|
||
|
||
LOG_FUNCTION_START("%d", (int)gain);
|
||
gain &= ~0x40;
|
||
if (gain > 7) {
|
||
gain = 7;
|
||
}
|
||
GainTotal = gain | 0x40;
|
||
|
||
status = VL6180x_WrByte(dev, SYSALS_ANALOGUE_GAIN, GainTotal);
|
||
if (!status) {
|
||
VL6180xDevDataSet(dev, AlsGainCode, gain);
|
||
}
|
||
|
||
LOG_FUNCTION_END_FMT(status, "%d %d", (int)gain, (int)GainTotal);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_AlsSetInterMeasurementPeriod(VL6180xDev_t dev,
|
||
uint16_t intermeasurement_period_ms) {
|
||
int status;
|
||
|
||
LOG_FUNCTION_START("%d", (int)intermeasurement_period_ms);
|
||
/* clipping: range is 0-2550ms */
|
||
if (intermeasurement_period_ms >= 255 * 10)
|
||
intermeasurement_period_ms = 255 * 10;
|
||
status = VL6180x_WrByte(dev, SYSALS_INTERMEASUREMENT_PERIOD,
|
||
(uint8_t)(intermeasurement_period_ms / 10));
|
||
|
||
LOG_FUNCTION_END_FMT(status, "%d", (int)intermeasurement_period_ms);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_AlsSetIntegrationPeriod(VL6180xDev_t dev, uint16_t period_ms) {
|
||
int status;
|
||
uint16_t SetIntegrationPeriod;
|
||
|
||
LOG_FUNCTION_START("%d", (int)period_ms);
|
||
|
||
if (period_ms >= 1)
|
||
SetIntegrationPeriod = period_ms - 1;
|
||
else
|
||
SetIntegrationPeriod = period_ms;
|
||
|
||
if (SetIntegrationPeriod > 464) {
|
||
SetIntegrationPeriod = 464;
|
||
} else if (SetIntegrationPeriod == 255) {
|
||
SetIntegrationPeriod++; /* can't write 255 since this causes the device to
|
||
lock out.*/
|
||
}
|
||
|
||
status = VL6180x_WrWord(dev, SYSALS_INTEGRATION_PERIOD, SetIntegrationPeriod);
|
||
if (!status) {
|
||
VL6180xDevDataSet(dev, IntegrationPeriod, SetIntegrationPeriod);
|
||
}
|
||
LOG_FUNCTION_END_FMT(status, "%d", (int)SetIntegrationPeriod);
|
||
return status;
|
||
}
|
||
|
||
#endif /* HAVE_ALS_SUPPORT */
|
||
|
||
int VL6180x_RangePollMeasurement(VL6180xDev_t dev,
|
||
VL6180x_RangeData_t *pRangeData) {
|
||
int status;
|
||
int ClrStatus;
|
||
IntrStatus_t IntStatus;
|
||
|
||
LOG_FUNCTION_START("");
|
||
/* start single range measurement */
|
||
|
||
#if VL6180X_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 = VL6180x_RangeClearInterrupt(dev);
|
||
if (status) {
|
||
VL6180x_ErrLog("VL6180x_RangeClearInterrupt fail");
|
||
goto done;
|
||
}
|
||
#endif
|
||
/* //![single_shot_snipet] */
|
||
status = VL6180x_RangeSetSystemMode(dev, MODE_START_STOP | MODE_SINGLESHOT);
|
||
if (status) {
|
||
VL6180x_ErrLog("VL6180x_RangeSetSystemMode fail");
|
||
goto done;
|
||
}
|
||
|
||
/* poll for new sample ready */
|
||
while (1) {
|
||
status = VL6180x_RangeGetInterruptStatus(dev, &IntStatus.val);
|
||
if (status) {
|
||
break;
|
||
}
|
||
if (IntStatus.status.Range == RES_INT_STAT_GPIO_NEW_SAMPLE_READY ||
|
||
IntStatus.status.Error != 0) {
|
||
break;
|
||
}
|
||
|
||
VL6180x_PollDelay(dev);
|
||
}
|
||
/* //![single_shot_snipet] */
|
||
|
||
if (!status) {
|
||
status = VL6180x_RangeGetMeasurement(dev, pRangeData);
|
||
}
|
||
|
||
/* clear range interrupt source */
|
||
ClrStatus = VL6180x_RangeClearInterrupt(dev);
|
||
if (ClrStatus) {
|
||
VL6180x_ErrLog("VL6180x_RangeClearInterrupt fail");
|
||
/* leave initial status if already in error */
|
||
if (!status) {
|
||
status = ClrStatus;
|
||
}
|
||
}
|
||
done:
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
#if VL6180x_CACHED_REG
|
||
|
||
int VL6180x_GetCachedDWord(VL6180xDev_t dev, uint16_t index, uint32_t *pValue) {
|
||
int status;
|
||
uint32_t Value;
|
||
if (VL6180xDevDataGet(dev, CacheFilled) != 0 &&
|
||
index >= VL6180x_FIRST_CACHED_INDEX &&
|
||
index <= (VL6180x_LAST_CACHED_INDEX - 3)) {
|
||
uint8_t *pBytes =
|
||
&VL6180xDevDataGet(dev, CachedRegs[index - VL6180x_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 = VL6180x_RdDWord(dev, index, pValue);
|
||
}
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_GetCachedWord(VL6180xDev_t dev, uint16_t index, uint16_t *pValue) {
|
||
int status;
|
||
uint32_t Value;
|
||
if (VL6180xDevDataGet(dev, CacheFilled) != 0 &&
|
||
index >= VL6180x_FIRST_CACHED_INDEX &&
|
||
index <= (VL6180x_LAST_CACHED_INDEX - 1)) {
|
||
uint8_t *pBytes =
|
||
&VL6180xDevDataGet(dev, CachedRegs[index - VL6180x_FIRST_CACHED_INDEX]);
|
||
Value = ((uint32_t)pBytes[0] << 8) | (uint32_t)pBytes[1];
|
||
*pValue = Value;
|
||
status = 0;
|
||
} else {
|
||
status = VL6180x_RdWord(dev, index, pValue);
|
||
}
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_GetCachedByte(VL6180xDev_t dev, uint16_t index, uint8_t *pValue) {
|
||
int status;
|
||
uint8_t Value;
|
||
if (VL6180xDevDataGet(dev, CacheFilled) != 0 &&
|
||
index >= VL6180x_FIRST_CACHED_INDEX &&
|
||
index <= VL6180x_LAST_CACHED_INDEX) {
|
||
Value =
|
||
VL6180xDevDataGet(dev, CachedRegs[index - VL6180x_FIRST_CACHED_INDEX]);
|
||
*pValue = Value;
|
||
status = 0;
|
||
} else {
|
||
status = VL6180x_RdByte(dev, index, pValue);
|
||
}
|
||
return status;
|
||
}
|
||
|
||
int _CachedRegs_Fetch(VL6180xDev_t dev) {
|
||
int status;
|
||
uint8_t *Buffer;
|
||
if (VL6180xDevDataGet(dev, CacheFilled) == 0) {
|
||
VL6180xDevDataSet(dev, CacheFilled, 1);
|
||
Buffer = &VL6180xDevDataGet(dev, CachedRegs[0]);
|
||
status = VL6180x_RdMulti(dev, VL6180x_FIRST_CACHED_INDEX, Buffer,
|
||
VL6180x_CACHED_REG_CNT);
|
||
} else {
|
||
status = 0;
|
||
}
|
||
return status;
|
||
}
|
||
|
||
void _CachedRegs_Flush(VL6180xDev_t dev) {
|
||
VL6180xDevDataSet(dev, CacheFilled, 0);
|
||
}
|
||
|
||
#else
|
||
#define _CachedRegs_Fetch(...) 0
|
||
#define _CachedRegs_Flush(...) (void)0
|
||
#define _Fetch_CachedRegs(...) 0
|
||
#define VL6180x_GetCachedByte(dev, index, pValue) \
|
||
VL6180x_RdByte(dev, index, pValue)
|
||
#define VL6180x_GetCachedWord(dev, index, pValue) \
|
||
VL6180x_RdWord(dev, index, pValue)
|
||
#define VL6180x_GetCachedDWord(dev, index, pValue) \
|
||
VL6180x_RdDWord(dev, index, pValue)
|
||
#endif /* VL6180x_CACHED_REG */
|
||
|
||
int VL6180x_RangeGetMeasurement(VL6180xDev_t dev,
|
||
VL6180x_RangeData_t *pRangeData) {
|
||
int status;
|
||
uint16_t RawRate;
|
||
uint8_t RawStatus;
|
||
|
||
LOG_FUNCTION_START("");
|
||
|
||
status = _CachedRegs_Fetch(dev);
|
||
if (status) {
|
||
VL6180x_ErrLog("Cache register read fail");
|
||
goto error;
|
||
}
|
||
status = VL6180x_RangeGetResult(dev, &pRangeData->range_mm);
|
||
if (!status) {
|
||
status = VL6180x_GetCachedWord(dev, RESULT_RANGE_SIGNAL_RATE, &RawRate);
|
||
if (!status) {
|
||
pRangeData->signalRate_mcps = VL6180x_9to7Conv(RawRate);
|
||
status = VL6180x_GetCachedByte(dev, RESULT_RANGE_STATUS, &RawStatus);
|
||
if (!status) {
|
||
pRangeData->errorStatus = RawStatus >> 4;
|
||
} else {
|
||
VL6180x_ErrLog("Rd RESULT_RANGE_STATUS fail");
|
||
}
|
||
#if VL6180x_WRAP_AROUND_FILTER_SUPPORT || VL6180x_HAVE_DMAX_RANGING
|
||
status = _GetRateResult(dev, pRangeData);
|
||
if (status)
|
||
goto error;
|
||
#endif
|
||
#if VL6180x_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 VL6180x_HAVE_DMAX_RANGING
|
||
if (_IsDMaxActive(dev)) {
|
||
_DMax_Compute(dev, pRangeData);
|
||
}
|
||
#endif
|
||
} else {
|
||
VL6180x_ErrLog("Rd RESULT_RANGE_SIGNAL_RATE fail");
|
||
}
|
||
} else {
|
||
VL6180x_ErrLog("VL6180x_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 VL6180x_RangeGetMeasurementIfReady(VL6180xDev_t dev,
|
||
VL6180x_RangeData_t *pRangeData) {
|
||
int status;
|
||
IntrStatus_t IntStatus;
|
||
|
||
LOG_FUNCTION_START();
|
||
status = VL6180x_RangeGetInterruptStatus(dev, &IntStatus.val);
|
||
if (status == 0) {
|
||
if (IntStatus.status.Range == RES_INT_STAT_GPIO_NEW_SAMPLE_READY ||
|
||
IntStatus.status.Error != 0) {
|
||
status = VL6180x_RangeGetMeasurement(dev, pRangeData);
|
||
if (status == 0) {
|
||
/* clear range interrupt source */
|
||
status = VL6180x_RangeClearInterrupt(dev);
|
||
if (status) {
|
||
VL6180x_ErrLog("VL6180x_RangeClearInterrupt fail");
|
||
}
|
||
}
|
||
} else {
|
||
pRangeData->errorStatus = DataNotReady;
|
||
}
|
||
} else {
|
||
VL6180x_ErrLog("fail to get interrupt status");
|
||
}
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_FilterSetState(VL6180xDev_t dev, int state) {
|
||
int status;
|
||
LOG_FUNCTION_START("%d", state);
|
||
#if VL6180x_WRAP_AROUND_FILTER_SUPPORT
|
||
VL6180xDevDataSet(dev, WrapAroundFilterActive, state);
|
||
status = 0;
|
||
#else
|
||
status = NOT_SUPPORTED;
|
||
#endif
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_FilterGetState(VL6180xDev_t dev) {
|
||
int status;
|
||
LOG_FUNCTION_START("");
|
||
#if VL6180x_WRAP_AROUND_FILTER_SUPPORT
|
||
status = VL6180xDevDataGet(dev, WrapAroundFilterActive);
|
||
#else
|
||
status = 0;
|
||
#endif
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_RangeGetResult(VL6180xDev_t dev, int32_t *pRange_mm) {
|
||
int status;
|
||
uint8_t RawRange;
|
||
int32_t Upscale;
|
||
|
||
LOG_FUNCTION_START("%p", pRange_mm);
|
||
|
||
status = VL6180x_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 VL6180x_RangeSetRawThresholds(VL6180xDev_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 = VL6180x_WrByte(dev, SYSRANGE_THRESH_HIGH, high);
|
||
if (!status) {
|
||
status = VL6180x_WrByte(dev, SYSRANGE_THRESH_LOW, low);
|
||
}
|
||
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_RangeSetThresholds(VL6180xDev_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 = VL6180x_SetGroupParamHold(dev, 1);
|
||
if (status)
|
||
break;
|
||
}
|
||
status = VL6180x_RangeSetRawThresholds(dev, (uint8_t)(low / scale),
|
||
(uint8_t)(high / scale));
|
||
if (status) {
|
||
VL6180x_ErrLog("VL6180x_RangeSetRawThresholds fail");
|
||
}
|
||
if (UseSafeParamHold) {
|
||
int HoldStatus;
|
||
/* tryt to unset param hold vene if previous fail */
|
||
HoldStatus = VL6180x_SetGroupParamHold(dev, 0);
|
||
if (!status)
|
||
status = HoldStatus;
|
||
}
|
||
} while (0);
|
||
}
|
||
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_RangeGetThresholds(VL6180xDev_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 = VL6180x_RdByte(dev, SYSRANGE_THRESH_HIGH, &RawHigh);
|
||
if (status) {
|
||
VL6180x_ErrLog("rd SYSRANGE_THRESH_HIGH fail");
|
||
break;
|
||
}
|
||
*high = (uint16_t)RawHigh * scale;
|
||
}
|
||
if (low != NULL) {
|
||
status = VL6180x_RdByte(dev, SYSRANGE_THRESH_LOW, &RawLow);
|
||
if (status) {
|
||
VL6180x_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 VL6180x_RangeGetInterruptStatus(VL6180xDev_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 = VL6180x_GetCachedByte(dev, RESULT_INTERRUPT_STATUS_GPIO, &IntStatus);
|
||
*pIntStatus = IntStatus & 0xC7;
|
||
|
||
LOG_FUNCTION_END_FMT(status, "%d", (int)*pIntStatus);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_GetInterruptStatus(VL6180xDev_t dev, uint8_t *IntStatus) {
|
||
int status;
|
||
LOG_FUNCTION_START("%p", IntStatus);
|
||
status = VL6180x_RdByte(dev, RESULT_INTERRUPT_STATUS_GPIO, IntStatus);
|
||
LOG_FUNCTION_END_FMT(status, "%d", (int)*IntStatus);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_ClearInterrupt(VL6180xDev_t dev, uint8_t IntClear) {
|
||
int status;
|
||
LOG_FUNCTION_START("%d", (int)IntClear);
|
||
if (IntClear <= 7) {
|
||
status = VL6180x_WrByte(dev, SYSTEM_INTERRUPT_CLEAR, IntClear);
|
||
} else {
|
||
status = INVALID_PARAMS;
|
||
}
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
static int VL6180x_RangeStaticInit(VL6180xDev_t dev) {
|
||
int status;
|
||
LOG_FUNCTION_START("");
|
||
|
||
/* REGISTER_TUNING_SR03_270514_CustomerView.txt */
|
||
VL6180x_WrByte(dev, 0x0207, 0x01);
|
||
VL6180x_WrByte(dev, 0x0208, 0x01);
|
||
VL6180x_WrByte(dev, 0x0096, 0x00);
|
||
VL6180x_WrByte(dev, 0x0097, 0xfd);
|
||
VL6180x_WrByte(dev, 0x00e3, 0x01);
|
||
VL6180x_WrByte(dev, 0x00e4, 0x03);
|
||
VL6180x_WrByte(dev, 0x00e5, 0x02);
|
||
VL6180x_WrByte(dev, 0x00e6, 0x01);
|
||
VL6180x_WrByte(dev, 0x00e7, 0x03);
|
||
VL6180x_WrByte(dev, 0x00f5, 0x02);
|
||
VL6180x_WrByte(dev, 0x00d9, 0x05);
|
||
VL6180x_WrByte(dev, 0x00db, 0xce);
|
||
VL6180x_WrByte(dev, 0x00dc, 0x03);
|
||
VL6180x_WrByte(dev, 0x00dd, 0xf8);
|
||
VL6180x_WrByte(dev, 0x009f, 0x00);
|
||
VL6180x_WrByte(dev, 0x00a3, 0x3c);
|
||
VL6180x_WrByte(dev, 0x00b7, 0x00);
|
||
VL6180x_WrByte(dev, 0x00bb, 0x3c); /*Recommended value = 0x28 in case very
|
||
fast ranging frequency (~ 100 Hz)*/
|
||
VL6180x_WrByte(dev, 0x00b2, 0x09);
|
||
VL6180x_WrByte(dev, 0x00ca, 0x09);
|
||
VL6180x_WrByte(dev, 0x0198, 0x01);
|
||
VL6180x_WrByte(dev, 0x01b0, 0x17);
|
||
VL6180x_WrByte(dev, 0x01ad, 0x00);
|
||
VL6180x_WrByte(dev, 0x00ff, 0x05);
|
||
VL6180x_WrByte(dev, 0x0100, 0x05);
|
||
VL6180x_WrByte(dev, 0x0199, 0x05);
|
||
VL6180x_WrByte(dev, 0x01a6, 0x1b);
|
||
VL6180x_WrByte(dev, 0x01ac, 0x3e);
|
||
VL6180x_WrByte(dev, 0x01a7, 0x1f);
|
||
VL6180x_WrByte(dev, 0x0030, 0x00);
|
||
|
||
/* Recommended : Public registers - See data sheet for more detail */
|
||
VL6180x_WrByte(dev, 0x0011, 0x10); /* Enables polling for New Sample ready
|
||
when measurement completes */
|
||
VL6180x_WrByte(dev, 0x010a,
|
||
0x30); /* Set the averaging sample period (compromise between
|
||
lower noise and increased execution time) */
|
||
VL6180x_WrByte(dev, 0x003f,
|
||
0x46); /* Sets the light and dark gain (upper nibble). Dark
|
||
gain should not be changed.*/
|
||
VL6180x_WrByte(dev, 0x0031,
|
||
0xFF); /* sets the # of range measurements after which auto
|
||
calibration of system is performed */
|
||
VL6180x_WrByte(dev, 0x0040, 0x63); /* Set ALS integration time to 100ms */
|
||
VL6180x_WrByte(dev, 0x002e, 0x01); /* perform a single temperature calibration
|
||
of the ranging sensor */
|
||
|
||
/* Optional: Public registers - See data sheet for more detail */
|
||
VL6180x_WrByte(
|
||
dev, 0x001b,
|
||
0x09); /* Set default ranging inter-measurement period to 100ms */
|
||
VL6180x_WrByte(dev, 0x003e,
|
||
0x31); /* Set default ALS inter-measurement period to 500ms */
|
||
VL6180x_WrByte(dev, 0x0014,
|
||
0x24); /* Configures interrupt on New sample ready */
|
||
|
||
status = VL6180x_RangeSetMaxConvergenceTime(
|
||
dev, 50); /* Calculate ece value on initialization (use max conv) */
|
||
LOG_FUNCTION_END(status);
|
||
|
||
return status;
|
||
}
|
||
|
||
#if VL6180x_UPSCALE_SUPPORT != 1
|
||
|
||
static int _UpscaleInitPatch0(VL6180xDev_t dev) {
|
||
int status;
|
||
uint32_t CalValue = 0;
|
||
CalValue = VL6180xDevDataGet(dev, Part2PartAmbNVM);
|
||
status = VL6180x_WrDWord(dev, 0xDA, CalValue);
|
||
return status;
|
||
}
|
||
|
||
/* only include up-scaling register setting when up-scale support is configured
|
||
* in */
|
||
int VL6180x_UpscaleRegInit(VL6180xDev_t dev) {
|
||
/* apply REGISTER_TUNING_ER02_100614_CustomerView.txt */
|
||
VL6180x_WrByte(dev, 0x0207, 0x01);
|
||
VL6180x_WrByte(dev, 0x0208, 0x01);
|
||
VL6180x_WrByte(dev, 0x0096, 0x00);
|
||
VL6180x_WrByte(dev, 0x0097, 0x54);
|
||
VL6180x_WrByte(dev, 0x00e3, 0x01);
|
||
VL6180x_WrByte(dev, 0x00e4, 0x03);
|
||
VL6180x_WrByte(dev, 0x00e5, 0x02);
|
||
VL6180x_WrByte(dev, 0x00e6, 0x01);
|
||
VL6180x_WrByte(dev, 0x00e7, 0x03);
|
||
VL6180x_WrByte(dev, 0x00f5, 0x02);
|
||
VL6180x_WrByte(dev, 0x00d9, 0x05);
|
||
|
||
_UpscaleInitPatch0(dev);
|
||
|
||
VL6180x_WrByte(dev, 0x009f, 0x00);
|
||
VL6180x_WrByte(dev, 0x00a3, 0x28);
|
||
VL6180x_WrByte(dev, 0x00b7, 0x00);
|
||
VL6180x_WrByte(dev, 0x00bb, 0x28);
|
||
VL6180x_WrByte(dev, 0x00b2, 0x09);
|
||
VL6180x_WrByte(dev, 0x00ca, 0x09);
|
||
VL6180x_WrByte(dev, 0x0198, 0x01);
|
||
VL6180x_WrByte(dev, 0x01b0, 0x17);
|
||
VL6180x_WrByte(dev, 0x01ad, 0x00);
|
||
VL6180x_WrByte(dev, 0x00ff, 0x05);
|
||
VL6180x_WrByte(dev, 0x0100, 0x05);
|
||
VL6180x_WrByte(dev, 0x0199, 0x05);
|
||
VL6180x_WrByte(dev, 0x01a6, 0x1b);
|
||
VL6180x_WrByte(dev, 0x01ac, 0x3e);
|
||
VL6180x_WrByte(dev, 0x01a7, 0x1f);
|
||
VL6180x_WrByte(dev, 0x0030, 0x00);
|
||
VL6180x_WrByte(dev, 0x0011, 0x10);
|
||
VL6180x_WrByte(dev, 0x010a, 0x30);
|
||
VL6180x_WrByte(dev, 0x003f, 0x46);
|
||
VL6180x_WrByte(dev, 0x0031, 0xFF);
|
||
VL6180x_WrByte(dev, 0x0040, 0x63);
|
||
VL6180x_WrByte(dev, 0x002e, 0x01);
|
||
VL6180x_WrByte(dev, 0x002c, 0xff);
|
||
VL6180x_WrByte(dev, 0x001b, 0x09);
|
||
VL6180x_WrByte(dev, 0x003e, 0x31);
|
||
VL6180x_WrByte(dev, 0x0014, 0x24);
|
||
#if VL6180x_EXTENDED_RANGE
|
||
VL6180x_RangeSetMaxConvergenceTime(dev, 63);
|
||
#else
|
||
VL6180x_RangeSetMaxConvergenceTime(dev, 50);
|
||
#endif
|
||
return 0;
|
||
}
|
||
#else
|
||
#define VL6180x_UpscaleRegInit(...) -1
|
||
#endif
|
||
|
||
int VL6180x_UpscaleSetScaling(VL6180xDev_t dev, uint8_t scaling) {
|
||
int status;
|
||
uint16_t Scaler;
|
||
uint16_t ValidHeight;
|
||
int8_t Offset;
|
||
|
||
LOG_FUNCTION_START("%d", (int)scaling);
|
||
|
||
#ifdef VL6180x_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 VL6180x_UPSCALE_SUPPORT
|
||
#define max_scaling VL6180x_UPSCALE_SUPPORT
|
||
#endif
|
||
|
||
if (scaling >= min_scaling && scaling <= max_scaling) {
|
||
|
||
Scaler = ScalerLookUP[scaling - 1];
|
||
status = VL6180x_WrWord(dev, RANGE_SCALER, Scaler);
|
||
_SetUpscale(dev, scaling);
|
||
|
||
/* Apply scaling on part-2-part offset */
|
||
Offset = VL6180xDevDataGet(dev, Part2PartOffsetNVM) / scaling;
|
||
status = VL6180x_WrByte(dev, SYSRANGE_PART_TO_PART_RANGE_OFFSET, Offset);
|
||
|
||
/* Apply scaling on CrossTalkValidHeight */
|
||
if (status == 0) {
|
||
status = VL6180x_WrByte(dev, SYSRANGE_CROSSTALK_VALID_HEIGHT,
|
||
DEF_CROSS_TALK_VALID_HEIGHT_VALUE / scaling);
|
||
}
|
||
/* Apply scaling on RangeIgnore ValidHeight if enabled */
|
||
if (status == 0) {
|
||
if (VL6180xDevDataGet(dev, RangeIgnore.Enabled) != 0) {
|
||
ValidHeight = VL6180xDevDataGet(dev, RangeIgnore.ValidHeight);
|
||
ValidHeight /= _GetUpscale(dev);
|
||
if (ValidHeight > 255)
|
||
ValidHeight = 255;
|
||
|
||
status = VL6180x_WrByte(dev, SYSRANGE_RANGE_IGNORE_VALID_HEIGHT,
|
||
ValidHeight);
|
||
}
|
||
}
|
||
|
||
#if !VL6180x_EXTENDED_RANGE
|
||
if (status == 0) {
|
||
status = VL6180x_RangeSetEceState(
|
||
dev, scaling == 1); /* enable ece only at 1x scaling */
|
||
}
|
||
if (status == 0 && !VL6180x_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 VL6180x_UpscaleGetScaling(VL6180xDev_t dev) {
|
||
int status;
|
||
LOG_FUNCTION_START("");
|
||
status = _GetUpscale(dev);
|
||
LOG_FUNCTION_END(status);
|
||
|
||
return status;
|
||
}
|
||
|
||
static int VL6180x_UpscaleStaticInit(VL6180xDev_t dev) {
|
||
/* todo make these a fail macro in case only 1x is suppoted */
|
||
int status;
|
||
|
||
LOG_FUNCTION_START("");
|
||
do {
|
||
status = VL6180x_UpscaleRegInit(dev);
|
||
if (status) {
|
||
VL6180x_ErrLog("regInit fail");
|
||
break;
|
||
}
|
||
#if VL6180x_EXTENDED_RANGE
|
||
status = VL6180x_RangeSetEceState(dev, 0);
|
||
if (status) {
|
||
VL6180x_ErrLog("VL6180x_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 = VL6180x_UpscaleSetScaling(dev, _GetUpscale(dev));
|
||
VL6180x_WrByte(dev, 0x016, 0x00); /* change fresh out of set status to 0 */
|
||
}
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_SetGPIOxPolarity(VL6180xDev_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 = VL6180x_UpdateByte(dev, RegIndex,
|
||
(uint8_t)~GPIOx_POLARITY_SELECT_MASK, DataSet);
|
||
} else {
|
||
VL6180x_ErrLog("Invalid pin param %d", (int)pin);
|
||
status = INVALID_PARAMS;
|
||
}
|
||
|
||
LOG_FUNCTION_END(status);
|
||
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_SetGPIOxFunctionality(VL6180xDev_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 = VL6180x_UpdateByte(
|
||
dev, RegIndex, (uint8_t)~GPIOx_FUNCTIONALITY_SELECT_MASK,
|
||
functionality << GPIOx_FUNCTIONALITY_SELECT_SHIFT);
|
||
if (status) {
|
||
VL6180x_ErrLog("Update SYSTEM_MODE_GPIO%d fail", (int)pin);
|
||
}
|
||
} else {
|
||
VL6180x_ErrLog("Invalid pin %d or function %d", (int)pin,
|
||
(int)functionality);
|
||
status = INVALID_PARAMS;
|
||
}
|
||
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_SetupGPIOx(VL6180xDev_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 = VL6180x_WrByte(dev, RegIndex, value);
|
||
if (status) {
|
||
VL6180x_ErrLog("SYSTEM_MODE_GPIO%d wr fail",
|
||
(int)pin - SYSTEM_MODE_GPIO0);
|
||
}
|
||
} else {
|
||
VL6180x_ErrLog("Invalid pin %d or function %d", (int)pin, (int)IntFunction);
|
||
status = INVALID_PARAMS;
|
||
}
|
||
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_DisableGPIOxOut(VL6180xDev_t dev, int pin) {
|
||
int status;
|
||
|
||
LOG_FUNCTION_START("%d", (int)pin);
|
||
|
||
status = VL6180x_SetGPIOxFunctionality(dev, pin, GPIOx_SELECT_OFF);
|
||
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_SetupGPIO1(VL6180xDev_t dev, uint8_t IntFunction, int ActiveHigh) {
|
||
int status;
|
||
LOG_FUNCTION_START("%d %d", (int)IntFunction, (int)ActiveHigh);
|
||
status = VL6180x_SetupGPIOx(dev, 1, IntFunction, ActiveHigh);
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_RangeConfigInterrupt(VL6180xDev_t dev, uint8_t ConfigGpioInt) {
|
||
int status;
|
||
|
||
if (ConfigGpioInt <= CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY) {
|
||
status =
|
||
VL6180x_UpdateByte(dev, SYSTEM_INTERRUPT_CONFIG_GPIO,
|
||
(uint8_t)(~CONFIG_GPIO_RANGE_MASK), ConfigGpioInt);
|
||
} else {
|
||
VL6180x_ErrLog("Invalid config mode param %d", (int)ConfigGpioInt);
|
||
status = INVALID_PARAMS;
|
||
}
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_RangeSetEceFactor(VL6180xDev_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) {
|
||
VL6180xDevDataSet(dev, EceFactorM, FactorM);
|
||
VL6180xDevDataSet(dev, EceFactorD, FactorD);
|
||
/* read and re-apply max conv time to get new ece factor set */
|
||
status = VL6180x_RdByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, &u8);
|
||
if (status) {
|
||
VL6180x_ErrLog("SYSRANGE_MAX_CONVERGENCE_TIME rd fail ");
|
||
break;
|
||
}
|
||
status = VL6180x_RangeSetMaxConvergenceTime(dev, u8);
|
||
if (status < 0) {
|
||
VL6180x_ErrLog("fail to apply time after ece m/d change");
|
||
break;
|
||
}
|
||
} else {
|
||
VL6180x_ErrLog("invalid factor %d/%d", (int)FactorM, (int)FactorD);
|
||
status = INVALID_PARAMS;
|
||
}
|
||
} while (0);
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_RangeSetEceState(VL6180xDev_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 = VL6180x_UpdateByte(dev, SYSRANGE_RANGE_CHECK_ENABLES,
|
||
~RANGE_CHECK_ECE_ENABLE_MASK, or_mask);
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_RangeSetMaxConvergenceTime(VL6180xDev_t dev,
|
||
uint8_t MaxConTime_msec) {
|
||
int status = 0;
|
||
LOG_FUNCTION_START("%d", (int)MaxConTime_msec);
|
||
do {
|
||
status =
|
||
VL6180x_WrByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, MaxConTime_msec);
|
||
if (status) {
|
||
break;
|
||
}
|
||
status = VL6180x_RangeSetEarlyConvergenceEestimateThreshold(dev);
|
||
if (status) {
|
||
break;
|
||
}
|
||
status = _DMax_InitData(dev);
|
||
} while (0);
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_RangeSetInterMeasPeriod(VL6180xDev_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 = VL6180x_WrByte(dev, SYSRANGE_INTERMEASUREMENT_PERIOD, SetTime);
|
||
if (status) {
|
||
VL6180x_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 VL6180x_RangeGetDeviceReady(VL6180xDev_t dev, int *Ready) {
|
||
int status;
|
||
uint8_t u8;
|
||
LOG_FUNCTION_START("%p", (int)Ready);
|
||
status = VL6180x_RdByte(dev, RESULT_RANGE_STATUS, &u8);
|
||
if (!status)
|
||
*Ready = u8 & RANGE_DEVICE_READY_MASK;
|
||
LOG_FUNCTION_END_FMT(status, "%d", *Ready);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_RangeWaitDeviceReady(VL6180xDev_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 = VL6180x_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 VL6180x_RangeSetSystemMode(VL6180xDev_t dev, uint8_t mode) {
|
||
int status;
|
||
LOG_FUNCTION_START("%d", (int)mode);
|
||
/* FIXME we are not checking device is ready via @a
|
||
* VL6180x_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 = VL6180x_WrByte(dev, SYSRANGE_START, mode);
|
||
if (status) {
|
||
VL6180x_ErrLog("SYSRANGE_START wr fail");
|
||
}
|
||
} else {
|
||
status = INVALID_PARAMS;
|
||
}
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_RangeStartContinuousMode(VL6180xDev_t dev) {
|
||
int status;
|
||
LOG_FUNCTION_START("");
|
||
status = VL6180x_RangeSetSystemMode(dev, MODE_START_STOP | MODE_CONTINUOUS);
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_RangeStartSingleShot(VL6180xDev_t dev) {
|
||
int status;
|
||
LOG_FUNCTION_START("");
|
||
status = VL6180x_RangeSetSystemMode(dev, MODE_START_STOP | MODE_SINGLESHOT);
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
static int
|
||
VL6180x_RangeSetEarlyConvergenceEestimateThreshold(VL6180xDev_t dev) {
|
||
int status;
|
||
|
||
const uint32_t cMicroSecPerMilliSec = 1000;
|
||
const uint32_t cEceSampleTime_us = 500;
|
||
uint32_t ece_factor_m = VL6180xDevDataGet(dev, EceFactorM);
|
||
uint32_t ece_factor_d = VL6180xDevDataGet(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 = VL6180x_RdByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, &u8);
|
||
if (status) {
|
||
VL6180x_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 = VL6180x_RdDWord(dev, 0xB8, &fineThresh);
|
||
if (status) {
|
||
VL6180x_ErrLog("reg 0xB8 rd fail");
|
||
break;
|
||
}
|
||
fineThresh *= 256;
|
||
eceThresh = ece_factor_m * cEceSampleTime_us * fineThresh /
|
||
(convergTime_us * ece_factor_d);
|
||
|
||
status = VL6180x_WrWord(dev, SYSRANGE_EARLY_CONVERGENCE_ESTIMATE,
|
||
(uint16_t)eceThresh);
|
||
} while (0);
|
||
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
static int _RangeIgnore_UpdateDevice(VL6180xDev_t dev) {
|
||
int status;
|
||
int enable;
|
||
int threshold;
|
||
int range;
|
||
int or_mask;
|
||
enable = VL6180xDevDataGet(dev, RangeIgnore.Enabled);
|
||
if (enable) {
|
||
// if to be nabled program first range value and threshold
|
||
range = VL6180xDevDataGet(dev, RangeIgnore.ValidHeight);
|
||
range /= _GetUpscale(dev);
|
||
if (range > 255)
|
||
range = 255;
|
||
|
||
status = VL6180x_WrByte(dev, SYSRANGE_RANGE_IGNORE_VALID_HEIGHT, range);
|
||
if (status) {
|
||
goto done;
|
||
}
|
||
|
||
threshold = VL6180xDevDataGet(dev, RangeIgnore.IgnoreThreshold);
|
||
status = VL6180x_WrWord(dev, SYSRANGE_RANGE_IGNORE_THRESHOLD, threshold);
|
||
if (status) {
|
||
goto done;
|
||
}
|
||
or_mask = RANGE_CHECK_RANGE_ENABLE_MASK;
|
||
} else {
|
||
or_mask = 0;
|
||
}
|
||
status = VL6180x_UpdateByte(dev, SYSRANGE_RANGE_CHECK_ENABLES,
|
||
~RANGE_CHECK_RANGE_ENABLE_MASK, or_mask);
|
||
_DMax_InitData(dev);
|
||
done:
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_RangeIgnoreSetEnable(VL6180xDev_t dev, int EnableState) {
|
||
int CurEnable;
|
||
int status;
|
||
LOG_FUNCTION_START("enable %d", EnableState);
|
||
|
||
if (EnableState)
|
||
EnableState = 1;
|
||
|
||
CurEnable = VL6180xDevDataGet(dev, RangeIgnore.Enabled);
|
||
if (EnableState != CurEnable) {
|
||
VL6180xDevDataSet(dev, RangeIgnore.Enabled, EnableState);
|
||
status = _RangeIgnore_UpdateDevice(dev);
|
||
}
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_RangeIgnoreConfigure(VL6180xDev_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 = VL6180xDevDataGet(dev, RangeIgnore.Enabled);
|
||
VL6180xDevDataSet(dev, RangeIgnore.ValidHeight, ValidHeight_mm);
|
||
VL6180xDevDataSet(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(VL6180xDev_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 = VL6180x_RdByte(dev, 0x109, &u8);
|
||
if (status) {
|
||
VL6180x_ErrLog("rd 0x109 fail");
|
||
return -1;
|
||
}
|
||
Samples = u8 & cMeasMask;
|
||
status = VL6180x_RdByte(dev, READOUT_AVERAGING_SAMPLE_PERIOD, &u8);
|
||
if (status) {
|
||
VL6180x_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 VL6180x_HAVE_DMAX_RANGING
|
||
#define _GetDMaxDataRetSignalAt400mm(dev) \
|
||
VL6180xDevDataGet(dev, DMaxData.retSignalAt400mm)
|
||
#else
|
||
#define _GetDMaxDataRetSignalAt400mm(dev) 375 /* Use a default high value */
|
||
#endif
|
||
|
||
#if VL6180x_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) VL6180xDevDataGet(dev, FilterData.field)
|
||
/*
|
||
* One time init
|
||
*/
|
||
int _filter_Init(VL6180xDev_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(VL6180xDev_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 VL6180x_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 = VL6180xDevDataGet(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 = VL6180x_RdByte(dev, 0x01AC, &u8);
|
||
if (status) {
|
||
VL6180x_ErrLog("0x01AC rd fail");
|
||
goto done_err;
|
||
}
|
||
registerValue = u8;
|
||
|
||
/* Read data for filtering */
|
||
#if VL6180x_HAVE_MULTI_READ
|
||
status =
|
||
VL6180x_RdMulti(dev, 0x10C, MultiReadBuf, 8); /* read only 8 lsb bits */
|
||
if (status) {
|
||
VL6180x_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 = VL6180x_RdDWord(dev, 0x10C,
|
||
®ister32BitsValue1); /* read 32 bits, lower 17
|
||
bits are the one useful */
|
||
if (status) {
|
||
VL6180x_ErrLog("0x010C rd fail");
|
||
goto done_err;
|
||
}
|
||
status = VL6180x_RdDWord(dev, 0x0110,
|
||
®ister32BitsValue2); /* read 32 bits, lower 17
|
||
bits are the one useful */
|
||
if (status) {
|
||
VL6180x_ErrLog("0x0110 rd fail");
|
||
goto done_err;
|
||
}
|
||
#endif
|
||
|
||
if ((FlushFilter == 1) || ((bypassFilter == 1) && (resetVAVGData == 1))) {
|
||
if (registerValue != 0x3E) {
|
||
status = VL6180x_WrByte(dev, 0x1AC, 0x3E);
|
||
if (status) {
|
||
VL6180x_ErrLog("0x01AC bypass wr fail");
|
||
goto done_err;
|
||
}
|
||
// status = VL6180x_WrByte(dev, 0x0F2, 0x01);
|
||
// if (status) {
|
||
// VL6180x_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 = VL6180x_WrByte(dev, 0x01AC, u8);
|
||
if (status) {
|
||
VL6180x_ErrLog("0x01AC wr fail");
|
||
goto done_err;
|
||
}
|
||
// status = VL6180x_WrByte(dev, 0x0F2, u8_2);
|
||
// if (status) {
|
||
// VL6180x_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(VL6180xDev_t dev,
|
||
VL6180x_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 = VL6180x_GetCachedByte(dev, RESULT_RANGE_RAW, &u8);
|
||
if (status) {
|
||
VL6180x_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 /* VL6180x_WRAP_AROUND_FILTER_SUPPORT */
|
||
|
||
#ifdef VL6180x_HAVE_RATE_DATA
|
||
|
||
static int _GetRateResult(VL6180xDev_t dev, VL6180x_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 = VL6180x_GetCachedDWord(dev, RESULT_RANGE_RETURN_SIGNAL_COUNT,
|
||
&m_rtnSignalCount);
|
||
if (status) {
|
||
VL6180x_ErrLog("RESULT_RANGE_RETURN_SIGNAL_COUNT rd fail");
|
||
break;
|
||
}
|
||
if (m_rtnSignalCount > cRtnSignalCountMax) {
|
||
m_rtnSignalCount = 0;
|
||
}
|
||
|
||
status = VL6180x_GetCachedDWord(dev, RESULT_RANGE_RETURN_AMB_COUNT,
|
||
&m_rtnAmbientCount);
|
||
if (status) {
|
||
VL6180x_ErrLog("RESULT_RANGE_RETURN_AMB_COUNTrd fail");
|
||
break;
|
||
}
|
||
|
||
status = VL6180x_GetCachedDWord(dev, RESULT_RANGE_RETURN_CONV_TIME,
|
||
&m_rtnConvTime);
|
||
if (status) {
|
||
VL6180x_ErrLog("RESULT_RANGE_RETURN_CONV_TIME rd fail");
|
||
break;
|
||
}
|
||
|
||
status = VL6180x_GetCachedDWord(dev, RESULT_RANGE_REFERENCE_CONV_TIME,
|
||
&m_refConvTime);
|
||
if (status) {
|
||
VL6180x_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 /* VL6180x_HAVE_RATE_DATA */
|
||
|
||
int VL6180x_DMaxSetState(VL6180xDev_t dev, int state) {
|
||
int status;
|
||
LOG_FUNCTION_START("%d", state);
|
||
#if VL6180x_HAVE_DMAX_RANGING
|
||
VL6180xDevDataSet(dev, DMaxEnable, state);
|
||
if (state) {
|
||
status = _DMax_InitData(dev);
|
||
} else {
|
||
status = 0;
|
||
}
|
||
#else
|
||
status = NOT_SUPPORTED;
|
||
#endif
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
int VL6180x_DMaxGetState(VL6180xDev_t dev) {
|
||
int status;
|
||
LOG_FUNCTION_START("");
|
||
#if VL6180x_HAVE_DMAX_RANGING
|
||
status = VL6180xDevDataGet(dev, DMaxEnable);
|
||
#else
|
||
status = 0;
|
||
#endif
|
||
LOG_FUNCTION_END(status);
|
||
return status;
|
||
}
|
||
|
||
#if VL6180x_HAVE_DMAX_RANGING
|
||
|
||
#define _DMaxData(field) VL6180xDevDataGet(dev, DMaxData.field)
|
||
/*
|
||
* Convert fix point x.7 to KCpount per sec
|
||
*/
|
||
|
||
#ifndef VL6180x_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 VL6180x_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(VL6180xDev_t dev) {
|
||
_DMaxData(ambTuningWindowFactor_K) = DEF_AMBIENT_TUNING;
|
||
}
|
||
|
||
static uint32_t _DMax_RawValueAtRateKCps(VL6180xDev_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 = VL6180x_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(VL6180xDev_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 = VL6180x_RdByte(dev, 0x02A, &u8);
|
||
if (status) {
|
||
VL6180x_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 =
|
||
VL6180x_RdByte(dev, SYSRANGE_RANGE_CHECK_ENABLES, &SysRangeCheckEn);
|
||
if (status) {
|
||
VL6180x_ErrLog("SYSRANGE_RANGE_CHECK_ENABLES rd fail ");
|
||
break;
|
||
}
|
||
|
||
status = VL6180x_RdByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, &MaxConvTime);
|
||
if (status) {
|
||
VL6180x_ErrLog("SYSRANGE_MAX_CONVERGENCE_TIME rd fail ");
|
||
break;
|
||
}
|
||
|
||
status = VL6180x_RdDWord(dev, 0x0B8, &RegB8);
|
||
if (status) {
|
||
VL6180x_ErrLog("reg 0x0B8 rd fail ");
|
||
break;
|
||
}
|
||
|
||
status = VL6180x_RdByte(dev, SYSRANGE_MAX_AMBIENT_LEVEL_MULT, &snrLimit);
|
||
if (status) {
|
||
VL6180x_ErrLog("SYSRANGE_MAX_AMBIENT_LEVEL_MULT rd fail ");
|
||
break;
|
||
}
|
||
_DMaxData(snrLimit_K) = (int32_t)16 * 1000 / snrLimit;
|
||
XTalkCompRate_KCps = VL6180xDevDataGet(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 = VL6180x_RdWord(dev, SYSRANGE_RANGE_IGNORE_THRESHOLD, &u16);
|
||
if (status) {
|
||
VL6180x_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(VL6180xDev_t dev, VL6180x_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 /* VL6180x_HAVE_DMAX_RANGING */
|