Mustapha hamdi / Light_Sensor_Nucleo

INSAT Mini Project

Dependencies:   ST_INTERFACES X_NUCLEO_COMMON

Fork of X_NUCLEO_6180XA1 by ST

Components/VL6180X/vl6180x_class.cpp@9:9cbc1736f148, 2015-09-29 (annotated)

Committer:
licio.mapelli@st.com
Date:
Tue Sep 29 15:40:08 2015 +0200
Revision:
9:9cbc1736f148
Parent:
8:f943a1fca15f
Child:
10:4954b09b72d8
Fixed i2c wr

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gallonm 1:1de1ea2994d9 1 /**
gallonm 1:1de1ea2994d9 2 ******************************************************************************
gallonm 1:1de1ea2994d9 3 * @file vl6180x_class.cpp
gallonm 1:1de1ea2994d9 4 * @author AST / EST
gallonm 1:1de1ea2994d9 5 * @version V0.0.1
gallonm 1:1de1ea2994d9 6 * @date 14-April-2015
gallonm 1:1de1ea2994d9 7 * @brief Implementation file for the HTS221 driver class
gallonm 1:1de1ea2994d9 8 ******************************************************************************
gallonm 1:1de1ea2994d9 9 * @attention
gallonm 1:1de1ea2994d9 10 *
gallonm 1:1de1ea2994d9 11 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
gallonm 1:1de1ea2994d9 12 *
gallonm 1:1de1ea2994d9 13 * Redistribution and use in source and binary forms, with or without modification,
gallonm 1:1de1ea2994d9 14 * are permitted provided that the following conditions are met:
gallonm 1:1de1ea2994d9 15 * 1. Redistributions of source code must retain the above copyright notice,
gallonm 1:1de1ea2994d9 16 * this list of conditions and the following disclaimer.
gallonm 1:1de1ea2994d9 17 * 2. Redistributions in binary form must reproduce the above copyright notice,
gallonm 1:1de1ea2994d9 18 * this list of conditions and the following disclaimer in the documentation
gallonm 1:1de1ea2994d9 19 * and/or other materials provided with the distribution.
gallonm 1:1de1ea2994d9 20 * 3. Neither the name of STMicroelectronics nor the names of its contributors
gallonm 1:1de1ea2994d9 21 * may be used to endorse or promote products derived from this software
gallonm 1:1de1ea2994d9 22 * without specific prior written permission.
gallonm 1:1de1ea2994d9 23 *
gallonm 1:1de1ea2994d9 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
gallonm 1:1de1ea2994d9 25 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
gallonm 1:1de1ea2994d9 26 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
gallonm 1:1de1ea2994d9 27 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
gallonm 1:1de1ea2994d9 28 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
gallonm 1:1de1ea2994d9 29 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
gallonm 1:1de1ea2994d9 30 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
gallonm 1:1de1ea2994d9 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
gallonm 1:1de1ea2994d9 32 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
gallonm 1:1de1ea2994d9 33 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
gallonm 1:1de1ea2994d9 34 *
gallonm 1:1de1ea2994d9 35 ******************************************************************************
gallonm 1:1de1ea2994d9 36 */
gallonm 1:1de1ea2994d9 37
gallonm 7:2dc81120c917 38 /* Includes */
gallonm 1:1de1ea2994d9 39 #include "vl6180x_class.h"
licio.mapelli@st.com 8:f943a1fca15f 40
licio.mapelli@st.com 8:f943a1fca15f 41
licio.mapelli@st.com 8:f943a1fca15f 42 #ifndef VL6180x_RANGE_STATUS_ERRSTRING
licio.mapelli@st.com 8:f943a1fca15f 43 #warning "VL6180x_RANGE_STATUS_ERRSTRING not defined ?"
licio.mapelli@st.com 8:f943a1fca15f 44 /* TODO you may remove or comment these #warning and keep the default below to keep compatibility
licio.mapelli@st.com 8:f943a1fca15f 45 or update your vl6180x_platform.h file */
licio.mapelli@st.com 8:f943a1fca15f 46 /**
licio.mapelli@st.com 8:f943a1fca15f 47 * force VL6180x_RANGE_STATUS_ERRSTRING to not supported when not part of any cfg file
licio.mapelli@st.com 8:f943a1fca15f 48 */
licio.mapelli@st.com 8:f943a1fca15f 49 #define VL6180x_RANGE_STATUS_ERRSTRING 0
licio.mapelli@st.com 8:f943a1fca15f 50 #endif
licio.mapelli@st.com 8:f943a1fca15f 51
licio.mapelli@st.com 8:f943a1fca15f 52 #ifndef VL6180X_SAFE_POLLING_ENTER
licio.mapelli@st.com 8:f943a1fca15f 53 #warning "VL6180X_SAFE_POLLING_ENTER not defined, likely old vl6180x_cfg.h file ?"
licio.mapelli@st.com 8:f943a1fca15f 54 /* TODO you may remove or comment these #warning and keep the default below to keep compatibility
licio.mapelli@st.com 8:f943a1fca15f 55 or update your vl6180x_platform.h file */
licio.mapelli@st.com 8:f943a1fca15f 56 /**
licio.mapelli@st.com 8:f943a1fca15f 57 * force VL6180X_SAFE_POLLING_ENTER to off when not in cfg file
licio.mapelli@st.com 8:f943a1fca15f 58 */
licio.mapelli@st.com 8:f943a1fca15f 59 #define VL6180X_SAFE_POLLING_ENTER 0 /* off by default as in api 2.0 */
licio.mapelli@st.com 8:f943a1fca15f 60 #endif
licio.mapelli@st.com 8:f943a1fca15f 61
licio.mapelli@st.com 8:f943a1fca15f 62 #ifndef VL6180X_LOG_ENABLE
licio.mapelli@st.com 8:f943a1fca15f 63 /**
licio.mapelli@st.com 8:f943a1fca15f 64 * Force VL6180X_LOG_ENABLE to none as default
licio.mapelli@st.com 8:f943a1fca15f 65 */
licio.mapelli@st.com 8:f943a1fca15f 66 #define VL6180X_LOG_ENABLE 0
licio.mapelli@st.com 8:f943a1fca15f 67 #endif
licio.mapelli@st.com 8:f943a1fca15f 68
licio.mapelli@st.com 8:f943a1fca15f 69 #if VL6180x_RANGE_STATUS_ERRSTRING
licio.mapelli@st.com 8:f943a1fca15f 70 /**@def VL6180x_HAVE_RANGE_STATUS_ERRSTRING
licio.mapelli@st.com 8:f943a1fca15f 71 * @brief is defined when @a #VL6180x_RANGE_STATUS_ERRSTRING is enable
licio.mapelli@st.com 8:f943a1fca15f 72 */
licio.mapelli@st.com 8:f943a1fca15f 73 #define VL6180x_HAVE_RANGE_STATUS_ERRSTRING
licio.mapelli@st.com 8:f943a1fca15f 74 #endif
licio.mapelli@st.com 8:f943a1fca15f 75
licio.mapelli@st.com 8:f943a1fca15f 76
licio.mapelli@st.com 8:f943a1fca15f 77 /** @brief Get API version as "hex integer" 0xMMnnss
licio.mapelli@st.com 8:f943a1fca15f 78 */
licio.mapelli@st.com 8:f943a1fca15f 79 #define VL6180x_ApiRevInt ((VL6180x_API_REV_MAJOR<<24)+(VL6180x_API_REV_MINOR<<16)+VL6180x_API_REV_SUB)
licio.mapelli@st.com 8:f943a1fca15f 80
licio.mapelli@st.com 8:f943a1fca15f 81 /** Get API version as string for exe "2.1.12" "
licio.mapelli@st.com 8:f943a1fca15f 82 */
licio.mapelli@st.com 8:f943a1fca15f 83 #define VL6180x_ApiRevStr VL6180X_STR(VL6180x_API_REV_MAJOR) "." VL6180X_STR(VL6180x_API_REV_MINOR) "." VL6180X_STR(VL6180x_API_REV_SUB)
licio.mapelli@st.com 8:f943a1fca15f 84
licio.mapelli@st.com 8:f943a1fca15f 85 /** @defgroup api_init Init functions
licio.mapelli@st.com 8:f943a1fca15f 86 * @brief API init functions
licio.mapelli@st.com 8:f943a1fca15f 87 * @ingroup api_hl
licio.mapelli@st.com 8:f943a1fca15f 88 * @{
licio.mapelli@st.com 8:f943a1fca15f 89 */
licio.mapelli@st.com 8:f943a1fca15f 90
gallonm 1:1de1ea2994d9 91
gallonm 7:2dc81120c917 92 /****************** define for i2c configuration *******************************/
gallonm 4:a5abf7757947 93 #define TEMP_BUF_SIZE 32
gallonm 4:a5abf7757947 94
gallonm 7:2dc81120c917 95 #define IsValidGPIOFunction(x) ((x)==GPIOx_SELECT_GPIO_INTERRUPT_OUTPUT || (x)==GPIOx_SELECT_OFF)
licio.mapelli@st.com 8:f943a1fca15f 96 /**
licio.mapelli@st.com 8:f943a1fca15f 97 * @brief Clear All interrupt causes (als+range+error)
licio.mapelli@st.com 8:f943a1fca15f 98 *
licio.mapelli@st.com 8:f943a1fca15f 99 * @param dev The device
licio.mapelli@st.com 8:f943a1fca15f 100 * @return 0 On success
licio.mapelli@st.com 8:f943a1fca15f 101 */
licio.mapelli@st.com 8:f943a1fca15f 102 #define VL6180x_ClearAllInterrupt(dev) VL6180x_ClearInterrupt(dev, INTERRUPT_CLEAR_ERROR|INTERRUPT_CLEAR_RANGING|INTERRUPT_CLEAR_ALS)
licio.mapelli@st.com 8:f943a1fca15f 103 /**
licio.mapelli@st.com 8:f943a1fca15f 104 * @brief Clear ALS interrupt
licio.mapelli@st.com 8:f943a1fca15f 105 *
licio.mapelli@st.com 8:f943a1fca15f 106 * @param dev The device
licio.mapelli@st.com 8:f943a1fca15f 107 * @return 0 On success
licio.mapelli@st.com 8:f943a1fca15f 108 */
licio.mapelli@st.com 8:f943a1fca15f 109 #define VL6180x_AlsClearInterrupt(dev) VL6180x_ClearInterrupt(dev, INTERRUPT_CLEAR_ALS)
licio.mapelli@st.com 8:f943a1fca15f 110 /**
licio.mapelli@st.com 8:f943a1fca15f 111 * @brief Clear range interrupt
licio.mapelli@st.com 8:f943a1fca15f 112 *
licio.mapelli@st.com 8:f943a1fca15f 113 * @param dev The device
licio.mapelli@st.com 8:f943a1fca15f 114 * @return 0 On success
licio.mapelli@st.com 8:f943a1fca15f 115 */
licio.mapelli@st.com 8:f943a1fca15f 116 #define VL6180x_RangeClearInterrupt(dev) VL6180x_ClearInterrupt(dev, INTERRUPT_CLEAR_RANGING)
gallonm 7:2dc81120c917 117
gallonm 7:2dc81120c917 118 /******************************************************************************/
gallonm 7:2dc81120c917 119 /******************************* file api.c ***********************************/
gallonm 7:2dc81120c917 120
gallonm 7:2dc81120c917 121 #define VL6180x_9to7Conv(x) (x)
gallonm 7:2dc81120c917 122
gallonm 7:2dc81120c917 123 /* TODO when set all "cached" value with "default init" are updated after init from register read back */
gallonm 7:2dc81120c917 124 #define REFRESH_CACHED_DATA_AFTER_INIT 1
gallonm 7:2dc81120c917 125
gallonm 7:2dc81120c917 126
gallonm 7:2dc81120c917 127 #define IsValidGPIOFunction(x) ((x)==GPIOx_SELECT_GPIO_INTERRUPT_OUTPUT || (x)==GPIOx_SELECT_OFF)
gallonm 7:2dc81120c917 128
gallonm 7:2dc81120c917 129
gallonm 7:2dc81120c917 130 /** default value ECE factor Molecular */
gallonm 7:2dc81120c917 131 #define DEF_ECE_FACTOR_M 85
gallonm 7:2dc81120c917 132 /** default value ECE factor Denominator */
gallonm 7:2dc81120c917 133 #define DEF_ECE_FACTOR_D 100
gallonm 7:2dc81120c917 134 /** default value ALS integration time */
gallonm 7:2dc81120c917 135 #define DEF_INT_PEFRIOD 100
gallonm 7:2dc81120c917 136 /** default value ALS gain */
gallonm 7:2dc81120c917 137 #define DEF_ALS_GAIN 1
gallonm 7:2dc81120c917 138 /** default value ALS scaler */
gallonm 7:2dc81120c917 139 #define DEF_ALS_SCALER 1
gallonm 7:2dc81120c917 140 /** default value for DMAX Enbale */
gallonm 7:2dc81120c917 141 #define DEF_DMAX_ENABLE 1
gallonm 7:2dc81120c917 142 /** default ambient tuning factor %x1000 */
gallonm 7:2dc81120c917 143 #define DEF_AMBIENT_TUNING 80
gallonm 7:2dc81120c917 144
gallonm 7:2dc81120c917 145
gallonm 7:2dc81120c917 146 #define LUXRES_FIX_PREC 8
gallonm 7:2dc81120c917 147 #define GAIN_FIX_PREC 8 /* ! if not sme as LUX_PREC then :( adjust GetLux */
gallonm 7:2dc81120c917 148 #define AN_GAIN_MULT (1<<GAIN_FIX_PREC)
gallonm 7:2dc81120c917 149
gallonm 7:2dc81120c917 150
gallonm 7:2dc81120c917 151 static int32_t _GetAveTotalTime(VL6180xDev_t dev);
gallonm 7:2dc81120c917 152 static int VL6180x_RangeSetEarlyConvergenceEestimateThreshold(VL6180xDev_t dev);
gallonm 7:2dc81120c917 153
gallonm 7:2dc81120c917 154 /**
gallonm 7:2dc81120c917 155 * ScalerLookUP scaling factor-1 to register #RANGE_SCALER lookup
gallonm 7:2dc81120c917 156 */
gallonm 7:2dc81120c917 157 static const uint16_t ScalerLookUP[] ROMABLE_DATA ={ 253, 127, 84}; /* lookup table for scaling->scalar 1x2x 3x */
gallonm 7:2dc81120c917 158 /**
gallonm 7:2dc81120c917 159 * scaling factor to Upper limit look up
gallonm 7:2dc81120c917 160 */
gallonm 7:2dc81120c917 161 static const uint16_t UpperLimitLookUP[] ROMABLE_DATA ={ 185, 370, 580}; /* lookup table for scaling->limit 1x2x3x */
gallonm 7:2dc81120c917 162 /**
gallonm 7:2dc81120c917 163 * Als Code gain to fix point gain lookup
gallonm 7:2dc81120c917 164 */
gallonm 7:2dc81120c917 165 static const uint16_t AlsGainLookUp[8] ROMABLE_DATA = {
gallonm 7:2dc81120c917 166 (uint16_t)(20.0f * AN_GAIN_MULT),
gallonm 7:2dc81120c917 167 (uint16_t)(10.0f * AN_GAIN_MULT),
gallonm 7:2dc81120c917 168 (uint16_t)(5.0f * AN_GAIN_MULT),
gallonm 7:2dc81120c917 169 (uint16_t)(2.5f * AN_GAIN_MULT),
gallonm 7:2dc81120c917 170 (uint16_t)(1.67f * AN_GAIN_MULT),
gallonm 7:2dc81120c917 171 (uint16_t)(1.25f * AN_GAIN_MULT),
gallonm 7:2dc81120c917 172 (uint16_t)(1.0f * AN_GAIN_MULT),
gallonm 7:2dc81120c917 173 (uint16_t)(40.0f * AN_GAIN_MULT),
gallonm 7:2dc81120c917 174 };
gallonm 7:2dc81120c917 175
gallonm 7:2dc81120c917 176
gallonm 7:2dc81120c917 177 #if VL6180x_RANGE_STATUS_ERRSTRING
gallonm 7:2dc81120c917 178 const char * ROMABLE_DATA VL6180x_RangeStatusErrString[]={
gallonm 7:2dc81120c917 179 "No Error",
gallonm 7:2dc81120c917 180 "VCSEL Continuity Test",
gallonm 7:2dc81120c917 181 "VCSEL Watchdog Test",
gallonm 7:2dc81120c917 182 "VCSEL Watchdog",
gallonm 7:2dc81120c917 183 "PLL1 Lock",
gallonm 7:2dc81120c917 184 "PLL2 Lock",
gallonm 7:2dc81120c917 185 "Early Convergence Estimate",
gallonm 7:2dc81120c917 186 "Max Convergence",
gallonm 7:2dc81120c917 187 "No Target Ignore",
gallonm 7:2dc81120c917 188 "Not used 9",
gallonm 7:2dc81120c917 189 "Not used 10",
gallonm 7:2dc81120c917 190 "Max Signal To Noise Ratio",
gallonm 7:2dc81120c917 191 "Raw Ranging Algo Underflow",
gallonm 7:2dc81120c917 192 "Raw Ranging Algo Overflow",
gallonm 7:2dc81120c917 193 "Ranging Algo Underflow",
gallonm 7:2dc81120c917 194 "Ranging Algo Overflow",
gallonm 7:2dc81120c917 195
gallonm 7:2dc81120c917 196 "Filtered by post processing"
gallonm 7:2dc81120c917 197 };
gallonm 7:2dc81120c917 198
gallonm 7:2dc81120c917 199 const char * VL6180x_RangeGetStatusErrString(uint8_t RangeErrCode){
gallonm 7:2dc81120c917 200 if( RangeErrCode > sizeof(VL6180x_RangeStatusErrString)/sizeof(VL6180x_RangeStatusErrString[0]) )
gallonm 7:2dc81120c917 201 return NULL;
gallonm 7:2dc81120c917 202 return VL6180x_RangeStatusErrString[RangeErrCode];
gallonm 7:2dc81120c917 203 }
gallonm 7:2dc81120c917 204 #endif
gallonm 7:2dc81120c917 205
gallonm 7:2dc81120c917 206 #if VL6180x_UPSCALE_SUPPORT == 1
gallonm 7:2dc81120c917 207 #define _GetUpscale(dev, ... ) 1
gallonm 7:2dc81120c917 208 #define _SetUpscale(...) -1
gallonm 7:2dc81120c917 209 #define DEF_UPSCALE 1
gallonm 7:2dc81120c917 210 #elif VL6180x_UPSCALE_SUPPORT == 2
gallonm 7:2dc81120c917 211 #define _GetUpscale(dev, ... ) 2
gallonm 7:2dc81120c917 212 #define _SetUpscale(...)
gallonm 7:2dc81120c917 213 #define DEF_UPSCALE 2
gallonm 7:2dc81120c917 214 #elif VL6180x_UPSCALE_SUPPORT == 3
gallonm 7:2dc81120c917 215 #define _GetUpscale(dev, ... ) 3
gallonm 7:2dc81120c917 216 #define _SetUpscale(...)
gallonm 7:2dc81120c917 217 #define DEF_UPSCALE 3
gallonm 7:2dc81120c917 218 #else
gallonm 7:2dc81120c917 219 #define DEF_UPSCALE (-(VL6180x_UPSCALE_SUPPORT))
gallonm 7:2dc81120c917 220 #define _GetUpscale(dev, ... ) VL6180xDevDataGet(dev, UpscaleFactor)
gallonm 7:2dc81120c917 221 #define _SetUpscale(dev, Scaling ) VL6180xDevDataSet(dev, UpscaleFactor, Scaling)
gallonm 7:2dc81120c917 222 #endif
gallonm 7:2dc81120c917 223
gallonm 7:2dc81120c917 224 #define Fix7_2_KCPs(x) ((((uint32_t)(x))*1000)>>7)
gallonm 7:2dc81120c917 225
gallonm 7:2dc81120c917 226
gallonm 7:2dc81120c917 227 #if VL6180x_WRAP_AROUND_FILTER_SUPPORT || VL6180x_HAVE_DMAX_RANGING
gallonm 7:2dc81120c917 228 static int _GetRateResult(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData);
gallonm 7:2dc81120c917 229 #endif
gallonm 7:2dc81120c917 230
gallonm 7:2dc81120c917 231 #if VL6180x_WRAP_AROUND_FILTER_SUPPORT
gallonm 7:2dc81120c917 232 static int _filter_Init(VL6180xDev_t dev);
gallonm 7:2dc81120c917 233 static int _filter_GetResult(VL6180xDev_t dev, VL6180x_RangeData_t *pData);
gallonm 7:2dc81120c917 234 #define _IsWrapArroundActive(dev) VL6180xDevDataGet(dev,WrapAroundFilterActive)
gallonm 7:2dc81120c917 235 #else
gallonm 7:2dc81120c917 236 #define _IsWrapArroundActive(dev) 0
gallonm 7:2dc81120c917 237 #endif
gallonm 7:2dc81120c917 238
gallonm 7:2dc81120c917 239
gallonm 7:2dc81120c917 240 #if VL6180x_HAVE_DMAX_RANGING
gallonm 7:2dc81120c917 241 void _DMax_OneTimeInit(VL6180xDev_t dev);
gallonm 7:2dc81120c917 242 static int _DMax_InitData(VL6180xDev_t dev);
gallonm 7:2dc81120c917 243 static int _DMax_Compute(VL6180xDev_t dev, VL6180x_RangeData_t *pRange);
gallonm 7:2dc81120c917 244 #define _IsDMaxActive(dev) VL6180xDevDataGet(dev,DMaxEnable)
gallonm 7:2dc81120c917 245 #else
gallonm 7:2dc81120c917 246 #define _DMax_InitData(...) 0 /* success */
gallonm 7:2dc81120c917 247 #define _DMax_OneTimeInit(...) (void)0
gallonm 7:2dc81120c917 248 #define _IsDMaxActive(...) 0
gallonm 7:2dc81120c917 249 #endif
gallonm 7:2dc81120c917 250
gallonm 7:2dc81120c917 251 static int VL6180x_RangeStaticInit(VL6180xDev_t dev);
gallonm 7:2dc81120c917 252 static int VL6180x_UpscaleStaticInit(VL6180xDev_t dev);
gallonm 7:2dc81120c917 253
gallonm 7:2dc81120c917 254 int VL6180X::VL6180x_WaitDeviceBooted(VL6180xDev_t dev){
gallonm 7:2dc81120c917 255 uint8_t FreshOutReset;
gallonm 7:2dc81120c917 256 int status;
gallonm 7:2dc81120c917 257 LOG_FUNCTION_START("");
gallonm 7:2dc81120c917 258 do{
gallonm 7:2dc81120c917 259 status = VL6180x_RdByte(dev,SYSTEM_FRESH_OUT_OF_RESET, &FreshOutReset);
gallonm 7:2dc81120c917 260 }
gallonm 7:2dc81120c917 261 while( FreshOutReset!=1 && status==0);
gallonm 7:2dc81120c917 262 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 263 return status;
gallonm 7:2dc81120c917 264 }
gallonm 7:2dc81120c917 265
gallonm 7:2dc81120c917 266 int VL6180X::VL6180x_InitData(VL6180xDev_t dev){
gallonm 3:454541a079f4 267 int status, dmax_status ;
gallonm 3:454541a079f4 268 int8_t offset;
gallonm 3:454541a079f4 269 uint8_t FreshOutReset;
gallonm 3:454541a079f4 270 uint32_t CalValue;
gallonm 3:454541a079f4 271 uint16_t u16;
gallonm 3:454541a079f4 272 uint32_t XTalkCompRate_KCps;
gallonm 3:454541a079f4 273
gallonm 3:454541a079f4 274 LOG_FUNCTION_START("");
gallonm 3:454541a079f4 275
gallonm 3:454541a079f4 276 VL6180xDevDataSet(dev, EceFactorM , DEF_ECE_FACTOR_M);
gallonm 3:454541a079f4 277 VL6180xDevDataSet(dev, EceFactorD , DEF_ECE_FACTOR_D);
gallonm 3:454541a079f4 278
gallonm 3:454541a079f4 279 #ifdef VL6180x_HAVE_UPSCALE_DATA
gallonm 3:454541a079f4 280 VL6180xDevDataSet(dev, UpscaleFactor , DEF_UPSCALE);
gallonm 3:454541a079f4 281 #endif
gallonm 3:454541a079f4 282
gallonm 3:454541a079f4 283 #ifdef VL6180x_HAVE_ALS_DATA
gallonm 3:454541a079f4 284 VL6180xDevDataSet(dev, IntegrationPeriod, DEF_INT_PEFRIOD);
gallonm 3:454541a079f4 285 VL6180xDevDataSet(dev, AlsGainCode, DEF_ALS_GAIN);
gallonm 3:454541a079f4 286 VL6180xDevDataSet(dev, AlsScaler, DEF_ALS_SCALER);
gallonm 3:454541a079f4 287 #endif
gallonm 3:454541a079f4 288
gallonm 3:454541a079f4 289 #ifdef VL6180x_HAVE_WRAP_AROUND_DATA
gallonm 3:454541a079f4 290 VL6180xDevDataSet(dev, WrapAroundFilterActive, (VL6180x_WRAP_AROUND_FILTER_SUPPORT >0));
gallonm 3:454541a079f4 291 VL6180xDevDataSet(dev, DMaxEnable, DEF_DMAX_ENABLE);
gallonm 3:454541a079f4 292 #endif
gallonm 3:454541a079f4 293
gallonm 3:454541a079f4 294 _DMax_OneTimeInit(dev);
gallonm 3:454541a079f4 295 do{
gallonm 3:454541a079f4 296
gallonm 3:454541a079f4 297 /* backup offset initial value from nvm these must be done prior any over call that use offset */
gallonm 7:2dc81120c917 298 status = VL6180X::VL6180x_RdByte(dev,SYSRANGE_PART_TO_PART_RANGE_OFFSET, (uint8_t*)&offset);
gallonm 3:454541a079f4 299 if( status ){
gallonm 3:454541a079f4 300 VL6180x_ErrLog("SYSRANGE_PART_TO_PART_RANGE_OFFSET rd fail");
gallonm 3:454541a079f4 301 break;
gallonm 3:454541a079f4 302 }
gallonm 3:454541a079f4 303 VL6180xDevDataSet(dev, Part2PartOffsetNVM, offset);
gallonm 1:1de1ea2994d9 304
gallonm 7:2dc81120c917 305 status=VL6180x_RdDWord( dev, SYSRANGE_RANGE_IGNORE_THRESHOLD, &CalValue);
gallonm 3:454541a079f4 306 if( status ){
gallonm 3:454541a079f4 307 VL6180x_ErrLog("Part2PartAmbNVM rd fail");
gallonm 3:454541a079f4 308 break;
gallonm 3:454541a079f4 309 }
gallonm 3:454541a079f4 310 if( (CalValue&0xFFFF0000) == 0 ){
gallonm 3:454541a079f4 311 CalValue=0x00CE03F8;
gallonm 3:454541a079f4 312 }
gallonm 3:454541a079f4 313 VL6180xDevDataSet(dev, Part2PartAmbNVM, CalValue);
gallonm 3:454541a079f4 314
gallonm 7:2dc81120c917 315 status = VL6180x_RdWord(dev, SYSRANGE_CROSSTALK_COMPENSATION_RATE ,&u16);
gallonm 3:454541a079f4 316 if( status){
gallonm 3:454541a079f4 317 VL6180x_ErrLog("SYSRANGE_CROSSTALK_COMPENSATION_RATE rd fail ");
gallonm 3:454541a079f4 318 break;
gallonm 3:454541a079f4 319 }
gallonm 3:454541a079f4 320 XTalkCompRate_KCps = Fix7_2_KCPs(u16);
gallonm 3:454541a079f4 321 VL6180xDevDataSet(dev, XTalkCompRate_KCps , XTalkCompRate_KCps );
gallonm 1:1de1ea2994d9 322
gallonm 3:454541a079f4 323 dmax_status = _DMax_InitData(dev);
gallonm 3:454541a079f4 324 if( dmax_status < 0 ){
gallonm 3:454541a079f4 325 VL6180x_ErrLog("DMax init failure");
gallonm 3:454541a079f4 326 break;
gallonm 3:454541a079f4 327 }
gallonm 3:454541a079f4 328
gallonm 3:454541a079f4 329 /* Read or wait for fresh out of reset */
gallonm 7:2dc81120c917 330 status = VL6180x_RdByte(dev,SYSTEM_FRESH_OUT_OF_RESET, &FreshOutReset);
gallonm 3:454541a079f4 331 if( status ) {
gallonm 3:454541a079f4 332 VL6180x_ErrLog("SYSTEM_FRESH_OUT_OF_RESET rd fail");
gallonm 3:454541a079f4 333 break;
gallonm 3:454541a079f4 334 }
gallonm 3:454541a079f4 335 if( FreshOutReset!= 1 || dmax_status )
gallonm 3:454541a079f4 336 status = CALIBRATION_WARNING;
gallonm 3:454541a079f4 337
gallonm 3:454541a079f4 338 }
gallonm 3:454541a079f4 339 while(0);
gallonm 3:454541a079f4 340
gallonm 3:454541a079f4 341 LOG_FUNCTION_END(status);
gallonm 3:454541a079f4 342 return status;
gallonm 1:1de1ea2994d9 343 }
gallonm 1:1de1ea2994d9 344
gallonm 7:2dc81120c917 345 int8_t VL6180X::VL6180x_GetOffsetCalibrationData(VL6180xDev_t dev)
gallonm 7:2dc81120c917 346 {
gallonm 7:2dc81120c917 347 int8_t offset;
gallonm 7:2dc81120c917 348 LOG_FUNCTION_START("");
gallonm 7:2dc81120c917 349 offset = VL6180xDevDataGet(dev, Part2PartOffsetNVM);
gallonm 7:2dc81120c917 350 LOG_FUNCTION_END( offset );
gallonm 7:2dc81120c917 351 return offset;
gallonm 7:2dc81120c917 352 }
gallonm 7:2dc81120c917 353
gallonm 7:2dc81120c917 354 void VL6180X::VL6180x_SetOffsetCalibrationData(VL6180xDev_t dev, int8_t offset)
gallonm 7:2dc81120c917 355 {
gallonm 7:2dc81120c917 356 LOG_FUNCTION_START("%d", offset);
gallonm 7:2dc81120c917 357 VL6180xDevDataSet(dev, Part2PartOffsetNVM, offset);
gallonm 7:2dc81120c917 358 LOG_FUNCTION_END(0);
gallonm 7:2dc81120c917 359 }
gallonm 7:2dc81120c917 360
gallonm 7:2dc81120c917 361 int VL6180X::VL6180x_SetXTalkCompensationRate(VL6180xDev_t dev, FixPoint97_t Rate)
gallonm 7:2dc81120c917 362 {
gallonm 7:2dc81120c917 363 int status;
gallonm 7:2dc81120c917 364 LOG_FUNCTION_START("%d", Rate);
gallonm 7:2dc81120c917 365 status = VL6180x_WrWord(dev, SYSRANGE_CROSSTALK_COMPENSATION_RATE, Rate);
gallonm 7:2dc81120c917 366 if( status ==0 ){
gallonm 7:2dc81120c917 367 uint32_t XTalkCompRate_KCps;
gallonm 7:2dc81120c917 368 XTalkCompRate_KCps = Fix7_2_KCPs(Rate);
gallonm 7:2dc81120c917 369 VL6180xDevDataSet(dev, XTalkCompRate_KCps , XTalkCompRate_KCps );
gallonm 7:2dc81120c917 370 /* update dmax whenever xtalk rate changes */
gallonm 7:2dc81120c917 371 status = _DMax_InitData(dev);
gallonm 7:2dc81120c917 372 }
gallonm 7:2dc81120c917 373 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 374 return status;
gallonm 7:2dc81120c917 375 }
gallonm 7:2dc81120c917 376
gallonm 7:2dc81120c917 377 int VL6180X::VL6180x_SetI2CAddress(VL6180xDev_t dev, uint8_t NewAddress){
gallonm 7:2dc81120c917 378 int status;
gallonm 7:2dc81120c917 379 LOG_FUNCTION_START("");
gallonm 7:2dc81120c917 380
licio.mapelli@st.com 8:f943a1fca15f 381 status = VL6180x_WrByte(dev, I2C_SLAVE_DEVICE_ADDRESS, NewAddress);
gallonm 7:2dc81120c917 382 if( status ){
gallonm 7:2dc81120c917 383 VL6180x_ErrLog("new i2c addr Wr fail");
gallonm 7:2dc81120c917 384 }
licio.mapelli@st.com 9:9cbc1736f148 385 DevData.I2cAddr = NewAddress;
gallonm 7:2dc81120c917 386 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 387 return status;
gallonm 7:2dc81120c917 388 }
gallonm 7:2dc81120c917 389
licio.mapelli@st.com 9:9cbc1736f148 390
gallonm 7:2dc81120c917 391 uint16_t VL6180X::VL6180x_GetUpperLimit(VL6180xDev_t dev) {
gallonm 7:2dc81120c917 392 uint16_t limit;
gallonm 7:2dc81120c917 393 int scaling;
gallonm 7:2dc81120c917 394
gallonm 7:2dc81120c917 395 LOG_FUNCTION_START("");
gallonm 7:2dc81120c917 396
gallonm 7:2dc81120c917 397 scaling = _GetUpscale(dev);
gallonm 7:2dc81120c917 398 /* FIXME we do assume here _GetUpscale is valid if user call us prior to init we may overflow the LUT mem area */
gallonm 7:2dc81120c917 399 limit = UpperLimitLookUP[scaling - 1];
gallonm 7:2dc81120c917 400
gallonm 7:2dc81120c917 401 LOG_FUNCTION_END((int )limit);
gallonm 7:2dc81120c917 402 return limit;
gallonm 7:2dc81120c917 403 }
gallonm 7:2dc81120c917 404
gallonm 7:2dc81120c917 405
gallonm 7:2dc81120c917 406
gallonm 7:2dc81120c917 407 int VL6180X::VL6180x_StaticInit(VL6180xDev_t dev){
gallonm 7:2dc81120c917 408 int status=0, init_status;
gallonm 7:2dc81120c917 409 LOG_FUNCTION_START("");
gallonm 7:2dc81120c917 410
gallonm 7:2dc81120c917 411 /* TODO doc When using configurable scaling but using 1x as start condition
gallonm 7:2dc81120c917 412 * load tunning upscale or not ??? */
gallonm 7:2dc81120c917 413 if( _GetUpscale(dev) == 1 && !(VL6180x_UPSCALE_SUPPORT<0))
gallonm 7:2dc81120c917 414 init_status=VL6180x_RangeStaticInit(dev);
gallonm 7:2dc81120c917 415 else
gallonm 7:2dc81120c917 416 init_status=VL6180x_UpscaleStaticInit(dev);
gallonm 7:2dc81120c917 417
gallonm 7:2dc81120c917 418 if( init_status <0 ){
gallonm 7:2dc81120c917 419 VL6180x_ErrLog("StaticInit fail");
gallonm 7:2dc81120c917 420 goto error;
gallonm 7:2dc81120c917 421 }
gallonm 7:2dc81120c917 422 else if(init_status > 0){
gallonm 7:2dc81120c917 423 VL6180x_ErrLog("StaticInit warning");
gallonm 7:2dc81120c917 424 }
gallonm 7:2dc81120c917 425
gallonm 7:2dc81120c917 426 #if REFRESH_CACHED_DATA_AFTER_INIT
gallonm 7:2dc81120c917 427 /* update cached value after tuning applied */
gallonm 7:2dc81120c917 428 do{
gallonm 7:2dc81120c917 429 #ifdef VL6180x_HAVE_ALS_DATA
gallonm 7:2dc81120c917 430 uint8_t data;
gallonm 7:2dc81120c917 431 status= VL6180x_RdByte(dev, FW_ALS_RESULT_SCALER, &data);
gallonm 7:2dc81120c917 432 if( status ) break;
gallonm 7:2dc81120c917 433 VL6180xDevDataSet(dev, AlsScaler, data);
gallonm 7:2dc81120c917 434
gallonm 7:2dc81120c917 435 status= VL6180x_RdByte(dev, SYSALS_ANALOGUE_GAIN, &data);
gallonm 7:2dc81120c917 436 if( status ) break;
gallonm 7:2dc81120c917 437 VL6180x_AlsSetAnalogueGain(dev, data);
gallonm 7:2dc81120c917 438 #endif
gallonm 7:2dc81120c917 439 }
gallonm 7:2dc81120c917 440 while(0);
gallonm 7:2dc81120c917 441 #endif /* REFRESH_CACHED_DATA_AFTER_INIT */
gallonm 7:2dc81120c917 442 if( status < 0 ){
gallonm 7:2dc81120c917 443 VL6180x_ErrLog("StaticInit fail");
gallonm 7:2dc81120c917 444 }
gallonm 7:2dc81120c917 445 if( !status && init_status){
gallonm 7:2dc81120c917 446 status = init_status;
gallonm 7:2dc81120c917 447 }
gallonm 7:2dc81120c917 448 error:
gallonm 7:2dc81120c917 449 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 450 return status;
gallonm 7:2dc81120c917 451 }
gallonm 7:2dc81120c917 452
gallonm 7:2dc81120c917 453
gallonm 7:2dc81120c917 454 int VL6180X::VL6180x_SetGroupParamHold(VL6180xDev_t dev, int Hold)
gallonm 7:2dc81120c917 455 {
gallonm 7:2dc81120c917 456 int status;
gallonm 7:2dc81120c917 457 uint8_t value;
gallonm 7:2dc81120c917 458
gallonm 7:2dc81120c917 459 LOG_FUNCTION_START("%d", Hold);
gallonm 7:2dc81120c917 460 if( Hold )
gallonm 7:2dc81120c917 461 value = 1;
gallonm 7:2dc81120c917 462 else
gallonm 7:2dc81120c917 463 value = 0;
gallonm 7:2dc81120c917 464 status = VL6180x_WrByte(dev, SYSTEM_GROUPED_PARAMETER_HOLD, value);
gallonm 7:2dc81120c917 465
gallonm 7:2dc81120c917 466 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 467 return status;
gallonm 7:2dc81120c917 468
gallonm 7:2dc81120c917 469 }
gallonm 7:2dc81120c917 470
gallonm 7:2dc81120c917 471 int VL6180X::VL6180x_Prepare(VL6180xDev_t dev)
gallonm 7:2dc81120c917 472 {
gallonm 7:2dc81120c917 473 int status;
gallonm 7:2dc81120c917 474 LOG_FUNCTION_START("");
gallonm 7:2dc81120c917 475
gallonm 7:2dc81120c917 476 do{
gallonm 7:2dc81120c917 477 status=VL6180x_StaticInit(dev);
gallonm 7:2dc81120c917 478 if( status<0) break;
gallonm 7:2dc81120c917 479
gallonm 7:2dc81120c917 480 /* set range InterruptMode to new sample */
gallonm 7:2dc81120c917 481 status=VL6180x_RangeConfigInterrupt(dev, CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY );
gallonm 7:2dc81120c917 482 if( status)
gallonm 7:2dc81120c917 483 break;
gallonm 7:2dc81120c917 484
gallonm 7:2dc81120c917 485 /* set default threshold */
gallonm 7:2dc81120c917 486 status=VL6180x_RangeSetRawThresholds(dev, 10, 200);
gallonm 7:2dc81120c917 487 if( status ){
gallonm 7:2dc81120c917 488 VL6180x_ErrLog("VL6180x_RangeSetRawThresholds fail");
gallonm 7:2dc81120c917 489 break;
gallonm 7:2dc81120c917 490 }
gallonm 7:2dc81120c917 491 #if VL6180x_ALS_SUPPORT
gallonm 7:2dc81120c917 492 status =VL6180x_AlsSetIntegrationPeriod(dev, 100);
gallonm 7:2dc81120c917 493 if( status ) break;
gallonm 7:2dc81120c917 494 status = VL6180x_AlsSetInterMeasurementPeriod(dev, 200);
gallonm 7:2dc81120c917 495 if( status ) break;
gallonm 7:2dc81120c917 496 status = VL6180x_AlsSetAnalogueGain(dev, 0);
gallonm 7:2dc81120c917 497 if( status ) break;
gallonm 7:2dc81120c917 498 status = VL6180x_AlsSetThresholds(dev, 0, 0xFF);
gallonm 7:2dc81120c917 499 if( status ) break;
gallonm 7:2dc81120c917 500 /* set Als InterruptMode to new sample */
gallonm 7:2dc81120c917 501 status=VL6180x_AlsConfigInterrupt(dev, CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY);
gallonm 7:2dc81120c917 502 if( status ) {
gallonm 7:2dc81120c917 503 VL6180x_ErrLog("VL6180x_AlsConfigInterrupt fail");
gallonm 7:2dc81120c917 504 break;
gallonm 7:2dc81120c917 505 }
gallonm 7:2dc81120c917 506 #endif
gallonm 7:2dc81120c917 507 #if VL6180x_WRAP_AROUND_FILTER_SUPPORT
gallonm 7:2dc81120c917 508 _filter_Init(dev);
gallonm 7:2dc81120c917 509 #endif
gallonm 7:2dc81120c917 510 /* make sure to reset any left previous condition that can hangs first poll */
gallonm 7:2dc81120c917 511 status=VL6180x_ClearAllInterrupt(dev);
gallonm 7:2dc81120c917 512 }
gallonm 7:2dc81120c917 513 while(0);
gallonm 7:2dc81120c917 514 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 515
gallonm 7:2dc81120c917 516 return status;
gallonm 7:2dc81120c917 517 }
gallonm 7:2dc81120c917 518
gallonm 7:2dc81120c917 519 #if VL6180x_ALS_SUPPORT
gallonm 7:2dc81120c917 520 int VL6180X::VL6180x_AlsGetLux(VL6180xDev_t dev, lux_t *pLux)
gallonm 7:2dc81120c917 521 {
gallonm 7:2dc81120c917 522 int status;
gallonm 7:2dc81120c917 523 uint16_t RawAls;
gallonm 7:2dc81120c917 524 uint32_t luxValue = 0;
gallonm 7:2dc81120c917 525 uint32_t IntPeriod;
gallonm 7:2dc81120c917 526 uint32_t AlsAnGain;
gallonm 7:2dc81120c917 527 uint32_t GainFix;
gallonm 7:2dc81120c917 528 uint32_t AlsScaler;
gallonm 7:2dc81120c917 529
gallonm 7:2dc81120c917 530 #if LUXRES_FIX_PREC != GAIN_FIX_PREC
gallonm 7:2dc81120c917 531 #error "LUXRES_FIX_PREC != GAIN_FIX_PREC review these code to be correct"
gallonm 7:2dc81120c917 532 #endif
gallonm 7:2dc81120c917 533 const uint32_t LuxResxIntIme =(uint32_t)(0.56f* DEF_INT_PEFRIOD *(1<<LUXRES_FIX_PREC));
gallonm 7:2dc81120c917 534
gallonm 7:2dc81120c917 535 LOG_FUNCTION_START("%p", pLux);
gallonm 7:2dc81120c917 536
gallonm 7:2dc81120c917 537 status = VL6180x_RdWord( dev, RESULT_ALS_VAL, &RawAls);
gallonm 7:2dc81120c917 538 if( !status){
gallonm 7:2dc81120c917 539 /* wer are yet here at no fix point */
gallonm 7:2dc81120c917 540 IntPeriod=VL6180xDevDataGet(dev, IntegrationPeriod);
gallonm 7:2dc81120c917 541 AlsScaler=VL6180xDevDataGet(dev, AlsScaler);
gallonm 7:2dc81120c917 542 IntPeriod++; /* what stored is real time ms -1 and it can be 0 for or 0 or 1ms */
gallonm 7:2dc81120c917 543 luxValue = (uint32_t)RawAls * LuxResxIntIme; /* max # 16+8bits + 6bit (0.56*100) */
gallonm 7:2dc81120c917 544 luxValue /= IntPeriod; /* max # 16+8bits + 6bit 16+8+1 to 9 bit */
gallonm 7:2dc81120c917 545 /* between 29 - 21 bit */
gallonm 7:2dc81120c917 546 AlsAnGain = VL6180xDevDataGet(dev, AlsGainCode);
gallonm 7:2dc81120c917 547 GainFix = AlsGainLookUp[AlsAnGain];
gallonm 7:2dc81120c917 548 luxValue = luxValue / (AlsScaler * GainFix);
gallonm 7:2dc81120c917 549 *pLux=luxValue;
gallonm 7:2dc81120c917 550 }
gallonm 7:2dc81120c917 551
gallonm 7:2dc81120c917 552 LOG_FUNCTION_END_FMT(status, "%x",(int)*pLux);
gallonm 7:2dc81120c917 553 return status;
gallonm 7:2dc81120c917 554 }
gallonm 7:2dc81120c917 555
gallonm 7:2dc81120c917 556 int VL6180X::VL6180x_AlsGetMeasurement(VL6180xDev_t dev, VL6180x_AlsData_t *pAlsData)
gallonm 7:2dc81120c917 557 {
gallonm 7:2dc81120c917 558 int status;
gallonm 7:2dc81120c917 559 uint8_t ErrStatus;
gallonm 7:2dc81120c917 560
gallonm 7:2dc81120c917 561 LOG_FUNCTION_START("%p", pAlsData);
gallonm 7:2dc81120c917 562
gallonm 7:2dc81120c917 563 status = VL6180x_AlsGetLux(dev, &pAlsData->lux);
gallonm 7:2dc81120c917 564 if( !status ){
gallonm 7:2dc81120c917 565 status = VL6180x_RdByte(dev, RESULT_ALS_STATUS, & ErrStatus);
gallonm 7:2dc81120c917 566 pAlsData->errorStatus = ErrStatus>>4;
gallonm 7:2dc81120c917 567 }
gallonm 7:2dc81120c917 568 LOG_FUNCTION_END_FMT(status,"%d %d", (int)pAlsData->lux, (int)pAlsData->errorStatus);
gallonm 7:2dc81120c917 569
gallonm 7:2dc81120c917 570 return status;
gallonm 7:2dc81120c917 571 }
gallonm 7:2dc81120c917 572
gallonm 7:2dc81120c917 573
gallonm 7:2dc81120c917 574 int VL6180X::VL6180x_AlsPollMeasurement(VL6180xDev_t dev, VL6180x_AlsData_t *pAlsData) {
gallonm 7:2dc81120c917 575 int status;
gallonm 7:2dc81120c917 576 int ClrStatus;
gallonm 7:2dc81120c917 577 uint8_t IntStatus;
gallonm 7:2dc81120c917 578
gallonm 7:2dc81120c917 579 LOG_FUNCTION_START("%p", pAlsData);
gallonm 7:2dc81120c917 580 #if VL6180X_SAFE_POLLING_ENTER
gallonm 7:2dc81120c917 581 /* if device get stopped with left interrupt uncleared , it is required to clear them now or poll for new condition will never occur*/
gallonm 7:2dc81120c917 582 status=VL6180x_AlsClearInterrupt(dev);
gallonm 7:2dc81120c917 583 if(status){
gallonm 7:2dc81120c917 584 VL6180x_ErrLog("VL6180x_AlsClearInterrupt fail");
gallonm 7:2dc81120c917 585 goto over;
gallonm 7:2dc81120c917 586 }
gallonm 7:2dc81120c917 587 #endif
gallonm 7:2dc81120c917 588
gallonm 7:2dc81120c917 589 status=VL6180x_AlsSetSystemMode(dev, MODE_START_STOP|MODE_SINGLESHOT);
gallonm 7:2dc81120c917 590 if( status){
gallonm 7:2dc81120c917 591 VL6180x_ErrLog("VL6180x_AlsSetSystemMode fail");
gallonm 7:2dc81120c917 592 goto over;
gallonm 7:2dc81120c917 593 }
gallonm 7:2dc81120c917 594
gallonm 7:2dc81120c917 595 /* poll for new sample ready */
gallonm 7:2dc81120c917 596 while (1 ) {
gallonm 7:2dc81120c917 597 status = VL6180x_AlsGetInterruptStatus(dev, &IntStatus);
gallonm 7:2dc81120c917 598 if (status) {
gallonm 7:2dc81120c917 599 break;
gallonm 7:2dc81120c917 600 }
gallonm 7:2dc81120c917 601 if (IntStatus == RES_INT_STAT_GPIO_NEW_SAMPLE_READY) {
gallonm 7:2dc81120c917 602 break; /* break on new data (status is 0) */
gallonm 7:2dc81120c917 603 }
gallonm 7:2dc81120c917 604
gallonm 7:2dc81120c917 605 VL6180x_PollDelay(dev);
gallonm 7:2dc81120c917 606 };
gallonm 7:2dc81120c917 607
gallonm 7:2dc81120c917 608 if (!status) {
gallonm 7:2dc81120c917 609 status = VL6180x_AlsGetMeasurement(dev, pAlsData);
gallonm 7:2dc81120c917 610 }
gallonm 7:2dc81120c917 611
gallonm 7:2dc81120c917 612 ClrStatus = VL6180x_AlsClearInterrupt(dev);
gallonm 7:2dc81120c917 613 if (ClrStatus) {
gallonm 7:2dc81120c917 614 VL6180x_ErrLog("VL6180x_AlsClearInterrupt fail");
gallonm 7:2dc81120c917 615 if (!status) {
gallonm 7:2dc81120c917 616 status = ClrStatus; /* leave previous if already on error */
gallonm 7:2dc81120c917 617 }
gallonm 7:2dc81120c917 618 }
gallonm 7:2dc81120c917 619 over:
gallonm 7:2dc81120c917 620 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 621
gallonm 7:2dc81120c917 622 return status;
gallonm 7:2dc81120c917 623 }
gallonm 7:2dc81120c917 624
gallonm 7:2dc81120c917 625 int VL6180X::VL6180x_AlsGetInterruptStatus(VL6180xDev_t dev, uint8_t *pIntStatus) {
gallonm 7:2dc81120c917 626 int status;
gallonm 7:2dc81120c917 627 uint8_t IntStatus;
gallonm 7:2dc81120c917 628 LOG_FUNCTION_START("%p", pIntStatus);
gallonm 7:2dc81120c917 629
gallonm 7:2dc81120c917 630 status = VL6180x_RdByte(dev, RESULT_INTERRUPT_STATUS_GPIO, &IntStatus);
gallonm 7:2dc81120c917 631 *pIntStatus= (IntStatus>>3)&0x07;
gallonm 7:2dc81120c917 632
gallonm 7:2dc81120c917 633 LOG_FUNCTION_END_FMT(status, "%d", (int)*pIntStatus);
gallonm 7:2dc81120c917 634 return status;
gallonm 7:2dc81120c917 635 }
gallonm 7:2dc81120c917 636
gallonm 7:2dc81120c917 637 int VL6180X::VL6180x_AlsWaitDeviceReady(VL6180xDev_t dev, int MaxLoop ){
gallonm 7:2dc81120c917 638 int status;
gallonm 7:2dc81120c917 639 int n;
gallonm 7:2dc81120c917 640 uint8_t u8;
gallonm 7:2dc81120c917 641 LOG_FUNCTION_START("%d", (int)MaxLoop);
gallonm 7:2dc81120c917 642 if( MaxLoop<1){
gallonm 7:2dc81120c917 643 status=INVALID_PARAMS;
gallonm 7:2dc81120c917 644 }
gallonm 7:2dc81120c917 645 else{
gallonm 7:2dc81120c917 646 for( n=0; n < MaxLoop ; n++){
gallonm 7:2dc81120c917 647 status=VL6180x_RdByte(dev, RESULT_ALS_STATUS, &u8);
gallonm 7:2dc81120c917 648 if( status)
gallonm 7:2dc81120c917 649 break;
gallonm 7:2dc81120c917 650 u8 = u8 & ALS_DEVICE_READY_MASK;
gallonm 7:2dc81120c917 651 if( u8 )
gallonm 7:2dc81120c917 652 break;
gallonm 7:2dc81120c917 653
gallonm 7:2dc81120c917 654 }
gallonm 7:2dc81120c917 655 if( !status && !u8 ){
gallonm 7:2dc81120c917 656 status = TIME_OUT;
gallonm 7:2dc81120c917 657 }
gallonm 7:2dc81120c917 658 }
gallonm 7:2dc81120c917 659 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 660 return status;
gallonm 7:2dc81120c917 661 }
gallonm 7:2dc81120c917 662
gallonm 7:2dc81120c917 663 int VL6180X::VL6180x_AlsSetSystemMode(VL6180xDev_t dev, uint8_t mode)
gallonm 7:2dc81120c917 664 {
gallonm 7:2dc81120c917 665 int status;
gallonm 7:2dc81120c917 666 LOG_FUNCTION_START("%d", (int)mode);
gallonm 7:2dc81120c917 667 /* FIXME if we are called back to back real fast we are not checking
gallonm 7:2dc81120c917 668 * if previous mode "set" got absorbed => bit 0 must be 0 so that wr 1 work */
gallonm 7:2dc81120c917 669 if( mode <= 3){
gallonm 7:2dc81120c917 670 status=VL6180x_WrByte(dev, SYSALS_START, mode);
gallonm 7:2dc81120c917 671 }
gallonm 7:2dc81120c917 672 else{
gallonm 7:2dc81120c917 673 status = INVALID_PARAMS;
gallonm 7:2dc81120c917 674 }
gallonm 7:2dc81120c917 675 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 676 return status;
gallonm 7:2dc81120c917 677 }
gallonm 7:2dc81120c917 678
gallonm 7:2dc81120c917 679 int VL6180X::VL6180x_AlsConfigInterrupt(VL6180xDev_t dev, uint8_t ConfigGpioInt)
gallonm 7:2dc81120c917 680 {
gallonm 7:2dc81120c917 681 int status;
gallonm 7:2dc81120c917 682
gallonm 7:2dc81120c917 683 if( ConfigGpioInt<= CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY){
gallonm 7:2dc81120c917 684 status = VL6180x_UpdateByte(dev, SYSTEM_INTERRUPT_CONFIG_GPIO, (uint8_t)(~CONFIG_GPIO_ALS_MASK), (ConfigGpioInt<<3));
gallonm 7:2dc81120c917 685 }
gallonm 7:2dc81120c917 686 else{
gallonm 7:2dc81120c917 687 VL6180x_ErrLog("Invalid config mode param %d", (int)ConfigGpioInt);
gallonm 7:2dc81120c917 688 status = INVALID_PARAMS;
gallonm 7:2dc81120c917 689 }
gallonm 7:2dc81120c917 690 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 691 return status;
gallonm 7:2dc81120c917 692 }
gallonm 7:2dc81120c917 693
gallonm 7:2dc81120c917 694
gallonm 7:2dc81120c917 695
gallonm 7:2dc81120c917 696 int VL6180X::VL6180x_AlsSetThresholds(VL6180xDev_t dev, uint8_t low, uint8_t high) {
gallonm 7:2dc81120c917 697 int status;
gallonm 7:2dc81120c917 698
gallonm 7:2dc81120c917 699 LOG_FUNCTION_START("%d %d", (int )low, (int)high);
gallonm 7:2dc81120c917 700
gallonm 7:2dc81120c917 701 status = VL6180x_WrByte(dev, SYSALS_THRESH_LOW, low);
gallonm 7:2dc81120c917 702 if(!status ){
gallonm 7:2dc81120c917 703 status = VL6180x_WrByte(dev, SYSALS_THRESH_HIGH, high);
gallonm 7:2dc81120c917 704 }
gallonm 7:2dc81120c917 705
gallonm 7:2dc81120c917 706 LOG_FUNCTION_END(status) ;
gallonm 7:2dc81120c917 707 return status;
gallonm 7:2dc81120c917 708 }
gallonm 7:2dc81120c917 709
gallonm 7:2dc81120c917 710
gallonm 7:2dc81120c917 711 int VL6180X::VL6180x_AlsSetAnalogueGain(VL6180xDev_t dev, uint8_t gain) {
gallonm 7:2dc81120c917 712 int status;
gallonm 7:2dc81120c917 713 uint8_t GainTotal;
gallonm 7:2dc81120c917 714
gallonm 7:2dc81120c917 715 LOG_FUNCTION_START("%d", (int )gain);
gallonm 7:2dc81120c917 716 gain&=~0x40;
gallonm 7:2dc81120c917 717 if (gain > 7) {
gallonm 7:2dc81120c917 718 gain = 7;
gallonm 7:2dc81120c917 719 }
gallonm 7:2dc81120c917 720 GainTotal = gain|0x40;
gallonm 7:2dc81120c917 721
gallonm 7:2dc81120c917 722 status = VL6180x_WrByte(dev, SYSALS_ANALOGUE_GAIN, GainTotal);
gallonm 7:2dc81120c917 723 if( !status){
gallonm 7:2dc81120c917 724 VL6180xDevDataSet(dev, AlsGainCode, gain);
gallonm 7:2dc81120c917 725 }
gallonm 7:2dc81120c917 726
gallonm 7:2dc81120c917 727 LOG_FUNCTION_END_FMT(status, "%d %d", (int ) gain, (int )GainTotal);
gallonm 7:2dc81120c917 728 return status;
gallonm 7:2dc81120c917 729 }
gallonm 7:2dc81120c917 730
gallonm 7:2dc81120c917 731 int VL6180X::VL6180x_AlsSetInterMeasurementPeriod(VL6180xDev_t dev, uint16_t intermeasurement_period_ms)
gallonm 7:2dc81120c917 732 {
gallonm 7:2dc81120c917 733 int status;
gallonm 7:2dc81120c917 734
gallonm 7:2dc81120c917 735 LOG_FUNCTION_START("%d",(int)intermeasurement_period_ms);
gallonm 7:2dc81120c917 736 /* clipping: range is 0-2550ms */
gallonm 7:2dc81120c917 737 if (intermeasurement_period_ms >= 255 *10)
gallonm 7:2dc81120c917 738 intermeasurement_period_ms = 255 *10;
gallonm 7:2dc81120c917 739 status=VL6180x_WrByte(dev, SYSALS_INTERMEASUREMENT_PERIOD, (uint8_t)(intermeasurement_period_ms/10));
gallonm 7:2dc81120c917 740
gallonm 7:2dc81120c917 741 LOG_FUNCTION_END_FMT(status, "%d", (int) intermeasurement_period_ms);
gallonm 7:2dc81120c917 742 return status;
gallonm 7:2dc81120c917 743 }
gallonm 7:2dc81120c917 744
gallonm 7:2dc81120c917 745
gallonm 7:2dc81120c917 746 int VL6180X::VL6180x_AlsSetIntegrationPeriod(VL6180xDev_t dev, uint16_t period_ms)
gallonm 7:2dc81120c917 747 {
gallonm 7:2dc81120c917 748 int status;
gallonm 7:2dc81120c917 749 uint16_t SetIntegrationPeriod;
gallonm 7:2dc81120c917 750
gallonm 7:2dc81120c917 751 LOG_FUNCTION_START("%d", (int)period_ms);
gallonm 7:2dc81120c917 752
gallonm 7:2dc81120c917 753 if( period_ms>=1 )
gallonm 7:2dc81120c917 754 SetIntegrationPeriod = period_ms - 1;
gallonm 7:2dc81120c917 755 else
gallonm 7:2dc81120c917 756 SetIntegrationPeriod = period_ms;
gallonm 7:2dc81120c917 757
gallonm 7:2dc81120c917 758 if (SetIntegrationPeriod > 464) {
gallonm 7:2dc81120c917 759 SetIntegrationPeriod = 464;
gallonm 7:2dc81120c917 760 }
gallonm 7:2dc81120c917 761 else if (SetIntegrationPeriod == 255) {
gallonm 7:2dc81120c917 762 SetIntegrationPeriod++; /* can't write 255 since this causes the device to lock out.*/
gallonm 7:2dc81120c917 763 }
gallonm 7:2dc81120c917 764
gallonm 7:2dc81120c917 765 status =VL6180x_WrWord(dev, SYSALS_INTEGRATION_PERIOD, SetIntegrationPeriod);
gallonm 7:2dc81120c917 766 if( !status ){
gallonm 7:2dc81120c917 767 VL6180xDevDataSet(dev, IntegrationPeriod, SetIntegrationPeriod) ;
gallonm 7:2dc81120c917 768 }
gallonm 7:2dc81120c917 769 LOG_FUNCTION_END_FMT(status, "%d", (int)SetIntegrationPeriod);
gallonm 7:2dc81120c917 770 return status;
gallonm 7:2dc81120c917 771 }
gallonm 7:2dc81120c917 772
gallonm 7:2dc81120c917 773 #endif /* HAVE_ALS_SUPPORT */
gallonm 7:2dc81120c917 774
gallonm 7:2dc81120c917 775
gallonm 7:2dc81120c917 776 int VL6180X::VL6180x_RangePollMeasurement(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData)
gallonm 7:2dc81120c917 777 {
gallonm 7:2dc81120c917 778 int status;
gallonm 7:2dc81120c917 779 int ClrStatus;
gallonm 7:2dc81120c917 780 IntrStatus_t IntStatus;
gallonm 7:2dc81120c917 781
gallonm 7:2dc81120c917 782 LOG_FUNCTION_START("");
gallonm 7:2dc81120c917 783 /* start single range measurement */
gallonm 7:2dc81120c917 784
gallonm 7:2dc81120c917 785
gallonm 7:2dc81120c917 786 #if VL6180X_SAFE_POLLING_ENTER
gallonm 7:2dc81120c917 787 /* if device get stopped with left interrupt uncleared , it is required to clear them now or poll for new condition will never occur*/
gallonm 7:2dc81120c917 788 status=VL6180x_RangeClearInterrupt(dev);
gallonm 7:2dc81120c917 789 if(status){
gallonm 7:2dc81120c917 790 VL6180x_ErrLog("VL6180x_RangeClearInterrupt fail");
gallonm 7:2dc81120c917 791 goto done;
gallonm 7:2dc81120c917 792 }
gallonm 7:2dc81120c917 793 #endif
gallonm 7:2dc81120c917 794 /* //![single_shot_snipet] */
gallonm 7:2dc81120c917 795 status=VL6180x_RangeSetSystemMode(dev, MODE_START_STOP|MODE_SINGLESHOT);
gallonm 7:2dc81120c917 796 if( status ){
gallonm 7:2dc81120c917 797 VL6180x_ErrLog("VL6180x_RangeSetSystemMode fail");
gallonm 7:2dc81120c917 798 goto done;
gallonm 7:2dc81120c917 799 }
gallonm 7:2dc81120c917 800
gallonm 7:2dc81120c917 801
gallonm 7:2dc81120c917 802 /* poll for new sample ready */
gallonm 7:2dc81120c917 803 while(1 ){
gallonm 7:2dc81120c917 804 status=VL6180x_RangeGetInterruptStatus(dev, &IntStatus.val);
gallonm 7:2dc81120c917 805 if( status ){
gallonm 7:2dc81120c917 806 break;
gallonm 7:2dc81120c917 807 }
gallonm 7:2dc81120c917 808 if( IntStatus.status.Error !=0 ){
gallonm 7:2dc81120c917 809 VL6180x_ErrLog("GPIO int Error report %d",(int)IntStatus.val);
gallonm 7:2dc81120c917 810 status = RANGE_ERROR;
gallonm 7:2dc81120c917 811 break;
gallonm 7:2dc81120c917 812 }
gallonm 7:2dc81120c917 813 else
gallonm 7:2dc81120c917 814 if( IntStatus.status.Range == RES_INT_STAT_GPIO_NEW_SAMPLE_READY){
gallonm 7:2dc81120c917 815 break;
gallonm 7:2dc81120c917 816 }
gallonm 7:2dc81120c917 817
gallonm 7:2dc81120c917 818 VL6180x_PollDelay(dev);
gallonm 7:2dc81120c917 819 }
gallonm 7:2dc81120c917 820 /* //![single_shot_snipet] */
gallonm 7:2dc81120c917 821
gallonm 7:2dc81120c917 822 if ( !status ){
gallonm 7:2dc81120c917 823 status = VL6180x_RangeGetMeasurement(dev, pRangeData);
gallonm 7:2dc81120c917 824 }
gallonm 7:2dc81120c917 825
gallonm 7:2dc81120c917 826 /* clear range interrupt source */
gallonm 7:2dc81120c917 827 ClrStatus = VL6180x_RangeClearInterrupt(dev);
gallonm 7:2dc81120c917 828 if( ClrStatus ){
gallonm 7:2dc81120c917 829 VL6180x_ErrLog("VL6180x_RangeClearInterrupt fail");
gallonm 7:2dc81120c917 830 /* leave initial status if already in error */
gallonm 7:2dc81120c917 831 if( !status ){
gallonm 7:2dc81120c917 832 status=ClrStatus;
gallonm 7:2dc81120c917 833 }
gallonm 7:2dc81120c917 834 }
gallonm 7:2dc81120c917 835 done:
gallonm 7:2dc81120c917 836 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 837 return status;
gallonm 7:2dc81120c917 838 }
gallonm 7:2dc81120c917 839
gallonm 7:2dc81120c917 840
gallonm 7:2dc81120c917 841
gallonm 7:2dc81120c917 842 int VL6180X::VL6180x_RangeGetMeasurement(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData)
gallonm 7:2dc81120c917 843 {
gallonm 7:2dc81120c917 844 int status;
gallonm 7:2dc81120c917 845 uint16_t RawRate;
gallonm 7:2dc81120c917 846 uint8_t RawStatus;
gallonm 7:2dc81120c917 847
gallonm 7:2dc81120c917 848 LOG_FUNCTION_START("");
gallonm 7:2dc81120c917 849
gallonm 7:2dc81120c917 850 status = VL6180x_RangeGetResult(dev, &pRangeData->range_mm);
gallonm 7:2dc81120c917 851 if( !status ){
gallonm 7:2dc81120c917 852 status = VL6180x_RdWord(dev,RESULT_RANGE_SIGNAL_RATE, &RawRate );
gallonm 7:2dc81120c917 853 if( !status ){
gallonm 7:2dc81120c917 854 pRangeData->signalRate_mcps = VL6180x_9to7Conv(RawRate);
gallonm 7:2dc81120c917 855 status = VL6180x_RdByte(dev, RESULT_RANGE_STATUS, &RawStatus);
gallonm 7:2dc81120c917 856 if( !status ){
gallonm 7:2dc81120c917 857 pRangeData->errorStatus = RawStatus >>4;
gallonm 7:2dc81120c917 858 }
gallonm 7:2dc81120c917 859 else{
gallonm 7:2dc81120c917 860 VL6180x_ErrLog("Rd RESULT_RANGE_STATUS fail");
gallonm 7:2dc81120c917 861 }
gallonm 7:2dc81120c917 862 #if VL6180x_WRAP_AROUND_FILTER_SUPPORT || VL6180x_HAVE_DMAX_RANGING
gallonm 7:2dc81120c917 863 status = _GetRateResult(dev, pRangeData);
gallonm 7:2dc81120c917 864 if( status )
gallonm 7:2dc81120c917 865 goto error;
gallonm 7:2dc81120c917 866 #endif
gallonm 7:2dc81120c917 867 #if VL6180x_WRAP_AROUND_FILTER_SUPPORT
gallonm 7:2dc81120c917 868 /* if enabled run filter */
gallonm 7:2dc81120c917 869 if( _IsWrapArroundActive(dev) ){
gallonm 7:2dc81120c917 870 status=_filter_GetResult(dev, pRangeData);
gallonm 7:2dc81120c917 871 if( !status){
gallonm 7:2dc81120c917 872 /* patch the range status and measure if it is filtered */
gallonm 7:2dc81120c917 873 if( pRangeData->range_mm != pRangeData->FilteredData.range_mm) {
gallonm 7:2dc81120c917 874 pRangeData->errorStatus=RangingFiltered;
gallonm 7:2dc81120c917 875 pRangeData->range_mm = pRangeData->FilteredData.range_mm;
gallonm 7:2dc81120c917 876 }
gallonm 7:2dc81120c917 877 }
gallonm 7:2dc81120c917 878 }
gallonm 7:2dc81120c917 879 #endif
gallonm 7:2dc81120c917 880
gallonm 7:2dc81120c917 881 #if VL6180x_HAVE_DMAX_RANGING
gallonm 7:2dc81120c917 882 if(_IsDMaxActive(dev) ){
gallonm 7:2dc81120c917 883 _DMax_Compute(dev, pRangeData);
gallonm 7:2dc81120c917 884 }
gallonm 7:2dc81120c917 885 #endif
gallonm 7:2dc81120c917 886 }
gallonm 7:2dc81120c917 887 else{
gallonm 7:2dc81120c917 888 VL6180x_ErrLog("Rd RESULT_RANGE_SIGNAL_RATE fail");
gallonm 7:2dc81120c917 889 }
gallonm 7:2dc81120c917 890 }
gallonm 7:2dc81120c917 891 else{
gallonm 7:2dc81120c917 892 VL6180x_ErrLog("VL6180x_GetRangeResult fail");
gallonm 7:2dc81120c917 893 }
gallonm 7:2dc81120c917 894 error:
gallonm 7:2dc81120c917 895 LOG_FUNCTION_END_FMT(status, "%d %d %d", (int)pRangeData->range_mm, (int)pRangeData->signalRate_mcps, (int)pRangeData->errorStatus) ;
gallonm 7:2dc81120c917 896 return status;
gallonm 7:2dc81120c917 897 }
gallonm 7:2dc81120c917 898
gallonm 7:2dc81120c917 899
gallonm 7:2dc81120c917 900 int VL6180X::VL6180x_RangeGetMeasurementIfReady(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData)
gallonm 7:2dc81120c917 901 {
gallonm 7:2dc81120c917 902 int status;
gallonm 7:2dc81120c917 903 IntrStatus_t IntStatus;
gallonm 7:2dc81120c917 904
gallonm 7:2dc81120c917 905 LOG_FUNCTION_START();
gallonm 7:2dc81120c917 906
gallonm 7:2dc81120c917 907 status = VL6180x_RangeGetInterruptStatus(dev, &IntStatus.val);
gallonm 7:2dc81120c917 908 if( status ==0 ){
gallonm 7:2dc81120c917 909 if( IntStatus.status.Error !=0 ){
gallonm 7:2dc81120c917 910 VL6180x_ErrLog("GPIO int Error report %d",(int)IntStatus.val);
gallonm 7:2dc81120c917 911 status = RANGE_ERROR;
gallonm 7:2dc81120c917 912 }
gallonm 7:2dc81120c917 913 else
gallonm 7:2dc81120c917 914 if( IntStatus.status.Range == RES_INT_STAT_GPIO_NEW_SAMPLE_READY){
gallonm 7:2dc81120c917 915 status = VL6180x_RangeGetMeasurement(dev,pRangeData );
gallonm 7:2dc81120c917 916 if( status == 0){
gallonm 7:2dc81120c917 917 /* clear range interrupt source */
gallonm 7:2dc81120c917 918 status = VL6180x_RangeClearInterrupt(dev);
gallonm 7:2dc81120c917 919 if( status ){
gallonm 7:2dc81120c917 920 VL6180x_ErrLog("VL6180x_RangeClearInterrupt fail");
gallonm 7:2dc81120c917 921 }
gallonm 7:2dc81120c917 922 }
gallonm 7:2dc81120c917 923 }
gallonm 7:2dc81120c917 924 else{
gallonm 7:2dc81120c917 925 status = NOT_READY;
gallonm 7:2dc81120c917 926 }
gallonm 7:2dc81120c917 927 }
gallonm 7:2dc81120c917 928 else{
gallonm 7:2dc81120c917 929 VL6180x_ErrLog("fail to get interrupt status");
gallonm 7:2dc81120c917 930 }
gallonm 7:2dc81120c917 931 LOG_FUNCTION_END(status) ;
gallonm 7:2dc81120c917 932 return status;
gallonm 7:2dc81120c917 933 }
gallonm 7:2dc81120c917 934
gallonm 7:2dc81120c917 935 int VL6180X::VL6180x_FilterSetState(VL6180xDev_t dev, int state){
gallonm 7:2dc81120c917 936 int status;
gallonm 7:2dc81120c917 937 LOG_FUNCTION_START("%d", state);
gallonm 7:2dc81120c917 938 #if VL6180x_WRAP_AROUND_FILTER_SUPPORT
gallonm 7:2dc81120c917 939 VL6180xDevDataSet(dev,WrapAroundFilterActive, state);
gallonm 7:2dc81120c917 940 status = 0;
gallonm 7:2dc81120c917 941 #else
gallonm 7:2dc81120c917 942 status = NOT_SUPPORTED;
gallonm 7:2dc81120c917 943 #endif
gallonm 7:2dc81120c917 944 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 945 return status;
gallonm 7:2dc81120c917 946 }
gallonm 7:2dc81120c917 947
gallonm 7:2dc81120c917 948 int VL6180X::VL6180x_FilterGetState(VL6180xDev_t dev){
gallonm 7:2dc81120c917 949 int status;
gallonm 7:2dc81120c917 950 LOG_FUNCTION_START("");
gallonm 7:2dc81120c917 951 #if VL6180x_WRAP_AROUND_FILTER_SUPPORT
gallonm 7:2dc81120c917 952 status = VL6180xDevDataGet(dev,WrapAroundFilterActive);
gallonm 7:2dc81120c917 953 #else
gallonm 7:2dc81120c917 954 status = 0;
gallonm 7:2dc81120c917 955 #endif
gallonm 7:2dc81120c917 956 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 957 return status;
gallonm 7:2dc81120c917 958 }
gallonm 7:2dc81120c917 959
gallonm 7:2dc81120c917 960 int VL6180X::VL6180x_RangeGetResult(VL6180xDev_t dev, int32_t *pRange_mm) {
gallonm 7:2dc81120c917 961 int status;
gallonm 7:2dc81120c917 962 uint8_t RawRange;
gallonm 7:2dc81120c917 963 int32_t Upscale;
gallonm 7:2dc81120c917 964
gallonm 7:2dc81120c917 965 LOG_FUNCTION_START("%p",pRange_mm);
gallonm 7:2dc81120c917 966
gallonm 7:2dc81120c917 967 status = VL6180x_RdByte(dev, RESULT_RANGE_VAL, &RawRange);
gallonm 7:2dc81120c917 968 if( !status ){
gallonm 7:2dc81120c917 969 Upscale = _GetUpscale(dev);
gallonm 7:2dc81120c917 970 *pRange_mm= Upscale*(int32_t)RawRange;
gallonm 7:2dc81120c917 971 }
gallonm 7:2dc81120c917 972 LOG_FUNCTION_END_FMT(status, "%d", (int)*pRange_mm);
gallonm 7:2dc81120c917 973 return status;
gallonm 7:2dc81120c917 974 }
gallonm 7:2dc81120c917 975
gallonm 7:2dc81120c917 976 int VL6180X::VL6180x_RangeSetRawThresholds(VL6180xDev_t dev, uint8_t low, uint8_t high)
gallonm 7:2dc81120c917 977 {
gallonm 7:2dc81120c917 978 int status;
gallonm 7:2dc81120c917 979 LOG_FUNCTION_START("%d %d", (int) low, (int)high);
gallonm 7:2dc81120c917 980 /* TODO we can optimize here grouping high/low in a word but that's cpu endianness dependent */
gallonm 7:2dc81120c917 981 status=VL6180x_WrByte(dev, SYSRANGE_THRESH_HIGH,high);
gallonm 7:2dc81120c917 982 if( !status){
gallonm 7:2dc81120c917 983 status=VL6180x_WrByte(dev, SYSRANGE_THRESH_LOW, low);
gallonm 7:2dc81120c917 984 }
gallonm 7:2dc81120c917 985
gallonm 7:2dc81120c917 986 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 987 return status;
gallonm 7:2dc81120c917 988 }
gallonm 7:2dc81120c917 989
gallonm 7:2dc81120c917 990 int VL6180X::VL6180x_RangeSetThresholds(VL6180xDev_t dev, uint16_t low, uint16_t high, int UseSafeParamHold)
gallonm 7:2dc81120c917 991 {
gallonm 7:2dc81120c917 992 int status;
gallonm 7:2dc81120c917 993 int scale;
gallonm 7:2dc81120c917 994 LOG_FUNCTION_START("%d %d", (int) low, (int)high);
gallonm 7:2dc81120c917 995 scale=_GetUpscale(dev,UpscaleFactor);
gallonm 7:2dc81120c917 996 if( low>scale*255 || high >scale*255){
gallonm 7:2dc81120c917 997 status = INVALID_PARAMS;
gallonm 7:2dc81120c917 998 }
gallonm 7:2dc81120c917 999 else{
gallonm 7:2dc81120c917 1000 do{
gallonm 7:2dc81120c917 1001 if( UseSafeParamHold ){
gallonm 7:2dc81120c917 1002 status=VL6180x_SetGroupParamHold(dev, 1);
gallonm 7:2dc81120c917 1003 if( status )
gallonm 7:2dc81120c917 1004 break;
gallonm 7:2dc81120c917 1005 }
gallonm 7:2dc81120c917 1006 status=VL6180x_RangeSetRawThresholds(dev, (uint8_t)(low/scale), (uint8_t)(high/scale));
gallonm 7:2dc81120c917 1007 if( status ){
gallonm 7:2dc81120c917 1008 VL6180x_ErrLog("VL6180x_RangeSetRawThresholds fail");
gallonm 7:2dc81120c917 1009 }
gallonm 7:2dc81120c917 1010 if( UseSafeParamHold ){
gallonm 7:2dc81120c917 1011 int HoldStatus;
gallonm 7:2dc81120c917 1012 /* tryt to unset param hold vene if previous fail */
gallonm 7:2dc81120c917 1013 HoldStatus=VL6180x_SetGroupParamHold(dev, 0);
gallonm 7:2dc81120c917 1014 if( !status)
gallonm 7:2dc81120c917 1015 status=HoldStatus;
gallonm 7:2dc81120c917 1016 }
gallonm 7:2dc81120c917 1017 }
gallonm 7:2dc81120c917 1018 while(0);
gallonm 7:2dc81120c917 1019 }
gallonm 7:2dc81120c917 1020
gallonm 7:2dc81120c917 1021 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1022 return status;
gallonm 7:2dc81120c917 1023 }
gallonm 7:2dc81120c917 1024
gallonm 7:2dc81120c917 1025
gallonm 7:2dc81120c917 1026 int VL6180X::VL6180x_RangeGetThresholds(VL6180xDev_t dev, uint16_t *low, uint16_t *high)
gallonm 7:2dc81120c917 1027 {
gallonm 7:2dc81120c917 1028 int status;
gallonm 7:2dc81120c917 1029 uint8_t RawLow, RawHigh;
gallonm 7:2dc81120c917 1030 int scale;
gallonm 7:2dc81120c917 1031
gallonm 7:2dc81120c917 1032 LOG_FUNCTION_START("%p %p", low , high);
gallonm 7:2dc81120c917 1033
gallonm 7:2dc81120c917 1034 scale=_GetUpscale(dev,UpscaleFactor);
gallonm 7:2dc81120c917 1035 do{
gallonm 7:2dc81120c917 1036 if( high != NULL ){
gallonm 7:2dc81120c917 1037 status=VL6180x_RdByte(dev, SYSRANGE_THRESH_HIGH,&RawHigh);
gallonm 7:2dc81120c917 1038 if( status ){
gallonm 7:2dc81120c917 1039 VL6180x_ErrLog("rd SYSRANGE_THRESH_HIGH fail");
gallonm 7:2dc81120c917 1040 break;
gallonm 7:2dc81120c917 1041 }
gallonm 7:2dc81120c917 1042 *high=(uint16_t)RawHigh*scale;
gallonm 7:2dc81120c917 1043 }
gallonm 7:2dc81120c917 1044 if( low != NULL ) {
gallonm 7:2dc81120c917 1045 status=VL6180x_RdByte(dev, SYSRANGE_THRESH_LOW, &RawLow);
gallonm 7:2dc81120c917 1046 if( status ){
gallonm 7:2dc81120c917 1047 VL6180x_ErrLog("rd SYSRANGE_THRESH_LOW fail");
gallonm 7:2dc81120c917 1048 break;
gallonm 7:2dc81120c917 1049 }
gallonm 7:2dc81120c917 1050 *low=(uint16_t)RawLow*scale;
gallonm 7:2dc81120c917 1051 }
gallonm 7:2dc81120c917 1052 }
gallonm 7:2dc81120c917 1053 while(0);
gallonm 7:2dc81120c917 1054 LOG_FUNCTION_END_FMT(status, "%d %d",(int)*low ,(int)*high);
gallonm 7:2dc81120c917 1055 return status;
gallonm 7:2dc81120c917 1056 }
gallonm 7:2dc81120c917 1057
gallonm 7:2dc81120c917 1058
gallonm 7:2dc81120c917 1059 int VL6180X::VL6180x_RangeGetInterruptStatus(VL6180xDev_t dev, uint8_t *pIntStatus) {
gallonm 7:2dc81120c917 1060 int status;
gallonm 7:2dc81120c917 1061 uint8_t IntStatus;
gallonm 7:2dc81120c917 1062 LOG_FUNCTION_START("%p", pIntStatus);
gallonm 7:2dc81120c917 1063 /* FIXME we are grouping "error" with over status the user must check implicitly for it
gallonm 7:2dc81120c917 1064 * not just new sample or over status , that will nevr show up in case of error*/
gallonm 7:2dc81120c917 1065 status = VL6180x_RdByte(dev, RESULT_INTERRUPT_STATUS_GPIO, &IntStatus);
gallonm 7:2dc81120c917 1066 *pIntStatus= IntStatus&0xC7;
gallonm 7:2dc81120c917 1067
gallonm 7:2dc81120c917 1068 LOG_FUNCTION_END_FMT(status, "%d", (int)*pIntStatus);
gallonm 7:2dc81120c917 1069 return status;
gallonm 7:2dc81120c917 1070 }
gallonm 7:2dc81120c917 1071
gallonm 7:2dc81120c917 1072
gallonm 7:2dc81120c917 1073 int VL6180X::VL6180x_GetInterruptStatus(VL6180xDev_t dev, uint8_t *IntStatus)
gallonm 7:2dc81120c917 1074 {
gallonm 7:2dc81120c917 1075 int status;
gallonm 7:2dc81120c917 1076 LOG_FUNCTION_START("%p" , IntStatus);
gallonm 7:2dc81120c917 1077 status = VL6180x_RdByte(dev, RESULT_INTERRUPT_STATUS_GPIO, IntStatus);
gallonm 7:2dc81120c917 1078 LOG_FUNCTION_END_FMT(status, "%d", (int)*IntStatus);
gallonm 7:2dc81120c917 1079 return status;
gallonm 7:2dc81120c917 1080 }
gallonm 7:2dc81120c917 1081
gallonm 7:2dc81120c917 1082 int VL6180X::VL6180x_ClearInterrupt(VL6180xDev_t dev, uint8_t IntClear )
gallonm 7:2dc81120c917 1083 {
gallonm 7:2dc81120c917 1084 int status;
gallonm 7:2dc81120c917 1085 LOG_FUNCTION_START("%d" ,(int)IntClear);
gallonm 7:2dc81120c917 1086 if( IntClear <= 7 ){
gallonm 7:2dc81120c917 1087 status=VL6180x_WrByte( dev, SYSTEM_INTERRUPT_CLEAR, IntClear);
gallonm 7:2dc81120c917 1088 }
gallonm 7:2dc81120c917 1089 else{
gallonm 7:2dc81120c917 1090 status = INVALID_PARAMS;
gallonm 7:2dc81120c917 1091 }
gallonm 7:2dc81120c917 1092 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1093 return status;
gallonm 7:2dc81120c917 1094 }
gallonm 7:2dc81120c917 1095
gallonm 7:2dc81120c917 1096
gallonm 7:2dc81120c917 1097 int VL6180X::VL6180x_RangeStaticInit(VL6180xDev_t dev)
gallonm 7:2dc81120c917 1098 {
gallonm 7:2dc81120c917 1099 int status;
gallonm 7:2dc81120c917 1100 LOG_FUNCTION_START("");
gallonm 7:2dc81120c917 1101
gallonm 7:2dc81120c917 1102 /* REGISTER_TUNING_SR03_270514_CustomerView.txt */
gallonm 7:2dc81120c917 1103 VL6180x_WrByte( dev, 0x0207, 0x01);
gallonm 7:2dc81120c917 1104 VL6180x_WrByte( dev, 0x0208, 0x01);
gallonm 7:2dc81120c917 1105 VL6180x_WrByte( dev, 0x0096, 0x00);
gallonm 7:2dc81120c917 1106 VL6180x_WrByte( dev, 0x0097, 0xfd);
gallonm 7:2dc81120c917 1107 VL6180x_WrByte( dev, 0x00e3, 0x00);
gallonm 7:2dc81120c917 1108 VL6180x_WrByte( dev, 0x00e4, 0x04);
gallonm 7:2dc81120c917 1109 VL6180x_WrByte( dev, 0x00e5, 0x02);
gallonm 7:2dc81120c917 1110 VL6180x_WrByte( dev, 0x00e6, 0x01);
gallonm 7:2dc81120c917 1111 VL6180x_WrByte( dev, 0x00e7, 0x03);
gallonm 7:2dc81120c917 1112 VL6180x_WrByte( dev, 0x00f5, 0x02);
gallonm 7:2dc81120c917 1113 VL6180x_WrByte( dev, 0x00d9, 0x05);
gallonm 7:2dc81120c917 1114 VL6180x_WrByte( dev, 0x00db, 0xce);
gallonm 7:2dc81120c917 1115 VL6180x_WrByte( dev, 0x00dc, 0x03);
gallonm 7:2dc81120c917 1116 VL6180x_WrByte( dev, 0x00dd, 0xf8);
gallonm 7:2dc81120c917 1117 VL6180x_WrByte( dev, 0x009f, 0x00);
gallonm 7:2dc81120c917 1118 VL6180x_WrByte( dev, 0x00a3, 0x3c);
gallonm 7:2dc81120c917 1119 VL6180x_WrByte( dev, 0x00b7, 0x00);
gallonm 7:2dc81120c917 1120 VL6180x_WrByte( dev, 0x00bb, 0x3c);
gallonm 7:2dc81120c917 1121 VL6180x_WrByte( dev, 0x00b2, 0x09);
gallonm 7:2dc81120c917 1122 VL6180x_WrByte( dev, 0x00ca, 0x09);
gallonm 7:2dc81120c917 1123 VL6180x_WrByte( dev, 0x0198, 0x01);
gallonm 7:2dc81120c917 1124 VL6180x_WrByte( dev, 0x01b0, 0x17);
gallonm 7:2dc81120c917 1125 VL6180x_WrByte( dev, 0x01ad, 0x00);
gallonm 7:2dc81120c917 1126 VL6180x_WrByte( dev, 0x00ff, 0x05);
gallonm 7:2dc81120c917 1127 VL6180x_WrByte( dev, 0x0100, 0x05);
gallonm 7:2dc81120c917 1128 VL6180x_WrByte( dev, 0x0199, 0x05);
gallonm 7:2dc81120c917 1129 VL6180x_WrByte( dev, 0x01a6, 0x1b);
gallonm 7:2dc81120c917 1130 VL6180x_WrByte( dev, 0x01ac, 0x3e);
gallonm 7:2dc81120c917 1131 VL6180x_WrByte( dev, 0x01a7, 0x1f);
gallonm 7:2dc81120c917 1132 VL6180x_WrByte( dev, 0x0030, 0x00);
gallonm 7:2dc81120c917 1133
gallonm 7:2dc81120c917 1134 /* Recommended : Public registers - See data sheet for more detail */
gallonm 7:2dc81120c917 1135 VL6180x_WrByte( dev, 0x0011, 0x10); /* Enables polling for New Sample ready when measurement completes */
gallonm 7:2dc81120c917 1136 VL6180x_WrByte( dev, 0x010a, 0x30); /* Set the averaging sample period (compromise between lower noise and increased execution time) */
gallonm 7:2dc81120c917 1137 VL6180x_WrByte( dev, 0x003f, 0x46); /* Sets the light and dark gain (upper nibble). Dark gain should not be changed.*/
gallonm 7:2dc81120c917 1138 VL6180x_WrByte( dev, 0x0031, 0xFF); /* sets the # of range measurements after which auto calibration of system is performed */
gallonm 7:2dc81120c917 1139 VL6180x_WrByte( dev, 0x0040, 0x63); /* Set ALS integration time to 100ms */
gallonm 7:2dc81120c917 1140 VL6180x_WrByte( dev, 0x002e, 0x01); /* perform a single temperature calibration of the ranging sensor */
gallonm 7:2dc81120c917 1141
gallonm 7:2dc81120c917 1142 /* Optional: Public registers - See data sheet for more detail */
gallonm 7:2dc81120c917 1143 VL6180x_WrByte( dev, 0x001b, 0x09); /* Set default ranging inter-measurement period to 100ms */
gallonm 7:2dc81120c917 1144 VL6180x_WrByte( dev, 0x003e, 0x31); /* Set default ALS inter-measurement period to 500ms */
gallonm 7:2dc81120c917 1145 VL6180x_WrByte( dev, 0x0014, 0x24); /* Configures interrupt on New sample ready */
gallonm 7:2dc81120c917 1146
gallonm 7:2dc81120c917 1147
gallonm 7:2dc81120c917 1148 status=VL6180x_RangeSetMaxConvergenceTime(dev, 50); /* Calculate ece value on initialization (use max conv) */
gallonm 7:2dc81120c917 1149 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1150
gallonm 7:2dc81120c917 1151 return status;
gallonm 7:2dc81120c917 1152 }
gallonm 7:2dc81120c917 1153
gallonm 7:2dc81120c917 1154 #if VL6180x_UPSCALE_SUPPORT != 1
gallonm 7:2dc81120c917 1155
gallonm 7:2dc81120c917 1156 int VL6180X::_UpscaleInitPatch0(VL6180xDev_t dev){
gallonm 7:2dc81120c917 1157 int status;
gallonm 7:2dc81120c917 1158 uint32_t CalValue=0;
gallonm 7:2dc81120c917 1159 CalValue= VL6180xDevDataGet(dev, Part2PartAmbNVM);
gallonm 7:2dc81120c917 1160 status=VL6180x_WrDWord( dev, 0xDA, CalValue);
gallonm 7:2dc81120c917 1161 return status;
gallonm 7:2dc81120c917 1162 }
gallonm 7:2dc81120c917 1163
gallonm 7:2dc81120c917 1164 /* only include up-scaling register setting when up-scale support is configured in */
gallonm 7:2dc81120c917 1165 int VL6180X::VL6180x_UpscaleRegInit(VL6180xDev_t dev)
gallonm 7:2dc81120c917 1166 {
gallonm 7:2dc81120c917 1167 /* apply REGISTER_TUNING_ER02_100614_CustomerView.txt */
gallonm 7:2dc81120c917 1168 VL6180x_WrByte( dev, 0x0207, 0x01);
gallonm 7:2dc81120c917 1169 VL6180x_WrByte( dev, 0x0208, 0x01);
gallonm 7:2dc81120c917 1170 VL6180x_WrByte( dev, 0x0096, 0x00);
gallonm 7:2dc81120c917 1171 VL6180x_WrByte( dev, 0x0097, 0x54);
gallonm 7:2dc81120c917 1172 VL6180x_WrByte( dev, 0x00e3, 0x00);
gallonm 7:2dc81120c917 1173 VL6180x_WrByte( dev, 0x00e4, 0x04);
gallonm 7:2dc81120c917 1174 VL6180x_WrByte( dev, 0x00e5, 0x02);
gallonm 7:2dc81120c917 1175 VL6180x_WrByte( dev, 0x00e6, 0x01);
gallonm 7:2dc81120c917 1176 VL6180x_WrByte( dev, 0x00e7, 0x03);
gallonm 7:2dc81120c917 1177 VL6180x_WrByte( dev, 0x00f5, 0x02);
gallonm 7:2dc81120c917 1178 VL6180x_WrByte( dev, 0x00d9, 0x05);
gallonm 7:2dc81120c917 1179
gallonm 7:2dc81120c917 1180 _UpscaleInitPatch0(dev);
gallonm 7:2dc81120c917 1181
gallonm 7:2dc81120c917 1182 VL6180x_WrByte( dev, 0x009f, 0x00);
gallonm 7:2dc81120c917 1183 VL6180x_WrByte( dev, 0x00a3, 0x28);
gallonm 7:2dc81120c917 1184 VL6180x_WrByte( dev, 0x00b7, 0x00);
gallonm 7:2dc81120c917 1185 VL6180x_WrByte( dev, 0x00bb, 0x28);
gallonm 7:2dc81120c917 1186 VL6180x_WrByte( dev, 0x00b2, 0x09);
gallonm 7:2dc81120c917 1187 VL6180x_WrByte( dev, 0x00ca, 0x09);
gallonm 7:2dc81120c917 1188 VL6180x_WrByte( dev, 0x0198, 0x01);
gallonm 7:2dc81120c917 1189 VL6180x_WrByte( dev, 0x01b0, 0x17);
gallonm 7:2dc81120c917 1190 VL6180x_WrByte( dev, 0x01ad, 0x00);
gallonm 7:2dc81120c917 1191 VL6180x_WrByte( dev, 0x00ff, 0x05);
gallonm 7:2dc81120c917 1192 VL6180x_WrByte( dev, 0x0100, 0x05);
gallonm 7:2dc81120c917 1193 VL6180x_WrByte( dev, 0x0199, 0x05);
gallonm 7:2dc81120c917 1194 VL6180x_WrByte( dev, 0x01a6, 0x1b);
gallonm 7:2dc81120c917 1195 VL6180x_WrByte( dev, 0x01ac, 0x3e);
gallonm 7:2dc81120c917 1196 VL6180x_WrByte( dev, 0x01a7, 0x1f);
gallonm 7:2dc81120c917 1197 VL6180x_WrByte( dev, 0x0030, 0x00);
gallonm 7:2dc81120c917 1198 VL6180x_WrByte( dev, 0x0011, 0x10);
gallonm 7:2dc81120c917 1199 VL6180x_WrByte( dev, 0x010a, 0x30);
gallonm 7:2dc81120c917 1200 VL6180x_WrByte( dev, 0x003f, 0x46);
gallonm 7:2dc81120c917 1201 VL6180x_WrByte( dev, 0x0031, 0xFF);
gallonm 7:2dc81120c917 1202 VL6180x_WrByte( dev, 0x0040, 0x63);
gallonm 7:2dc81120c917 1203 VL6180x_WrByte( dev, 0x002e, 0x01);
gallonm 7:2dc81120c917 1204 VL6180x_WrByte( dev, 0x002c, 0xff);
gallonm 7:2dc81120c917 1205 VL6180x_WrByte( dev, 0x001b, 0x09);
gallonm 7:2dc81120c917 1206 VL6180x_WrByte( dev, 0x003e, 0x31);
gallonm 7:2dc81120c917 1207 VL6180x_WrByte( dev, 0x0014, 0x24);
gallonm 7:2dc81120c917 1208 #if VL6180x_EXTENDED_RANGE
gallonm 7:2dc81120c917 1209 VL6180x_RangeSetMaxConvergenceTime(dev, 63);
gallonm 7:2dc81120c917 1210 #else
gallonm 7:2dc81120c917 1211 VL6180x_RangeSetMaxConvergenceTime(dev, 50);
gallonm 7:2dc81120c917 1212 #endif
gallonm 7:2dc81120c917 1213 return 0;
gallonm 7:2dc81120c917 1214 }
gallonm 7:2dc81120c917 1215 #else
gallonm 7:2dc81120c917 1216 #define VL6180x_UpscaleRegInit(...) -1
gallonm 7:2dc81120c917 1217 #endif
gallonm 7:2dc81120c917 1218
gallonm 7:2dc81120c917 1219 int VL6180X::VL6180x_UpscaleSetScaling(VL6180xDev_t dev, uint8_t scaling)
gallonm 7:2dc81120c917 1220 {
gallonm 7:2dc81120c917 1221 int status;
gallonm 7:2dc81120c917 1222 uint16_t Scaler;
gallonm 7:2dc81120c917 1223 int8_t Offset;
gallonm 7:2dc81120c917 1224
gallonm 7:2dc81120c917 1225 LOG_FUNCTION_START("%d",(int) scaling);
gallonm 7:2dc81120c917 1226
gallonm 7:2dc81120c917 1227 #ifdef VL6180x_HAVE_UPSCALE_DATA
gallonm 7:2dc81120c917 1228 #define min_scaling 1
gallonm 7:2dc81120c917 1229 #define max_scaling sizeof(ScalerLookUP)/sizeof(ScalerLookUP[0])
gallonm 7:2dc81120c917 1230 #else
gallonm 7:2dc81120c917 1231 /* we are in fixed config so only allow configured factor */
gallonm 7:2dc81120c917 1232 #define min_scaling VL6180x_UPSCALE_SUPPORT
gallonm 7:2dc81120c917 1233 #define max_scaling VL6180x_UPSCALE_SUPPORT
gallonm 7:2dc81120c917 1234 #endif
gallonm 7:2dc81120c917 1235
gallonm 7:2dc81120c917 1236 if( scaling>=min_scaling && scaling<= max_scaling ){
gallonm 7:2dc81120c917 1237
gallonm 7:2dc81120c917 1238 Scaler = ScalerLookUP[scaling-1];
gallonm 7:2dc81120c917 1239 status = VL6180x_WrWord(dev, RANGE_SCALER, Scaler);
gallonm 7:2dc81120c917 1240 _SetUpscale(dev, scaling );
gallonm 7:2dc81120c917 1241
gallonm 7:2dc81120c917 1242 /* Apply scaling on part-2-part offset */
gallonm 7:2dc81120c917 1243 Offset = VL6180xDevDataGet(dev, Part2PartOffsetNVM)/scaling;
gallonm 7:2dc81120c917 1244 status = VL6180x_WrByte(dev, SYSRANGE_PART_TO_PART_RANGE_OFFSET, Offset);
gallonm 7:2dc81120c917 1245 #if ! VL6180x_EXTENDED_RANGE
gallonm 7:2dc81120c917 1246 if( status ==0 ){
gallonm 7:2dc81120c917 1247 status = VL6180x_RangeSetEceState(dev, scaling == 1); /* enable ece only at 1x scaling */
gallonm 7:2dc81120c917 1248 }
gallonm 7:2dc81120c917 1249 if( status == 0 && !VL6180x_EXTENDED_RANGE && scaling!=1 ){
gallonm 7:2dc81120c917 1250 status = NOT_GUARANTEED ;
gallonm 7:2dc81120c917 1251 }
gallonm 7:2dc81120c917 1252 #endif
gallonm 7:2dc81120c917 1253 }
gallonm 7:2dc81120c917 1254 else{
gallonm 7:2dc81120c917 1255 status = INVALID_PARAMS;
gallonm 7:2dc81120c917 1256 }
gallonm 7:2dc81120c917 1257 #undef min_scaling
gallonm 7:2dc81120c917 1258 #undef max_scaling
gallonm 7:2dc81120c917 1259 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1260 return status;
gallonm 7:2dc81120c917 1261 }
gallonm 7:2dc81120c917 1262
gallonm 7:2dc81120c917 1263
gallonm 7:2dc81120c917 1264 int VL6180X::VL6180x_UpscaleGetScaling(VL6180xDev_t dev)
gallonm 7:2dc81120c917 1265 {
gallonm 7:2dc81120c917 1266 int status;
gallonm 7:2dc81120c917 1267 LOG_FUNCTION_START("");
gallonm 7:2dc81120c917 1268 status=_GetUpscale(dev );
gallonm 7:2dc81120c917 1269 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1270
gallonm 7:2dc81120c917 1271 return status;
gallonm 7:2dc81120c917 1272 }
gallonm 7:2dc81120c917 1273
gallonm 7:2dc81120c917 1274
gallonm 7:2dc81120c917 1275 int VL6180X::VL6180x_UpscaleStaticInit(VL6180xDev_t dev)
gallonm 7:2dc81120c917 1276 {
gallonm 7:2dc81120c917 1277 /* todo make these a fail macro in case only 1x is suppoted */
gallonm 7:2dc81120c917 1278 int status;
gallonm 7:2dc81120c917 1279
gallonm 7:2dc81120c917 1280 LOG_FUNCTION_START("");
gallonm 7:2dc81120c917 1281 do{
gallonm 7:2dc81120c917 1282 status=VL6180x_UpscaleRegInit(dev);
gallonm 7:2dc81120c917 1283 if( status){
gallonm 7:2dc81120c917 1284 VL6180x_ErrLog("regInit fail");
gallonm 7:2dc81120c917 1285 break;
gallonm 7:2dc81120c917 1286 }
gallonm 7:2dc81120c917 1287 #if VL6180x_EXTENDED_RANGE
gallonm 7:2dc81120c917 1288 status = VL6180x_RangeSetEceState(dev, 0);
gallonm 7:2dc81120c917 1289 if( status){
gallonm 7:2dc81120c917 1290 VL6180x_ErrLog("VL6180x_RangeSetEceState fail");
gallonm 7:2dc81120c917 1291 break;
gallonm 7:2dc81120c917 1292 }
gallonm 7:2dc81120c917 1293 #endif
gallonm 7:2dc81120c917 1294 } while(0);
gallonm 7:2dc81120c917 1295 if( !status){
gallonm 7:2dc81120c917 1296 /* must write the scaler at least once to the device to ensure the scaler is in a known state. */
gallonm 7:2dc81120c917 1297 status=VL6180x_UpscaleSetScaling(dev, _GetUpscale(dev));
gallonm 7:2dc81120c917 1298 VL6180x_WrByte( dev, 0x016, 0x00); /* change fresh out of set status to 0 */
gallonm 7:2dc81120c917 1299 }
gallonm 7:2dc81120c917 1300 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1301 return status;
gallonm 7:2dc81120c917 1302 }
gallonm 7:2dc81120c917 1303
gallonm 7:2dc81120c917 1304
gallonm 7:2dc81120c917 1305 int VL6180X::VL6180x_SetGPIOxPolarity(VL6180xDev_t dev, int pin, int active_high)
gallonm 7:2dc81120c917 1306 {
gallonm 7:2dc81120c917 1307 int status;
gallonm 7:2dc81120c917 1308 LOG_FUNCTION_START("%d %d",(int) pin, (int)active_high);
gallonm 7:2dc81120c917 1309
gallonm 7:2dc81120c917 1310 if( pin ==0 || pin ==1 ){
gallonm 7:2dc81120c917 1311 uint16_t RegIndex;
gallonm 7:2dc81120c917 1312 uint8_t DataSet;
gallonm 7:2dc81120c917 1313 if( pin==0 )
gallonm 7:2dc81120c917 1314 RegIndex= SYSTEM_MODE_GPIO0;
gallonm 7:2dc81120c917 1315 else
gallonm 7:2dc81120c917 1316 RegIndex= SYSTEM_MODE_GPIO1;
gallonm 7:2dc81120c917 1317
gallonm 7:2dc81120c917 1318 if (active_high )
gallonm 7:2dc81120c917 1319 DataSet = GPIOx_POLARITY_SELECT_MASK;
gallonm 7:2dc81120c917 1320 else
gallonm 7:2dc81120c917 1321 DataSet = 0;
gallonm 7:2dc81120c917 1322
gallonm 7:2dc81120c917 1323 status = VL6180x_UpdateByte(dev, RegIndex, (uint8_t)~GPIOx_POLARITY_SELECT_MASK, DataSet);
gallonm 7:2dc81120c917 1324 }
gallonm 7:2dc81120c917 1325 else{
gallonm 7:2dc81120c917 1326 VL6180x_ErrLog("Invalid pin param %d", (int)pin);
gallonm 7:2dc81120c917 1327 status = INVALID_PARAMS;
gallonm 7:2dc81120c917 1328 }
gallonm 7:2dc81120c917 1329
gallonm 7:2dc81120c917 1330 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1331
gallonm 7:2dc81120c917 1332 return status;
gallonm 7:2dc81120c917 1333 }
gallonm 7:2dc81120c917 1334
gallonm 7:2dc81120c917 1335 int VL6180X::VL6180x_SetGPIOxFunctionality(VL6180xDev_t dev, int pin, uint8_t functionality)
gallonm 7:2dc81120c917 1336 {
gallonm 7:2dc81120c917 1337 int status;
gallonm 7:2dc81120c917 1338
gallonm 7:2dc81120c917 1339 LOG_FUNCTION_START("%d %d",(int) pin, (int)functionality);
gallonm 7:2dc81120c917 1340
gallonm 7:2dc81120c917 1341 if( ((pin ==0) || (pin ==1)) && IsValidGPIOFunction(functionality) ){
gallonm 7:2dc81120c917 1342 uint16_t RegIndex;
gallonm 7:2dc81120c917 1343
gallonm 7:2dc81120c917 1344 if( pin==0 )
gallonm 7:2dc81120c917 1345 RegIndex= SYSTEM_MODE_GPIO0;
gallonm 7:2dc81120c917 1346 else
gallonm 7:2dc81120c917 1347 RegIndex= SYSTEM_MODE_GPIO1;
gallonm 7:2dc81120c917 1348
gallonm 7:2dc81120c917 1349 status = VL6180x_UpdateByte(dev, RegIndex, (uint8_t)~GPIOx_FUNCTIONALITY_SELECT_MASK, functionality<<GPIOx_FUNCTIONALITY_SELECT_SHIFT);
gallonm 7:2dc81120c917 1350 if( status){
gallonm 7:2dc81120c917 1351 VL6180x_ErrLog("Update SYSTEM_MODE_GPIO%d fail", (int)pin);
gallonm 7:2dc81120c917 1352 }
gallonm 7:2dc81120c917 1353 }
gallonm 7:2dc81120c917 1354 else{
gallonm 7:2dc81120c917 1355 VL6180x_ErrLog("Invalid pin %d or function %d", (int)pin, (int) functionality);
gallonm 7:2dc81120c917 1356 status = INVALID_PARAMS;
gallonm 7:2dc81120c917 1357 }
gallonm 7:2dc81120c917 1358
gallonm 7:2dc81120c917 1359 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1360 return status;
gallonm 7:2dc81120c917 1361 }
gallonm 7:2dc81120c917 1362
gallonm 7:2dc81120c917 1363
gallonm 7:2dc81120c917 1364 int VL6180X::VL6180x_SetupGPIOx(VL6180xDev_t dev, int pin, uint8_t IntFunction, int ActiveHigh)
gallonm 7:2dc81120c917 1365 {
gallonm 7:2dc81120c917 1366 int status;
gallonm 7:2dc81120c917 1367
gallonm 7:2dc81120c917 1368 LOG_FUNCTION_START("%d %d",(int) pin, (int)IntFunction);
gallonm 7:2dc81120c917 1369
gallonm 7:2dc81120c917 1370 if( ((pin ==0) || (pin ==1)) && IsValidGPIOFunction(IntFunction) ){
gallonm 7:2dc81120c917 1371 uint16_t RegIndex;
gallonm 7:2dc81120c917 1372 uint8_t value=0;
gallonm 7:2dc81120c917 1373
gallonm 7:2dc81120c917 1374 if( pin==0 )
gallonm 7:2dc81120c917 1375 RegIndex= SYSTEM_MODE_GPIO0;
gallonm 7:2dc81120c917 1376 else
gallonm 7:2dc81120c917 1377 RegIndex= SYSTEM_MODE_GPIO1;
gallonm 7:2dc81120c917 1378
gallonm 7:2dc81120c917 1379 if( ActiveHigh )
gallonm 7:2dc81120c917 1380 value|=GPIOx_POLARITY_SELECT_MASK;
gallonm 7:2dc81120c917 1381
gallonm 7:2dc81120c917 1382 value |= IntFunction<<GPIOx_FUNCTIONALITY_SELECT_SHIFT;
gallonm 7:2dc81120c917 1383 status = VL6180x_WrByte(dev, RegIndex, value);
gallonm 7:2dc81120c917 1384 if( status ){
gallonm 7:2dc81120c917 1385 VL6180x_ErrLog("SYSTEM_MODE_GPIO%d wr fail", (int)pin-SYSTEM_MODE_GPIO0);
gallonm 7:2dc81120c917 1386 }
gallonm 7:2dc81120c917 1387 }
gallonm 7:2dc81120c917 1388 else{
gallonm 7:2dc81120c917 1389 VL6180x_ErrLog("Invalid pin %d or function %d", (int)pin, (int) IntFunction);
gallonm 7:2dc81120c917 1390 status = INVALID_PARAMS;
gallonm 7:2dc81120c917 1391 }
gallonm 7:2dc81120c917 1392
gallonm 7:2dc81120c917 1393 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1394 return status;
gallonm 7:2dc81120c917 1395 }
gallonm 7:2dc81120c917 1396
gallonm 7:2dc81120c917 1397
gallonm 7:2dc81120c917 1398 int VL6180X::VL6180x_DisableGPIOxOut(VL6180xDev_t dev, int pin) {
gallonm 7:2dc81120c917 1399 int status;
gallonm 7:2dc81120c917 1400
gallonm 7:2dc81120c917 1401 LOG_FUNCTION_START("%d",(int)pin);
gallonm 7:2dc81120c917 1402
gallonm 7:2dc81120c917 1403 status=VL6180x_SetGPIOxFunctionality(dev, pin, GPIOx_SELECT_OFF);
gallonm 7:2dc81120c917 1404
gallonm 7:2dc81120c917 1405 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1406 return status;
gallonm 7:2dc81120c917 1407 }
gallonm 7:2dc81120c917 1408
gallonm 7:2dc81120c917 1409
gallonm 7:2dc81120c917 1410 int VL6180X::VL6180x_SetupGPIO1(VL6180xDev_t dev, uint8_t IntFunction, int ActiveHigh)
gallonm 7:2dc81120c917 1411 {
gallonm 7:2dc81120c917 1412 int status;
gallonm 7:2dc81120c917 1413 LOG_FUNCTION_START("%d %d",(int)IntFunction, (int)ActiveHigh );
gallonm 7:2dc81120c917 1414 status=VL6180x_SetupGPIOx(dev, 1 , IntFunction, ActiveHigh);
gallonm 7:2dc81120c917 1415 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1416 return status;
gallonm 7:2dc81120c917 1417 }
gallonm 7:2dc81120c917 1418
gallonm 7:2dc81120c917 1419 int VL6180X::VL6180x_RangeConfigInterrupt(VL6180xDev_t dev, uint8_t ConfigGpioInt)
gallonm 7:2dc81120c917 1420 {
gallonm 7:2dc81120c917 1421 int status;
gallonm 7:2dc81120c917 1422
gallonm 7:2dc81120c917 1423 if( ConfigGpioInt<= CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY){
gallonm 7:2dc81120c917 1424 status = VL6180x_UpdateByte(dev, SYSTEM_INTERRUPT_CONFIG_GPIO, (uint8_t)(~CONFIG_GPIO_RANGE_MASK), ConfigGpioInt);
gallonm 7:2dc81120c917 1425 }
gallonm 7:2dc81120c917 1426 else{
gallonm 7:2dc81120c917 1427 VL6180x_ErrLog("Invalid config mode param %d", (int)ConfigGpioInt);
gallonm 7:2dc81120c917 1428 status = INVALID_PARAMS;
gallonm 7:2dc81120c917 1429 }
gallonm 7:2dc81120c917 1430 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1431 return status;
gallonm 7:2dc81120c917 1432 }
gallonm 7:2dc81120c917 1433
gallonm 7:2dc81120c917 1434
gallonm 7:2dc81120c917 1435 int VL6180X::VL6180x_RangeSetEceFactor(VL6180xDev_t dev, uint16_t FactorM, uint16_t FactorD){
gallonm 7:2dc81120c917 1436 int status;
gallonm 7:2dc81120c917 1437 uint8_t u8;
gallonm 7:2dc81120c917 1438
gallonm 7:2dc81120c917 1439 LOG_FUNCTION_START("%d %d", (int)FactorM, (int)FactorD );
gallonm 7:2dc81120c917 1440 do{
gallonm 7:2dc81120c917 1441 /* D cannot be 0 M must be <=D and >= 0 */
gallonm 7:2dc81120c917 1442 if( FactorM <= FactorD && FactorD> 0){
gallonm 7:2dc81120c917 1443 VL6180xDevDataSet(dev, EceFactorM, FactorM);
gallonm 7:2dc81120c917 1444 VL6180xDevDataSet(dev, EceFactorD, FactorD);
gallonm 7:2dc81120c917 1445 /* read and re-apply max conv time to get new ece factor set */
gallonm 7:2dc81120c917 1446 status = VL6180x_RdByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, &u8);
gallonm 7:2dc81120c917 1447 if( status){
gallonm 7:2dc81120c917 1448 VL6180x_ErrLog("SYSRANGE_MAX_CONVERGENCE_TIME rd fail ");
gallonm 7:2dc81120c917 1449 break;
gallonm 7:2dc81120c917 1450 }
gallonm 7:2dc81120c917 1451 status = VL6180x_RangeSetMaxConvergenceTime(dev, u8);
gallonm 7:2dc81120c917 1452 if( status <0 ){
gallonm 7:2dc81120c917 1453 VL6180x_ErrLog("fail to apply time after ece m/d change");
gallonm 7:2dc81120c917 1454 break;
gallonm 7:2dc81120c917 1455 }
gallonm 7:2dc81120c917 1456 }
gallonm 7:2dc81120c917 1457 else{
gallonm 7:2dc81120c917 1458 VL6180x_ErrLog("invalid factor %d/%d", (int)FactorM, (int)FactorD );
gallonm 7:2dc81120c917 1459 status = INVALID_PARAMS;
gallonm 7:2dc81120c917 1460 }
gallonm 7:2dc81120c917 1461 }
gallonm 7:2dc81120c917 1462 while(0);
gallonm 7:2dc81120c917 1463 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1464 return status;
gallonm 7:2dc81120c917 1465 }
gallonm 7:2dc81120c917 1466
gallonm 7:2dc81120c917 1467 int VL6180X::VL6180x_RangeSetEceState(VL6180xDev_t dev, int enable ){
gallonm 7:2dc81120c917 1468 int status;
gallonm 7:2dc81120c917 1469 uint8_t or_mask;
gallonm 7:2dc81120c917 1470
gallonm 7:2dc81120c917 1471 LOG_FUNCTION_START("%d", (int)enable);
gallonm 7:2dc81120c917 1472 if( enable )
gallonm 7:2dc81120c917 1473 or_mask = RANGE_CHECK_ECE_ENABLE_MASK;
gallonm 7:2dc81120c917 1474 else
gallonm 7:2dc81120c917 1475 or_mask = 0;
gallonm 7:2dc81120c917 1476
gallonm 7:2dc81120c917 1477 status =VL6180x_UpdateByte(dev, SYSRANGE_RANGE_CHECK_ENABLES, ~RANGE_CHECK_ECE_ENABLE_MASK, or_mask);
gallonm 7:2dc81120c917 1478 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1479 return status;
gallonm 7:2dc81120c917 1480 }
gallonm 7:2dc81120c917 1481
gallonm 7:2dc81120c917 1482
gallonm 7:2dc81120c917 1483 int VL6180X::VL6180x_RangeSetMaxConvergenceTime(VL6180xDev_t dev, uint8_t MaxConTime_msec)
gallonm 7:2dc81120c917 1484 {
gallonm 7:2dc81120c917 1485 int status = 0;
gallonm 7:2dc81120c917 1486 LOG_FUNCTION_START("%d",(int)MaxConTime_msec);
gallonm 7:2dc81120c917 1487 do{
gallonm 7:2dc81120c917 1488 status=VL6180x_WrByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, MaxConTime_msec);
gallonm 7:2dc81120c917 1489 if( status ){
gallonm 7:2dc81120c917 1490 break;
gallonm 7:2dc81120c917 1491 }
gallonm 7:2dc81120c917 1492 status=VL6180x_RangeSetEarlyConvergenceEestimateThreshold(dev);
gallonm 7:2dc81120c917 1493 if( status){
gallonm 7:2dc81120c917 1494 break;
gallonm 7:2dc81120c917 1495 }
gallonm 7:2dc81120c917 1496 status = _DMax_InitData(dev);
gallonm 7:2dc81120c917 1497 }
gallonm 7:2dc81120c917 1498 while(0);
gallonm 7:2dc81120c917 1499 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1500 return status;
gallonm 7:2dc81120c917 1501 }
gallonm 7:2dc81120c917 1502
gallonm 7:2dc81120c917 1503 int VL6180X::VL6180x_RangeSetInterMeasPeriod(VL6180xDev_t dev, uint32_t InterMeasTime_msec){
gallonm 7:2dc81120c917 1504 uint8_t SetTime;
gallonm 7:2dc81120c917 1505 int status;
gallonm 7:2dc81120c917 1506
gallonm 7:2dc81120c917 1507 LOG_FUNCTION_START("%d",(int)InterMeasTime_msec);
gallonm 7:2dc81120c917 1508 do {
gallonm 7:2dc81120c917 1509 if( InterMeasTime_msec > 2550 ){
gallonm 7:2dc81120c917 1510 status = INVALID_PARAMS;
gallonm 7:2dc81120c917 1511 break;
gallonm 7:2dc81120c917 1512 }
gallonm 7:2dc81120c917 1513 /* doc in not 100% clear and confusing about the limit practically all value are OK but 0
gallonm 7:2dc81120c917 1514 * that can hang device in continuous mode */
gallonm 7:2dc81120c917 1515 if( InterMeasTime_msec < 10 ) {
gallonm 7:2dc81120c917 1516 InterMeasTime_msec=10;
gallonm 7:2dc81120c917 1517 }
gallonm 7:2dc81120c917 1518 SetTime=(uint8_t)(InterMeasTime_msec/10);
gallonm 7:2dc81120c917 1519 status=VL6180x_WrByte(dev, SYSRANGE_INTERMEASUREMENT_PERIOD, SetTime);
gallonm 7:2dc81120c917 1520 if( status ){
gallonm 7:2dc81120c917 1521 VL6180x_ErrLog("SYSRANGE_INTERMEASUREMENT_PERIOD wr fail");
gallonm 7:2dc81120c917 1522 }
gallonm 7:2dc81120c917 1523 else
gallonm 7:2dc81120c917 1524 if( SetTime != InterMeasTime_msec /10 ) {
gallonm 7:2dc81120c917 1525 status = MIN_CLIPED; /* on success change status to clip if it did */
gallonm 7:2dc81120c917 1526 }
gallonm 7:2dc81120c917 1527 }while(0);
gallonm 7:2dc81120c917 1528 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1529 return status;
gallonm 7:2dc81120c917 1530 }
gallonm 7:2dc81120c917 1531
gallonm 7:2dc81120c917 1532
gallonm 7:2dc81120c917 1533 int VL6180X::VL6180x_RangeGetDeviceReady(VL6180xDev_t dev, int * Ready){
gallonm 7:2dc81120c917 1534 int status;
gallonm 7:2dc81120c917 1535 uint8_t u8;
gallonm 7:2dc81120c917 1536 LOG_FUNCTION_START("%p", (int)Ready);
gallonm 7:2dc81120c917 1537 status=VL6180x_RdByte(dev, RESULT_RANGE_STATUS, &u8);
gallonm 7:2dc81120c917 1538 if( !status)
gallonm 7:2dc81120c917 1539 *Ready = u8&RANGE_DEVICE_READY_MASK;
gallonm 7:2dc81120c917 1540 LOG_FUNCTION_END_FMT(status,"%d", *Ready);
gallonm 7:2dc81120c917 1541 return status;
gallonm 7:2dc81120c917 1542 }
gallonm 7:2dc81120c917 1543
gallonm 7:2dc81120c917 1544
gallonm 7:2dc81120c917 1545 int VL6180X::VL6180x_RangeWaitDeviceReady(VL6180xDev_t dev, int MaxLoop ){
gallonm 7:2dc81120c917 1546 int status; /* if user specify an invalid <=0 loop count we'll return error */
gallonm 7:2dc81120c917 1547 int n;
gallonm 7:2dc81120c917 1548 uint8_t u8;
gallonm 7:2dc81120c917 1549 LOG_FUNCTION_START("%d", (int)MaxLoop);
gallonm 7:2dc81120c917 1550 if( MaxLoop<1){
gallonm 7:2dc81120c917 1551 status=INVALID_PARAMS;
gallonm 7:2dc81120c917 1552 }
gallonm 7:2dc81120c917 1553 else{
gallonm 7:2dc81120c917 1554 for( n=0; n < MaxLoop ; n++){
gallonm 7:2dc81120c917 1555 status=VL6180x_RdByte(dev, RESULT_RANGE_STATUS, &u8);
gallonm 7:2dc81120c917 1556 if( status)
gallonm 7:2dc81120c917 1557 break;
gallonm 7:2dc81120c917 1558 u8 = u8 & RANGE_DEVICE_READY_MASK;
gallonm 7:2dc81120c917 1559 if( u8 )
gallonm 7:2dc81120c917 1560 break;
gallonm 7:2dc81120c917 1561
gallonm 7:2dc81120c917 1562 }
gallonm 7:2dc81120c917 1563 if( !status && !u8 ){
gallonm 7:2dc81120c917 1564 status = TIME_OUT;
gallonm 7:2dc81120c917 1565 }
gallonm 7:2dc81120c917 1566 }
gallonm 7:2dc81120c917 1567 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1568 return status;
gallonm 7:2dc81120c917 1569 }
gallonm 7:2dc81120c917 1570
gallonm 7:2dc81120c917 1571 int VL6180X::VL6180x_RangeSetSystemMode(VL6180xDev_t dev, uint8_t mode)
gallonm 7:2dc81120c917 1572 {
gallonm 7:2dc81120c917 1573 int status;
gallonm 7:2dc81120c917 1574 LOG_FUNCTION_START("%d", (int)mode);
gallonm 7:2dc81120c917 1575 /* FIXME we are not checking device is ready via @a VL6180x_RangeWaitDeviceReady
gallonm 7:2dc81120c917 1576 * so if called back to back real fast we are not checking
gallonm 7:2dc81120c917 1577 * if previous mode "set" got absorbed => bit 0 must be 0 so that it work
gallonm 7:2dc81120c917 1578 */
gallonm 7:2dc81120c917 1579 if( mode <= 3){
gallonm 7:2dc81120c917 1580 status=VL6180x_WrByte(dev, SYSRANGE_START, mode);
gallonm 7:2dc81120c917 1581 if( status ){
gallonm 7:2dc81120c917 1582 VL6180x_ErrLog("SYSRANGE_START wr fail");
gallonm 7:2dc81120c917 1583 }
gallonm 7:2dc81120c917 1584 }
gallonm 7:2dc81120c917 1585 else{
gallonm 7:2dc81120c917 1586 status = INVALID_PARAMS;
gallonm 7:2dc81120c917 1587 }
gallonm 7:2dc81120c917 1588 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1589 return status;
gallonm 7:2dc81120c917 1590 }
gallonm 7:2dc81120c917 1591
gallonm 7:2dc81120c917 1592
gallonm 7:2dc81120c917 1593 int VL6180X::VL6180x_RangeStartContinuousMode(VL6180xDev_t dev)
gallonm 7:2dc81120c917 1594 {
gallonm 7:2dc81120c917 1595 int status;
gallonm 7:2dc81120c917 1596 LOG_FUNCTION_START("");
gallonm 7:2dc81120c917 1597 status= VL6180x_RangeSetSystemMode(dev, MODE_START_STOP | MODE_CONTINUOUS);
gallonm 7:2dc81120c917 1598 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1599 return status;
gallonm 7:2dc81120c917 1600 }
gallonm 7:2dc81120c917 1601
gallonm 7:2dc81120c917 1602 int VL6180X::VL6180x_RangeStartSingleShot(VL6180xDev_t dev) {
gallonm 7:2dc81120c917 1603 int status;
gallonm 7:2dc81120c917 1604 LOG_FUNCTION_START("");
gallonm 7:2dc81120c917 1605 status = VL6180x_RangeSetSystemMode(dev, MODE_START_STOP|MODE_SINGLESHOT);
gallonm 7:2dc81120c917 1606 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1607 return status;
gallonm 7:2dc81120c917 1608 }
gallonm 7:2dc81120c917 1609
gallonm 7:2dc81120c917 1610
gallonm 7:2dc81120c917 1611 int VL6180X::VL6180x_RangeSetEarlyConvergenceEestimateThreshold(VL6180xDev_t dev)
gallonm 7:2dc81120c917 1612 {
gallonm 7:2dc81120c917 1613 int status;
gallonm 7:2dc81120c917 1614
gallonm 7:2dc81120c917 1615 const uint32_t cMicroSecPerMilliSec = 1000;
gallonm 7:2dc81120c917 1616 const uint32_t cEceSampleTime_us = 500;
gallonm 7:2dc81120c917 1617 uint32_t ece_factor_m = VL6180xDevDataGet(dev, EceFactorM);
gallonm 7:2dc81120c917 1618 uint32_t ece_factor_d = VL6180xDevDataGet(dev, EceFactorD);
gallonm 7:2dc81120c917 1619 uint32_t convergTime_us;
gallonm 7:2dc81120c917 1620 uint32_t fineThresh;
gallonm 7:2dc81120c917 1621 uint32_t eceThresh;
gallonm 7:2dc81120c917 1622 uint8_t u8;
gallonm 7:2dc81120c917 1623 uint32_t maxConv_ms;
gallonm 7:2dc81120c917 1624 int32_t AveTime;
gallonm 7:2dc81120c917 1625
gallonm 7:2dc81120c917 1626 LOG_FUNCTION_START("");
gallonm 7:2dc81120c917 1627
gallonm 7:2dc81120c917 1628 do{
gallonm 7:2dc81120c917 1629 status = VL6180x_RdByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, &u8);
gallonm 7:2dc81120c917 1630 if( status ){
gallonm 7:2dc81120c917 1631 VL6180x_ErrLog("SYSRANGE_MAX_CONVERGENCE_TIME rd fail");
gallonm 7:2dc81120c917 1632 break;
gallonm 7:2dc81120c917 1633 }
gallonm 7:2dc81120c917 1634 maxConv_ms = u8;
gallonm 7:2dc81120c917 1635 AveTime = _GetAveTotalTime(dev);
gallonm 7:2dc81120c917 1636 if( AveTime <0 ){
gallonm 7:2dc81120c917 1637 status=-1;
gallonm 7:2dc81120c917 1638 break;
gallonm 7:2dc81120c917 1639 }
gallonm 7:2dc81120c917 1640
gallonm 7:2dc81120c917 1641 convergTime_us = maxConv_ms * cMicroSecPerMilliSec - AveTime;
gallonm 7:2dc81120c917 1642 status = VL6180x_RdDWord(dev, 0xB8, &fineThresh);
gallonm 7:2dc81120c917 1643 if( status ) {
gallonm 7:2dc81120c917 1644 VL6180x_ErrLog("reg 0xB8 rd fail");
gallonm 7:2dc81120c917 1645 break;
gallonm 7:2dc81120c917 1646 }
gallonm 7:2dc81120c917 1647 fineThresh*=256;
gallonm 7:2dc81120c917 1648 eceThresh = ece_factor_m * cEceSampleTime_us * fineThresh/(convergTime_us * ece_factor_d);
gallonm 7:2dc81120c917 1649
gallonm 7:2dc81120c917 1650 status=VL6180x_WrWord(dev, SYSRANGE_EARLY_CONVERGENCE_ESTIMATE, (uint16_t)eceThresh);
gallonm 7:2dc81120c917 1651 }
gallonm 7:2dc81120c917 1652 while(0);
gallonm 7:2dc81120c917 1653
gallonm 7:2dc81120c917 1654 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 1655 return status;
gallonm 7:2dc81120c917 1656 }
gallonm 7:2dc81120c917 1657
gallonm 7:2dc81120c917 1658 /*
gallonm 7:2dc81120c917 1659 * Return >0 = time
gallonm 7:2dc81120c917 1660 * <0 1 if fail to get read data from device to compute time
gallonm 7:2dc81120c917 1661 */
gallonm 7:2dc81120c917 1662 int32_t VL6180X::_GetAveTotalTime(VL6180xDev_t dev) {
gallonm 7:2dc81120c917 1663 uint32_t cFwOverhead_us = 24;
gallonm 7:2dc81120c917 1664 uint32_t cVcpSetupTime_us = 70;
gallonm 7:2dc81120c917 1665 uint32_t cPLL2_StartupDelay_us = 200;
gallonm 7:2dc81120c917 1666 uint8_t cMeasMask = 0x07;
gallonm 7:2dc81120c917 1667 uint32_t Samples;
gallonm 7:2dc81120c917 1668 uint32_t SamplePeriod;
gallonm 7:2dc81120c917 1669 uint32_t SingleTime_us;
gallonm 7:2dc81120c917 1670 int32_t TotalAveTime_us;
gallonm 7:2dc81120c917 1671 uint8_t u8;
gallonm 7:2dc81120c917 1672 int status;
gallonm 7:2dc81120c917 1673
gallonm 7:2dc81120c917 1674 LOG_FUNCTION_START("");
gallonm 7:2dc81120c917 1675
gallonm 7:2dc81120c917 1676 status = VL6180x_RdByte(dev, 0x109, &u8);
gallonm 7:2dc81120c917 1677 if (status) {
gallonm 7:2dc81120c917 1678 VL6180x_ErrLog("rd 0x109 fail");
gallonm 7:2dc81120c917 1679 return -1;
gallonm 7:2dc81120c917 1680 }
gallonm 7:2dc81120c917 1681 Samples = u8 & cMeasMask;
gallonm 7:2dc81120c917 1682 status = VL6180x_RdByte(dev, READOUT_AVERAGING_SAMPLE_PERIOD, &u8);
gallonm 7:2dc81120c917 1683 if (status) {
gallonm 7:2dc81120c917 1684 VL6180x_ErrLog("i2c READOUT_AVERAGING_SAMPLE_PERIOD fail");
gallonm 7:2dc81120c917 1685 return -1;
gallonm 7:2dc81120c917 1686 }
gallonm 7:2dc81120c917 1687 SamplePeriod = u8;
gallonm 7:2dc81120c917 1688 SingleTime_us = cFwOverhead_us + cVcpSetupTime_us + (SamplePeriod * 10);
gallonm 7:2dc81120c917 1689 TotalAveTime_us = (Samples + 1) * SingleTime_us + cPLL2_StartupDelay_us;
gallonm 7:2dc81120c917 1690
gallonm 7:2dc81120c917 1691 LOG_FUNCTION_END(TotalAveTime_us);
gallonm 7:2dc81120c917 1692 return TotalAveTime_us;
gallonm 7:2dc81120c917 1693 }
gallonm 7:2dc81120c917 1694
gallonm 7:2dc81120c917 1695 #if VL6180x_HAVE_DMAX_RANGING
gallonm 7:2dc81120c917 1696 #define _GetDMaxDataRetSignalAt400mm(dev) VL6180xDevDataGet(dev, DMaxData.retSignalAt400mm)
gallonm 7:2dc81120c917 1697 #else
gallonm 7:2dc81120c917 1698 #define _GetDMaxDataRetSignalAt400mm(dev) 375 // Use a default high value
gallonm 7:2dc81120c917 1699 #endif
gallonm 7:2dc81120c917 1700
gallonm 7:2dc81120c917 1701
gallonm 7:2dc81120c917 1702 #if VL6180x_WRAP_AROUND_FILTER_SUPPORT
gallonm 7:2dc81120c917 1703
gallonm 7:2dc81120c917 1704 #define FILTER_STDDEV_SAMPLES 6
gallonm 7:2dc81120c917 1705 #define MIN_FILTER_STDDEV_SAMPLES 3
gallonm 7:2dc81120c917 1706 #define MIN_FILTER_VALID_STDDEV_SAMPLES 3
gallonm 7:2dc81120c917 1707 #define FILTER_INVALID_DISTANCE 65535
gallonm 7:2dc81120c917 1708
gallonm 7:2dc81120c917 1709 #define _FilterData(field) VL6180xDevDataGet(dev, FilterData.field)
gallonm 7:2dc81120c917 1710 /*
gallonm 7:2dc81120c917 1711 * One time init
gallonm 7:2dc81120c917 1712 */
gallonm 7:2dc81120c917 1713 int _filter_Init( VL6180xDev_t dev) {
gallonm 7:2dc81120c917 1714 int i;
gallonm 7:2dc81120c917 1715 _FilterData(MeasurementIndex) = 0;
gallonm 7:2dc81120c917 1716
gallonm 7:2dc81120c917 1717 _FilterData(Default_ZeroVal) = 0;
gallonm 7:2dc81120c917 1718 _FilterData(Default_VAVGVal) = 0;
gallonm 7:2dc81120c917 1719 _FilterData(NoDelay_ZeroVal) = 0;
gallonm 7:2dc81120c917 1720 _FilterData(NoDelay_VAVGVal) = 0;
gallonm 7:2dc81120c917 1721 _FilterData(Previous_VAVGDiff) = 0;
gallonm 7:2dc81120c917 1722
gallonm 7:2dc81120c917 1723 _FilterData(StdFilteredReads) = 0;
gallonm 7:2dc81120c917 1724
gallonm 7:2dc81120c917 1725 for (i = 0; i < FILTER_NBOF_SAMPLES; i++) {
gallonm 7:2dc81120c917 1726 _FilterData(LastTrueRange)[i] = FILTER_INVALID_DISTANCE;
gallonm 7:2dc81120c917 1727 _FilterData(LastReturnRates)[i] = 0;
gallonm 7:2dc81120c917 1728 }
gallonm 7:2dc81120c917 1729 return 0;
gallonm 7:2dc81120c917 1730 }
gallonm 7:2dc81120c917 1731
gallonm 7:2dc81120c917 1732
gallonm 7:2dc81120c917 1733 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) {
gallonm 7:2dc81120c917 1734 uint32_t newStdDev;
gallonm 7:2dc81120c917 1735 uint16_t SNR;
gallonm 7:2dc81120c917 1736
gallonm 7:2dc81120c917 1737 if (AmbientRate > 0)
gallonm 7:2dc81120c917 1738 SNR = (uint16_t) ((100 * SignalRate) / AmbientRate);
gallonm 7:2dc81120c917 1739 else
gallonm 7:2dc81120c917 1740 SNR = 9999;
gallonm 7:2dc81120c917 1741
gallonm 7:2dc81120c917 1742 if (SNR >= StdDevLimitLowLightSNR) {
gallonm 7:2dc81120c917 1743 newStdDev = StdDevLimitLowLight;
gallonm 7:2dc81120c917 1744 } else {
gallonm 7:2dc81120c917 1745 if (SNR <= StdDevLimitHighLightSNR)
gallonm 7:2dc81120c917 1746 newStdDev = StdDevLimitHighLight;
gallonm 7:2dc81120c917 1747 else {
gallonm 7:2dc81120c917 1748 newStdDev = (uint32_t) (StdDevLimitHighLight + (SNR - StdDevLimitHighLightSNR) * (int) (StdDevLimitLowLight - StdDevLimitHighLight) / (StdDevLimitLowLightSNR - StdDevLimitHighLightSNR));
gallonm 7:2dc81120c917 1749 }
gallonm 7:2dc81120c917 1750 }
gallonm 7:2dc81120c917 1751
gallonm 7:2dc81120c917 1752 return newStdDev;
gallonm 7:2dc81120c917 1753 }
gallonm 7:2dc81120c917 1754
gallonm 7:2dc81120c917 1755
gallonm 7:2dc81120c917 1756 /*
gallonm 7:2dc81120c917 1757 * Return <0 on error
gallonm 7:2dc81120c917 1758 */
gallonm 7:2dc81120c917 1759 int32_t VL6180X::_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) {
gallonm 7:2dc81120c917 1760 int status;
gallonm 7:2dc81120c917 1761 uint16_t m_newTrueRange_mm = 0;
gallonm 7:2dc81120c917 1762
gallonm 7:2dc81120c917 1763 uint16_t i;
gallonm 7:2dc81120c917 1764 uint16_t bypassFilter = 0;
gallonm 7:2dc81120c917 1765
gallonm 7:2dc81120c917 1766 uint16_t registerValue;
gallonm 7:2dc81120c917 1767
gallonm 7:2dc81120c917 1768 uint32_t register32BitsValue1;
gallonm 7:2dc81120c917 1769 uint32_t register32BitsValue2;
gallonm 7:2dc81120c917 1770
gallonm 7:2dc81120c917 1771 uint16_t ValidDistance = 0;
gallonm 7:2dc81120c917 1772
gallonm 7:2dc81120c917 1773 uint16_t WrapAroundFlag = 0;
gallonm 7:2dc81120c917 1774 uint16_t NoWrapAroundFlag = 0;
gallonm 7:2dc81120c917 1775 uint16_t NoWrapAroundHighConfidenceFlag = 0;
gallonm 7:2dc81120c917 1776
gallonm 7:2dc81120c917 1777 uint16_t FlushFilter = 0;
gallonm 7:2dc81120c917 1778 uint32_t RateChange = 0;
gallonm 7:2dc81120c917 1779
gallonm 7:2dc81120c917 1780 uint16_t StdDevSamples = 0;
gallonm 7:2dc81120c917 1781 uint32_t StdDevDistanceSum = 0;
gallonm 7:2dc81120c917 1782 uint32_t StdDevDistanceMean = 0;
gallonm 7:2dc81120c917 1783 uint32_t StdDevDistance = 0;
gallonm 7:2dc81120c917 1784 uint32_t StdDevRateSum = 0;
gallonm 7:2dc81120c917 1785 uint32_t StdDevRateMean = 0;
gallonm 7:2dc81120c917 1786 uint32_t StdDevRate = 0;
gallonm 7:2dc81120c917 1787 uint32_t StdDevLimitWithTargetMove = 0;
gallonm 7:2dc81120c917 1788
gallonm 7:2dc81120c917 1789 uint32_t VAVGDiff;
gallonm 7:2dc81120c917 1790 uint32_t IdealVAVGDiff;
gallonm 7:2dc81120c917 1791 uint32_t MinVAVGDiff;
gallonm 7:2dc81120c917 1792 uint32_t MaxVAVGDiff;
gallonm 7:2dc81120c917 1793
gallonm 7:2dc81120c917 1794 /* Filter Parameters */
gallonm 7:2dc81120c917 1795 static const uint16_t ROMABLE_DATA WrapAroundLowRawRangeLimit = 60;
gallonm 7:2dc81120c917 1796 static const uint32_t ROMABLE_DATA WrapAroundLowReturnRateLimit_ROM = 800; // Shall be adapted depending on crossTalk
gallonm 7:2dc81120c917 1797 static const uint16_t ROMABLE_DATA WrapAroundLowRawRangeLimit2 = 165;
gallonm 7:2dc81120c917 1798 static const uint32_t ROMABLE_DATA WrapAroundLowReturnRateLimit2_ROM = 180; // Shall be adapted depending on crossTalk and device sensitivity
gallonm 7:2dc81120c917 1799
gallonm 7:2dc81120c917 1800 static const uint32_t ROMABLE_DATA WrapAroundLowReturnRateFilterLimit_ROM = 850; // Shall be adapted depending on crossTalk and device sensitivity
gallonm 7:2dc81120c917 1801 static const uint16_t ROMABLE_DATA WrapAroundHighRawRangeFilterLimit = 350;
gallonm 7:2dc81120c917 1802 static const uint32_t ROMABLE_DATA WrapAroundHighReturnRateFilterLimit_ROM = 1400; // Shall be adapted depending on crossTalk and device sensitivity
gallonm 7:2dc81120c917 1803
gallonm 7:2dc81120c917 1804 static const uint32_t ROMABLE_DATA WrapAroundMaximumAmbientRateFilterLimit = 7500;
gallonm 7:2dc81120c917 1805
gallonm 7:2dc81120c917 1806 /* Temporal filter data and flush values */
gallonm 7:2dc81120c917 1807 static const uint32_t ROMABLE_DATA MinReturnRateFilterFlush = 75;
gallonm 7:2dc81120c917 1808 static const uint32_t ROMABLE_DATA MaxReturnRateChangeFilterFlush = 50;
gallonm 7:2dc81120c917 1809
gallonm 7:2dc81120c917 1810 /* STDDEV values and damper values */
gallonm 7:2dc81120c917 1811
gallonm 7:2dc81120c917 1812 static const uint32_t ROMABLE_DATA StdDevLimitLowLight = 300;
gallonm 7:2dc81120c917 1813 static const uint32_t ROMABLE_DATA StdDevLimitLowLightSNR = 30; /* 0.3 */
gallonm 7:2dc81120c917 1814 static const uint32_t ROMABLE_DATA StdDevLimitHighLight = 2500;
gallonm 7:2dc81120c917 1815 static const uint32_t ROMABLE_DATA StdDevLimitHighLightSNR = 5; /* 0.05 */
gallonm 7:2dc81120c917 1816
gallonm 7:2dc81120c917 1817 static const uint32_t ROMABLE_DATA StdDevHighConfidenceSNRLimit = 8;
gallonm 7:2dc81120c917 1818
gallonm 7:2dc81120c917 1819 static const uint32_t ROMABLE_DATA StdDevMovingTargetStdDevLimit = 90000;
gallonm 7:2dc81120c917 1820
gallonm 7:2dc81120c917 1821 static const uint32_t ROMABLE_DATA StdDevMovingTargetReturnRateLimit = 3500;
gallonm 7:2dc81120c917 1822 static const uint32_t ROMABLE_DATA StdDevMovingTargetStdDevForReturnRateLimit = 5000;
gallonm 7:2dc81120c917 1823
gallonm 7:2dc81120c917 1824 static const uint32_t ROMABLE_DATA MAX_VAVGDiff = 1800;
gallonm 7:2dc81120c917 1825
gallonm 7:2dc81120c917 1826 /* WrapAroundDetection variables */
gallonm 7:2dc81120c917 1827 static const uint16_t ROMABLE_DATA WrapAroundNoDelayCheckPeriod = 2;
gallonm 7:2dc81120c917 1828 static const uint16_t ROMABLE_DATA StdFilteredReadsIncrement = 2;
gallonm 7:2dc81120c917 1829 static const uint16_t ROMABLE_DATA StdMaxFilteredReads = 4;
gallonm 7:2dc81120c917 1830
gallonm 7:2dc81120c917 1831 uint32_t SignalRateDMax;
gallonm 7:2dc81120c917 1832 uint32_t WrapAroundLowReturnRateLimit;
gallonm 7:2dc81120c917 1833 uint32_t WrapAroundLowReturnRateLimit2;
gallonm 7:2dc81120c917 1834 uint32_t WrapAroundLowReturnRateFilterLimit;
gallonm 7:2dc81120c917 1835 uint32_t WrapAroundHighReturnRateFilterLimit;
gallonm 7:2dc81120c917 1836
gallonm 7:2dc81120c917 1837 uint8_t u8, u8_2;
gallonm 7:2dc81120c917 1838 uint32_t XTalkCompRate_KCps;
gallonm 7:2dc81120c917 1839 uint32_t StdDevLimit = 300;
gallonm 7:2dc81120c917 1840 uint32_t MaxOrInvalidDistance = 255*_GetUpscale(dev);
gallonm 7:2dc81120c917 1841 /* #define MaxOrInvalidDistance (uint16_t) (255 * 3) */
gallonm 7:2dc81120c917 1842
gallonm 7:2dc81120c917 1843 /* Check if distance is Valid or not */
gallonm 7:2dc81120c917 1844 switch (errorCode) {
gallonm 7:2dc81120c917 1845 case 0x0C:
gallonm 7:2dc81120c917 1846 m_trueRange_mm = MaxOrInvalidDistance;
gallonm 7:2dc81120c917 1847 ValidDistance = 0;
gallonm 7:2dc81120c917 1848 break;
gallonm 7:2dc81120c917 1849 case 0x0D:
gallonm 7:2dc81120c917 1850 m_trueRange_mm = MaxOrInvalidDistance;
gallonm 7:2dc81120c917 1851 ValidDistance = 1;
gallonm 7:2dc81120c917 1852 break;
gallonm 7:2dc81120c917 1853 case 0x0F:
gallonm 7:2dc81120c917 1854 m_trueRange_mm = MaxOrInvalidDistance;
gallonm 7:2dc81120c917 1855 ValidDistance = 1;
gallonm 7:2dc81120c917 1856 break;
gallonm 7:2dc81120c917 1857 default:
gallonm 7:2dc81120c917 1858 if (m_rawRange_mm >= MaxOrInvalidDistance) {
gallonm 7:2dc81120c917 1859 ValidDistance = 0;
gallonm 7:2dc81120c917 1860 } else {
gallonm 7:2dc81120c917 1861 ValidDistance = 1;
gallonm 7:2dc81120c917 1862 }
gallonm 7:2dc81120c917 1863 break;
gallonm 7:2dc81120c917 1864 }
gallonm 7:2dc81120c917 1865 m_newTrueRange_mm = m_trueRange_mm;
gallonm 7:2dc81120c917 1866
gallonm 7:2dc81120c917 1867 XTalkCompRate_KCps = VL6180xDevDataGet(dev, XTalkCompRate_KCps );
gallonm 7:2dc81120c917 1868
gallonm 7:2dc81120c917 1869
gallonm 7:2dc81120c917 1870 //Update signal rate limits depending on crosstalk
gallonm 7:2dc81120c917 1871 SignalRateDMax = (uint32_t)_GetDMaxDataRetSignalAt400mm(dev) + XTalkCompRate_KCps;
gallonm 7:2dc81120c917 1872 WrapAroundLowReturnRateLimit = WrapAroundLowReturnRateLimit_ROM + XTalkCompRate_KCps;
gallonm 7:2dc81120c917 1873 WrapAroundLowReturnRateLimit2 = ((WrapAroundLowReturnRateLimit2_ROM * SignalRateDMax) / 312) + XTalkCompRate_KCps;
gallonm 7:2dc81120c917 1874 WrapAroundLowReturnRateFilterLimit = ((WrapAroundLowReturnRateFilterLimit_ROM * SignalRateDMax) / 312) + XTalkCompRate_KCps;
gallonm 7:2dc81120c917 1875 WrapAroundHighReturnRateFilterLimit = ((WrapAroundHighReturnRateFilterLimit_ROM * SignalRateDMax) / 312) + XTalkCompRate_KCps;
gallonm 7:2dc81120c917 1876
gallonm 7:2dc81120c917 1877
gallonm 7:2dc81120c917 1878 /* Checks on low range data */
gallonm 7:2dc81120c917 1879 if ((m_rawRange_mm < WrapAroundLowRawRangeLimit) && (m_rtnSignalRate < WrapAroundLowReturnRateLimit)) {
gallonm 7:2dc81120c917 1880 m_newTrueRange_mm = MaxOrInvalidDistance;
gallonm 7:2dc81120c917 1881 bypassFilter = 1;
gallonm 7:2dc81120c917 1882 }
gallonm 7:2dc81120c917 1883 if ((m_rawRange_mm < WrapAroundLowRawRangeLimit2) && (m_rtnSignalRate < WrapAroundLowReturnRateLimit2)) {
gallonm 7:2dc81120c917 1884 m_newTrueRange_mm = MaxOrInvalidDistance;
gallonm 7:2dc81120c917 1885 bypassFilter = 1;
gallonm 7:2dc81120c917 1886 }
gallonm 7:2dc81120c917 1887
gallonm 7:2dc81120c917 1888 /* Checks on Ambient rate level */
gallonm 7:2dc81120c917 1889 if (m_rtnAmbientRate > WrapAroundMaximumAmbientRateFilterLimit) {
gallonm 7:2dc81120c917 1890 /* Too high ambient rate */
gallonm 7:2dc81120c917 1891 FlushFilter = 1;
gallonm 7:2dc81120c917 1892 bypassFilter = 1;
gallonm 7:2dc81120c917 1893 }
gallonm 7:2dc81120c917 1894 /* Checks on Filter flush */
gallonm 7:2dc81120c917 1895 if (m_rtnSignalRate < MinReturnRateFilterFlush) {
gallonm 7:2dc81120c917 1896 /* Completely lost target, so flush the filter */
gallonm 7:2dc81120c917 1897 FlushFilter = 1;
gallonm 7:2dc81120c917 1898 bypassFilter = 1;
gallonm 7:2dc81120c917 1899 }
gallonm 7:2dc81120c917 1900 if (_FilterData(LastReturnRates)[0] != 0) {
gallonm 7:2dc81120c917 1901 if (m_rtnSignalRate > _FilterData(LastReturnRates)[0])
gallonm 7:2dc81120c917 1902 RateChange = (100 * (m_rtnSignalRate - _FilterData(LastReturnRates)[0])) / _FilterData(LastReturnRates)[0];
gallonm 7:2dc81120c917 1903 else
gallonm 7:2dc81120c917 1904 RateChange = (100 * (_FilterData(LastReturnRates)[0] - m_rtnSignalRate)) / _FilterData(LastReturnRates)[0];
gallonm 7:2dc81120c917 1905 } else
gallonm 7:2dc81120c917 1906 RateChange = 0;
gallonm 7:2dc81120c917 1907 if (RateChange > MaxReturnRateChangeFilterFlush) {
gallonm 7:2dc81120c917 1908 FlushFilter = 1;
gallonm 7:2dc81120c917 1909 }
gallonm 7:2dc81120c917 1910 /* TODO optimize filter using circular buffer */
gallonm 7:2dc81120c917 1911 if (FlushFilter == 1) {
gallonm 7:2dc81120c917 1912 _FilterData(MeasurementIndex) = 0;
gallonm 7:2dc81120c917 1913 for (i = 0; i < FILTER_NBOF_SAMPLES; i++) {
gallonm 7:2dc81120c917 1914 _FilterData(LastTrueRange)[i] = FILTER_INVALID_DISTANCE;
gallonm 7:2dc81120c917 1915 _FilterData(LastReturnRates)[i] = 0;
gallonm 7:2dc81120c917 1916 }
gallonm 7:2dc81120c917 1917 } else {
gallonm 7:2dc81120c917 1918 for (i = (uint16_t) (FILTER_NBOF_SAMPLES - 1); i > 0; i--) {
gallonm 7:2dc81120c917 1919 _FilterData(LastTrueRange)[i] = _FilterData(LastTrueRange)[i - 1];
gallonm 7:2dc81120c917 1920 _FilterData(LastReturnRates)[i] = _FilterData(LastReturnRates)[i - 1];
gallonm 7:2dc81120c917 1921 }
gallonm 7:2dc81120c917 1922 }
gallonm 7:2dc81120c917 1923 if (ValidDistance == 1)
gallonm 7:2dc81120c917 1924 _FilterData(LastTrueRange)[0] = m_trueRange_mm;
gallonm 7:2dc81120c917 1925 else
gallonm 7:2dc81120c917 1926 _FilterData(LastTrueRange)[0] = FILTER_INVALID_DISTANCE;
gallonm 7:2dc81120c917 1927 _FilterData(LastReturnRates)[0] = m_rtnSignalRate;
gallonm 7:2dc81120c917 1928
gallonm 7:2dc81120c917 1929 /* Check if we need to go through the filter or not */
gallonm 7:2dc81120c917 1930 if (!(((m_rawRange_mm < WrapAroundHighRawRangeFilterLimit) && (m_rtnSignalRate < WrapAroundLowReturnRateFilterLimit)) || ((m_rawRange_mm >= WrapAroundHighRawRangeFilterLimit) && (m_rtnSignalRate < WrapAroundHighReturnRateFilterLimit))))
gallonm 7:2dc81120c917 1931 bypassFilter = 1;
gallonm 7:2dc81120c917 1932
gallonm 7:2dc81120c917 1933 /* Check which kind of measurement has been made */
gallonm 7:2dc81120c917 1934 status = VL6180x_RdByte(dev, 0x01AC, &u8 );
gallonm 7:2dc81120c917 1935 if( status ){
gallonm 7:2dc81120c917 1936 VL6180x_ErrLog("0x01AC rd fail");
gallonm 7:2dc81120c917 1937 goto done_err;
gallonm 7:2dc81120c917 1938 }
gallonm 7:2dc81120c917 1939 registerValue =u8;
gallonm 7:2dc81120c917 1940
gallonm 7:2dc81120c917 1941 /* Read data for filtering */
gallonm 7:2dc81120c917 1942 status = VL6180x_RdByte(dev, 0x10C, &u8 ); /* read only 8 lsb bits */
gallonm 7:2dc81120c917 1943 if( status ){
gallonm 7:2dc81120c917 1944 VL6180x_ErrLog("0x010C rd fail");
gallonm 7:2dc81120c917 1945 goto done_err;
gallonm 7:2dc81120c917 1946 }
gallonm 7:2dc81120c917 1947 register32BitsValue1=u8;
gallonm 7:2dc81120c917 1948 status = VL6180x_RdByte(dev, 0x0110, &u8); /* read only 8 lsb bits */
gallonm 7:2dc81120c917 1949 if( status ){
gallonm 7:2dc81120c917 1950 VL6180x_ErrLog("0x0110 rd fail");
gallonm 7:2dc81120c917 1951 goto done_err;
gallonm 7:2dc81120c917 1952 }
gallonm 7:2dc81120c917 1953 register32BitsValue2 = u8;
gallonm 7:2dc81120c917 1954
gallonm 7:2dc81120c917 1955 if (registerValue == 0x3E) {
gallonm 7:2dc81120c917 1956 _FilterData(Default_ZeroVal) = register32BitsValue1;
gallonm 7:2dc81120c917 1957 _FilterData(Default_VAVGVal) = register32BitsValue2;
gallonm 7:2dc81120c917 1958 } else {
gallonm 7:2dc81120c917 1959 _FilterData(NoDelay_ZeroVal) = register32BitsValue1;
gallonm 7:2dc81120c917 1960 _FilterData(NoDelay_VAVGVal) = register32BitsValue2;
gallonm 7:2dc81120c917 1961 }
gallonm 7:2dc81120c917 1962
gallonm 7:2dc81120c917 1963 if (bypassFilter == 1) {
gallonm 7:2dc81120c917 1964 /* Do not go through the filter */
gallonm 7:2dc81120c917 1965 if (registerValue != 0x3E) {
gallonm 7:2dc81120c917 1966 status = VL6180x_WrByte(dev, 0x1AC, 0x3E);
gallonm 7:2dc81120c917 1967 if( status ){
gallonm 7:2dc81120c917 1968 VL6180x_ErrLog("0x01AC bypass wr fail");
gallonm 7:2dc81120c917 1969 goto done_err;
gallonm 7:2dc81120c917 1970 }
gallonm 7:2dc81120c917 1971 status = VL6180x_WrByte(dev, 0x0F2, 0x01);
gallonm 7:2dc81120c917 1972 if( status ){
gallonm 7:2dc81120c917 1973 VL6180x_ErrLog("0x0F2 bypass wr fail");
gallonm 7:2dc81120c917 1974 goto done_err;
gallonm 7:2dc81120c917 1975 }
gallonm 7:2dc81120c917 1976 }
gallonm 7:2dc81120c917 1977 /* Set both Default and NoDelay To same value */
gallonm 7:2dc81120c917 1978 _FilterData(Default_ZeroVal) = register32BitsValue1;
gallonm 7:2dc81120c917 1979 _FilterData(Default_VAVGVal) = register32BitsValue2;
gallonm 7:2dc81120c917 1980 _FilterData(NoDelay_ZeroVal) = register32BitsValue1;
gallonm 7:2dc81120c917 1981 _FilterData(NoDelay_VAVGVal) = register32BitsValue2;
gallonm 7:2dc81120c917 1982 _FilterData(MeasurementIndex) = 0;
gallonm 7:2dc81120c917 1983
gallonm 7:2dc81120c917 1984 return m_newTrueRange_mm;
gallonm 7:2dc81120c917 1985 }
gallonm 7:2dc81120c917 1986
gallonm 7:2dc81120c917 1987 if (_FilterData(MeasurementIndex) % WrapAroundNoDelayCheckPeriod == 0) {
gallonm 7:2dc81120c917 1988 u8=0x3C;
gallonm 7:2dc81120c917 1989 u8_2 = 0x05;
gallonm 7:2dc81120c917 1990 } else {
gallonm 7:2dc81120c917 1991 u8=0x3E;
gallonm 7:2dc81120c917 1992 u8_2 = 0x01;
gallonm 7:2dc81120c917 1993 }
gallonm 7:2dc81120c917 1994 status = VL6180x_WrByte(dev, 0x01AC, u8);
gallonm 7:2dc81120c917 1995 if( status ){
gallonm 7:2dc81120c917 1996 VL6180x_ErrLog("0x01AC wr fail");
gallonm 7:2dc81120c917 1997 goto done_err;
gallonm 7:2dc81120c917 1998 }
gallonm 7:2dc81120c917 1999 status = VL6180x_WrByte(dev, 0x0F2, u8_2);
gallonm 7:2dc81120c917 2000 if( status ){
gallonm 7:2dc81120c917 2001 VL6180x_ErrLog("0x0F2 wr fail");
gallonm 7:2dc81120c917 2002 goto done_err;
gallonm 7:2dc81120c917 2003 }
gallonm 7:2dc81120c917 2004
gallonm 7:2dc81120c917 2005
gallonm 7:2dc81120c917 2006 _FilterData(MeasurementIndex)++;
gallonm 7:2dc81120c917 2007
gallonm 7:2dc81120c917 2008 /* Computes current VAVGDiff */
gallonm 7:2dc81120c917 2009 if (_FilterData(Default_VAVGVal) > _FilterData(NoDelay_VAVGVal))
gallonm 7:2dc81120c917 2010 VAVGDiff = _FilterData(Default_VAVGVal) - _FilterData(NoDelay_VAVGVal);
gallonm 7:2dc81120c917 2011 else
gallonm 7:2dc81120c917 2012 VAVGDiff = 0;
gallonm 7:2dc81120c917 2013 _FilterData(Previous_VAVGDiff) = VAVGDiff;
gallonm 7:2dc81120c917 2014
gallonm 7:2dc81120c917 2015 /* Check the VAVGDiff */
gallonm 7:2dc81120c917 2016 if (_FilterData(Default_ZeroVal) > _FilterData(NoDelay_ZeroVal))
gallonm 7:2dc81120c917 2017 IdealVAVGDiff = _FilterData(Default_ZeroVal) - _FilterData(NoDelay_ZeroVal);
gallonm 7:2dc81120c917 2018 else
gallonm 7:2dc81120c917 2019 IdealVAVGDiff = _FilterData(NoDelay_ZeroVal) - _FilterData(Default_ZeroVal);
gallonm 7:2dc81120c917 2020 if (IdealVAVGDiff > MAX_VAVGDiff)
gallonm 7:2dc81120c917 2021 MinVAVGDiff = IdealVAVGDiff - MAX_VAVGDiff;
gallonm 7:2dc81120c917 2022 else
gallonm 7:2dc81120c917 2023 MinVAVGDiff = 0;
gallonm 7:2dc81120c917 2024 MaxVAVGDiff = IdealVAVGDiff + MAX_VAVGDiff;
gallonm 7:2dc81120c917 2025 if (VAVGDiff < MinVAVGDiff || VAVGDiff > MaxVAVGDiff) {
gallonm 7:2dc81120c917 2026 WrapAroundFlag = 1;
gallonm 7:2dc81120c917 2027 } else {
gallonm 7:2dc81120c917 2028 /* Go through filtering check */
gallonm 7:2dc81120c917 2029
gallonm 7:2dc81120c917 2030 /* StdDevLimit Damper on SNR */
gallonm 7:2dc81120c917 2031 StdDevLimit = _filter_StdDevDamper(m_rtnAmbientRate, m_rtnSignalRate, StdDevLimitLowLight, StdDevLimitLowLightSNR, StdDevLimitHighLight, StdDevLimitHighLightSNR);
gallonm 7:2dc81120c917 2032
gallonm 7:2dc81120c917 2033 /* Standard deviations computations */
gallonm 7:2dc81120c917 2034 StdDevSamples = 0;
gallonm 7:2dc81120c917 2035 StdDevDistanceSum = 0;
gallonm 7:2dc81120c917 2036 StdDevDistanceMean = 0;
gallonm 7:2dc81120c917 2037 StdDevDistance = 0;
gallonm 7:2dc81120c917 2038 StdDevRateSum = 0;
gallonm 7:2dc81120c917 2039 StdDevRateMean = 0;
gallonm 7:2dc81120c917 2040 StdDevRate = 0;
gallonm 7:2dc81120c917 2041 for (i = 0; (i < FILTER_NBOF_SAMPLES) && (StdDevSamples < FILTER_STDDEV_SAMPLES); i++) {
gallonm 7:2dc81120c917 2042 if (_FilterData(LastTrueRange)[i] != FILTER_INVALID_DISTANCE) {
gallonm 7:2dc81120c917 2043 StdDevSamples = (uint16_t) (StdDevSamples + 1);
gallonm 7:2dc81120c917 2044 StdDevDistanceSum = (uint32_t) (StdDevDistanceSum + _FilterData(LastTrueRange)[i]);
gallonm 7:2dc81120c917 2045 StdDevRateSum = (uint32_t) (StdDevRateSum + _FilterData(LastReturnRates)[i]);
gallonm 7:2dc81120c917 2046 }
gallonm 7:2dc81120c917 2047 }
gallonm 7:2dc81120c917 2048 if (StdDevSamples > 0) {
gallonm 7:2dc81120c917 2049 StdDevDistanceMean = (uint32_t) (StdDevDistanceSum / StdDevSamples);
gallonm 7:2dc81120c917 2050 StdDevRateMean = (uint32_t) (StdDevRateSum / StdDevSamples);
gallonm 7:2dc81120c917 2051 }
gallonm 7:2dc81120c917 2052 /* TODO optimize shorten Std dev in aisngle loop computation using sum of x2 - (sum of x)2 */
gallonm 7:2dc81120c917 2053 StdDevSamples = 0;
gallonm 7:2dc81120c917 2054 StdDevDistanceSum = 0;
gallonm 7:2dc81120c917 2055 StdDevRateSum = 0;
gallonm 7:2dc81120c917 2056 for (i = 0; (i < FILTER_NBOF_SAMPLES) && (StdDevSamples < FILTER_STDDEV_SAMPLES); i++) {
gallonm 7:2dc81120c917 2057 if (_FilterData(LastTrueRange)[i] != FILTER_INVALID_DISTANCE) {
gallonm 7:2dc81120c917 2058 StdDevSamples = (uint16_t) (StdDevSamples + 1);
gallonm 7:2dc81120c917 2059 StdDevDistanceSum = (uint32_t) (StdDevDistanceSum + (int) (_FilterData(LastTrueRange)[i] - StdDevDistanceMean) * (int) (_FilterData(LastTrueRange)[i] - StdDevDistanceMean));
gallonm 7:2dc81120c917 2060 StdDevRateSum = (uint32_t) (StdDevRateSum + (int) (_FilterData(LastReturnRates)[i] - StdDevRateMean) * (int) (_FilterData(LastReturnRates)[i] - StdDevRateMean));
gallonm 7:2dc81120c917 2061 }
gallonm 7:2dc81120c917 2062 }
gallonm 7:2dc81120c917 2063 if (StdDevSamples >= MIN_FILTER_STDDEV_SAMPLES) {
gallonm 7:2dc81120c917 2064 StdDevDistance = (uint16_t) (StdDevDistanceSum / StdDevSamples);
gallonm 7:2dc81120c917 2065 StdDevRate = (uint16_t) (StdDevRateSum / StdDevSamples);
gallonm 7:2dc81120c917 2066 } else {
gallonm 7:2dc81120c917 2067 StdDevDistance = 0;
gallonm 7:2dc81120c917 2068 StdDevRate = 0;
gallonm 7:2dc81120c917 2069 }
gallonm 7:2dc81120c917 2070
gallonm 7:2dc81120c917 2071 /* Check Return rate standard deviation */
gallonm 7:2dc81120c917 2072 if (StdDevRate < StdDevMovingTargetStdDevLimit) {
gallonm 7:2dc81120c917 2073 if (StdDevSamples < MIN_FILTER_VALID_STDDEV_SAMPLES) {
gallonm 7:2dc81120c917 2074 m_newTrueRange_mm = MaxOrInvalidDistance;
gallonm 7:2dc81120c917 2075 } else {
gallonm 7:2dc81120c917 2076 /* Check distance standard deviation */
gallonm 7:2dc81120c917 2077 if (StdDevRate < StdDevMovingTargetReturnRateLimit)
gallonm 7:2dc81120c917 2078 StdDevLimitWithTargetMove = StdDevLimit + (((StdDevMovingTargetStdDevForReturnRateLimit - StdDevLimit) * StdDevRate) / StdDevMovingTargetReturnRateLimit);
gallonm 7:2dc81120c917 2079 else
gallonm 7:2dc81120c917 2080 StdDevLimitWithTargetMove = StdDevMovingTargetStdDevForReturnRateLimit;
gallonm 7:2dc81120c917 2081
gallonm 7:2dc81120c917 2082 if ((StdDevDistance * StdDevHighConfidenceSNRLimit) < StdDevLimitWithTargetMove) {
gallonm 7:2dc81120c917 2083 NoWrapAroundHighConfidenceFlag = 1;
gallonm 7:2dc81120c917 2084 } else {
gallonm 7:2dc81120c917 2085 if (StdDevDistance < StdDevLimitWithTargetMove) {
gallonm 7:2dc81120c917 2086 if (StdDevSamples >= MIN_FILTER_VALID_STDDEV_SAMPLES) {
gallonm 7:2dc81120c917 2087 NoWrapAroundFlag = 1;
gallonm 7:2dc81120c917 2088 } else {
gallonm 7:2dc81120c917 2089 m_newTrueRange_mm = MaxOrInvalidDistance;
gallonm 7:2dc81120c917 2090 }
gallonm 7:2dc81120c917 2091 } else {
gallonm 7:2dc81120c917 2092 WrapAroundFlag = 1;
gallonm 7:2dc81120c917 2093 }
gallonm 7:2dc81120c917 2094 }
gallonm 7:2dc81120c917 2095 }
gallonm 7:2dc81120c917 2096 } else {
gallonm 7:2dc81120c917 2097 WrapAroundFlag = 1;
gallonm 7:2dc81120c917 2098 }
gallonm 7:2dc81120c917 2099 }
gallonm 7:2dc81120c917 2100
gallonm 7:2dc81120c917 2101 if (m_newTrueRange_mm == MaxOrInvalidDistance) {
gallonm 7:2dc81120c917 2102 if (_FilterData(StdFilteredReads) > 0)
gallonm 7:2dc81120c917 2103 _FilterData(StdFilteredReads) = (uint16_t) (_FilterData(StdFilteredReads) - 1);
gallonm 7:2dc81120c917 2104 } else {
gallonm 7:2dc81120c917 2105 if (WrapAroundFlag == 1) {
gallonm 7:2dc81120c917 2106 m_newTrueRange_mm = MaxOrInvalidDistance;
gallonm 7:2dc81120c917 2107 _FilterData(StdFilteredReads) = (uint16_t) (_FilterData(StdFilteredReads) + StdFilteredReadsIncrement);
gallonm 7:2dc81120c917 2108 if (_FilterData(StdFilteredReads) > StdMaxFilteredReads)
gallonm 7:2dc81120c917 2109 _FilterData(StdFilteredReads) = StdMaxFilteredReads;
gallonm 7:2dc81120c917 2110 } else {
gallonm 7:2dc81120c917 2111 if (NoWrapAroundFlag == 1) {
gallonm 7:2dc81120c917 2112 if (_FilterData(StdFilteredReads) > 0) {
gallonm 7:2dc81120c917 2113 m_newTrueRange_mm = MaxOrInvalidDistance;
gallonm 7:2dc81120c917 2114 if (_FilterData(StdFilteredReads) > StdFilteredReadsIncrement)
gallonm 7:2dc81120c917 2115 _FilterData(StdFilteredReads) = (uint16_t) (_FilterData(StdFilteredReads) - StdFilteredReadsIncrement);
gallonm 7:2dc81120c917 2116 else
gallonm 7:2dc81120c917 2117 _FilterData(StdFilteredReads) = 0;
gallonm 7:2dc81120c917 2118 }
gallonm 7:2dc81120c917 2119 } else {
gallonm 7:2dc81120c917 2120 if (NoWrapAroundHighConfidenceFlag == 1) {
gallonm 7:2dc81120c917 2121 _FilterData(StdFilteredReads) = 0;
gallonm 7:2dc81120c917 2122 }
gallonm 7:2dc81120c917 2123 }
gallonm 7:2dc81120c917 2124 }
gallonm 7:2dc81120c917 2125 }
gallonm 7:2dc81120c917 2126
gallonm 7:2dc81120c917 2127 return m_newTrueRange_mm;
gallonm 7:2dc81120c917 2128 done_err:
gallonm 7:2dc81120c917 2129 return -1;
gallonm 7:2dc81120c917 2130
gallonm 7:2dc81120c917 2131 #undef MaxOrInvalidDistance
gallonm 7:2dc81120c917 2132 }
gallonm 7:2dc81120c917 2133
gallonm 7:2dc81120c917 2134
gallonm 7:2dc81120c917 2135 int VL6180X::_filter_GetResult(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData) {
gallonm 7:2dc81120c917 2136 uint32_t m_rawRange_mm = 0;
gallonm 7:2dc81120c917 2137 int32_t FilteredRange;
gallonm 7:2dc81120c917 2138 const uint8_t scaler = _GetUpscale(dev);
gallonm 7:2dc81120c917 2139 uint8_t u8;
gallonm 7:2dc81120c917 2140 int status;
gallonm 7:2dc81120c917 2141
gallonm 7:2dc81120c917 2142 do {
gallonm 7:2dc81120c917 2143 status = VL6180x_RdByte(dev, RESULT_RANGE_RAW, &u8);
gallonm 7:2dc81120c917 2144 if (status) {
gallonm 7:2dc81120c917 2145 VL6180x_ErrLog("RESULT_RANGE_RAW rd fail");
gallonm 7:2dc81120c917 2146 break;
gallonm 7:2dc81120c917 2147 }
gallonm 7:2dc81120c917 2148 m_rawRange_mm = u8;
gallonm 7:2dc81120c917 2149
gallonm 7:2dc81120c917 2150 FilteredRange = _filter_Start(dev, pRangeData->range_mm, (m_rawRange_mm * scaler), pRangeData->rtnRate, pRangeData->rtnAmbRate, pRangeData->errorStatus);
gallonm 7:2dc81120c917 2151 if( FilteredRange<0 ){
gallonm 7:2dc81120c917 2152 status = -1;
gallonm 7:2dc81120c917 2153 break;
gallonm 7:2dc81120c917 2154 }
gallonm 7:2dc81120c917 2155 pRangeData->FilteredData.range_mm= FilteredRange;
gallonm 7:2dc81120c917 2156 pRangeData->FilteredData.rawRange_mm = m_rawRange_mm * scaler;
gallonm 7:2dc81120c917 2157 } while (0);
gallonm 7:2dc81120c917 2158 return status;
gallonm 7:2dc81120c917 2159 }
gallonm 7:2dc81120c917 2160
gallonm 7:2dc81120c917 2161 #undef _FilterData
gallonm 7:2dc81120c917 2162 #undef FILTER_STDDEV_SAMPLES
gallonm 7:2dc81120c917 2163 #undef MIN_FILTER_STDDEV_SAMPLES
gallonm 7:2dc81120c917 2164 #undef MIN_FILTER_VALID_STDDEV_SAMPLES
gallonm 7:2dc81120c917 2165 #undef FILTER_INVALID_DISTANCE
gallonm 7:2dc81120c917 2166
gallonm 7:2dc81120c917 2167 #endif /* VL6180x_WRAP_AROUND_FILTER_SUPPORT */
gallonm 7:2dc81120c917 2168
gallonm 7:2dc81120c917 2169 #ifdef VL6180x_HAVE_RATE_DATA
gallonm 7:2dc81120c917 2170
gallonm 7:2dc81120c917 2171 int VL6180X::_GetRateResult(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData) {
gallonm 7:2dc81120c917 2172 uint32_t m_rtnConvTime = 0;
gallonm 7:2dc81120c917 2173 uint32_t m_rtnSignalRate = 0;
gallonm 7:2dc81120c917 2174 uint32_t m_rtnAmbientRate = 0;
gallonm 7:2dc81120c917 2175 uint32_t m_rtnSignalCount = 0;
gallonm 7:2dc81120c917 2176 uint32_t m_rtnAmbientCount = 0;
gallonm 7:2dc81120c917 2177 uint32_t m_refConvTime = 0;
gallonm 7:2dc81120c917 2178 uint32_t cRtnSignalCountMax = 0x7FFFFFFF;
gallonm 7:2dc81120c917 2179 uint32_t cDllPeriods = 6;
gallonm 7:2dc81120c917 2180 uint32_t calcConvTime = 0;
gallonm 7:2dc81120c917 2181
gallonm 7:2dc81120c917 2182 int status;
gallonm 7:2dc81120c917 2183
gallonm 7:2dc81120c917 2184 do {
gallonm 7:2dc81120c917 2185
gallonm 7:2dc81120c917 2186 status = VL6180x_RdDWord(dev, RESULT_RANGE_RETURN_SIGNAL_COUNT, &m_rtnSignalCount);
gallonm 7:2dc81120c917 2187 if (status) {
gallonm 7:2dc81120c917 2188 VL6180x_ErrLog("RESULT_RANGE_RETURN_SIGNAL_COUNT rd fail");
gallonm 7:2dc81120c917 2189 break;
gallonm 7:2dc81120c917 2190 }
gallonm 7:2dc81120c917 2191 if (m_rtnSignalCount > cRtnSignalCountMax) {
gallonm 7:2dc81120c917 2192 m_rtnSignalCount = 0;
gallonm 7:2dc81120c917 2193 }
gallonm 7:2dc81120c917 2194
gallonm 7:2dc81120c917 2195 status = VL6180x_RdDWord(dev, RESULT_RANGE_RETURN_AMB_COUNT, &m_rtnAmbientCount);
gallonm 7:2dc81120c917 2196 if (status) {
gallonm 7:2dc81120c917 2197 VL6180x_ErrLog("RESULT_RANGE_RETURN_AMB_COUNTrd fail");
gallonm 7:2dc81120c917 2198 break;
gallonm 7:2dc81120c917 2199 }
gallonm 7:2dc81120c917 2200
gallonm 7:2dc81120c917 2201
gallonm 7:2dc81120c917 2202 status = VL6180x_RdDWord(dev, RESULT_RANGE_RETURN_CONV_TIME, &m_rtnConvTime);
gallonm 7:2dc81120c917 2203 if (status) {
gallonm 7:2dc81120c917 2204 VL6180x_ErrLog("RESULT_RANGE_RETURN_CONV_TIME rd fail");
gallonm 7:2dc81120c917 2205 break;
gallonm 7:2dc81120c917 2206 }
gallonm 7:2dc81120c917 2207
gallonm 7:2dc81120c917 2208 status = VL6180x_RdDWord(dev, RESULT_RANGE_REFERENCE_CONV_TIME, &m_refConvTime);
gallonm 7:2dc81120c917 2209 if (status) {
gallonm 7:2dc81120c917 2210 VL6180x_ErrLog("RESULT_RANGE_REFERENCE_CONV_TIME rd fail");
gallonm 7:2dc81120c917 2211 break;
gallonm 7:2dc81120c917 2212 }
gallonm 7:2dc81120c917 2213
gallonm 7:2dc81120c917 2214 pRangeData->rtnConvTime = m_rtnConvTime;
gallonm 7:2dc81120c917 2215 pRangeData->refConvTime = m_refConvTime;
gallonm 7:2dc81120c917 2216
gallonm 7:2dc81120c917 2217 calcConvTime = m_refConvTime;
gallonm 7:2dc81120c917 2218 if (m_rtnConvTime > m_refConvTime) {
gallonm 7:2dc81120c917 2219 calcConvTime = m_rtnConvTime;
gallonm 7:2dc81120c917 2220 }
gallonm 7:2dc81120c917 2221 if (calcConvTime == 0)
gallonm 7:2dc81120c917 2222 calcConvTime = 63000;
gallonm 7:2dc81120c917 2223
gallonm 7:2dc81120c917 2224 m_rtnSignalRate = (m_rtnSignalCount * 1000) / calcConvTime;
gallonm 7:2dc81120c917 2225 m_rtnAmbientRate = (m_rtnAmbientCount * cDllPeriods * 1000) / calcConvTime;
gallonm 7:2dc81120c917 2226
gallonm 7:2dc81120c917 2227 pRangeData->rtnRate = m_rtnSignalRate;
gallonm 7:2dc81120c917 2228 pRangeData->rtnAmbRate = m_rtnAmbientRate;
gallonm 7:2dc81120c917 2229
gallonm 7:2dc81120c917 2230
gallonm 7:2dc81120c917 2231 } while (0);
gallonm 7:2dc81120c917 2232 return status;
gallonm 7:2dc81120c917 2233 }
gallonm 7:2dc81120c917 2234 #endif /* VL6180x_HAVE_RATE_DATA */
gallonm 7:2dc81120c917 2235
gallonm 7:2dc81120c917 2236
gallonm 7:2dc81120c917 2237 int VL6180X::VL6180x_DMaxSetState(VL6180xDev_t dev, int state){
gallonm 7:2dc81120c917 2238 int status;
gallonm 7:2dc81120c917 2239 LOG_FUNCTION_START("%d", state);
gallonm 7:2dc81120c917 2240 #if VL6180x_HAVE_DMAX_RANGING
gallonm 7:2dc81120c917 2241 VL6180xDevDataSet(dev,DMaxEnable, state);
gallonm 7:2dc81120c917 2242 if( state ){
gallonm 7:2dc81120c917 2243 status = _DMax_InitData(dev);
gallonm 7:2dc81120c917 2244 }
gallonm 7:2dc81120c917 2245 else {
gallonm 7:2dc81120c917 2246 status = 0;
gallonm 7:2dc81120c917 2247 }
gallonm 7:2dc81120c917 2248 #else
gallonm 7:2dc81120c917 2249 status = NOT_SUPPORTED;
gallonm 7:2dc81120c917 2250 #endif
gallonm 7:2dc81120c917 2251 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 2252 return status;
gallonm 7:2dc81120c917 2253 }
gallonm 7:2dc81120c917 2254
gallonm 7:2dc81120c917 2255 int VL6180X::VL6180x_DMaxGetState(VL6180xDev_t dev){
gallonm 7:2dc81120c917 2256 int status;
gallonm 7:2dc81120c917 2257 LOG_FUNCTION_START("");
gallonm 7:2dc81120c917 2258 #if VL6180x_HAVE_DMAX_RANGING
gallonm 7:2dc81120c917 2259 status = VL6180xDevDataGet(dev,DMaxEnable);
gallonm 7:2dc81120c917 2260 #else
gallonm 7:2dc81120c917 2261 status = 0;
gallonm 7:2dc81120c917 2262 #endif
gallonm 7:2dc81120c917 2263 LOG_FUNCTION_END(status);
gallonm 7:2dc81120c917 2264 return status;
gallonm 7:2dc81120c917 2265 }
gallonm 7:2dc81120c917 2266
gallonm 7:2dc81120c917 2267
gallonm 7:2dc81120c917 2268 #if VL6180x_HAVE_DMAX_RANGING
gallonm 1:1de1ea2994d9 2269
gallonm 4:a5abf7757947 2270 #define _DMaxData(field) VL6180xDevDataGet(dev, DMaxData.field)
gallonm 7:2dc81120c917 2271 /*
gallonm 7:2dc81120c917 2272 * Convert fix point x.7 to KCpount per sec
gallonm 7:2dc81120c917 2273 */
gallonm 4:a5abf7757947 2274
gallonm 4:a5abf7757947 2275 #ifndef VL6180x_PLATFORM_PROVIDE_SQRT
gallonm 4:a5abf7757947 2276
gallonm 7:2dc81120c917 2277 /*
gallonm 7:2dc81120c917 2278 * 32 bit integer square root with not so bad precision (integer result) and is quite fast
gallonm 7:2dc81120c917 2279 * see http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
gallonm 7:2dc81120c917 2280 */
gallonm 7:2dc81120c917 2281 uint32_t VL6180x_SqrtUint32(uint32_t num) {
gallonm 4:a5abf7757947 2282 uint32_t res = 0;
gallonm 4:a5abf7757947 2283 uint32_t bit = 1 << 30; /* The second-to-top bit is set: 1 << 30 for 32 bits */
gallonm 4:a5abf7757947 2284
gallonm 4:a5abf7757947 2285 /* "bit" starts at the highest power of four <= the argument. */
gallonm 4:a5abf7757947 2286 while (bit > num)
gallonm 4:a5abf7757947 2287 bit >>= 2;
gallonm 1:1de1ea2994d9 2288
gallonm 4:a5abf7757947 2289 while (bit != 0) {
gallonm 4:a5abf7757947 2290 if (num >= res + bit) {
gallonm 4:a5abf7757947 2291 num -= res + bit;
gallonm 4:a5abf7757947 2292 res = (res >> 1) + bit;
gallonm 4:a5abf7757947 2293 }
gallonm 4:a5abf7757947 2294 else
gallonm 4:a5abf7757947 2295 res >>= 1;
gallonm 4:a5abf7757947 2296 bit >>= 2;
gallonm 1:1de1ea2994d9 2297 }
gallonm 4:a5abf7757947 2298 return res;
gallonm 4:a5abf7757947 2299 }
gallonm 4:a5abf7757947 2300 #endif
gallonm 4:a5abf7757947 2301
gallonm 4:a5abf7757947 2302
gallonm 7:2dc81120c917 2303 /* DMax one time init */
gallonm 7:2dc81120c917 2304 void _DMax_OneTimeInit(VL6180xDev_t dev){
gallonm 4:a5abf7757947 2305 _DMaxData(ambTuningWindowFactor_K)=DEF_AMBIENT_TUNING;
gallonm 1:1de1ea2994d9 2306 }
gallonm 1:1de1ea2994d9 2307
gallonm 1:1de1ea2994d9 2308
gallonm 7:2dc81120c917 2309 static uint32_t _DMax_RawValueAtRateKCps(VL6180xDev_t dev, int32_t rate){
gallonm 4:a5abf7757947 2310 uint32_t snrLimit_K;
gallonm 4:a5abf7757947 2311 int32_t DMaxSq;
gallonm 4:a5abf7757947 2312 uint32_t RawDMax;
gallonm 4:a5abf7757947 2313 DMaxFix_t retSignalAt400mm;
gallonm 4:a5abf7757947 2314 uint32_t ambTuningWindowFactor_K;
gallonm 3:454541a079f4 2315
gallonm 3:454541a079f4 2316
gallonm 4:a5abf7757947 2317 ambTuningWindowFactor_K = _DMaxData(ambTuningWindowFactor_K);
gallonm 4:a5abf7757947 2318 snrLimit_K = _DMaxData(snrLimit_K);
gallonm 4:a5abf7757947 2319 retSignalAt400mm = _DMaxData(retSignalAt400mm); /* 12 to 18 bits Kcps */
gallonm 4:a5abf7757947 2320 if( rate > 0 ){
gallonm 4:a5abf7757947 2321 DMaxSq = 400*400*1000 / rate -(400*400/330); /* K of (1/RtnAmb -1/330 )=> 30bit- (12-18)bit => 12-18 bits*/
gallonm 4:a5abf7757947 2322 if( DMaxSq<= 0){
gallonm 4:a5abf7757947 2323 RawDMax = 0;
gallonm 4:a5abf7757947 2324 }
gallonm 4:a5abf7757947 2325 else{
gallonm 4:a5abf7757947 2326 /* value can be more 32 bit so base on raneg apply *retSignalAt400mm before or after division to presevr accuracy */
gallonm 4:a5abf7757947 2327 if( DMaxSq< (2<<12) ){
gallonm 4:a5abf7757947 2328 DMaxSq = DMaxSq*retSignalAt400mm/(snrLimit_K+ambTuningWindowFactor_K); /* max 12 + 12 to 18 -10 => 12-26 bit */
gallonm 4:a5abf7757947 2329 }else{
gallonm 4:a5abf7757947 2330 DMaxSq = DMaxSq/(snrLimit_K+ambTuningWindowFactor_K)*retSignalAt400mm; /* 12 to 18 -10 + 12 to 18 *=> 12-26 bit */
gallonm 4:a5abf7757947 2331 }
gallonm 4:a5abf7757947 2332 RawDMax=VL6180x_SqrtUint32(DMaxSq);
gallonm 4:a5abf7757947 2333 }
gallonm 4:a5abf7757947 2334 }
gallonm 4:a5abf7757947 2335 else{
gallonm 4:a5abf7757947 2336 RawDMax = 0x7FFFFFFF; /* bigest possibmle 32bit signed value */
gallonm 4:a5abf7757947 2337 }
gallonm 4:a5abf7757947 2338 return RawDMax;
gallonm 4:a5abf7757947 2339 }
gallonm 3:454541a079f4 2340
gallonm 7:2dc81120c917 2341 /*
gallonm 7:2dc81120c917 2342 * fetch static data from register to avoid re-read
gallonm 7:2dc81120c917 2343 * precompute all intermediate constant and cliipings
gallonm 7:2dc81120c917 2344 *
gallonm 7:2dc81120c917 2345 * to be re-used/call on changes of :
gallonm 7:2dc81120c917 2346 * 0x2A
gallonm 7:2dc81120c917 2347 * SYSRANGE_MAX_AMBIENT_LEVEL_MULT
gallonm 7:2dc81120c917 2348 * Dev Data XtalkComRate_KCPs
gallonm 7:2dc81120c917 2349 * SYSRANGE_MAX_CONVERGENCE_TIME
gallonm 7:2dc81120c917 2350 * SYSRANGE_RANGE_CHECK_ENABLES mask RANGE_CHECK_RANGE_ENABLE_MASK
gallonm 7:2dc81120c917 2351 * range 0xb8-0xbb (0xbb)
gallonm 7:2dc81120c917 2352 */
gallonm 7:2dc81120c917 2353 int VL6180X::_DMax_InitData(VL6180xDev_t dev){
gallonm 3:454541a079f4 2354 int status, warning;
gallonm 3:454541a079f4 2355 uint8_t u8;
gallonm 3:454541a079f4 2356 uint16_t u16;
gallonm 3:454541a079f4 2357 uint32_t u32;
gallonm 3:454541a079f4 2358 uint32_t Reg2A_KCps;
gallonm 3:454541a079f4 2359 uint32_t RegB8;
gallonm 3:454541a079f4 2360 uint8_t MaxConvTime;
gallonm 3:454541a079f4 2361 uint32_t XTalkCompRate_KCps;
gallonm 3:454541a079f4 2362 uint32_t RangeIgnoreThreshold;
gallonm 3:454541a079f4 2363 int32_t minSignalNeeded;
gallonm 3:454541a079f4 2364 uint8_t SysRangeCheckEn;
gallonm 3:454541a079f4 2365 uint8_t snrLimit;
gallonm 3:454541a079f4 2366 warning=0;
gallonm 3:454541a079f4 2367
gallonm 3:454541a079f4 2368 static const int ROMABLE_DATA MaxConvTimeAdjust=-4;
gallonm 3:454541a079f4 2369
gallonm 3:454541a079f4 2370 LOG_FUNCTION_START("");
gallonm 3:454541a079f4 2371 do{
gallonm 7:2dc81120c917 2372 status = VL6180x_RdByte(dev, 0x02A ,&u8);
gallonm 3:454541a079f4 2373 if( status ){
gallonm 3:454541a079f4 2374 VL6180x_ErrLog("Reg 0x02A rd fail");
gallonm 3:454541a079f4 2375 break;
gallonm 3:454541a079f4 2376 }
gallonm 3:454541a079f4 2377
gallonm 3:454541a079f4 2378 if( u8 == 0 ) {
gallonm 3:454541a079f4 2379 warning = CALIBRATION_WARNING;
gallonm 3:454541a079f4 2380 u8 = 40; /* use a default average value */
gallonm 3:454541a079f4 2381 }
gallonm 3:454541a079f4 2382 Reg2A_KCps = Fix7_2_KCPs(u8); /* convert to KCPs */
gallonm 3:454541a079f4 2383
gallonm 7:2dc81120c917 2384 status = VL6180x_RdByte(dev, SYSRANGE_RANGE_CHECK_ENABLES, &SysRangeCheckEn);
gallonm 3:454541a079f4 2385 if (status) {
gallonm 3:454541a079f4 2386 VL6180x_ErrLog("SYSRANGE_RANGE_CHECK_ENABLES rd fail ");
gallonm 3:454541a079f4 2387 break;
gallonm 3:454541a079f4 2388 }
gallonm 3:454541a079f4 2389
gallonm 7:2dc81120c917 2390 status = VL6180x_RdByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, &MaxConvTime);
gallonm 3:454541a079f4 2391 if( status){
gallonm 3:454541a079f4 2392 VL6180x_ErrLog("SYSRANGE_MAX_CONVERGENCE_TIME rd fail ");
gallonm 3:454541a079f4 2393 break;
gallonm 3:454541a079f4 2394 }
gallonm 3:454541a079f4 2395
gallonm 7:2dc81120c917 2396 status = VL6180x_RdDWord(dev, 0x0B8, &RegB8);
gallonm 3:454541a079f4 2397 if( status ){
gallonm 3:454541a079f4 2398 VL6180x_ErrLog("reg 0x0B8 rd fail ");
gallonm 3:454541a079f4 2399 break;
gallonm 3:454541a079f4 2400 }
gallonm 3:454541a079f4 2401
gallonm 7:2dc81120c917 2402 status = VL6180x_RdByte(dev, SYSRANGE_MAX_AMBIENT_LEVEL_MULT, &snrLimit);
gallonm 3:454541a079f4 2403 if( status){
gallonm 3:454541a079f4 2404 VL6180x_ErrLog("SYSRANGE_MAX_AMBIENT_LEVEL_MULT rd fail ");
gallonm 3:454541a079f4 2405 break;
gallonm 3:454541a079f4 2406 }
gallonm 3:454541a079f4 2407 _DMaxData(snrLimit_K) = (int32_t)16*1000/snrLimit;
gallonm 3:454541a079f4 2408 XTalkCompRate_KCps = VL6180xDevDataGet(dev, XTalkCompRate_KCps );
gallonm 3:454541a079f4 2409
gallonm 3:454541a079f4 2410 if( Reg2A_KCps >= XTalkCompRate_KCps){
gallonm 3:454541a079f4 2411 _DMaxData(retSignalAt400mm)=( Reg2A_KCps - XTalkCompRate_KCps);
gallonm 3:454541a079f4 2412 }
gallonm 3:454541a079f4 2413 else{
gallonm 3:454541a079f4 2414 _DMaxData(retSignalAt400mm)=0; /* Reg2A_K - XTalkCompRate_KCp <0 is invalid */
gallonm 3:454541a079f4 2415 }
gallonm 3:454541a079f4 2416
gallonm 3:454541a079f4 2417 /* if xtalk range check is off omit it in snr clipping */
gallonm 3:454541a079f4 2418 if( SysRangeCheckEn&RANGE_CHECK_RANGE_ENABLE_MASK ){
gallonm 7:2dc81120c917 2419 status = VL6180x_RdWord(dev, SYSRANGE_RANGE_IGNORE_THRESHOLD, &u16);
gallonm 3:454541a079f4 2420 if( status){
gallonm 3:454541a079f4 2421 VL6180x_ErrLog("SYSRANGE_RANGE_IGNORE_THRESHOLD rd fail ");
gallonm 3:454541a079f4 2422 break;
gallonm 3:454541a079f4 2423 }
gallonm 3:454541a079f4 2424 RangeIgnoreThreshold = Fix7_2_KCPs(u16);
gallonm 3:454541a079f4 2425 }
gallonm 3:454541a079f4 2426 else{
gallonm 3:454541a079f4 2427 RangeIgnoreThreshold = 0;
gallonm 3:454541a079f4 2428 }
gallonm 3:454541a079f4 2429
gallonm 3:454541a079f4 2430 minSignalNeeded = (RegB8*256)/((int32_t)MaxConvTime+(int32_t)MaxConvTimeAdjust); /* KCps 8+8 bit -(1 to 6 bit) => 15-10 bit */
gallonm 3:454541a079f4 2431 /* minSignalNeeded = max ( minSignalNeeded, RangeIgnoreThreshold - XTalkCompRate_KCps) */
gallonm 3:454541a079f4 2432 if( minSignalNeeded <= RangeIgnoreThreshold - XTalkCompRate_KCps )
gallonm 3:454541a079f4 2433 minSignalNeeded = RangeIgnoreThreshold - XTalkCompRate_KCps;
gallonm 3:454541a079f4 2434
gallonm 3:454541a079f4 2435 u32 = (minSignalNeeded*(uint32_t)snrLimit)/16;
gallonm 3:454541a079f4 2436 _DMaxData(ClipSnrLimit ) = _DMax_RawValueAtRateKCps(dev, u32 ); /* clip to dmax to min signal snr limit rate*/
gallonm 3:454541a079f4 2437 }
gallonm 3:454541a079f4 2438 while(0);
gallonm 3:454541a079f4 2439 if( !status )
gallonm 3:454541a079f4 2440 status = warning;
gallonm 3:454541a079f4 2441 LOG_FUNCTION_END(status);
gallonm 3:454541a079f4 2442 return status;
gallonm 1:1de1ea2994d9 2443 }
gallonm 1:1de1ea2994d9 2444
gallonm 7:2dc81120c917 2445 static int _DMax_Compute(VL6180xDev_t dev, VL6180x_RangeData_t *pRange){
gallonm 7:2dc81120c917 2446 uint32_t rtnAmbRate;
gallonm 7:2dc81120c917 2447 int32_t DMax;
gallonm 7:2dc81120c917 2448 int scaling;
gallonm 7:2dc81120c917 2449 uint16_t HwLimitAtScale;
gallonm 7:2dc81120c917 2450 static const int ROMABLE_DATA rtnAmbLowLimit_KCps=330*1000;
gallonm 7:2dc81120c917 2451
gallonm 7:2dc81120c917 2452 rtnAmbRate = pRange->rtnAmbRate;
gallonm 7:2dc81120c917 2453 if( rtnAmbRate < rtnAmbLowLimit_KCps ){
gallonm 7:2dc81120c917 2454 DMax = _DMax_RawValueAtRateKCps( dev, rtnAmbRate);
gallonm 7:2dc81120c917 2455 scaling = _GetUpscale(dev);
gallonm 7:2dc81120c917 2456 HwLimitAtScale=UpperLimitLookUP[scaling - 1];
gallonm 7:2dc81120c917 2457
gallonm 7:2dc81120c917 2458 if( DMax > _DMaxData(ClipSnrLimit) ){
gallonm 7:2dc81120c917 2459 DMax=_DMaxData(ClipSnrLimit);
gallonm 7:2dc81120c917 2460 }
gallonm 7:2dc81120c917 2461 if( DMax > HwLimitAtScale ){
gallonm 7:2dc81120c917 2462 DMax=HwLimitAtScale;
gallonm 7:2dc81120c917 2463 }
gallonm 7:2dc81120c917 2464 pRange->DMax=DMax;
gallonm 7:2dc81120c917 2465 }
gallonm 7:2dc81120c917 2466 else{
gallonm 7:2dc81120c917 2467 pRange->DMax = 0;
gallonm 7:2dc81120c917 2468 }
gallonm 7:2dc81120c917 2469 return 0;
gallonm 7:2dc81120c917 2470 }
gallonm 7:2dc81120c917 2471
gallonm 7:2dc81120c917 2472 #undef _DMaxData
gallonm 3:454541a079f4 2473 #undef Fix7_2_KCPs
gallonm 1:1de1ea2994d9 2474
gallonm 7:2dc81120c917 2475 #endif /* VL6180x_HAVE_DMAX_RANGING */
gallonm 7:2dc81120c917 2476
gallonm 7:2dc81120c917 2477
gallonm 7:2dc81120c917 2478 /******************************************************************************/
gallonm 7:2dc81120c917 2479 /******************************************************************************/
gallonm 7:2dc81120c917 2480
gallonm 7:2dc81120c917 2481
gallonm 7:2dc81120c917 2482
gallonm 7:2dc81120c917 2483 /****************** Write and read functions from I2C *************************/
gallonm 4:a5abf7757947 2484
gallonm 4:a5abf7757947 2485 int VL6180X::VL6180x_WrByte(VL6180xDev_t dev, uint16_t index, uint8_t data)
gallonm 4:a5abf7757947 2486 {
gallonm 4:a5abf7757947 2487 int status;
licio.mapelli@st.com 8:f943a1fca15f 2488
licio.mapelli@st.com 8:f943a1fca15f 2489 status=VL6180x_I2CWrite(dev.I2cAddr, index, &data,(uint8_t)1);
gallonm 4:a5abf7757947 2490 return status;
gallonm 4:a5abf7757947 2491 }
gallonm 4:a5abf7757947 2492
gallonm 4:a5abf7757947 2493 int VL6180X::VL6180x_WrWord(VL6180xDev_t dev, uint16_t index, uint16_t data)
gallonm 4:a5abf7757947 2494 {
gallonm 4:a5abf7757947 2495 int status;
licio.mapelli@st.com 8:f943a1fca15f 2496 status=VL6180x_I2CWrite(dev.I2cAddr, index, (uint8_t *)&data,(uint8_t)2);
gallonm 4:a5abf7757947 2497 return status;
gallonm 4:a5abf7757947 2498 }
gallonm 4:a5abf7757947 2499
gallonm 4:a5abf7757947 2500 int VL6180X::VL6180x_WrDWord(VL6180xDev_t dev, uint16_t index, uint32_t data)
gallonm 4:a5abf7757947 2501 {
gallonm 4:a5abf7757947 2502 int status;
licio.mapelli@st.com 8:f943a1fca15f 2503 status=VL6180x_I2CWrite(dev.I2cAddr, index, (uint8_t *)&data,(uint8_t)4);
gallonm 4:a5abf7757947 2504 return status;
gallonm 4:a5abf7757947 2505 }
gallonm 4:a5abf7757947 2506
gallonm 4:a5abf7757947 2507 int VL6180X::VL6180x_RdByte(VL6180xDev_t dev, uint16_t index, uint8_t *data)
gallonm 4:a5abf7757947 2508 {
gallonm 4:a5abf7757947 2509 int status;
gallonm 4:a5abf7757947 2510 uint8_t buffer;
licio.mapelli@st.com 8:f943a1fca15f 2511 status=VL6180x_I2CRead(dev.I2cAddr, index, &buffer,1);
gallonm 4:a5abf7757947 2512 if( !status ){
gallonm 4:a5abf7757947 2513 *data=buffer;
gallonm 4:a5abf7757947 2514 }
gallonm 4:a5abf7757947 2515 return status;
gallonm 4:a5abf7757947 2516 }
gallonm 4:a5abf7757947 2517
gallonm 4:a5abf7757947 2518 int VL6180X::VL6180x_RdWord(VL6180xDev_t dev, uint16_t index, uint16_t *data)
gallonm 4:a5abf7757947 2519 {
gallonm 4:a5abf7757947 2520 int status;
licio.mapelli@st.com 8:f943a1fca15f 2521 uint8_t buffer[2];
licio.mapelli@st.com 8:f943a1fca15f 2522 status=VL6180x_I2CRead(dev.I2cAddr, index, buffer, 2);
gallonm 4:a5abf7757947 2523 return status;
gallonm 4:a5abf7757947 2524 }
gallonm 4:a5abf7757947 2525
gallonm 4:a5abf7757947 2526 int VL6180X::VL6180x_RdDWord(VL6180xDev_t dev, uint16_t index, uint32_t *data)
gallonm 4:a5abf7757947 2527 {
gallonm 4:a5abf7757947 2528 int status;
licio.mapelli@st.com 8:f943a1fca15f 2529 uint8_t buffer[4];
gallonm 4:a5abf7757947 2530
licio.mapelli@st.com 8:f943a1fca15f 2531 status=VL6180x_I2CRead(dev.I2cAddr, index, buffer,4);
gallonm 4:a5abf7757947 2532 return status;
gallonm 4:a5abf7757947 2533 }
gallonm 4:a5abf7757947 2534
licio.mapelli@st.com 8:f943a1fca15f 2535 int VL6180X::VL6180x_UpdateByte(VL6180xDev_t dev, uint16_t index, uint8_t AndData, uint8_t OrData)
licio.mapelli@st.com 8:f943a1fca15f 2536 {
gallonm 7:2dc81120c917 2537 int status;
licio.mapelli@st.com 8:f943a1fca15f 2538 uint8_t buffer;
licio.mapelli@st.com 8:f943a1fca15f 2539
licio.mapelli@st.com 8:f943a1fca15f 2540 status=VL6180x_I2CWrite(dev.I2cAddr, index, (uint8_t *)buffer,(uint8_t)0);
gallonm 7:2dc81120c917 2541 if( !status ){
gallonm 7:2dc81120c917 2542 /* read data direct onto buffer */
licio.mapelli@st.com 8:f943a1fca15f 2543 status=VL6180x_I2CRead(dev.I2cAddr, index, &buffer,1);
gallonm 7:2dc81120c917 2544 if( !status ){
licio.mapelli@st.com 8:f943a1fca15f 2545 buffer=(buffer & AndData)|OrData;
licio.mapelli@st.com 8:f943a1fca15f 2546 status=VL6180x_I2CWrite(dev.I2cAddr, index, &buffer, (uint8_t)1);
gallonm 7:2dc81120c917 2547 }
gallonm 7:2dc81120c917 2548 }
gallonm 7:2dc81120c917 2549
gallonm 7:2dc81120c917 2550 return status;
gallonm 7:2dc81120c917 2551 }
gallonm 7:2dc81120c917 2552
gallonm 4:a5abf7757947 2553 int VL6180X::VL6180x_I2CWrite(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToWrite)
gallonm 4:a5abf7757947 2554 {
gallonm 4:a5abf7757947 2555 int ret;
gallonm 4:a5abf7757947 2556 int i;
gallonm 4:a5abf7757947 2557 uint8_t tmp[TEMP_BUF_SIZE];
gallonm 4:a5abf7757947 2558 uint16_t myRegisterAddr = RegisterAddr;
gallonm 7:2dc81120c917 2559 uint16_t WriteDeviceAddr=0;
licio.mapelli@st.com 8:f943a1fca15f 2560 /* First, prepare 8 bits device address in 7bits i2ci format */
gallonm 7:2dc81120c917 2561 WriteDeviceAddr=DeviceAddr*2;
gallonm 4:a5abf7757947 2562 if(NumByteToWrite >= TEMP_BUF_SIZE) return -2;
gallonm 4:a5abf7757947 2563
licio.mapelli@st.com 8:f943a1fca15f 2564 /* then prepare 16 bits register address in BE format. Then, send data and STOP condition */
gallonm 4:a5abf7757947 2565 tmp[0] = *(((uint8_t*)&myRegisterAddr)+1);
gallonm 4:a5abf7757947 2566 tmp[1] = (uint8_t)RegisterAddr;
gallonm 4:a5abf7757947 2567
licio.mapelli@st.com 8:f943a1fca15f 2568 if(NumByteToWrite>1) /* swap data endianess */
gallonm 4:a5abf7757947 2569 {
gallonm 4:a5abf7757947 2570 for(i=0;i<NumByteToWrite;i++)
gallonm 4:a5abf7757947 2571 {
licio.mapelli@st.com 8:f943a1fca15f 2572 tmp[NumByteToWrite+sizeof(RegisterAddr)-i]=pBuffer[i];
gallonm 4:a5abf7757947 2573 }
gallonm 4:a5abf7757947 2574 }
gallonm 7:2dc81120c917 2575 else
gallonm 7:2dc81120c917 2576 {
licio.mapelli@st.com 8:f943a1fca15f 2577 memcpy(tmp+sizeof(RegisterAddr), pBuffer, NumByteToWrite);
gallonm 7:2dc81120c917 2578 }
licio.mapelli@st.com 9:9cbc1736f148 2579 ret = dev_i2c.write(WriteDeviceAddr, (const char*)tmp, NumByteToWrite+sizeof(RegisterAddr), false);
gallonm 4:a5abf7757947 2580
gallonm 4:a5abf7757947 2581 if(ret) return -1;
gallonm 4:a5abf7757947 2582 return 0;
gallonm 4:a5abf7757947 2583 }
gallonm 4:a5abf7757947 2584
gallonm 4:a5abf7757947 2585 int VL6180X::VL6180x_I2CRead(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToRead)
gallonm 4:a5abf7757947 2586 {
licio.mapelli@st.com 8:f943a1fca15f 2587 int ret,i;
licio.mapelli@st.com 8:f943a1fca15f 2588 uint8_t tmp[TEMP_BUF_SIZE];
gallonm 4:a5abf7757947 2589 uint16_t myRegisterAddr = RegisterAddr;
gallonm 4:a5abf7757947 2590 uint16_t myRegisterAddrBE;
gallonm 7:2dc81120c917 2591 uint16_t ReadDeviceAddr=DeviceAddr;
gallonm 7:2dc81120c917 2592
gallonm 7:2dc81120c917 2593 ReadDeviceAddr=DeviceAddr*2;
gallonm 4:a5abf7757947 2594 myRegisterAddrBE = *(((uint8_t*)&myRegisterAddr)+1);
gallonm 4:a5abf7757947 2595 *(((uint8_t*)&myRegisterAddrBE)+1) = (uint8_t)myRegisterAddr;
gallonm 4:a5abf7757947 2596
gallonm 4:a5abf7757947 2597 /* Send 8 bits device address and 16 bits register address in BE format, with no STOP condition */
gallonm 7:2dc81120c917 2598 ret = dev_i2c.write(ReadDeviceAddr, (const char*)&myRegisterAddrBE, sizeof(RegisterAddr), true);
gallonm 4:a5abf7757947 2599 if(!ret) {
gallonm 7:2dc81120c917 2600 ReadDeviceAddr|=0x001;
gallonm 4:a5abf7757947 2601 /* Read data, with STOP condition */
licio.mapelli@st.com 8:f943a1fca15f 2602 ret = dev_i2c.read(ReadDeviceAddr, (char*)tmp, NumByteToRead, false);
licio.mapelli@st.com 8:f943a1fca15f 2603 }
licio.mapelli@st.com 8:f943a1fca15f 2604 if(ret) return -1;
licio.mapelli@st.com 8:f943a1fca15f 2605
licio.mapelli@st.com 8:f943a1fca15f 2606 if(NumByteToRead>1) /* swap data endianess */
licio.mapelli@st.com 8:f943a1fca15f 2607 {
licio.mapelli@st.com 8:f943a1fca15f 2608 for(i=0;i<NumByteToRead;i++)
licio.mapelli@st.com 8:f943a1fca15f 2609 {
licio.mapelli@st.com 8:f943a1fca15f 2610 pBuffer[i] = tmp[NumByteToRead-1-i];
licio.mapelli@st.com 8:f943a1fca15f 2611 }
gallonm 4:a5abf7757947 2612 }
licio.mapelli@st.com 8:f943a1fca15f 2613 else
licio.mapelli@st.com 8:f943a1fca15f 2614 {
licio.mapelli@st.com 8:f943a1fca15f 2615 memcpy(pBuffer, tmp, NumByteToRead);
licio.mapelli@st.com 8:f943a1fca15f 2616 }
gallonm 4:a5abf7757947 2617 return 0;
gallonm 4:a5abf7757947 2618 }
gallonm 4:a5abf7757947 2619
gallonm 7:2dc81120c917 2620 int VL6180X::VL6180X_ReadID()
gallonm 4:a5abf7757947 2621 {
licio.mapelli@st.com 8:f943a1fca15f 2622 uint8_t rd_id;
licio.mapelli@st.com 8:f943a1fca15f 2623 VL6180x_RdByte( DevData, IDENTIFICATION_MODEL_ID, &rd_id);
licio.mapelli@st.com 8:f943a1fca15f 2624 return rd_id;
gallonm 4:a5abf7757947 2625 }
gallonm 4:a5abf7757947 2626
gallonm 4:a5abf7757947 2627
gallonm 7:2dc81120c917 2628
gallonm 1:1de1ea2994d9 2629 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/