Librairie adaptée au laboratoire 2

Dependencies:   ST_INTERFACES X_NUCLEO_COMMON

Fork of X_NUCLEO_6180XA1 by ST

Files at this revision

API Documentation at this revision

Comitter:
gallonm
Date:
Fri Sep 25 12:12:51 2015 +0200
Parent:
6:6a09fe77ad3c
Child:
8:f943a1fca15f
Commit message:
update files

Changed in this revision

Components/Display/Display_class.h Show annotated file Show diff for this revision Revisions of this file
Components/GPIO_expander/GPIO_expander_class.cpp Show diff for this revision Revisions of this file
Components/GPIO_expander/GPIO_expander_class.h Show diff for this revision Revisions of this file
Components/GPIO_expander/GpioDigital/GpioDigitalInOut.cpp Show diff for this revision Revisions of this file
Components/GPIO_expander/GpioDigital/GpioDigitalInOut.h Show diff for this revision Revisions of this file
Components/GPIO_expander/GpioDigital/GpioDigitalOut.cpp Show diff for this revision Revisions of this file
Components/GPIO_expander/GpioDigital/GpioDigitalOut.h Show diff for this revision Revisions of this file
Components/STMPE1600/STMPE1600.cpp Show diff for this revision Revisions of this file
Components/STMPE1600/STMPE1600.h Show diff for this revision Revisions of this file
Components/STMPE1600/stmpe1600_class.h Show annotated file Show diff for this revision Revisions of this file
Components/VL6180X/vl6180x_api.h Show annotated file Show diff for this revision Revisions of this file
Components/VL6180X/vl6180x_appcfg.h Show annotated file Show diff for this revision Revisions of this file
Components/VL6180X/vl6180x_class.cpp Show annotated file Show diff for this revision Revisions of this file
Components/VL6180X/vl6180x_class.h Show annotated file Show diff for this revision Revisions of this file
x_nucleo_6180xa1.h Show annotated file Show diff for this revision Revisions of this file
--- a/Components/Display/Display_class.h	Fri Sep 18 13:19:50 2015 +0000
+++ b/Components/Display/Display_class.h	Fri Sep 25 12:12:51 2015 +0200
@@ -0,0 +1,69 @@
+/**
+ ******************************************************************************
+ * @file    Display.h
+ * @author  AST / EST
+ * @version V0.0.1
+ * @date    14-April-2015
+ * @brief   Header file for display
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *   1. Redistributions of source code must retain the above copyright notice,
+ *      this list of conditions and the following disclaimer.
+ *   2. Redistributions in binary form must reproduce the above copyright notice,
+ *      this list of conditions and the following disclaimer in the documentation
+ *      and/or other materials provided with the distribution.
+ *   3. Neither the name of STMicroelectronics nor the names of its contributors
+ *      may be used to endorse or promote products derived from this software
+ *       without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+*/
+
+#ifndef __DISPLAY_H
+#define __DISPLAY_H
+
+/* Includes ------------------------------------------------------------------*/
+#include "mbed.h"
+
+
+/* Classes -------------------------------------------------------------------*/
+/** Class representing GPIO expander
+ */
+ 
+/*
+class Display : 
+{
+ public:
+
+    Display(DevI2C &i2c) : device_i2c(i2c);
+    
+    virtual ~Display() 
+    {
+        //nella classe board ci sara' un puntatore ad un oggetto di tipo Display che verra' creato con una new
+		    //qui devo fare il delete[] del new 
+    } 
+    
+    
+ private:
+    DevI2C &device_i2c;
+  
+};
+*/
+
+#endif // __GPIO_EXPANDER_CLASS_H
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Components/STMPE1600/stmpe1600_class.h	Fri Sep 25 12:12:51 2015 +0200
@@ -0,0 +1,162 @@
+/**
+ ******************************************************************************
+ * @file    vl6180x_class.h
+ * @author  AST / EST
+ * @version V0.0.1
+ * @date    14-April-2015
+ * @brief   Header file for component VL6180X
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *   1. Redistributions of source code must retain the above copyright notice,
+ *      this list of conditions and the following disclaimer.
+ *   2. Redistributions in binary form must reproduce the above copyright notice,
+ *      this list of conditions and the following disclaimer in the documentation
+ *      and/or other materials provided with the distribution.
+ *   3. Neither the name of STMicroelectronics nor the names of its contributors
+ *      may be used to endorse or promote products derived from this software
+ *       without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+*/
+#ifndef     __STMPE1600_CLASS
+#define     __STMPE1600_CLASS
+/* Includes ------------------------------------------------------------------*/
+#include    "mbed.h"
+#include		<assert.h>
+
+#define STMPE1600_DEF_DEVICE_ADDRESS  (uint8_t)0x42*2   // in 7 bits I2C format
+#define STMPE1600_DEF_DIGIOUT_LVL      0
+
+/**  STMPE1600 registr map **/
+#define VersionId			(uint8_t)0x02
+#define SystemControl (uint8_t)0x03
+#define IEGPIOR_0_7		(uint8_t)0x08
+#define IEGPIOR_8_15	(uint8_t)0x09
+#define ISGPIOR_0_7		(uint8_t)0x0A
+#define ISGPIOR_8_15	(uint8_t)0x0B
+#define GPMR_0_7			(uint8_t)0x10
+#define GPMR_8_15			(uint8_t)0x11
+#define GPSR_0_7			(uint8_t)0x12
+#define GPSR_8_15			(uint8_t)0x13
+#define GPDR_0_7			(uint8_t)0x14
+#define GPDR_8_15			(uint8_t)0x15
+#define GPIR_0_7			(uint8_t)0x16
+#define GPIR_8_15			(uint8_t)0x17
+
+  typedef enum {
+    // GPIO Expander pin names
+    GPIO_0,
+    GPIO_1,
+    GPIO_2,
+    GPIO_3,            
+    GPIO_4,
+    GPIO_5,
+    GPIO_6,
+    GPIO_7,            
+    GPIO_8,
+    GPIO_9,
+    GPIO_10,
+    GPIO_11,            
+    GPIO_12,
+    GPIO_13,
+    GPIO_14,
+    GPIO_15
+} ExpGpioPinName;   
+
+typedef enum {
+    INPUT,
+    OUTPUT,
+	  NOT_CONNECTED
+}ExpGpioPinDirection;
+
+
+class STMPE1600DigiOut {
+	
+ public: 
+    STMPE1600DigiOut (DevI2C &i2c, ExpGpioPinName outpinname, uint8_t DevAddr=STMPE1600_DEF_DEVICE_ADDRESS, bool lvl=STMPE1600_DEF_DIGIOUT_LVL): dev_i2c(i2c), expdevaddr(DevAddr), exppinname(outpinname) 
+		{
+			uint8_t data[2];				
+			/* set the exppinname as output */
+			dev_i2c.i2c_read(data, expdevaddr, GPDR_0_7, 1);
+			dev_i2c.i2c_read(&data[1], expdevaddr, GPDR_8_15, 1);			
+			*(uint16_t*)data = *(uint16_t*)data | (1<<(uint16_t)exppinname);  // set gopio as out 			
+			dev_i2c.i2c_write(data, expdevaddr, GPDR_0_7, 1);
+			dev_i2c.i2c_write(&data[1], expdevaddr, GPDR_8_15, 1);			
+			write(lvl);
+			
+		}         
+
+		void write (int lvl) {
+			uint8_t data[2];			
+			/* set the exppinname state to lvl */
+			dev_i2c.i2c_read(data, expdevaddr, GPSR_0_7, 1);
+			dev_i2c.i2c_read(&data[1], expdevaddr, GPSR_8_15, 1);			
+			*(uint16_t*)data = *(uint16_t*)data & (~(1<<(uint16_t)exppinname));  // set pin mask 			
+			if (lvl) *(uint16_t*)data = *(uint16_t*)data | ( 1<<(uint16_t)exppinname);
+			dev_i2c.i2c_write(data, expdevaddr, GPSR_0_7, 1);
+			dev_i2c.i2c_write(&data[1], expdevaddr, GPSR_8_15, 1);			
+		}
+		
+		STMPE1600DigiOut& operator=(int lvl) {
+			write (lvl);
+			return *this;
+		}		
+		
+ private:
+    DevI2C          &dev_i2c; 
+    uint8_t     		expdevaddr;
+    ExpGpioPinName 	exppinname; 	
+};
+
+class STMPE1600DigiIn {
+	
+ public: 
+    STMPE1600DigiIn (DevI2C &i2c, ExpGpioPinName inpinname, uint8_t DevAddr=STMPE1600_DEF_DEVICE_ADDRESS): dev_i2c(i2c), expdevaddr(DevAddr), exppinname(inpinname) 
+		{
+			uint8_t data[2];
+			/* set the exppinname as input pin direction */
+			dev_i2c.i2c_read(data, expdevaddr, GPDR_0_7, 1);
+			dev_i2c.i2c_read(&data[1], expdevaddr, GPDR_8_15, 1);			
+			*(uint16_t*)data = *(uint16_t*)data & (~(1<<(uint16_t)exppinname));  // set gopio as in			
+			dev_i2c.i2c_write(data, expdevaddr, GPDR_0_7, 1);
+			dev_i2c.i2c_write(&data[1], expdevaddr, GPDR_8_15, 1);						
+		}         
+
+		bool read () {
+			uint8_t data[2];
+			/* read the exppinname */
+			dev_i2c.i2c_read(data, expdevaddr, GPMR_0_7, 1);
+			dev_i2c.i2c_read(&data[1], expdevaddr, GPMR_8_15, 1);			
+			*(uint16_t*)data = *(uint16_t*)data & (1<<(uint16_t)exppinname);  // mask the in gpio
+			if (data[0] || data[1]) return 1;
+			return 0;
+		}
+		
+		operator int() {		
+			return read();
+		}		
+		
+ private:
+    DevI2C          &dev_i2c; 
+    uint8_t     		expdevaddr;
+    ExpGpioPinName 	exppinname;      
+};
+
+
+#endif // __STMPE1600_CLASS
--- a/Components/VL6180X/vl6180x_api.h	Fri Sep 18 13:19:50 2015 +0000
+++ b/Components/VL6180X/vl6180x_api.h	Fri Sep 25 12:12:51 2015 +0200
@@ -175,7 +175,7 @@
   * @param dev   The device
   * @return      0 on success
   */
- int VL6180x_Prepare(VL6180xDev_t dev);
+int VL6180x_Prepare(VL6180xDev_t dev);
  
  /** @}  */
  
--- a/Components/VL6180X/vl6180x_appcfg.h	Fri Sep 18 13:19:50 2015 +0000
+++ b/Components/VL6180X/vl6180x_appcfg.h	Fri Sep 25 12:12:51 2015 +0200
@@ -48,7 +48,7 @@
  * value 0 =>  multiple device capable user must review "device" structure and type in porting files
  * @ingroup Configuration
  */
-#define VL6180x_SINGLE_DEVICE_DRIVER 1
+#define VL6180x_SINGLE_DEVICE_DRIVER 1 // fix_me: da sistemare il valore di single_device_driver (0 o 1)
 
 
 /**
--- a/Components/VL6180X/vl6180x_class.cpp	Fri Sep 18 13:19:50 2015 +0000
+++ b/Components/VL6180X/vl6180x_class.cpp	Fri Sep 25 12:12:51 2015 +0200
@@ -35,76 +35,10 @@
  ******************************************************************************
 */
 
-/* Includes ------------------------------------------------------------------*/
+/* Includes */
 #include "vl6180x_class.h"
 
-/* define for inizialization -------------------------------------------------*/
-
-#if VL6180x_UPSCALE_SUPPORT == 1
-    #define _GetUpscale(dev, ... )  1
-    #define _SetUpscale(...) -1
-    #define DEF_UPSCALE 1
-#elif VL6180x_UPSCALE_SUPPORT == 2
-    #define _GetUpscale(dev, ... )  2
-    #define _SetUpscale(...)
-    #define DEF_UPSCALE 2
-#elif  VL6180x_UPSCALE_SUPPORT == 3
-    #define _GetUpscale(dev, ... )  3
-    #define _SetUpscale(...)
-    #define DEF_UPSCALE 3
-#else
-    #define DEF_UPSCALE (-(VL6180x_UPSCALE_SUPPORT))
-    #define _GetUpscale(dev, ... ) VL6180xDevDataGet(dev, UpscaleFactor)
-    #define _SetUpscale(dev, Scaling ) VL6180xDevDataSet(dev, UpscaleFactor, Scaling)
-#endif
-
-
-#if VL6180x_SINGLE_DEVICE_DRIVER
-extern  struct VL6180xDevData_t SingleVL6180xDevData;
-#define VL6180xDevDataGet(dev, field) (SingleVL6180xDevData.field)
-#define VL6180xDevDataSet(dev, field, data) SingleVL6180xDevData.field=(data)
-#endif
-
-
-#if VL6180x_UPSCALE_SUPPORT == 1
-    #define _GetUpscale(dev, ... )  1
-    #define _SetUpscale(...) -1
-    #define DEF_UPSCALE 1
-#elif VL6180x_UPSCALE_SUPPORT == 2
-    #define _GetUpscale(dev, ... )  2
-    #define _SetUpscale(...)
-    #define DEF_UPSCALE 2
-#elif  VL6180x_UPSCALE_SUPPORT == 3
-    #define _GetUpscale(dev, ... )  3
-    #define _SetUpscale(...)
-    #define DEF_UPSCALE 3
-#else
-    #define DEF_UPSCALE (-(VL6180x_UPSCALE_SUPPORT))
-    #define _GetUpscale(dev, ... ) VL6180xDevDataGet(dev, UpscaleFactor)
-    #define _SetUpscale(dev, Scaling ) VL6180xDevDataSet(dev, UpscaleFactor, Scaling)
-#endif
-
-
-struct VL6180xDevData_t VL6180x_DEV_DATA_ATTR  SingleVL6180xDevData={
-        .EceFactorM        = DEF_ECE_FACTOR_M,
-        .EceFactorD        = DEF_ECE_FACTOR_D,
-#ifdef VL6180x_HAVE_UPSCALE_DATA
-        .UpscaleFactor     = DEF_UPSCALE,
-#endif
-#ifdef VL6180x_HAVE_ALS_DATA
-        .IntegrationPeriod = DEF_INT_PEFRIOD,
-        .AlsGainCode       = DEF_ALS_GAIN,
-        .AlsScaler         = DEF_ALS_SCALER,
-#endif
-#ifdef VL6180x_HAVE_DMAX_RANGING
-        .DMaxEnable =   DEF_DMAX_ENABLE,
-#endif
-};
-
-
-#define Fix7_2_KCPs(x) ((((uint32_t)(x))*1000)>>7)
-
-/* define for i2c configuration ----------------------------------------------*/
+/****************** define for i2c configuration *******************************/
 
 #define I2C_BUFFER_CONFIG 1
 #define VL6180x_I2C_USER_VAR
@@ -134,10 +68,188 @@
 #error "invalid I2C_BUFFER_CONFIG "
 #endif
 
-/* Initialization functions --------------------------------------------------*/
-
-int VL6180X::VL6180x_InitData(VL6180xDev_t dev)
-{
+#define IsValidGPIOFunction(x) ((x)==GPIOx_SELECT_GPIO_INTERRUPT_OUTPUT || (x)==GPIOx_SELECT_OFF)
+
+
+
+/******************************************************************************/
+/******************************* file api.c ***********************************/
+
+#define VL6180x_9to7Conv(x) (x)
+
+/* TODO when set all "cached" value with "default init" are updated after init from register read back */
+#define REFRESH_CACHED_DATA_AFTER_INIT  1
+
+
+#define IsValidGPIOFunction(x) ((x)==GPIOx_SELECT_GPIO_INTERRUPT_OUTPUT || (x)==GPIOx_SELECT_OFF)
+
+
+/** default value ECE factor Molecular */
+#define DEF_ECE_FACTOR_M    85
+/** default value ECE factor Denominator */
+#define DEF_ECE_FACTOR_D    100
+/** default value ALS integration time */
+#define DEF_INT_PEFRIOD     100
+/** default value ALS gain */
+#define DEF_ALS_GAIN        1
+/** default value ALS scaler */
+#define DEF_ALS_SCALER      1
+/** default value for DMAX Enbale */
+#define DEF_DMAX_ENABLE     1
+/** default ambient tuning factor %x1000 */
+#define DEF_AMBIENT_TUNING  80
+
+#if VL6180x_SINGLE_DEVICE_DRIVER
+extern  struct VL6180xDevData_t SingleVL6180xDevData;
+#define VL6180xDevDataGet(dev, field) (SingleVL6180xDevData.field)
+#define VL6180xDevDataSet(dev, field, data) SingleVL6180xDevData.field=(data)
+#endif
+
+#define LUXRES_FIX_PREC 8
+#define GAIN_FIX_PREC    8  /* ! if not sme as LUX_PREC then :( adjust GetLux */
+#define AN_GAIN_MULT    (1<<GAIN_FIX_PREC)
+
+
+static int32_t _GetAveTotalTime(VL6180xDev_t dev);
+static int VL6180x_RangeSetEarlyConvergenceEestimateThreshold(VL6180xDev_t dev);
+
+/**
+ * ScalerLookUP scaling factor-1 to register #RANGE_SCALER lookup
+ */
+static const uint16_t ScalerLookUP[]      ROMABLE_DATA ={ 253, 127,  84}; /* lookup table for scaling->scalar 1x2x 3x */
+/**
+ * scaling factor to Upper limit look up
+ */
+static const uint16_t UpperLimitLookUP[]  ROMABLE_DATA ={ 185, 370,  580}; /* lookup table for scaling->limit  1x2x3x */
+/**
+ * Als Code gain to fix point gain lookup
+ */
+static const uint16_t AlsGainLookUp[8]    ROMABLE_DATA = {
+        (uint16_t)(20.0f * AN_GAIN_MULT),
+        (uint16_t)(10.0f * AN_GAIN_MULT),
+        (uint16_t)(5.0f  * AN_GAIN_MULT),
+        (uint16_t)(2.5f  * AN_GAIN_MULT),
+        (uint16_t)(1.67f * AN_GAIN_MULT),
+        (uint16_t)(1.25f * AN_GAIN_MULT),
+        (uint16_t)(1.0f  * AN_GAIN_MULT),
+        (uint16_t)(40.0f * AN_GAIN_MULT),
+};
+
+
+#if VL6180x_RANGE_STATUS_ERRSTRING
+const char * ROMABLE_DATA VL6180x_RangeStatusErrString[]={
+       "No Error",
+       "VCSEL Continuity Test",
+       "VCSEL Watchdog Test",
+       "VCSEL Watchdog",
+       "PLL1 Lock",
+       "PLL2 Lock",
+       "Early Convergence Estimate",
+       "Max Convergence",
+       "No Target Ignore",
+       "Not used 9",
+       "Not used 10",
+       "Max Signal To Noise Ratio",
+       "Raw Ranging Algo Underflow",
+       "Raw Ranging Algo Overflow",
+       "Ranging Algo Underflow",
+       "Ranging Algo Overflow",
+
+       "Filtered by post processing"
+};
+
+const char * VL6180x_RangeGetStatusErrString(uint8_t RangeErrCode){
+    if( RangeErrCode  > sizeof(VL6180x_RangeStatusErrString)/sizeof(VL6180x_RangeStatusErrString[0]) )
+        return NULL;
+    return  VL6180x_RangeStatusErrString[RangeErrCode];
+}
+#endif
+
+#if VL6180x_UPSCALE_SUPPORT == 1
+    #define _GetUpscale(dev, ... )  1
+    #define _SetUpscale(...) -1
+    #define DEF_UPSCALE 1
+#elif VL6180x_UPSCALE_SUPPORT == 2
+    #define _GetUpscale(dev, ... )  2
+    #define _SetUpscale(...)
+    #define DEF_UPSCALE 2
+#elif  VL6180x_UPSCALE_SUPPORT == 3
+    #define _GetUpscale(dev, ... )  3
+    #define _SetUpscale(...)
+    #define DEF_UPSCALE 3
+#else
+    #define DEF_UPSCALE (-(VL6180x_UPSCALE_SUPPORT))
+    #define _GetUpscale(dev, ... ) VL6180xDevDataGet(dev, UpscaleFactor)
+    #define _SetUpscale(dev, Scaling ) VL6180xDevDataSet(dev, UpscaleFactor, Scaling)
+#endif
+
+
+#if VL6180x_SINGLE_DEVICE_DRIVER
+/**
+ * the unique driver data  When single device driver is active
+ */
+struct VL6180xDevData_t VL6180x_DEV_DATA_ATTR  SingleVL6180xDevData={
+        .EceFactorM        = DEF_ECE_FACTOR_M,
+        .EceFactorD        = DEF_ECE_FACTOR_D,
+#ifdef VL6180x_HAVE_UPSCALE_DATA
+        .UpscaleFactor     = DEF_UPSCALE,
+#endif
+#ifdef VL6180x_HAVE_ALS_DATA
+        .IntegrationPeriod = DEF_INT_PEFRIOD,
+        .AlsGainCode       = DEF_ALS_GAIN,
+        .AlsScaler         = DEF_ALS_SCALER,
+#endif
+#ifdef VL6180x_HAVE_DMAX_RANGING
+        .DMaxEnable =   DEF_DMAX_ENABLE,
+#endif
+};
+#endif /* VL6180x_SINGLE_DEVICE_DRIVER */
+
+
+
+#define Fix7_2_KCPs(x) ((((uint32_t)(x))*1000)>>7)
+
+
+#if VL6180x_WRAP_AROUND_FILTER_SUPPORT || VL6180x_HAVE_DMAX_RANGING
+static int _GetRateResult(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData);
+#endif
+
+#if VL6180x_WRAP_AROUND_FILTER_SUPPORT
+static int _filter_Init(VL6180xDev_t dev);
+static int _filter_GetResult(VL6180xDev_t dev, VL6180x_RangeData_t *pData);
+    #define _IsWrapArroundActive(dev) VL6180xDevDataGet(dev,WrapAroundFilterActive)
+#else
+    #define _IsWrapArroundActive(dev) 0
+#endif
+
+
+#if VL6180x_HAVE_DMAX_RANGING
+   void _DMax_OneTimeInit(VL6180xDev_t dev);
+   static int _DMax_InitData(VL6180xDev_t dev);
+   static int _DMax_Compute(VL6180xDev_t dev, VL6180x_RangeData_t *pRange);
+    #define _IsDMaxActive(dev) VL6180xDevDataGet(dev,DMaxEnable)
+#else
+    #define _DMax_InitData(...) 0 /* success */
+    #define _DMax_OneTimeInit(...) (void)0
+    #define _IsDMaxActive(...) 0
+#endif
+
+static int VL6180x_RangeStaticInit(VL6180xDev_t dev);
+static int  VL6180x_UpscaleStaticInit(VL6180xDev_t dev);
+
+int VL6180X::VL6180x_WaitDeviceBooted(VL6180xDev_t dev){
+	uint8_t FreshOutReset;
+	int status;
+	LOG_FUNCTION_START("");
+	do{
+		status = VL6180x_RdByte(dev,SYSTEM_FRESH_OUT_OF_RESET, &FreshOutReset);
+	}
+	while( FreshOutReset!=1 && status==0);
+	LOG_FUNCTION_END(status);
+	return status;
+}
+
+int VL6180X::VL6180x_InitData(VL6180xDev_t dev){
     int status, dmax_status ;
     int8_t offset;
     uint8_t FreshOutReset;
@@ -169,14 +281,14 @@
     do{
 
         /* backup offset initial value from nvm these must be done prior any over call that use offset */
-        status = VL6180x_RdByte(MyDevice,SYSRANGE_PART_TO_PART_RANGE_OFFSET, (uint8_t*)&offset);
+        status = VL6180X::VL6180x_RdByte(dev,SYSRANGE_PART_TO_PART_RANGE_OFFSET, (uint8_t*)&offset);
         if( status ){
             VL6180x_ErrLog("SYSRANGE_PART_TO_PART_RANGE_OFFSET rd fail");
             break;
         }
         VL6180xDevDataSet(dev, Part2PartOffsetNVM, offset);
 
-        status=VL6180x_RdDWord(MyDevice, SYSRANGE_RANGE_IGNORE_THRESHOLD, &CalValue);
+        status=VL6180x_RdDWord( dev, SYSRANGE_RANGE_IGNORE_THRESHOLD, &CalValue);
         if( status ){
             VL6180x_ErrLog("Part2PartAmbNVM rd fail");
             break;
@@ -186,7 +298,7 @@
         }
         VL6180xDevDataSet(dev, Part2PartAmbNVM, CalValue);
 
-        status = VL6180x_RdWord(MyDevice, SYSRANGE_CROSSTALK_COMPENSATION_RATE ,&u16);
+        status = VL6180x_RdWord(dev, SYSRANGE_CROSSTALK_COMPENSATION_RATE ,&u16);
         if( status){
             VL6180x_ErrLog("SYSRANGE_CROSSTALK_COMPENSATION_RATE rd fail ");
             break;
@@ -201,7 +313,7 @@
         }
 
         /* Read or wait for fresh out of reset  */
-        status = VL6180x_RdByte(MyDevice,SYSTEM_FRESH_OUT_OF_RESET, &FreshOutReset);
+        status = VL6180x_RdByte(dev,SYSTEM_FRESH_OUT_OF_RESET, &FreshOutReset);
         if( status )  {
             VL6180x_ErrLog("SYSTEM_FRESH_OUT_OF_RESET rd fail");
             break;
@@ -216,13 +328,1941 @@
     return status;
 }
 
+int8_t VL6180X::VL6180x_GetOffsetCalibrationData(VL6180xDev_t dev)
+{
+    int8_t offset;
+    LOG_FUNCTION_START("");
+    offset = VL6180xDevDataGet(dev, Part2PartOffsetNVM);
+    LOG_FUNCTION_END( offset );
+    return offset;
+}
+
+void VL6180X::VL6180x_SetOffsetCalibrationData(VL6180xDev_t dev, int8_t offset)
+{
+    LOG_FUNCTION_START("%d", offset);
+    VL6180xDevDataSet(dev, Part2PartOffsetNVM, offset);
+    LOG_FUNCTION_END(0);
+}
+
+int VL6180X::VL6180x_SetXTalkCompensationRate(VL6180xDev_t dev, FixPoint97_t Rate)
+{
+    int status;
+    LOG_FUNCTION_START("%d", Rate);
+    status = VL6180x_WrWord(dev, SYSRANGE_CROSSTALK_COMPENSATION_RATE, Rate);
+    if( status ==0 ){
+        uint32_t XTalkCompRate_KCps;
+        XTalkCompRate_KCps = Fix7_2_KCPs(Rate);
+        VL6180xDevDataSet(dev, XTalkCompRate_KCps , XTalkCompRate_KCps );
+        /* update dmax whenever xtalk rate changes */
+        status = _DMax_InitData(dev);
+    }
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+int VL6180X::VL6180x_SetI2CAddress(VL6180xDev_t dev, uint8_t NewAddress){
+    int status;
+    LOG_FUNCTION_START("");
+
+    status = VL6180x_WrByte(dev, I2C_SLAVE_DEVICE_ADDRESS, NewAddress/2);
+    if( status ){
+        VL6180x_ErrLog("new i2c addr Wr fail");
+    }
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+uint16_t VL6180X::VL6180x_GetUpperLimit(VL6180xDev_t dev) {
+    uint16_t limit;
+    int scaling;
+
+    LOG_FUNCTION_START("");
+
+    scaling = _GetUpscale(dev);
+    /* FIXME we do assume here _GetUpscale is valid if  user call us prior to init we may overflow the LUT  mem area */
+    limit = UpperLimitLookUP[scaling - 1];
+
+    LOG_FUNCTION_END((int )limit);
+    return limit;
+}
+
+
+
+int VL6180X::VL6180x_StaticInit(VL6180xDev_t dev){
+    int status=0, init_status;
+    LOG_FUNCTION_START("");
+
+    /* TODO doc When using configurable scaling but using 1x as start condition
+     * load tunning upscale  or not ??? */
+    if( _GetUpscale(dev) == 1 && !(VL6180x_UPSCALE_SUPPORT<0))
+    	init_status=VL6180x_RangeStaticInit(dev);
+    else
+    	init_status=VL6180x_UpscaleStaticInit(dev);
+
+    if( init_status <0 ){
+    	VL6180x_ErrLog("StaticInit fail");
+    	goto error;
+    }
+    else if(init_status > 0){
+    	VL6180x_ErrLog("StaticInit warning");
+    }
+
+#if REFRESH_CACHED_DATA_AFTER_INIT
+    /* update cached value after tuning applied */
+    do{
+#ifdef  VL6180x_HAVE_ALS_DATA
+        uint8_t data;
+        status=  VL6180x_RdByte(dev, FW_ALS_RESULT_SCALER, &data);
+        if( status ) break;
+        VL6180xDevDataSet(dev, AlsScaler, data);
+
+        status=  VL6180x_RdByte(dev, SYSALS_ANALOGUE_GAIN, &data);
+        if( status ) break;
+        VL6180x_AlsSetAnalogueGain(dev, data);
+#endif
+    }
+    while(0);
+#endif /* REFRESH_CACHED_DATA_AFTER_INIT */
+    if( status < 0 ){
+        VL6180x_ErrLog("StaticInit fail");
+    }
+    if( !status && init_status){
+    	status = init_status;
+    }
+error:
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+
+int VL6180X::VL6180x_SetGroupParamHold(VL6180xDev_t dev, int Hold)
+{
+    int status;
+    uint8_t value;
+
+    LOG_FUNCTION_START("%d", Hold);
+    if( Hold )
+        value = 1;
+    else
+        value = 0;
+    status = VL6180x_WrByte(dev, SYSTEM_GROUPED_PARAMETER_HOLD, value);
+
+    LOG_FUNCTION_END(status);
+    return status;
+
+}
+
+int VL6180X::VL6180x_Prepare(VL6180xDev_t dev)
+{
+    int status;
+    LOG_FUNCTION_START("");
+
+    do{
+        status=VL6180x_StaticInit(dev);
+        if( status<0) break;
+
+        /* set range InterruptMode to new sample */
+        status=VL6180x_RangeConfigInterrupt(dev, CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY );
+        if( status)
+            break;
+
+        /* set default threshold */
+        status=VL6180x_RangeSetRawThresholds(dev, 10, 200);
+        if( status ){
+            VL6180x_ErrLog("VL6180x_RangeSetRawThresholds fail");
+            break;
+        }
+#if VL6180x_ALS_SUPPORT
+        status =VL6180x_AlsSetIntegrationPeriod(dev, 100);
+        if( status ) break;
+        status = VL6180x_AlsSetInterMeasurementPeriod(dev,  200);
+        if( status ) break;
+        status = VL6180x_AlsSetAnalogueGain(dev,  0);
+        if( status ) break;
+        status = VL6180x_AlsSetThresholds(dev, 0, 0xFF);
+        if( status ) break;
+        /* set Als InterruptMode to new sample */
+        status=VL6180x_AlsConfigInterrupt(dev, CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY);
+        if( status ) {
+            VL6180x_ErrLog("VL6180x_AlsConfigInterrupt fail");
+            break;
+        }
+#endif
+#if VL6180x_WRAP_AROUND_FILTER_SUPPORT
+        _filter_Init(dev);
+#endif
+        /* make sure to reset any left previous condition that can hangs first poll */
+        status=VL6180x_ClearAllInterrupt(dev);
+    }
+    while(0);
+    LOG_FUNCTION_END(status);
+
+    return status;
+}
+
+#if VL6180x_ALS_SUPPORT
+int VL6180X::VL6180x_AlsGetLux(VL6180xDev_t dev, lux_t *pLux)
+{
+    int status;
+    uint16_t RawAls;
+    uint32_t luxValue = 0;
+    uint32_t IntPeriod;
+    uint32_t AlsAnGain;
+    uint32_t GainFix;
+    uint32_t AlsScaler;
+
+#if LUXRES_FIX_PREC !=  GAIN_FIX_PREC
+#error "LUXRES_FIX_PREC != GAIN_FIX_PREC  review these code to be correct"
+#endif
+    const uint32_t LuxResxIntIme =(uint32_t)(0.56f* DEF_INT_PEFRIOD *(1<<LUXRES_FIX_PREC));
+
+    LOG_FUNCTION_START("%p", pLux);
+
+    status = VL6180x_RdWord( dev, RESULT_ALS_VAL, &RawAls);
+    if( !status){
+        /* wer are yet here at no fix point */
+        IntPeriod=VL6180xDevDataGet(dev, IntegrationPeriod);
+        AlsScaler=VL6180xDevDataGet(dev, AlsScaler);
+        IntPeriod++; /* what stored is real time  ms -1 and it can be 0 for or 0 or 1ms */
+        luxValue = (uint32_t)RawAls * LuxResxIntIme; /* max # 16+8bits + 6bit (0.56*100)  */
+        luxValue /= IntPeriod;                         /* max # 16+8bits + 6bit 16+8+1 to 9 bit */
+        /* between  29 - 21 bit */
+        AlsAnGain = VL6180xDevDataGet(dev, AlsGainCode);
+        GainFix = AlsGainLookUp[AlsAnGain];
+        luxValue = luxValue / (AlsScaler * GainFix);
+        *pLux=luxValue;
+    }
+
+    LOG_FUNCTION_END_FMT(status, "%x",(int)*pLux);
+    return status;
+}
+
+int VL6180X::VL6180x_AlsGetMeasurement(VL6180xDev_t dev, VL6180x_AlsData_t *pAlsData)
+{
+    int status;
+    uint8_t ErrStatus;
+
+    LOG_FUNCTION_START("%p", pAlsData);
+
+    status = VL6180x_AlsGetLux(dev, &pAlsData->lux);
+    if( !status ){
+        status = VL6180x_RdByte(dev, RESULT_ALS_STATUS, & ErrStatus);
+        pAlsData->errorStatus = ErrStatus>>4;
+    }
+    LOG_FUNCTION_END_FMT(status,"%d %d", (int)pAlsData->lux,  (int)pAlsData->errorStatus);
+
+    return status;
+}
+
+
+int VL6180X::VL6180x_AlsPollMeasurement(VL6180xDev_t dev, VL6180x_AlsData_t *pAlsData) {
+    int status;
+    int ClrStatus;
+    uint8_t IntStatus;
+
+    LOG_FUNCTION_START("%p", pAlsData);
+#if VL6180X_SAFE_POLLING_ENTER
+    /* if device get stopped with left interrupt uncleared , it is required to clear them now or poll for new condition will never occur*/
+    status=VL6180x_AlsClearInterrupt(dev);
+    if(status){
+        VL6180x_ErrLog("VL6180x_AlsClearInterrupt fail");
+        goto over;
+    }
+#endif
+
+    status=VL6180x_AlsSetSystemMode(dev, MODE_START_STOP|MODE_SINGLESHOT);
+    if( status){
+        VL6180x_ErrLog("VL6180x_AlsSetSystemMode fail");
+        goto over;
+    }
+
+    /* poll for new sample ready */
+    while (1 ) {
+        status = VL6180x_AlsGetInterruptStatus(dev, &IntStatus);
+        if (status) {
+            break;
+        }
+        if (IntStatus == RES_INT_STAT_GPIO_NEW_SAMPLE_READY) {
+            break; /* break on new data (status is 0)  */
+        }
+
+        VL6180x_PollDelay(dev);
+    };
+
+    if (!status) {
+        status = VL6180x_AlsGetMeasurement(dev, pAlsData);
+    }
+
+    ClrStatus = VL6180x_AlsClearInterrupt(dev);
+    if (ClrStatus) {
+        VL6180x_ErrLog("VL6180x_AlsClearInterrupt fail");
+        if (!status) {
+            status = ClrStatus; /* leave previous if already on error */
+        }
+    }
+over:
+    LOG_FUNCTION_END(status);
+
+    return status;
+}
+
+int VL6180X::VL6180x_AlsGetInterruptStatus(VL6180xDev_t dev, uint8_t *pIntStatus) {
+    int status;
+    uint8_t IntStatus;
+    LOG_FUNCTION_START("%p", pIntStatus);
+
+    status = VL6180x_RdByte(dev, RESULT_INTERRUPT_STATUS_GPIO, &IntStatus);
+    *pIntStatus= (IntStatus>>3)&0x07;
+
+    LOG_FUNCTION_END_FMT(status, "%d", (int)*pIntStatus);
+    return status;
+}
+
+int VL6180X::VL6180x_AlsWaitDeviceReady(VL6180xDev_t dev, int MaxLoop ){
+    int status;
+    int  n;
+    uint8_t u8;
+    LOG_FUNCTION_START("%d", (int)MaxLoop);
+    if( MaxLoop<1){
+          status=INVALID_PARAMS;
+     }
+    else{
+        for( n=0; n < MaxLoop ; n++){
+            status=VL6180x_RdByte(dev, RESULT_ALS_STATUS, &u8);
+            if( status)
+                break;
+            u8 = u8 & ALS_DEVICE_READY_MASK;
+            if( u8 )
+                break;
+
+        }
+        if( !status && !u8 ){
+            status = TIME_OUT;
+        }
+    }
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+int VL6180X::VL6180x_AlsSetSystemMode(VL6180xDev_t dev, uint8_t mode)
+{
+    int status;
+    LOG_FUNCTION_START("%d", (int)mode);
+    /* FIXME if we are called back to back real fast we are not checking
+     * if previous mode "set" got absorbed => bit 0 must be 0 so that wr 1 work */
+    if( mode <= 3){
+        status=VL6180x_WrByte(dev, SYSALS_START, mode);
+    }
+    else{
+        status = INVALID_PARAMS;
+    }
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+int VL6180X::VL6180x_AlsConfigInterrupt(VL6180xDev_t dev, uint8_t ConfigGpioInt)
+{
+    int status;
+
+    if( ConfigGpioInt<= CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY){
+        status = VL6180x_UpdateByte(dev, SYSTEM_INTERRUPT_CONFIG_GPIO, (uint8_t)(~CONFIG_GPIO_ALS_MASK), (ConfigGpioInt<<3));
+    }
+    else{
+        VL6180x_ErrLog("Invalid config mode param %d", (int)ConfigGpioInt);
+        status = INVALID_PARAMS;
+    }
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+
+
+int VL6180X::VL6180x_AlsSetThresholds(VL6180xDev_t dev, uint8_t low, uint8_t high) {
+    int status;
+
+    LOG_FUNCTION_START("%d %d", (int )low, (int)high);
+
+    status = VL6180x_WrByte(dev, SYSALS_THRESH_LOW, low);
+    if(!status ){
+        status = VL6180x_WrByte(dev, SYSALS_THRESH_HIGH, high);
+    }
+
+    LOG_FUNCTION_END(status) ;
+    return status;
+}
+
+
+int VL6180X::VL6180x_AlsSetAnalogueGain(VL6180xDev_t dev, uint8_t gain) {
+    int status;
+    uint8_t GainTotal;
+
+    LOG_FUNCTION_START("%d", (int )gain);
+    gain&=~0x40;
+    if (gain > 7) {
+        gain = 7;
+    }
+    GainTotal = gain|0x40;
+
+    status = VL6180x_WrByte(dev, SYSALS_ANALOGUE_GAIN, GainTotal);
+    if( !status){
+        VL6180xDevDataSet(dev, AlsGainCode, gain);
+    }
+
+    LOG_FUNCTION_END_FMT(status, "%d %d", (int ) gain, (int )GainTotal);
+    return status;
+}
+
+int VL6180X::VL6180x_AlsSetInterMeasurementPeriod(VL6180xDev_t dev,  uint16_t intermeasurement_period_ms)
+{
+    int status;
+
+    LOG_FUNCTION_START("%d",(int)intermeasurement_period_ms);
+        /* clipping: range is 0-2550ms */
+        if (intermeasurement_period_ms >= 255 *10)
+                intermeasurement_period_ms = 255 *10;
+    status=VL6180x_WrByte(dev, SYSALS_INTERMEASUREMENT_PERIOD, (uint8_t)(intermeasurement_period_ms/10));
+
+    LOG_FUNCTION_END_FMT(status, "%d", (int) intermeasurement_period_ms);
+    return status;
+}
+
+
+int VL6180X::VL6180x_AlsSetIntegrationPeriod(VL6180xDev_t dev, uint16_t period_ms)
+{
+    int status;
+    uint16_t SetIntegrationPeriod;
+
+    LOG_FUNCTION_START("%d", (int)period_ms);
+
+    if( period_ms>=1 )
+        SetIntegrationPeriod = period_ms - 1;
+    else
+        SetIntegrationPeriod = period_ms;
+
+    if (SetIntegrationPeriod > 464) {
+        SetIntegrationPeriod = 464;
+    }
+    else if (SetIntegrationPeriod == 255)   {
+        SetIntegrationPeriod++; /* can't write 255 since this causes the device to lock out.*/
+    }
+
+    status =VL6180x_WrWord(dev, SYSALS_INTEGRATION_PERIOD, SetIntegrationPeriod);
+    if( !status ){
+        VL6180xDevDataSet(dev, IntegrationPeriod, SetIntegrationPeriod) ;
+    }
+    LOG_FUNCTION_END_FMT(status, "%d", (int)SetIntegrationPeriod);
+    return status;
+}
+
+#endif /* HAVE_ALS_SUPPORT */
+
+
+int VL6180X::VL6180x_RangePollMeasurement(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData)
+{
+    int status;
+    int ClrStatus;
+    IntrStatus_t IntStatus;
+
+    LOG_FUNCTION_START("");
+    /* start single range measurement */
+
+
+#if VL6180X_SAFE_POLLING_ENTER
+    /* if device get stopped with left interrupt uncleared , it is required to clear them now or poll for new condition will never occur*/
+    status=VL6180x_RangeClearInterrupt(dev);
+    if(status){
+        VL6180x_ErrLog("VL6180x_RangeClearInterrupt fail");
+        goto done;
+    }
+#endif
+    /* //![single_shot_snipet] */
+    status=VL6180x_RangeSetSystemMode(dev, MODE_START_STOP|MODE_SINGLESHOT);
+    if( status ){
+        VL6180x_ErrLog("VL6180x_RangeSetSystemMode fail");
+        goto done;
+    }
+
+
+    /* poll for new sample ready */
+    while(1 ){
+        status=VL6180x_RangeGetInterruptStatus(dev, &IntStatus.val);
+        if( status ){
+            break;
+        }
+        if( IntStatus.status.Error !=0 ){
+          VL6180x_ErrLog("GPIO int Error report %d",(int)IntStatus.val);
+          status = RANGE_ERROR;
+          break;
+        }
+        else
+        if( IntStatus.status.Range == RES_INT_STAT_GPIO_NEW_SAMPLE_READY){
+            break;
+        }
+
+        VL6180x_PollDelay(dev);
+    }
+ /* //![single_shot_snipet] */
+
+    if ( !status ){
+        status = VL6180x_RangeGetMeasurement(dev, pRangeData);
+    }
+
+    /*  clear range interrupt source */
+    ClrStatus = VL6180x_RangeClearInterrupt(dev);
+    if( ClrStatus ){
+        VL6180x_ErrLog("VL6180x_RangeClearInterrupt fail");
+        /*  leave initial status if already in error  */
+        if( !status ){
+            status=ClrStatus;
+        }
+    }
+done:
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+
+
+int VL6180X::VL6180x_RangeGetMeasurement(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData)
+{
+    int status;
+    uint16_t RawRate;
+    uint8_t RawStatus;
+
+    LOG_FUNCTION_START("");
+
+    status = VL6180x_RangeGetResult(dev, &pRangeData->range_mm);
+    if( !status ){
+        status = VL6180x_RdWord(dev,RESULT_RANGE_SIGNAL_RATE, &RawRate );
+        if( !status ){
+            pRangeData->signalRate_mcps = VL6180x_9to7Conv(RawRate);
+            status = VL6180x_RdByte(dev, RESULT_RANGE_STATUS, &RawStatus);
+            if( !status ){
+                pRangeData->errorStatus = RawStatus >>4;
+            }
+            else{
+                VL6180x_ErrLog("Rd RESULT_RANGE_STATUS fail");
+            }
+	#if VL6180x_WRAP_AROUND_FILTER_SUPPORT || VL6180x_HAVE_DMAX_RANGING
+            status = _GetRateResult(dev, pRangeData);
+            if( status )
+            	goto error;
+	#endif
+    #if VL6180x_WRAP_AROUND_FILTER_SUPPORT
+            /* if enabled run filter */
+            if( _IsWrapArroundActive(dev) ){
+                status=_filter_GetResult(dev, pRangeData);
+                if( !status){
+                    /* patch the range status and measure if it is filtered */
+                    if( pRangeData->range_mm != pRangeData->FilteredData.range_mm) {
+                        pRangeData->errorStatus=RangingFiltered;
+                        pRangeData->range_mm = pRangeData->FilteredData.range_mm;
+                    }
+                }
+            }
+    #endif
+
+#if VL6180x_HAVE_DMAX_RANGING
+            if(_IsDMaxActive(dev) ){
+                _DMax_Compute(dev, pRangeData);
+            }
+#endif
+        }
+        else{
+            VL6180x_ErrLog("Rd RESULT_RANGE_SIGNAL_RATE fail");
+        }
+    }
+    else{
+        VL6180x_ErrLog("VL6180x_GetRangeResult fail");
+    }
+error:
+    LOG_FUNCTION_END_FMT(status, "%d %d %d", (int)pRangeData->range_mm, (int)pRangeData->signalRate_mcps,  (int)pRangeData->errorStatus) ;
+    return status;
+}
+
+
+int VL6180X::VL6180x_RangeGetMeasurementIfReady(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData)
+{
+    int status;
+    IntrStatus_t IntStatus;
+
+    LOG_FUNCTION_START();
+
+    status = VL6180x_RangeGetInterruptStatus(dev, &IntStatus.val);
+    if( status ==0 ){
+        if( IntStatus.status.Error !=0 ){
+            VL6180x_ErrLog("GPIO int Error report %d",(int)IntStatus.val);
+            status = RANGE_ERROR;
+        }
+        else
+        if( IntStatus.status.Range == RES_INT_STAT_GPIO_NEW_SAMPLE_READY){
+           status = VL6180x_RangeGetMeasurement(dev,pRangeData );
+           if( status == 0){
+               /*  clear range interrupt source */
+               status = VL6180x_RangeClearInterrupt(dev);
+               if( status ){
+                   VL6180x_ErrLog("VL6180x_RangeClearInterrupt fail");
+               }
+           }
+        }
+        else{
+            status = NOT_READY;
+        }
+    }
+    else{
+        VL6180x_ErrLog("fail to get interrupt status");
+    }
+    LOG_FUNCTION_END(status) ;
+    return status;
+}
+
+int VL6180X::VL6180x_FilterSetState(VL6180xDev_t dev, int state){
+    int status;
+    LOG_FUNCTION_START("%d", state);
+#if VL6180x_WRAP_AROUND_FILTER_SUPPORT
+    VL6180xDevDataSet(dev,WrapAroundFilterActive, state);
+    status = 0;
+#else
+    status =  NOT_SUPPORTED;
+#endif
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+int VL6180X::VL6180x_FilterGetState(VL6180xDev_t dev){
+    int status;
+    LOG_FUNCTION_START("");
+#if VL6180x_WRAP_AROUND_FILTER_SUPPORT
+    status = VL6180xDevDataGet(dev,WrapAroundFilterActive);
+#else
+    status = 0;
+#endif
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+int VL6180X::VL6180x_RangeGetResult(VL6180xDev_t dev, int32_t *pRange_mm) {
+    int status;
+    uint8_t RawRange;
+    int32_t Upscale;
+
+    LOG_FUNCTION_START("%p",pRange_mm);
+
+    status = VL6180x_RdByte(dev, RESULT_RANGE_VAL, &RawRange);
+    if( !status ){
+        Upscale = _GetUpscale(dev);
+        *pRange_mm= Upscale*(int32_t)RawRange;
+    }
+    LOG_FUNCTION_END_FMT(status, "%d", (int)*pRange_mm);
+    return status;
+}
+
+int VL6180X::VL6180x_RangeSetRawThresholds(VL6180xDev_t dev, uint8_t low, uint8_t high)
+{
+    int status;
+    LOG_FUNCTION_START("%d %d", (int) low, (int)high);
+    /* TODO we can optimize here grouping high/low in a word but that's cpu endianness dependent */
+    status=VL6180x_WrByte(dev, SYSRANGE_THRESH_HIGH,high);
+    if( !status){
+        status=VL6180x_WrByte(dev, SYSRANGE_THRESH_LOW, low);
+    }
+
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+int VL6180X::VL6180x_RangeSetThresholds(VL6180xDev_t dev, uint16_t low, uint16_t high, int UseSafeParamHold)
+{
+    int status;
+    int scale;
+    LOG_FUNCTION_START("%d %d", (int) low, (int)high);
+    scale=_GetUpscale(dev,UpscaleFactor);
+    if( low>scale*255 || high >scale*255){
+        status = INVALID_PARAMS;
+    }
+    else{
+        do{
+            if( UseSafeParamHold ){
+                status=VL6180x_SetGroupParamHold(dev, 1);
+                if( status )
+                    break;
+            }
+            status=VL6180x_RangeSetRawThresholds(dev, (uint8_t)(low/scale), (uint8_t)(high/scale));
+            if( status ){
+                VL6180x_ErrLog("VL6180x_RangeSetRawThresholds fail");
+            }
+            if( UseSafeParamHold ){
+                int HoldStatus;
+                /* tryt to unset param hold vene if previous fail */
+                HoldStatus=VL6180x_SetGroupParamHold(dev, 0);
+                if( !status)
+                    status=HoldStatus;
+            }
+        }
+        while(0);
+    }
+
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+
+int VL6180X::VL6180x_RangeGetThresholds(VL6180xDev_t dev, uint16_t *low, uint16_t *high)
+{
+    int status;
+    uint8_t RawLow, RawHigh;
+    int scale;
+
+    LOG_FUNCTION_START("%p %p", low , high);
+
+    scale=_GetUpscale(dev,UpscaleFactor);
+    do{
+        if( high != NULL ){
+            status=VL6180x_RdByte(dev, SYSRANGE_THRESH_HIGH,&RawHigh);
+            if( status ){
+                VL6180x_ErrLog("rd SYSRANGE_THRESH_HIGH fail");
+                break;
+            }
+            *high=(uint16_t)RawHigh*scale;
+        }
+        if( low != NULL ) {
+            status=VL6180x_RdByte(dev, SYSRANGE_THRESH_LOW, &RawLow);
+            if( status ){
+                VL6180x_ErrLog("rd SYSRANGE_THRESH_LOW fail");
+                break;
+            }
+            *low=(uint16_t)RawLow*scale;
+        }
+    }
+    while(0);
+    LOG_FUNCTION_END_FMT(status, "%d %d",(int)*low ,(int)*high);
+    return status;
+}
+
+
+int VL6180X::VL6180x_RangeGetInterruptStatus(VL6180xDev_t dev, uint8_t *pIntStatus) {
+    int status;
+    uint8_t IntStatus;
+    LOG_FUNCTION_START("%p", pIntStatus);
+    /* FIXME we are grouping "error" with over status the user must check implicitly for it
+     * not just new sample or over status , that will nevr show up in case of error*/
+    status = VL6180x_RdByte(dev, RESULT_INTERRUPT_STATUS_GPIO, &IntStatus);
+    *pIntStatus= IntStatus&0xC7;
+
+    LOG_FUNCTION_END_FMT(status, "%d", (int)*pIntStatus);
+    return status;
+}
+
+
+int VL6180X::VL6180x_GetInterruptStatus(VL6180xDev_t dev, uint8_t *IntStatus)
+{
+    int status;
+    LOG_FUNCTION_START("%p" , IntStatus);
+    status = VL6180x_RdByte(dev, RESULT_INTERRUPT_STATUS_GPIO, IntStatus);
+    LOG_FUNCTION_END_FMT(status, "%d", (int)*IntStatus);
+    return status;
+}
+
+int VL6180X::VL6180x_ClearInterrupt(VL6180xDev_t dev, uint8_t IntClear )
+{
+    int status;
+    LOG_FUNCTION_START("%d" ,(int)IntClear);
+    if( IntClear <= 7 ){
+        status=VL6180x_WrByte( dev, SYSTEM_INTERRUPT_CLEAR, IntClear);
+    }
+    else{
+        status = INVALID_PARAMS;
+    }
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+
+int VL6180X::VL6180x_RangeStaticInit(VL6180xDev_t dev)
+{
+    int status;
+    LOG_FUNCTION_START("");
+
+    /* REGISTER_TUNING_SR03_270514_CustomerView.txt */
+    VL6180x_WrByte( dev, 0x0207, 0x01);
+    VL6180x_WrByte( dev, 0x0208, 0x01);
+    VL6180x_WrByte( dev, 0x0096, 0x00);
+    VL6180x_WrByte( dev, 0x0097, 0xfd);
+    VL6180x_WrByte( dev, 0x00e3, 0x00);
+    VL6180x_WrByte( dev, 0x00e4, 0x04);
+    VL6180x_WrByte( dev, 0x00e5, 0x02);
+    VL6180x_WrByte( dev, 0x00e6, 0x01);
+    VL6180x_WrByte( dev, 0x00e7, 0x03);
+    VL6180x_WrByte( dev, 0x00f5, 0x02);
+    VL6180x_WrByte( dev, 0x00d9, 0x05);
+    VL6180x_WrByte( dev, 0x00db, 0xce);
+    VL6180x_WrByte( dev, 0x00dc, 0x03);
+    VL6180x_WrByte( dev, 0x00dd, 0xf8);
+    VL6180x_WrByte( dev, 0x009f, 0x00);
+    VL6180x_WrByte( dev, 0x00a3, 0x3c);
+    VL6180x_WrByte( dev, 0x00b7, 0x00);
+    VL6180x_WrByte( dev, 0x00bb, 0x3c);
+    VL6180x_WrByte( dev, 0x00b2, 0x09);
+    VL6180x_WrByte( dev, 0x00ca, 0x09);
+    VL6180x_WrByte( dev, 0x0198, 0x01);
+    VL6180x_WrByte( dev, 0x01b0, 0x17);
+    VL6180x_WrByte( dev, 0x01ad, 0x00);
+    VL6180x_WrByte( dev, 0x00ff, 0x05);
+    VL6180x_WrByte( dev, 0x0100, 0x05);
+    VL6180x_WrByte( dev, 0x0199, 0x05);
+    VL6180x_WrByte( dev, 0x01a6, 0x1b);
+    VL6180x_WrByte( dev, 0x01ac, 0x3e);
+    VL6180x_WrByte( dev, 0x01a7, 0x1f);
+    VL6180x_WrByte( dev, 0x0030, 0x00);
+
+    /* Recommended : Public registers - See data sheet for more detail */
+    VL6180x_WrByte( dev, 0x0011, 0x10); /* Enables polling for New Sample ready when measurement completes */
+    VL6180x_WrByte( dev, 0x010a, 0x30); /* Set the averaging sample period (compromise between lower noise and increased execution time) */
+    VL6180x_WrByte( dev, 0x003f, 0x46); /* Sets the light and dark gain (upper nibble). Dark gain should not be changed.*/
+    VL6180x_WrByte( dev, 0x0031, 0xFF); /* sets the # of range measurements after which auto calibration of system is performed */
+    VL6180x_WrByte( dev, 0x0040, 0x63); /* Set ALS integration time to 100ms */
+    VL6180x_WrByte( dev, 0x002e, 0x01); /* perform a single temperature calibration of the ranging sensor */
+
+    /* Optional: Public registers - See data sheet for more detail */
+    VL6180x_WrByte( dev, 0x001b, 0x09); /* Set default ranging inter-measurement period to 100ms */
+    VL6180x_WrByte( dev, 0x003e, 0x31); /* Set default ALS inter-measurement period to 500ms */
+    VL6180x_WrByte( dev, 0x0014, 0x24); /* Configures interrupt on New sample ready */
+
+
+    status=VL6180x_RangeSetMaxConvergenceTime(dev, 50); /*  Calculate ece value on initialization (use max conv) */
+    LOG_FUNCTION_END(status);
+
+    return status;
+}
+
+#if VL6180x_UPSCALE_SUPPORT != 1
+
+int VL6180X::_UpscaleInitPatch0(VL6180xDev_t dev){
+    int status;
+    uint32_t CalValue=0;
+    CalValue= VL6180xDevDataGet(dev, Part2PartAmbNVM);
+    status=VL6180x_WrDWord( dev, 0xDA, CalValue);
+    return status;
+}
+
+/* only include up-scaling register setting when up-scale support is configured in */
+int VL6180X::VL6180x_UpscaleRegInit(VL6180xDev_t dev)
+{
+    /*  apply REGISTER_TUNING_ER02_100614_CustomerView.txt */
+    VL6180x_WrByte( dev, 0x0207, 0x01);
+    VL6180x_WrByte( dev, 0x0208, 0x01);
+    VL6180x_WrByte( dev, 0x0096, 0x00);
+    VL6180x_WrByte( dev, 0x0097, 0x54);
+    VL6180x_WrByte( dev, 0x00e3, 0x00);
+    VL6180x_WrByte( dev, 0x00e4, 0x04);
+    VL6180x_WrByte( dev, 0x00e5, 0x02);
+    VL6180x_WrByte( dev, 0x00e6, 0x01);
+    VL6180x_WrByte( dev, 0x00e7, 0x03);
+    VL6180x_WrByte( dev, 0x00f5, 0x02);
+    VL6180x_WrByte( dev, 0x00d9, 0x05);
+
+    _UpscaleInitPatch0(dev);
+
+    VL6180x_WrByte( dev, 0x009f, 0x00);
+    VL6180x_WrByte( dev, 0x00a3, 0x28);
+    VL6180x_WrByte( dev, 0x00b7, 0x00);
+    VL6180x_WrByte( dev, 0x00bb, 0x28);
+    VL6180x_WrByte( dev, 0x00b2, 0x09);
+    VL6180x_WrByte( dev, 0x00ca, 0x09);
+    VL6180x_WrByte( dev, 0x0198, 0x01);
+    VL6180x_WrByte( dev, 0x01b0, 0x17);
+    VL6180x_WrByte( dev, 0x01ad, 0x00);
+    VL6180x_WrByte( dev, 0x00ff, 0x05);
+    VL6180x_WrByte( dev, 0x0100, 0x05);
+    VL6180x_WrByte( dev, 0x0199, 0x05);
+    VL6180x_WrByte( dev, 0x01a6, 0x1b);
+    VL6180x_WrByte( dev, 0x01ac, 0x3e);
+    VL6180x_WrByte( dev, 0x01a7, 0x1f);
+    VL6180x_WrByte( dev, 0x0030, 0x00);
+    VL6180x_WrByte( dev, 0x0011, 0x10);
+    VL6180x_WrByte( dev, 0x010a, 0x30);
+    VL6180x_WrByte( dev, 0x003f, 0x46);
+    VL6180x_WrByte( dev, 0x0031, 0xFF);
+    VL6180x_WrByte( dev, 0x0040, 0x63);
+    VL6180x_WrByte( dev, 0x002e, 0x01);
+    VL6180x_WrByte( dev, 0x002c, 0xff);
+    VL6180x_WrByte( dev, 0x001b, 0x09);
+    VL6180x_WrByte( dev, 0x003e, 0x31);
+    VL6180x_WrByte( dev, 0x0014, 0x24);
+#if VL6180x_EXTENDED_RANGE
+    VL6180x_RangeSetMaxConvergenceTime(dev, 63);
+#else
+    VL6180x_RangeSetMaxConvergenceTime(dev, 50);
+#endif
+    return 0;
+}
+#else
+#define VL6180x_UpscaleRegInit(...) -1
+#endif
+
+int VL6180X::VL6180x_UpscaleSetScaling(VL6180xDev_t dev, uint8_t scaling)
+{
+    int status;
+    uint16_t Scaler;
+    int8_t  Offset;
+
+    LOG_FUNCTION_START("%d",(int) scaling);
+
+#ifdef VL6180x_HAVE_UPSCALE_DATA
+    #define min_scaling 1
+    #define max_scaling sizeof(ScalerLookUP)/sizeof(ScalerLookUP[0])
+#else
+     /* we are in fixed config so only allow configured factor */
+    #define min_scaling VL6180x_UPSCALE_SUPPORT
+    #define max_scaling VL6180x_UPSCALE_SUPPORT
+#endif
+
+    if( scaling>=min_scaling  && scaling<= max_scaling ){
+
+        Scaler = ScalerLookUP[scaling-1];
+        status = VL6180x_WrWord(dev, RANGE_SCALER, Scaler);
+        _SetUpscale(dev, scaling );
+
+        /* Apply scaling on  part-2-part offset */
+        Offset = VL6180xDevDataGet(dev, Part2PartOffsetNVM)/scaling;
+        status = VL6180x_WrByte(dev, SYSRANGE_PART_TO_PART_RANGE_OFFSET, Offset);
+#if ! VL6180x_EXTENDED_RANGE
+        if( status ==0 ){
+            status = VL6180x_RangeSetEceState(dev, scaling == 1); /* enable ece only at 1x scaling */
+        }
+        if( status == 0 && !VL6180x_EXTENDED_RANGE && scaling!=1 ){
+        	status = NOT_GUARANTEED ;
+        }
+#endif
+    }
+    else{
+        status = INVALID_PARAMS;
+    }
+#undef min_scaling
+#undef max_scaling
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+
+int VL6180X::VL6180x_UpscaleGetScaling(VL6180xDev_t dev)
+{
+    int status;
+    LOG_FUNCTION_START("");
+    status=_GetUpscale(dev );
+    LOG_FUNCTION_END(status);
+
+    return status;
+}
+
+
+int VL6180X::VL6180x_UpscaleStaticInit(VL6180xDev_t dev)
+{
+    /* todo make these a fail macro in case only 1x is suppoted */
+    int status;
+
+    LOG_FUNCTION_START("");
+    do{
+        status=VL6180x_UpscaleRegInit(dev);
+        if( status){
+            VL6180x_ErrLog("regInit fail");
+            break;
+        }
+#if VL6180x_EXTENDED_RANGE
+        status = VL6180x_RangeSetEceState(dev, 0);
+        if( status){
+            VL6180x_ErrLog("VL6180x_RangeSetEceState fail");
+            break;
+        }
+#endif
+    } while(0);
+    if( !status){
+        /*  must write the scaler at least once to the device to ensure the scaler is in a known state. */
+        status=VL6180x_UpscaleSetScaling(dev, _GetUpscale(dev));
+        VL6180x_WrByte( dev, 0x016, 0x00); /* change fresh out of set status to 0 */
+    }
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+
+int VL6180X::VL6180x_SetGPIOxPolarity(VL6180xDev_t dev, int pin, int active_high)
+{
+    int status;
+    LOG_FUNCTION_START("%d %d",(int) pin, (int)active_high);
+
+    if( pin ==0  || pin ==1  ){
+       uint16_t RegIndex;
+       uint8_t  DataSet;
+       if( pin==0 )
+           RegIndex= SYSTEM_MODE_GPIO0;
+       else
+           RegIndex= SYSTEM_MODE_GPIO1;
+
+       if (active_high )
+           DataSet = GPIOx_POLARITY_SELECT_MASK;
+       else
+           DataSet = 0;
+
+       status = VL6180x_UpdateByte(dev, RegIndex, (uint8_t)~GPIOx_POLARITY_SELECT_MASK, DataSet);
+    }
+    else{
+        VL6180x_ErrLog("Invalid pin param %d", (int)pin);
+        status = INVALID_PARAMS;
+    }
+
+    LOG_FUNCTION_END(status);
+
+    return status;
+}
+
+int VL6180X::VL6180x_SetGPIOxFunctionality(VL6180xDev_t dev, int pin, uint8_t functionality)
+{
+    int status;
+
+    LOG_FUNCTION_START("%d %d",(int) pin, (int)functionality);
+
+    if( ((pin ==0)  || (pin ==1))  && IsValidGPIOFunction(functionality)  ){
+       uint16_t RegIndex;
+
+       if( pin==0 )
+           RegIndex= SYSTEM_MODE_GPIO0;
+       else
+           RegIndex= SYSTEM_MODE_GPIO1;
+
+       status = VL6180x_UpdateByte(dev, RegIndex, (uint8_t)~GPIOx_FUNCTIONALITY_SELECT_MASK,  functionality<<GPIOx_FUNCTIONALITY_SELECT_SHIFT);
+       if( status){
+           VL6180x_ErrLog("Update SYSTEM_MODE_GPIO%d fail", (int)pin);
+       }
+    }
+    else{
+        VL6180x_ErrLog("Invalid pin %d  or function %d", (int)pin, (int) functionality);
+        status = INVALID_PARAMS;
+    }
+
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+
+int VL6180X::VL6180x_SetupGPIOx(VL6180xDev_t dev, int pin,  uint8_t IntFunction, int  ActiveHigh)
+{
+    int status;
+
+    LOG_FUNCTION_START("%d %d",(int) pin, (int)IntFunction);
+
+    if( ((pin ==0)  || (pin ==1))  && IsValidGPIOFunction(IntFunction)  ){
+       uint16_t RegIndex;
+       uint8_t value=0;
+
+       if( pin==0 )
+           RegIndex= SYSTEM_MODE_GPIO0;
+       else
+           RegIndex= SYSTEM_MODE_GPIO1;
+
+       if( ActiveHigh  )
+           value|=GPIOx_POLARITY_SELECT_MASK;
+
+       value |=  IntFunction<<GPIOx_FUNCTIONALITY_SELECT_SHIFT;
+       status = VL6180x_WrByte(dev, RegIndex, value);
+       if( status ){
+           VL6180x_ErrLog("SYSTEM_MODE_GPIO%d wr fail", (int)pin-SYSTEM_MODE_GPIO0);
+       }
+    }
+    else{
+        VL6180x_ErrLog("Invalid pin %d or function %d", (int)pin, (int) IntFunction);
+        status = INVALID_PARAMS;
+    }
+
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+
+int VL6180X::VL6180x_DisableGPIOxOut(VL6180xDev_t dev, int pin) {
+    int status;
+
+    LOG_FUNCTION_START("%d",(int)pin);
+
+    status=VL6180x_SetGPIOxFunctionality(dev, pin, GPIOx_SELECT_OFF);
+
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+
+int VL6180X::VL6180x_SetupGPIO1(VL6180xDev_t dev, uint8_t IntFunction, int ActiveHigh)
+{
+    int status;
+    LOG_FUNCTION_START("%d %d",(int)IntFunction, (int)ActiveHigh  );
+    status=VL6180x_SetupGPIOx(dev, 1 , IntFunction, ActiveHigh);
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+int VL6180X::VL6180x_RangeConfigInterrupt(VL6180xDev_t dev, uint8_t ConfigGpioInt)
+{
+    int status;
+
+    if( ConfigGpioInt<= CONFIG_GPIO_INTERRUPT_NEW_SAMPLE_READY){
+        status = VL6180x_UpdateByte(dev, SYSTEM_INTERRUPT_CONFIG_GPIO, (uint8_t)(~CONFIG_GPIO_RANGE_MASK), ConfigGpioInt);
+    }
+    else{
+        VL6180x_ErrLog("Invalid config mode param %d", (int)ConfigGpioInt);
+        status = INVALID_PARAMS;
+    }
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+
+int VL6180X::VL6180x_RangeSetEceFactor(VL6180xDev_t dev, uint16_t  FactorM, uint16_t FactorD){
+    int status;
+    uint8_t u8;
+
+    LOG_FUNCTION_START("%d %d", (int)FactorM, (int)FactorD );
+    do{
+        /* D cannot be 0 M must be <=D and >= 0 */
+        if( FactorM <= FactorD  && FactorD> 0){
+            VL6180xDevDataSet(dev, EceFactorM, FactorM);
+            VL6180xDevDataSet(dev, EceFactorD, FactorD);
+            /* read and re-apply max conv time to get new ece factor set */
+            status = VL6180x_RdByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, &u8);
+            if( status){
+               VL6180x_ErrLog("SYSRANGE_MAX_CONVERGENCE_TIME rd fail ");
+               break;
+            }
+            status = VL6180x_RangeSetMaxConvergenceTime(dev, u8);
+            if( status <0 ){
+                VL6180x_ErrLog("fail to apply time after ece m/d change");
+                break;
+            }
+        }
+        else{
+            VL6180x_ErrLog("invalid factor %d/%d", (int)FactorM, (int)FactorD );
+            status = INVALID_PARAMS;
+        }
+    }
+    while(0);
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+int VL6180X::VL6180x_RangeSetEceState(VL6180xDev_t dev, int enable ){
+    int status;
+    uint8_t or_mask;
+
+    LOG_FUNCTION_START("%d", (int)enable);
+    if( enable )
+        or_mask = RANGE_CHECK_ECE_ENABLE_MASK;
+    else
+        or_mask = 0;
+
+    status =VL6180x_UpdateByte(dev, SYSRANGE_RANGE_CHECK_ENABLES, ~RANGE_CHECK_ECE_ENABLE_MASK, or_mask);
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+
+int VL6180X::VL6180x_RangeSetMaxConvergenceTime(VL6180xDev_t dev, uint8_t  MaxConTime_msec)
+{
+    int status = 0;
+    LOG_FUNCTION_START("%d",(int)MaxConTime_msec);
+    do{
+        status=VL6180x_WrByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, MaxConTime_msec);
+        if( status ){
+            break;
+        }
+        status=VL6180x_RangeSetEarlyConvergenceEestimateThreshold(dev);
+        if( status){
+            break;
+        }
+        status = _DMax_InitData(dev);
+    }
+    while(0);
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+int VL6180X::VL6180x_RangeSetInterMeasPeriod(VL6180xDev_t dev, uint32_t  InterMeasTime_msec){
+    uint8_t SetTime;
+    int status;
+
+    LOG_FUNCTION_START("%d",(int)InterMeasTime_msec);
+    do {
+        if( InterMeasTime_msec > 2550 ){
+            status = INVALID_PARAMS;
+            break;
+        }
+        /* doc in not 100% clear and confusing about the limit practically all value are OK but 0
+         * that can hang device in continuous mode */
+        if( InterMeasTime_msec < 10 ) {
+            InterMeasTime_msec=10;
+        }
+        SetTime=(uint8_t)(InterMeasTime_msec/10);
+        status=VL6180x_WrByte(dev, SYSRANGE_INTERMEASUREMENT_PERIOD, SetTime);
+        if( status ){
+            VL6180x_ErrLog("SYSRANGE_INTERMEASUREMENT_PERIOD wr fail");
+        }
+        else
+        if( SetTime != InterMeasTime_msec /10 ) {
+            status = MIN_CLIPED;  /* on success change status to clip if it did */
+        }
+    }while(0);
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+
+int VL6180X::VL6180x_RangeGetDeviceReady(VL6180xDev_t dev, int * Ready){
+    int status;
+    uint8_t u8;
+    LOG_FUNCTION_START("%p", (int)Ready);
+    status=VL6180x_RdByte(dev, RESULT_RANGE_STATUS, &u8);
+    if( !status)
+        *Ready = u8&RANGE_DEVICE_READY_MASK;
+    LOG_FUNCTION_END_FMT(status,"%d", *Ready);
+    return status;
+}
+
+
+int VL6180X::VL6180x_RangeWaitDeviceReady(VL6180xDev_t dev, int MaxLoop ){
+    int status; /* if user specify an invalid <=0 loop count we'll return error */
+    int  n;
+    uint8_t u8;
+    LOG_FUNCTION_START("%d", (int)MaxLoop);
+    if( MaxLoop<1){
+        status=INVALID_PARAMS;
+    }
+    else{
+        for( n=0; n < MaxLoop ; n++){
+            status=VL6180x_RdByte(dev, RESULT_RANGE_STATUS, &u8);
+            if( status)
+                break;
+            u8 = u8 & RANGE_DEVICE_READY_MASK;
+            if( u8 )
+                break;
+
+        }
+        if( !status && !u8 ){
+            status = TIME_OUT;
+        }
+    }
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+int VL6180X::VL6180x_RangeSetSystemMode(VL6180xDev_t dev, uint8_t  mode)
+{
+    int status;
+    LOG_FUNCTION_START("%d", (int)mode);
+    /* FIXME we are not checking device is ready via @a VL6180x_RangeWaitDeviceReady
+     * so if called back to back real fast we are not checking
+     * if previous mode "set" got absorbed => bit 0 must be 0 so that it work
+     */
+    if( mode <= 3){
+        status=VL6180x_WrByte(dev, SYSRANGE_START, mode);
+        if( status ){
+            VL6180x_ErrLog("SYSRANGE_START wr fail");
+        }
+    }
+    else{
+        status = INVALID_PARAMS;
+    }
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+
+int VL6180X::VL6180x_RangeStartContinuousMode(VL6180xDev_t dev)
+{
+    int status;
+    LOG_FUNCTION_START("");
+    status= VL6180x_RangeSetSystemMode(dev, MODE_START_STOP | MODE_CONTINUOUS);
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+int VL6180X::VL6180x_RangeStartSingleShot(VL6180xDev_t dev) {
+    int status;
+    LOG_FUNCTION_START("");
+    status = VL6180x_RangeSetSystemMode(dev, MODE_START_STOP|MODE_SINGLESHOT);
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+
+int VL6180X::VL6180x_RangeSetEarlyConvergenceEestimateThreshold(VL6180xDev_t dev)
+{
+    int status;
+
+    const uint32_t cMicroSecPerMilliSec  = 1000;
+    const uint32_t cEceSampleTime_us     = 500;
+    uint32_t ece_factor_m          = VL6180xDevDataGet(dev, EceFactorM);
+    uint32_t ece_factor_d          = VL6180xDevDataGet(dev, EceFactorD);
+    uint32_t convergTime_us;
+    uint32_t fineThresh;
+    uint32_t eceThresh;
+    uint8_t  u8;
+    uint32_t maxConv_ms;
+    int32_t AveTime;
+
+    LOG_FUNCTION_START("");
+
+    do{
+        status = VL6180x_RdByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, &u8);
+        if( status ){
+            VL6180x_ErrLog("SYSRANGE_MAX_CONVERGENCE_TIME rd fail");
+            break;
+        }
+        maxConv_ms = u8;
+        AveTime = _GetAveTotalTime(dev);
+        if( AveTime <0 ){
+            status=-1;
+            break;
+        }
+
+        convergTime_us = maxConv_ms * cMicroSecPerMilliSec - AveTime;
+        status = VL6180x_RdDWord(dev, 0xB8, &fineThresh);
+        if( status ) {
+            VL6180x_ErrLog("reg 0xB8 rd fail");
+            break;
+        }
+        fineThresh*=256;
+        eceThresh      = ece_factor_m * cEceSampleTime_us * fineThresh/(convergTime_us * ece_factor_d);
+
+        status=VL6180x_WrWord(dev, SYSRANGE_EARLY_CONVERGENCE_ESTIMATE, (uint16_t)eceThresh);
+    }
+    while(0);
+
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+/*
+ * Return >0 = time
+ *       <0 1 if fail to get read data from device to compute time
+ */
+int32_t VL6180X::_GetAveTotalTime(VL6180xDev_t dev) {
+    uint32_t cFwOverhead_us = 24;
+    uint32_t cVcpSetupTime_us = 70;
+    uint32_t cPLL2_StartupDelay_us = 200;
+    uint8_t cMeasMask = 0x07;
+    uint32_t Samples;
+    uint32_t SamplePeriod;
+    uint32_t SingleTime_us;
+    int32_t TotalAveTime_us;
+    uint8_t u8;
+    int status;
+
+    LOG_FUNCTION_START("");
+
+    status = VL6180x_RdByte(dev, 0x109, &u8);
+    if (status) {
+        VL6180x_ErrLog("rd 0x109 fail");
+        return -1;
+    }
+    Samples = u8 & cMeasMask;
+    status = VL6180x_RdByte(dev, READOUT_AVERAGING_SAMPLE_PERIOD, &u8);
+    if (status) {
+        VL6180x_ErrLog("i2c READOUT_AVERAGING_SAMPLE_PERIOD fail");
+        return -1;
+    }
+    SamplePeriod = u8;
+    SingleTime_us = cFwOverhead_us + cVcpSetupTime_us + (SamplePeriod * 10);
+    TotalAveTime_us = (Samples + 1) * SingleTime_us + cPLL2_StartupDelay_us;
+
+    LOG_FUNCTION_END(TotalAveTime_us);
+    return TotalAveTime_us;
+}
+
+#if VL6180x_HAVE_DMAX_RANGING
+#define _GetDMaxDataRetSignalAt400mm(dev) VL6180xDevDataGet(dev, DMaxData.retSignalAt400mm)
+#else
+#define _GetDMaxDataRetSignalAt400mm(dev) 375 // Use a default high value
+#endif
+
+
+#if VL6180x_WRAP_AROUND_FILTER_SUPPORT
+
+#define FILTER_STDDEV_SAMPLES           6
+#define MIN_FILTER_STDDEV_SAMPLES       3
+#define MIN_FILTER_VALID_STDDEV_SAMPLES 3
+#define FILTER_INVALID_DISTANCE     65535
+
+#define _FilterData(field) VL6180xDevDataGet(dev, FilterData.field)
+/*
+ * One time init
+ */
+int _filter_Init( VL6180xDev_t dev) {
+    int i;
+    _FilterData(MeasurementIndex) = 0;
+
+    _FilterData(Default_ZeroVal) = 0;
+    _FilterData(Default_VAVGVal) = 0;
+    _FilterData(NoDelay_ZeroVal) = 0;
+    _FilterData(NoDelay_VAVGVal) = 0;
+    _FilterData(Previous_VAVGDiff) = 0;
+
+    _FilterData(StdFilteredReads) = 0;
+
+    for (i = 0; i < FILTER_NBOF_SAMPLES; i++) {
+        _FilterData(LastTrueRange)[i] = FILTER_INVALID_DISTANCE;
+        _FilterData(LastReturnRates)[i] = 0;
+    }
+    return 0;
+}
+
+
+static uint32_t _filter_StdDevDamper(uint32_t AmbientRate, uint32_t SignalRate, const uint32_t StdDevLimitLowLight, const uint32_t StdDevLimitLowLightSNR, const uint32_t StdDevLimitHighLight, const uint32_t StdDevLimitHighLightSNR) {
+    uint32_t newStdDev;
+    uint16_t SNR;
+
+    if (AmbientRate > 0)
+        SNR = (uint16_t) ((100 * SignalRate) / AmbientRate);
+    else
+        SNR = 9999;
+
+    if (SNR >= StdDevLimitLowLightSNR) {
+        newStdDev = StdDevLimitLowLight;
+    } else {
+        if (SNR <= StdDevLimitHighLightSNR)
+            newStdDev = StdDevLimitHighLight;
+        else {
+            newStdDev = (uint32_t) (StdDevLimitHighLight + (SNR - StdDevLimitHighLightSNR) * (int) (StdDevLimitLowLight - StdDevLimitHighLight) / (StdDevLimitLowLightSNR - StdDevLimitHighLightSNR));
+        }
+    }
+
+    return newStdDev;
+}
+
+
+/*
+ * Return <0 on error
+ */
+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) {
+    int status;
+    uint16_t m_newTrueRange_mm = 0;
+
+    uint16_t i;
+    uint16_t bypassFilter = 0;
+
+    uint16_t registerValue;
+
+    uint32_t register32BitsValue1;
+    uint32_t register32BitsValue2;
+
+    uint16_t ValidDistance = 0;
+
+    uint16_t WrapAroundFlag = 0;
+    uint16_t NoWrapAroundFlag = 0;
+    uint16_t NoWrapAroundHighConfidenceFlag = 0;
+
+    uint16_t FlushFilter = 0;
+    uint32_t RateChange = 0;
+
+    uint16_t StdDevSamples = 0;
+    uint32_t StdDevDistanceSum = 0;
+    uint32_t StdDevDistanceMean = 0;
+    uint32_t StdDevDistance = 0;
+    uint32_t StdDevRateSum = 0;
+    uint32_t StdDevRateMean = 0;
+    uint32_t StdDevRate = 0;
+    uint32_t StdDevLimitWithTargetMove = 0;
+
+    uint32_t VAVGDiff;
+    uint32_t IdealVAVGDiff;
+    uint32_t MinVAVGDiff;
+    uint32_t MaxVAVGDiff;
+
+    /* Filter Parameters */
+    static const uint16_t ROMABLE_DATA WrapAroundLowRawRangeLimit = 60;
+    static const uint32_t ROMABLE_DATA WrapAroundLowReturnRateLimit_ROM = 800; // Shall be adapted depending on crossTalk
+    static const uint16_t ROMABLE_DATA WrapAroundLowRawRangeLimit2 = 165;
+    static const uint32_t ROMABLE_DATA WrapAroundLowReturnRateLimit2_ROM = 180; // Shall be adapted depending on crossTalk and device sensitivity
+
+    static const uint32_t ROMABLE_DATA WrapAroundLowReturnRateFilterLimit_ROM = 850; // Shall be adapted depending on crossTalk and device sensitivity
+    static const uint16_t ROMABLE_DATA WrapAroundHighRawRangeFilterLimit = 350;
+    static const uint32_t ROMABLE_DATA WrapAroundHighReturnRateFilterLimit_ROM = 1400; // Shall be adapted depending on crossTalk and device sensitivity
+
+    static const uint32_t ROMABLE_DATA WrapAroundMaximumAmbientRateFilterLimit = 7500;
+
+    /*  Temporal filter data and flush values */
+    static const uint32_t ROMABLE_DATA MinReturnRateFilterFlush = 75;
+    static const uint32_t ROMABLE_DATA MaxReturnRateChangeFilterFlush = 50;
+
+    /* STDDEV values and damper values */
+
+    static const uint32_t ROMABLE_DATA StdDevLimitLowLight = 300;
+    static const uint32_t ROMABLE_DATA StdDevLimitLowLightSNR = 30; /* 0.3 */
+    static const uint32_t ROMABLE_DATA StdDevLimitHighLight = 2500;
+    static const uint32_t ROMABLE_DATA StdDevLimitHighLightSNR = 5; /* 0.05 */
+
+    static const uint32_t ROMABLE_DATA StdDevHighConfidenceSNRLimit = 8;
+
+    static const uint32_t ROMABLE_DATA StdDevMovingTargetStdDevLimit = 90000;
+
+    static const uint32_t ROMABLE_DATA StdDevMovingTargetReturnRateLimit = 3500;
+    static const uint32_t ROMABLE_DATA StdDevMovingTargetStdDevForReturnRateLimit = 5000;
+
+    static const uint32_t ROMABLE_DATA MAX_VAVGDiff = 1800;
+
+    /* WrapAroundDetection variables */
+    static const uint16_t ROMABLE_DATA WrapAroundNoDelayCheckPeriod = 2;
+    static const uint16_t ROMABLE_DATA StdFilteredReadsIncrement = 2;
+    static const uint16_t ROMABLE_DATA StdMaxFilteredReads = 4;
+    
+    uint32_t SignalRateDMax;
+    uint32_t WrapAroundLowReturnRateLimit; 
+    uint32_t WrapAroundLowReturnRateLimit2;
+    uint32_t WrapAroundLowReturnRateFilterLimit;
+    uint32_t WrapAroundHighReturnRateFilterLimit; 
+
+    uint8_t u8, u8_2;
+    uint32_t XTalkCompRate_KCps;
+    uint32_t StdDevLimit = 300;
+    uint32_t MaxOrInvalidDistance =   255*_GetUpscale(dev);
+    /* #define MaxOrInvalidDistance  (uint16_t) (255 * 3) */
+
+    /* Check if distance is Valid or not */
+    switch (errorCode) {
+    case 0x0C:
+        m_trueRange_mm = MaxOrInvalidDistance;
+        ValidDistance = 0;
+        break;
+    case 0x0D:
+        m_trueRange_mm = MaxOrInvalidDistance;
+        ValidDistance = 1;
+        break;
+    case 0x0F:
+        m_trueRange_mm = MaxOrInvalidDistance;
+        ValidDistance = 1;
+        break;
+    default:
+        if (m_rawRange_mm >= MaxOrInvalidDistance) {
+            ValidDistance = 0;
+        } else {
+            ValidDistance = 1;
+        }
+        break;
+    }
+    m_newTrueRange_mm = m_trueRange_mm;
+    
+    XTalkCompRate_KCps = VL6180xDevDataGet(dev, XTalkCompRate_KCps );
+
+    
+    //Update signal rate limits depending on crosstalk
+    SignalRateDMax = (uint32_t)_GetDMaxDataRetSignalAt400mm(dev) + XTalkCompRate_KCps;
+    WrapAroundLowReturnRateLimit = WrapAroundLowReturnRateLimit_ROM  + XTalkCompRate_KCps; 
+    WrapAroundLowReturnRateLimit2 = ((WrapAroundLowReturnRateLimit2_ROM * SignalRateDMax) / 312) + XTalkCompRate_KCps;
+    WrapAroundLowReturnRateFilterLimit = ((WrapAroundLowReturnRateFilterLimit_ROM * SignalRateDMax) / 312) + XTalkCompRate_KCps;
+    WrapAroundHighReturnRateFilterLimit = ((WrapAroundHighReturnRateFilterLimit_ROM * SignalRateDMax) / 312) + XTalkCompRate_KCps; 
+
+
+    /* Checks on low range data */
+    if ((m_rawRange_mm < WrapAroundLowRawRangeLimit) && (m_rtnSignalRate < WrapAroundLowReturnRateLimit)) {
+        m_newTrueRange_mm = MaxOrInvalidDistance;
+        bypassFilter = 1;
+    }
+    if ((m_rawRange_mm < WrapAroundLowRawRangeLimit2) && (m_rtnSignalRate < WrapAroundLowReturnRateLimit2)) {
+        m_newTrueRange_mm = MaxOrInvalidDistance;
+        bypassFilter = 1;
+    }
+
+    /* Checks on Ambient rate level */
+    if (m_rtnAmbientRate > WrapAroundMaximumAmbientRateFilterLimit) {
+        /* Too high ambient rate */
+        FlushFilter = 1;
+        bypassFilter = 1;
+    }
+    /*  Checks on Filter flush */
+    if (m_rtnSignalRate < MinReturnRateFilterFlush) {
+        /* Completely lost target, so flush the filter */
+        FlushFilter = 1;
+        bypassFilter = 1;
+    }
+    if (_FilterData(LastReturnRates)[0] != 0) {
+        if (m_rtnSignalRate > _FilterData(LastReturnRates)[0])
+            RateChange = (100 * (m_rtnSignalRate - _FilterData(LastReturnRates)[0])) / _FilterData(LastReturnRates)[0];
+        else
+            RateChange = (100 * (_FilterData(LastReturnRates)[0] - m_rtnSignalRate)) / _FilterData(LastReturnRates)[0];
+    } else
+        RateChange = 0;
+    if (RateChange > MaxReturnRateChangeFilterFlush) {
+        FlushFilter = 1;
+    }
+/* TODO optimize filter  using circular buffer */
+    if (FlushFilter == 1) {
+        _FilterData(MeasurementIndex) = 0;
+        for (i = 0; i < FILTER_NBOF_SAMPLES; i++) {
+            _FilterData(LastTrueRange)[i] = FILTER_INVALID_DISTANCE;
+            _FilterData(LastReturnRates)[i] = 0;
+        }
+    } else {
+        for (i = (uint16_t) (FILTER_NBOF_SAMPLES - 1); i > 0; i--) {
+            _FilterData(LastTrueRange)[i] = _FilterData(LastTrueRange)[i - 1];
+            _FilterData(LastReturnRates)[i] = _FilterData(LastReturnRates)[i - 1];
+        }
+    }
+    if (ValidDistance == 1)
+        _FilterData(LastTrueRange)[0] = m_trueRange_mm;
+    else
+        _FilterData(LastTrueRange)[0] = FILTER_INVALID_DISTANCE;
+    _FilterData(LastReturnRates)[0] = m_rtnSignalRate;
+
+    /* Check if we need to go through the filter or not */
+    if (!(((m_rawRange_mm < WrapAroundHighRawRangeFilterLimit) && (m_rtnSignalRate < WrapAroundLowReturnRateFilterLimit)) || ((m_rawRange_mm >= WrapAroundHighRawRangeFilterLimit) && (m_rtnSignalRate < WrapAroundHighReturnRateFilterLimit))))
+        bypassFilter = 1;
+
+    /* Check which kind of measurement has been made */
+    status = VL6180x_RdByte(dev, 0x01AC, &u8 );
+    if( status ){
+        VL6180x_ErrLog("0x01AC rd fail");
+        goto done_err;
+    }
+    registerValue =u8;
+
+    /* Read data for filtering */
+    status = VL6180x_RdByte(dev, 0x10C, &u8 ); /* read only 8 lsb bits */
+    if( status ){
+        VL6180x_ErrLog("0x010C rd fail");
+        goto done_err;
+    }
+    register32BitsValue1=u8;
+    status = VL6180x_RdByte(dev, 0x0110, &u8); /* read only 8 lsb bits */
+    if( status ){
+        VL6180x_ErrLog("0x0110 rd fail");
+        goto done_err;
+    }
+    register32BitsValue2 = u8;
+
+    if (registerValue == 0x3E) {
+        _FilterData(Default_ZeroVal) = register32BitsValue1;
+        _FilterData(Default_VAVGVal) = register32BitsValue2;
+    } else {
+        _FilterData(NoDelay_ZeroVal) = register32BitsValue1;
+        _FilterData(NoDelay_VAVGVal) = register32BitsValue2;
+    }
+
+    if (bypassFilter == 1) {
+        /* Do not go through the filter */
+        if (registerValue != 0x3E) {
+            status = VL6180x_WrByte(dev, 0x1AC, 0x3E);
+            if( status ){
+                VL6180x_ErrLog("0x01AC bypass wr fail");
+                goto done_err;
+            }
+            status = VL6180x_WrByte(dev, 0x0F2, 0x01);
+            if( status ){
+                VL6180x_ErrLog("0x0F2 bypass wr fail");
+                goto done_err;
+            }
+        }
+        /* Set both Default and NoDelay To same value */
+        _FilterData(Default_ZeroVal) = register32BitsValue1;
+        _FilterData(Default_VAVGVal) = register32BitsValue2;
+        _FilterData(NoDelay_ZeroVal) = register32BitsValue1;
+        _FilterData(NoDelay_VAVGVal) = register32BitsValue2;
+        _FilterData(MeasurementIndex) = 0;
+
+        return m_newTrueRange_mm;
+    }
+
+    if (_FilterData(MeasurementIndex) % WrapAroundNoDelayCheckPeriod == 0) {
+        u8=0x3C;
+		u8_2 = 0x05;
+    } else {
+        u8=0x3E;
+        u8_2 = 0x01;
+    }
+    status = VL6180x_WrByte(dev, 0x01AC, u8);
+    if( status ){
+        VL6180x_ErrLog("0x01AC wr fail");
+        goto done_err;
+    }
+    status = VL6180x_WrByte(dev, 0x0F2, u8_2);
+    if( status ){
+        VL6180x_ErrLog("0x0F2  wr fail");
+        goto done_err;
+    }
+
+
+    _FilterData(MeasurementIndex)++;
+
+    /* Computes current VAVGDiff */
+    if (_FilterData(Default_VAVGVal) > _FilterData(NoDelay_VAVGVal))
+        VAVGDiff = _FilterData(Default_VAVGVal) - _FilterData(NoDelay_VAVGVal);
+    else
+        VAVGDiff = 0;
+    _FilterData(Previous_VAVGDiff) = VAVGDiff;
+
+    /* Check the VAVGDiff */
+    if (_FilterData(Default_ZeroVal) > _FilterData(NoDelay_ZeroVal))
+        IdealVAVGDiff = _FilterData(Default_ZeroVal) - _FilterData(NoDelay_ZeroVal);
+    else
+        IdealVAVGDiff = _FilterData(NoDelay_ZeroVal) - _FilterData(Default_ZeroVal);
+    if (IdealVAVGDiff > MAX_VAVGDiff)
+        MinVAVGDiff = IdealVAVGDiff - MAX_VAVGDiff;
+    else
+        MinVAVGDiff = 0;
+    MaxVAVGDiff = IdealVAVGDiff + MAX_VAVGDiff;
+    if (VAVGDiff < MinVAVGDiff || VAVGDiff > MaxVAVGDiff) {
+        WrapAroundFlag = 1;
+    } else {
+        /* Go through filtering check */
+
+        /* StdDevLimit Damper on SNR */
+        StdDevLimit = _filter_StdDevDamper(m_rtnAmbientRate, m_rtnSignalRate, StdDevLimitLowLight, StdDevLimitLowLightSNR, StdDevLimitHighLight, StdDevLimitHighLightSNR);
+
+        /* Standard deviations computations */
+        StdDevSamples = 0;
+        StdDevDistanceSum = 0;
+        StdDevDistanceMean = 0;
+        StdDevDistance = 0;
+        StdDevRateSum = 0;
+        StdDevRateMean = 0;
+        StdDevRate = 0;
+        for (i = 0; (i < FILTER_NBOF_SAMPLES) && (StdDevSamples < FILTER_STDDEV_SAMPLES); i++) {
+            if (_FilterData(LastTrueRange)[i] != FILTER_INVALID_DISTANCE) {
+                StdDevSamples = (uint16_t) (StdDevSamples + 1);
+                StdDevDistanceSum = (uint32_t) (StdDevDistanceSum + _FilterData(LastTrueRange)[i]);
+                StdDevRateSum = (uint32_t) (StdDevRateSum + _FilterData(LastReturnRates)[i]);
+            }
+        }
+        if (StdDevSamples > 0) {
+            StdDevDistanceMean = (uint32_t) (StdDevDistanceSum / StdDevSamples);
+            StdDevRateMean = (uint32_t) (StdDevRateSum / StdDevSamples);
+        }
+        /* TODO optimize shorten Std dev in aisngle loop computation using sum of x2 - (sum of x)2 */
+        StdDevSamples = 0;
+        StdDevDistanceSum = 0;
+        StdDevRateSum = 0;
+        for (i = 0; (i < FILTER_NBOF_SAMPLES) && (StdDevSamples < FILTER_STDDEV_SAMPLES); i++) {
+            if (_FilterData(LastTrueRange)[i] != FILTER_INVALID_DISTANCE) {
+                StdDevSamples = (uint16_t) (StdDevSamples + 1);
+                StdDevDistanceSum = (uint32_t) (StdDevDistanceSum + (int) (_FilterData(LastTrueRange)[i] - StdDevDistanceMean) * (int) (_FilterData(LastTrueRange)[i] - StdDevDistanceMean));
+                StdDevRateSum = (uint32_t) (StdDevRateSum + (int) (_FilterData(LastReturnRates)[i] - StdDevRateMean) * (int) (_FilterData(LastReturnRates)[i] - StdDevRateMean));
+            }
+        }
+        if (StdDevSamples >= MIN_FILTER_STDDEV_SAMPLES) {
+            StdDevDistance = (uint16_t) (StdDevDistanceSum / StdDevSamples);
+            StdDevRate = (uint16_t) (StdDevRateSum / StdDevSamples);
+        } else {
+            StdDevDistance = 0;
+            StdDevRate = 0;
+        }
+
+        /* Check Return rate standard deviation */
+        if (StdDevRate < StdDevMovingTargetStdDevLimit) {
+            if (StdDevSamples < MIN_FILTER_VALID_STDDEV_SAMPLES) {
+                m_newTrueRange_mm = MaxOrInvalidDistance;
+            } else {
+                /* Check distance standard deviation */
+                if (StdDevRate < StdDevMovingTargetReturnRateLimit)
+                    StdDevLimitWithTargetMove = StdDevLimit + (((StdDevMovingTargetStdDevForReturnRateLimit - StdDevLimit) * StdDevRate) / StdDevMovingTargetReturnRateLimit);
+                else
+                    StdDevLimitWithTargetMove = StdDevMovingTargetStdDevForReturnRateLimit;
+
+                if ((StdDevDistance * StdDevHighConfidenceSNRLimit) < StdDevLimitWithTargetMove) {
+                    NoWrapAroundHighConfidenceFlag = 1;
+                } else {
+                    if (StdDevDistance < StdDevLimitWithTargetMove) {
+                        if (StdDevSamples >= MIN_FILTER_VALID_STDDEV_SAMPLES) {
+                            NoWrapAroundFlag = 1;
+                        } else {
+                            m_newTrueRange_mm = MaxOrInvalidDistance;
+                        }
+                    } else {
+                        WrapAroundFlag = 1;
+                    }
+                }
+            }
+        } else {
+            WrapAroundFlag = 1;
+        }
+    }
+
+    if (m_newTrueRange_mm == MaxOrInvalidDistance) {
+        if (_FilterData(StdFilteredReads) > 0)
+            _FilterData(StdFilteredReads) = (uint16_t) (_FilterData(StdFilteredReads) - 1);
+    } else {
+        if (WrapAroundFlag == 1) {
+            m_newTrueRange_mm = MaxOrInvalidDistance;
+            _FilterData(StdFilteredReads) = (uint16_t) (_FilterData(StdFilteredReads) + StdFilteredReadsIncrement);
+            if (_FilterData(StdFilteredReads) > StdMaxFilteredReads)
+                _FilterData(StdFilteredReads) = StdMaxFilteredReads;
+        } else {
+            if (NoWrapAroundFlag == 1) {
+                if (_FilterData(StdFilteredReads) > 0) {
+                    m_newTrueRange_mm = MaxOrInvalidDistance;
+                    if (_FilterData(StdFilteredReads) > StdFilteredReadsIncrement)
+                        _FilterData(StdFilteredReads) = (uint16_t) (_FilterData(StdFilteredReads) - StdFilteredReadsIncrement);
+                    else
+                        _FilterData(StdFilteredReads) = 0;
+                }
+            } else {
+                if (NoWrapAroundHighConfidenceFlag == 1) {
+                    _FilterData(StdFilteredReads) = 0;
+                }
+            }
+        }
+    }
+
+    return m_newTrueRange_mm;
+    done_err:
+    return -1;
+
+    #undef MaxOrInvalidDistance
+}
+
+
+int VL6180X::_filter_GetResult(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData) {
+    uint32_t m_rawRange_mm = 0;
+    int32_t  FilteredRange;
+    const uint8_t scaler = _GetUpscale(dev);
+    uint8_t u8;
+    int status;
+
+    do {
+        status = VL6180x_RdByte(dev, RESULT_RANGE_RAW, &u8);
+        if (status) {
+            VL6180x_ErrLog("RESULT_RANGE_RAW rd fail");
+            break;
+        }
+        m_rawRange_mm = u8;
+
+        FilteredRange = _filter_Start(dev, pRangeData->range_mm, (m_rawRange_mm * scaler), pRangeData->rtnRate, pRangeData->rtnAmbRate, pRangeData->errorStatus);
+        if( FilteredRange<0 ){
+            status = -1;
+            break;
+        }
+        pRangeData->FilteredData.range_mm= FilteredRange;
+        pRangeData->FilteredData.rawRange_mm = m_rawRange_mm * scaler;
+    } while (0);
+    return status;
+}
+
+#undef _FilterData
+#undef FILTER_STDDEV_SAMPLES
+#undef MIN_FILTER_STDDEV_SAMPLES
+#undef MIN_FILTER_VALID_STDDEV_SAMPLES
+#undef FILTER_INVALID_DISTANCE
+
+#endif /* VL6180x_WRAP_AROUND_FILTER_SUPPORT */
+
+#ifdef VL6180x_HAVE_RATE_DATA
+
+int VL6180X::_GetRateResult(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData) {
+    uint32_t m_rtnConvTime = 0;
+    uint32_t m_rtnSignalRate = 0;
+    uint32_t m_rtnAmbientRate = 0;
+    uint32_t m_rtnSignalCount = 0;
+    uint32_t m_rtnAmbientCount = 0;
+    uint32_t m_refConvTime = 0;
+    uint32_t cRtnSignalCountMax = 0x7FFFFFFF;
+    uint32_t cDllPeriods = 6;
+    uint32_t calcConvTime = 0;
+
+    int status;
+
+    do {
+
+        status = VL6180x_RdDWord(dev, RESULT_RANGE_RETURN_SIGNAL_COUNT, &m_rtnSignalCount);
+        if (status) {
+            VL6180x_ErrLog("RESULT_RANGE_RETURN_SIGNAL_COUNT rd fail");
+            break;
+        }
+        if (m_rtnSignalCount > cRtnSignalCountMax) {
+            m_rtnSignalCount = 0;
+        }
+
+        status = VL6180x_RdDWord(dev, RESULT_RANGE_RETURN_AMB_COUNT, &m_rtnAmbientCount);
+        if (status) {
+            VL6180x_ErrLog("RESULT_RANGE_RETURN_AMB_COUNTrd fail");
+            break;
+        }
+
+
+        status = VL6180x_RdDWord(dev, RESULT_RANGE_RETURN_CONV_TIME, &m_rtnConvTime);
+        if (status) {
+            VL6180x_ErrLog("RESULT_RANGE_RETURN_CONV_TIME rd fail");
+            break;
+        }
+
+        status = VL6180x_RdDWord(dev, RESULT_RANGE_REFERENCE_CONV_TIME, &m_refConvTime);
+        if (status) {
+            VL6180x_ErrLog("RESULT_RANGE_REFERENCE_CONV_TIME rd fail");
+            break;
+        }
+
+        pRangeData->rtnConvTime = m_rtnConvTime;
+        pRangeData->refConvTime = m_refConvTime;
+
+        calcConvTime = m_refConvTime;
+        if (m_rtnConvTime > m_refConvTime) {
+            calcConvTime = m_rtnConvTime;
+        }
+        if (calcConvTime == 0)
+            calcConvTime = 63000;
+
+        m_rtnSignalRate = (m_rtnSignalCount * 1000) / calcConvTime;
+        m_rtnAmbientRate = (m_rtnAmbientCount * cDllPeriods * 1000) / calcConvTime;
+
+        pRangeData->rtnRate = m_rtnSignalRate;
+        pRangeData->rtnAmbRate = m_rtnAmbientRate;
+
+
+    } while (0);
+    return status;
+}
+#endif /* VL6180x_HAVE_RATE_DATA */
+
+
+int VL6180X::VL6180x_DMaxSetState(VL6180xDev_t dev, int state){
+    int status;
+    LOG_FUNCTION_START("%d", state);
+#if VL6180x_HAVE_DMAX_RANGING
+    VL6180xDevDataSet(dev,DMaxEnable, state);
+    if( state ){
+        status = _DMax_InitData(dev);
+    }
+    else {
+        status = 0;
+    }
+#else
+    status =  NOT_SUPPORTED;
+#endif
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+int VL6180X::VL6180x_DMaxGetState(VL6180xDev_t dev){
+    int status;
+    LOG_FUNCTION_START("");
+#if VL6180x_HAVE_DMAX_RANGING
+    status = VL6180xDevDataGet(dev,DMaxEnable);
+#else
+    status = 0;
+#endif
+    LOG_FUNCTION_END(status);
+    return status;
+}
+
+
+#if VL6180x_HAVE_DMAX_RANGING
 
 #define _DMaxData(field) VL6180xDevDataGet(dev, DMaxData.field)
-
+/*
+ * Convert fix point  x.7 to KCpount per sec
+ */
 
 #ifndef VL6180x_PLATFORM_PROVIDE_SQRT
 
-uint32_t VL6180X::VL6180x_SqrtUint32(uint32_t num) {
+/*
+ * 32 bit integer square root with not so bad precision (integer result) and is quite fast
+ * see http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
+ */
+uint32_t VL6180x_SqrtUint32(uint32_t num) {
     uint32_t res = 0;
     uint32_t bit = 1 << 30; /* The second-to-top bit is set: 1 << 30 for 32 bits */
 
@@ -244,12 +2284,13 @@
 #endif
 
 
-void VL6180X::_DMax_OneTimeInit(VL6180xDev_t dev){
+/* DMax one time init */
+void _DMax_OneTimeInit(VL6180xDev_t dev){
     _DMaxData(ambTuningWindowFactor_K)=DEF_AMBIENT_TUNING;
 }
 
 
-uint32_t VL6180X::_DMax_RawValueAtRateKCps(VL6180xDev_t dev, int32_t rate){
+static uint32_t _DMax_RawValueAtRateKCps(VL6180xDev_t dev, int32_t rate){
     uint32_t snrLimit_K;
     int32_t DMaxSq;
     uint32_t RawDMax;
@@ -281,9 +2322,19 @@
     return RawDMax;
 }
 
-
-int VL6180X::_DMax_InitData(VL6180xDev_t dev)
-{
+/*
+ * fetch static data from register to avoid re-read
+ * precompute all intermediate constant and cliipings
+ *
+ * to be re-used/call on  changes of :
+ *  0x2A
+ *  SYSRANGE_MAX_AMBIENT_LEVEL_MULT
+ *  Dev Data XtalkComRate_KCPs
+ *  SYSRANGE_MAX_CONVERGENCE_TIME
+ *  SYSRANGE_RANGE_CHECK_ENABLES    mask RANGE_CHECK_RANGE_ENABLE_MASK
+ *  range 0xb8-0xbb (0xbb)
+ */
+int VL6180X::_DMax_InitData(VL6180xDev_t dev){
     int status, warning;
     uint8_t u8;
     uint16_t u16;
@@ -302,7 +2353,7 @@
 
     LOG_FUNCTION_START("");
     do{
-        status = VL6180x_RdByte(MyDevice, 0x02A ,&u8);
+        status = VL6180x_RdByte(dev, 0x02A ,&u8);
         if( status ){
             VL6180x_ErrLog("Reg 0x02A rd fail");
             break;
@@ -314,25 +2365,25 @@
         }
         Reg2A_KCps = Fix7_2_KCPs(u8); /* convert to KCPs */
 
-        status = VL6180x_RdByte(MyDevice, SYSRANGE_RANGE_CHECK_ENABLES, &SysRangeCheckEn);
+        status = VL6180x_RdByte(dev, SYSRANGE_RANGE_CHECK_ENABLES, &SysRangeCheckEn);
         if (status) {
             VL6180x_ErrLog("SYSRANGE_RANGE_CHECK_ENABLES rd fail ");
             break;
         }
 
-        status = VL6180x_RdByte(MyDevice, SYSRANGE_MAX_CONVERGENCE_TIME, &MaxConvTime);
+        status = VL6180x_RdByte(dev, SYSRANGE_MAX_CONVERGENCE_TIME, &MaxConvTime);
         if( status){
             VL6180x_ErrLog("SYSRANGE_MAX_CONVERGENCE_TIME rd fail ");
             break;
         }
 
-        status = VL6180x_RdDWord(MyDevice, 0x0B8, &RegB8);
+        status = VL6180x_RdDWord(dev, 0x0B8, &RegB8);
         if( status ){
             VL6180x_ErrLog("reg 0x0B8 rd fail ");
             break;
         }
 
-        status = VL6180x_RdByte(MyDevice, SYSRANGE_MAX_AMBIENT_LEVEL_MULT, &snrLimit);
+        status = VL6180x_RdByte(dev, SYSRANGE_MAX_AMBIENT_LEVEL_MULT, &snrLimit);
         if( status){
             VL6180x_ErrLog("SYSRANGE_MAX_AMBIENT_LEVEL_MULT rd fail ");
             break;
@@ -349,7 +2400,7 @@
 
         /* if xtalk range check is off omit it in snr clipping */
         if( SysRangeCheckEn&RANGE_CHECK_RANGE_ENABLE_MASK ){
-            status = VL6180x_RdWord(MyDevice, SYSRANGE_RANGE_IGNORE_THRESHOLD, &u16);
+            status = VL6180x_RdWord(dev, SYSRANGE_RANGE_IGNORE_THRESHOLD, &u16);
             if( status){
                 VL6180x_ErrLog("SYSRANGE_RANGE_IGNORE_THRESHOLD rd fail ");
                 break;
@@ -375,10 +2426,45 @@
     return status;
 }
 
-
+static int _DMax_Compute(VL6180xDev_t dev, VL6180x_RangeData_t *pRange){
+    uint32_t rtnAmbRate;
+    int32_t DMax;
+    int scaling;
+    uint16_t HwLimitAtScale;
+    static const int ROMABLE_DATA rtnAmbLowLimit_KCps=330*1000;
+
+    rtnAmbRate = pRange->rtnAmbRate;
+    if( rtnAmbRate  < rtnAmbLowLimit_KCps ){
+        DMax = _DMax_RawValueAtRateKCps( dev, rtnAmbRate);
+        scaling = _GetUpscale(dev);
+        HwLimitAtScale=UpperLimitLookUP[scaling - 1];
+
+        if( DMax > _DMaxData(ClipSnrLimit) ){
+            DMax=_DMaxData(ClipSnrLimit);
+        }
+        if( DMax > HwLimitAtScale ){
+            DMax=HwLimitAtScale;
+        }
+        pRange->DMax=DMax;
+    }
+    else{
+        pRange->DMax = 0;
+    }
+    return 0;
+}
+
+#undef _DMaxData
 #undef Fix7_2_KCPs
 
-/* Write and read functions from I2C -----------------------------------------*/
+#endif /* VL6180x_HAVE_DMAX_RANGING */
+
+
+/******************************************************************************/
+/******************************************************************************/
+
+
+
+/****************** Write and read functions from I2C *************************/
 
 int VL6180X::VL6180x_WrByte(VL6180xDev_t dev, uint16_t index, uint8_t data)
 {
@@ -396,7 +2482,7 @@
     DECL_I2C_BUFFER
     VL6180x_I2C_USER_VAR
 
-    status=VL6180x_I2CWrite(dev, index, (uint8_t *)&data,(uint8_t)4);
+    status=VL6180x_I2CWrite(MyDeviceAddress, index, (uint8_t *)&data,(uint8_t)4);
     return status;
 }
 
@@ -406,7 +2492,7 @@
     DECL_I2C_BUFFER
     int  status;
 
-	  status=VL6180x_I2CWrite(dev, index, (uint8_t *)&data,(uint8_t)6);
+	  status=VL6180x_I2CWrite(MyDeviceAddress, index, (uint8_t *)&data,(uint8_t)6);
     return status;
 }
 
@@ -431,7 +2517,7 @@
     uint8_t *buffer;
     DECL_I2C_BUFFER
 
-    status=VL6180x_I2CRead(dev, index, buffer,2);
+    status=VL6180x_I2CRead(MyDeviceAddress, index, buffer,2);
     if( !status ){
        /* VL6180x register are Big endian if cpu is be direct read direct into *data is possible */
        *data=((uint16_t)buffer[0]<<8)|(uint16_t)buffer[1];
@@ -447,7 +2533,7 @@
     DECL_I2C_BUFFER
 
     
-    status=VL6180x_I2CRead(dev, index, buffer,4);
+    status=VL6180x_I2CRead(MyDeviceAddress, index, buffer,4);
     if( !status ){
        /* VL6180x register are Big endian if cpu is be direct read direct into data is possible */
        *data=((uint32_t)buffer[0]<<24)|((uint32_t)buffer[1]<<16)|((uint32_t)buffer[2]<<8)|((uint32_t)buffer[3]);
@@ -455,14 +2541,34 @@
     return status;
 }
 
+int VL6180X::VL6180x_UpdateByte(VL6180xDev_t dev, uint16_t index, uint8_t AndData, uint8_t OrData){
+    VL6180x_I2C_USER_VAR
+    int  status;
+    uint8_t *buffer;
+    DECL_I2C_BUFFER
+
+    status=VL6180x_I2CWrite(MyDeviceAddress, index, (uint8_t *)buffer,(uint8_t)2);
+    if( !status ){
+        /* read data direct onto buffer */
+        status=VL6180x_I2CRead(dev, index, &buffer[2],1);
+        if( !status ){
+            buffer[2]=(buffer[2]&AndData)|OrData;
+            status=VL6180x_I2CWrite(dev, index, buffer, (uint8_t)3);
+        }
+    }
+
+    return status;
+}
+
 int VL6180X::VL6180x_I2CWrite(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToWrite)
 {
     int ret;
 	  int i;
     uint8_t tmp[TEMP_BUF_SIZE];
     uint16_t myRegisterAddr = RegisterAddr;
-    uint8_t *array;
+	  uint16_t WriteDeviceAddr=0;
     
+	  WriteDeviceAddr=DeviceAddr*2;
     if(NumByteToWrite >= TEMP_BUF_SIZE) return -2;
         
     /* First, send 8 bits device address and 16 bits register address in BE format. Then, send data and STOP condition */
@@ -471,16 +2577,16 @@
     
     if(NumByteToWrite>1)
     {
-    	  array=new uint8_t[NumByteToWrite];
     	  for(i=0;i<NumByteToWrite;i++)
     	  {
-    	  	 array[NumByteToWrite-1-i]=pBuffer[i];
+    	  	 tmp[NumByteToWrite+1-i]=pBuffer[i];
     	  }
     }
-    	  	 
-    memcpy(tmp+2, array, NumByteToWrite);
- 
-    ret = dev_i2c.write(DeviceAddr, (const char*)tmp, NumByteToWrite+sizeof(RegisterAddr), false);
+		else
+		{  	 
+        memcpy(tmp+2, pBuffer, NumByteToWrite);
+    }
+    ret = dev_i2c.write(WriteDeviceAddr, (const char*)tmp, NumByteToWrite+sizeof(RegisterAddr), false);
  
     if(ret) return -1;
     return 0;
@@ -491,39 +2597,40 @@
     int ret;
     uint16_t myRegisterAddr = RegisterAddr;
     uint16_t myRegisterAddrBE;
-        
+	  uint16_t ReadDeviceAddr=DeviceAddr;
+     
+    ReadDeviceAddr=DeviceAddr*2;	
     myRegisterAddrBE = *(((uint8_t*)&myRegisterAddr)+1);
     *(((uint8_t*)&myRegisterAddrBE)+1) = (uint8_t)myRegisterAddr;
     
     /* Send 8 bits device address and 16 bits register address in BE format, with no STOP condition */
-    ret = dev_i2c.write(DeviceAddr, (const char*)&myRegisterAddrBE, sizeof(RegisterAddr), true);
+    ret = dev_i2c.write(ReadDeviceAddr, (const char*)&myRegisterAddrBE, sizeof(RegisterAddr), true);
     if(!ret) {
+			  ReadDeviceAddr|=0x001;
         /* Read data, with STOP condition  */
-        ret = dev_i2c.read(DeviceAddr, (char*)pBuffer, NumByteToRead, false);
+        ret = dev_i2c.read(ReadDeviceAddr, (char*)pBuffer, NumByteToRead, false);
     }
     
     if(ret) return -1;
     return 0;
 } 
 
-/* IO read funcitons ---------------------------------------------------------*/
-
-int VL6180X::VL6180X_ReadID(uint8_t *rl_id)
+/****************** IO read funcitons *****************************************/
+
+int VL6180X::VL6180X_ReadID()
 {
-	  if(!rl_id)
-	  {
-	  	 return API_ERROR; // DA DEFINIRE IL TIPO DI ERRORE!!
-	  }
-	  return VL6180X_IO_Read(rl_id, IDENTIFICATION_MODEL_ID, 1);
+	  uint8_t rl_id;
+
+	  return VL6180X_IO_Read(&rl_id, IDENTIFICATION_MODEL_ID, 1);
 }
 
 
 int VL6180X::VL6180X_IO_Read(uint8_t *pBuffer, uint8_t RegisterAddress, uint16_t NumByteToRead)
 {
-    int lecture;
+    int status;
     
-    lecture=dev_i2c.i2c_read(pBuffer, MyDeviceAddress, RegisterAddress, NumByteToRead);
-    if(lecture!=0)
+	  status=VL6180x_I2CRead(MyDeviceAddress, RegisterAddress, pBuffer, NumByteToRead);
+    if(status!=0)
     {
     	  return INVALID_PARAMS; // DA DEFINIRE IL TIPO DI ERRORE!!
     }
@@ -531,4 +2638,6 @@
 }
 
 
+
+
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
--- a/Components/VL6180X/vl6180x_class.h	Fri Sep 18 13:19:50 2015 +0000
+++ b/Components/VL6180X/vl6180x_class.h	Fri Sep 25 12:12:51 2015 +0200
@@ -48,84 +48,84 @@
 #include "vl6180x_types.h"
 #include "vl6180x_platform.h"
 #include "vl6180x_appcfg.h"
+#include "STMPE1600_class.h"
 
 /** default device address */
 #define DEFAULT_DEVICE_ADDRESS		0x29
-/** default value ECE factor Molecular */
-#define DEF_ECE_FACTOR_M    85
-/** default value ECE factor Denominator */
-#define DEF_ECE_FACTOR_D    100
-/** default value ALS integration time */
-#define DEF_INT_PEFRIOD     100
-/** default value ALS gain */
-#define DEF_ALS_GAIN        1
-/** default value ALS scaler */
-#define DEF_ALS_SCALER      1
-/** default value for DMAX Enbale */
-#define DEF_DMAX_ENABLE     1
-/** default ambient tuning factor %x1000 */
-#define DEF_AMBIENT_TUNING  80
-
-/*
-typedef struct
-{
-		struct VL6180xDevData_t Data;
-		uint8_t I2cAddr;
-		uint8_t DevID;
-	
-    unsigned Present;
-    unsigned Ready;
-}MyVL6180xDev_t;
-*/
 
 /* Classes -------------------------------------------------------------------*/
 /** Class representing a VL6180X sensor component
  */
-class VL6180X : public RangeSensor, public LightSensor {
+class VL6180X : public RangeSensor, public LightSensor 
+{
  public:
     /** Constructor
      * @param[in] i2c device I2C to be used for communication
+     * @param[in] digital out pin/STMPE1600DigiOut pin to be used for GPIO expander communication
      * @param[in] device address, 0x29 by default 
-     */
-     VL6180X(DevI2C &i2c, uint8_t DevAddr=DEFAULT_DEVICE_ADDRESS) : RangeSensor(), LightSensor(), dev_i2c(i2c)
-     {
-        MyDeviceAddress=DevAddr;
-			  RangeLight_id=0;
-        //MyDevice.I2cAddr=DevAddr;
-        //MyDevice.DeviID=0;
-        //MyDevice.Present=0;
-        //MyDevice.Ready=0;
-        //oppure MyVL6180xDev_t MyDevice={DevAddr, 0, 0, 0};
-     }
-    
-    /** Destructor
+     * @param[in] device id, 0 by default
      */
-    /*virtual ~VL6180X() 
-    {
-        // delete[] memory allocated
-        VL6180x_SetupGPIOx(MyDevice, 0, 0, 0);	
-    } */
+
+		 
+		 VL6180X(DevI2C &i2c, DigitalOut &pin, uint8_t DevAddr=DEFAULT_DEVICE_ADDRESS) : RangeSensor(), LightSensor(), dev_i2c(i2c), gpio0(&pin)
+		 {
+		    MyDeviceAddress=DevAddr;
+        Present=0;
+        Ready=0;
+		 }  
+		 
+     VL6180X(DevI2C &i2c, STMPE1600DigiOut &pin, uint8_t DevAddr=DEFAULT_DEVICE_ADDRESS) : RangeSensor(), LightSensor(), dev_i2c(i2c), expgpio0(&pin)
+		 {
+		    MyDeviceAddress=DevAddr;
+        Present=0;
+        Ready=0;
+		 }  
+		 
     
-    // funzione che setta il paremetro DevID: SetDevID(int n)
-    // funzione che setta il parametro Present: SetPresent(int n)
-    // funzione che setta il parametro Ready: SetReady(int n)
-    // funzione che modifica lo slave address: SetI2CAddress(VL6180xDev_t MyDevice, uint8_t NewAddress))
-    
+   /** Destructor
+    */
+    //~VL6180X(){} 
+		 
+		 
+		 void VL6180x_On(void) {
+			if (gpio0) *gpio0=1;
+			else if (expgpio0) *expgpio0=1;
+		} 
+		
+    void VL6180x_Off(void) {
+			if (gpio0) *gpio0=0;
+			else if (expgpio0) *expgpio0=0;			
+		}
+		//Init sensor accende, legge id, inizializza il dispositivo e prepara per le misure (range, als)
+    //altrimenti errore e cancello dalla board		
+		
     /*** Interface Methods ***/
-    // nel momento in cui creo la classe x_nucleo_6180xa1, all'interno definisco un oggetto VL6180xDev_t MyDevice
-    // definisco una funzione Init() che chiama a sua volta la seguente funzione Init() e gli passo MyDevice
-    /*virtual*/ int Init() //par:void *init_struct
+    /*virtual*/ int Init() 
     {
-        return VL6180x_InitData(MyDevice); // a questa funzione devo passare il parametro di Init()
+        return VL6180x_InitData(MyDeviceAddress); // ritorna 0 se corretto 
     }
     
-    // nel momento in cui creo la classe x_nucleo_6180xa1, all'interno definisco un oggetto uint_t *RangeLight_id
-    // definisco una funzione ReadID() che chiama a sua volta la seguente funzione ReadID() e gli passo RangeLight_id
-    /*virtual*/ int ReadID() //par:uint8_t *rl_id
+    /*virtual*/ int ReadID() 
     {
-        return VL6180X_ReadID(&RangeLight_id); // a questa funzione devo passare il parametro di ReadID()
+        return VL6180X_ReadID(); // ritorna 0 se corretto e -2 se errore
     }
-    
+		/*
+
+		void SetPresent()
+		{
+			  MyDevice.Present=1;
+		}
+		
+		void SetDeviceID(int n)
+		{
+			  MyDevice.DevID=n;
+		}
+		
+		void SetReady()
+		{
+			  MyDevice.Ready=1;
+		}		
+	   
     /*
     virtual int GetRange(int *piData) {
         return VL6180X_GetRange(piData);
@@ -134,17 +134,73 @@
     virtual int GetLight(int *piData) {
         return VL6180X_GetLight(piData);
     }
-    */
-
-		/* Initialization functions */
-    int VL6180x_InitData(VL6180xDev_t dev);
-    /*static*/ int _DMax_InitData(VL6180xDev_t dev);
-    void _DMax_OneTimeInit(VL6180xDev_t dev);
-    /*static*/ uint32_t _DMax_RawValueAtRateKCps(VL6180xDev_t dev, int32_t rate);
-    uint32_t VL6180x_SqrtUint32(uint32_t num);
     
-    /* IO read funcitons */
-    int VL6180X_ReadID(uint8_t *rl_id);
+		/* api.c functions */
+		int VL6180x_WaitDeviceBooted(VL6180xDev_t dev);
+		int VL6180x_InitData(VL6180xDev_t dev );
+		int VL6180x_SetupGPIO1(VL6180xDev_t dev, uint8_t IntFunction, int ActiveHigh);
+		int VL6180x_Prepare(VL6180xDev_t dev);
+		int VL6180x_RangeStartContinuousMode(VL6180xDev_t dev);
+		int VL6180x_RangeStartSingleShot(VL6180xDev_t dev);
+		int VL6180x_RangeSetMaxConvergenceTime(VL6180xDev_t dev, uint8_t  MaxConTime_msec);
+		int VL6180x_RangePollMeasurement(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData);
+		int VL6180x_RangeGetMeasurementIfReady(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData);
+		int VL6180x_RangeGetMeasurement(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData);
+		int VL6180x_RangeGetResult(VL6180xDev_t dev, int32_t *pRange_mm);
+		int VL6180x_RangeConfigInterrupt(VL6180xDev_t dev, uint8_t ConfigGpioInt);
+		int VL6180x_RangeGetInterruptStatus(VL6180xDev_t dev, uint8_t *pIntStatus);
+		int VL6180x_AlsPollMeasurement(VL6180xDev_t dev, VL6180x_AlsData_t *pAlsData);
+		int VL6180x_AlsGetMeasurement(VL6180xDev_t dev, VL6180x_AlsData_t *pAlsData);
+		int VL6180x_AlsConfigInterrupt(VL6180xDev_t dev, uint8_t ConfigGpioInt);
+		int VL6180x_AlsSetIntegrationPeriod(VL6180xDev_t dev, uint16_t period_ms);
+		int VL6180x_AlsSetInterMeasurementPeriod(VL6180xDev_t dev,  uint16_t intermeasurement_period_ms);
+		int VL6180x_AlsSetAnalogueGain(VL6180xDev_t dev, uint8_t gain);
+		int VL6180x_AlsSetThresholds(VL6180xDev_t dev, uint8_t low, uint8_t high);
+		int VL6180x_AlsGetInterruptStatus(VL6180xDev_t dev, uint8_t *pIntStatus);
+		int VL6180x_StaticInit(VL6180xDev_t dev);
+		int VL6180x_RangeWaitDeviceReady(VL6180xDev_t dev, int MaxLoop );
+		int VL6180x_RangeSetInterMeasPeriod(VL6180xDev_t dev, uint32_t  InterMeasTime_msec);
+		int VL6180x_UpscaleSetScaling(VL6180xDev_t dev, uint8_t scaling);
+		int VL6180x_UpscaleGetScaling(VL6180xDev_t dev);
+		uint16_t VL6180x_GetUpperLimit(VL6180xDev_t dev);
+		int VL6180x_RangeSetThresholds(VL6180xDev_t dev, uint16_t low, uint16_t high, int SafeHold);
+		int VL6180x_RangeGetThresholds(VL6180xDev_t dev, uint16_t *low, uint16_t *high);
+		int VL6180x_RangeSetRawThresholds(VL6180xDev_t dev, uint8_t low, uint8_t high);
+		int VL6180x_RangeSetEceFactor(VL6180xDev_t dev, uint16_t  FactorM, uint16_t FactorD);
+		int VL6180x_RangeSetEceState(VL6180xDev_t dev, int enable );
+		int VL6180x_FilterSetState(VL6180xDev_t dev, int state);
+		int VL6180x_FilterGetState(VL6180xDev_t dev);
+		int VL6180x_DMaxSetState(VL6180xDev_t dev, int state);
+		int VL6180x_DMaxGetState(VL6180xDev_t dev);
+		int VL6180x_RangeSetSystemMode(VL6180xDev_t dev, uint8_t mode);
+		int8_t VL6180x_GetOffsetCalibrationData(VL6180xDev_t dev);
+		void VL6180x_SetOffsetCalibrationData(VL6180xDev_t dev, int8_t offset);
+		int VL6180x_SetXTalkCompensationRate(VL6180xDev_t dev, FixPoint97_t Rate);
+		int VL6180x_AlsWaitDeviceReady(VL6180xDev_t dev, int MaxLoop );
+		int VL6180x_AlsSetSystemMode(VL6180xDev_t dev, uint8_t mode); 
+		int VL6180x_SetGroupParamHold(VL6180xDev_t dev, int Hold);
+		int VL6180x_SetI2CAddress(VL6180xDev_t dev, uint8_t NewAddr);
+		int VL6180x_SetupGPIOx(VL6180xDev_t dev, int pin, uint8_t IntFunction, int ActiveHigh);
+		int VL6180x_SetGPIOxPolarity(VL6180xDev_t dev, int pin, int active_high);
+		int VL6180x_SetGPIOxFunctionality(VL6180xDev_t dev, int pin, uint8_t functionality);
+		int VL6180x_DisableGPIOxOut(VL6180xDev_t dev, int pin);
+		int VL6180x_GetInterruptStatus(VL6180xDev_t dev, uint8_t *status);
+		int VL6180x_ClearInterrupt(VL6180xDev_t dev, uint8_t IntClear );
+		int VL6180x_RangeStaticInit(VL6180xDev_t dev); 
+		int VL6180x_UpscaleRegInit(VL6180xDev_t dev);
+		int VL6180x_UpscaleStaticInit(VL6180xDev_t dev); 
+		int VL6180x_AlsGetLux(VL6180xDev_t dev, lux_t *pLux);
+		int _UpscaleInitPatch0(VL6180xDev_t dev); 
+		int VL6180x_RangeGetDeviceReady(VL6180xDev_t dev, int * Ready);
+		int VL6180x_RangeSetEarlyConvergenceEestimateThreshold(VL6180xDev_t dev);
+		int32_t _GetAveTotalTime(VL6180xDev_t dev); 
+		int32_t _filter_Start(VL6180xDev_t dev, uint16_t m_trueRange_mm, uint16_t m_rawRange_mm, uint32_t m_rtnSignalRate, uint32_t m_rtnAmbientRate, uint16_t errorCode);
+		int _filter_GetResult(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData);
+		int _GetRateResult(VL6180xDev_t dev, VL6180x_RangeData_t *pRangeData); 
+		int _DMax_InitData(VL6180xDev_t dev);
+		
+    /* Read functions from registers and write functions into registers  */
+    int VL6180X_ReadID();
     int VL6180X_IO_Read(uint8_t *pBuffer, uint8_t RegisterAddress, uint16_t NumByteToRead);
     
     /* Write and read functions from I2C */
@@ -154,28 +210,21 @@
     int VL6180x_RdByte(VL6180xDev_t dev, uint16_t index, uint8_t *data);
     int VL6180x_RdWord(VL6180xDev_t dev, uint16_t index, uint16_t *data);
     int VL6180x_RdDWord(VL6180xDev_t dev, uint16_t index, uint32_t *data);
+		int VL6180x_UpdateByte(VL6180xDev_t dev, uint16_t index, uint8_t AndData, uint8_t OrData);
     int VL6180x_I2CWrite(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t *pBuffer, uint16_t NumByteToWrite);
     int VL6180x_I2CRead(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t *pBuffer, uint16_t NumByteToRead);
-    
-
+		
 
 private:
-    /*** Methods ***/
-    
-    
-    
-    
     /*** Instance Variables ***/
     /* IO Device */
-    //temporaneo
-    DevI2C &dev_i2c; // creo questa variabile in x_nucleo_6180xa1.h allo stesso modo e poi passo al costruttore del MyDevice il suo indirizzo
-    //MyVL6180xDev_t MyDevice; in  sostituzione di VL6180xDev_t sotto 
+    DevI2C &dev_i2c;
+    DigitalOut *gpio0;
+    STMPE1600DigiOut *expgpio0;  
     
-    /* VL6180X Device */
-    uint8_t MyDeviceAddress;
-    //temporanei
-    VL6180xDev_t MyDevice; // creo questa variabile in x_nucleo_6180xa1.h allo stesso modo e poi passo alla funzione Init()
-    uint8_t RangeLight_id; // creo questa variabile in x_nucleo_6180xa1.h allo stesso modo e poi passo alla funzione ReadID() il suo indirizzo
+    VL6180xDev_t MyDeviceAddress;
+    unsigned Present;
+    unsigned Ready;
     
 };
 
--- a/x_nucleo_6180xa1.h	Fri Sep 18 13:19:50 2015 +0000
+++ b/x_nucleo_6180xa1.h	Fri Sep 25 12:12:51 2015 +0200
@@ -54,26 +54,37 @@
 	protected:
 	
 	public:
-		X_NUCLEO_VL6180XA1(); // verificare se necessario passare come argomento un dev_i2c
-		bool VL6180X_Init(); // controlla la presenza del sensore , chiamo Prepare() e setto ready
-		// modificare l'indirizzo i2c dei sensori (slave address)
+		X_NUCLEO_VL6180XA1(DevI2C *i2c)): dev_i2c(i2c), 
+			  vl6180x_top(new VL6180X()),
+			  vl6180x_left(new VL6180X()),
+			  vl6180x_bottom(new VL6180X()),
+			  vl6180x_right(new VL6180X()),
+			  display(new Display(i2c)),
+			  gpio_expander(i2c));			  
 		
 			
-		VL6180X *vl6180x_sensor;
+		VL6180X *vl6180x_top;
+		VL6180X *vl6180x_left;
+		VL6180X *vl6180x_bottom;
+		VL6180X *vl6180x_right;
 		Display *display;
 		GPIO_expander *gpio_expander;
 		DevI2C *dev_i2c;
 		
-		//posso creare 4 puntatori a istanze di tipo VL6180X, con i nomi della board(S1, left, bottom, right)
-		//oppure posso creare un vettore di VL6180X di dimensione 4 per contenere tutti i sensori
-		//dopo tengo traccia di quali sensori sono presenti e funzionanti 
+		X_NUCLEO_VL6180XA1 *Instance(DevI2C *i2c)
+		{
+			 if(_instance==NULL)
+			 	  _instance=new X_NUCLEO_6180XA1(i2c);
+			 else
+			    errore
+		}
 		
 	private:
-		static X_NUCLEO_IKS01A1 *instance;
+		static X_NUCLEO_6180XA1 *_instance;
 		
 }
 */
 
 
 
-#endif /* __X_NUCLEO_6180XA1_H */
\ No newline at end of file
+#endif /* __X_NUCLEO_6180XA1_H */