File content as of revision 60:972b4898a007:

/**
 ******************************************************************************
 * @file    VL6180X.cpp
 * @author  AST / EST
 * @version V0.0.1
 * @date    14-April-2015
 * @brief   Implementation file for the HTS221 driver class
 ******************************************************************************
 * @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.
 *
 ******************************************************************************
*/

//------- Feburary 1st, 2018 by JH1PJL / K.Arai --------------------------------
// Change X_NUCLEO_6180XA1 library to 6180XA1_simplest library
//    modified -> all related files
//------------------------------------------------------------------------------

/* Includes */
#include "VL6180X.h"

#define VL6180X_DEV_DATA_ATTR
#define ROMABLE_DATA

//-------- NOT USE FOLLOING FUNCTIONS -------------
//void OnErrLog(void);
#define LOG_FUNCTION_START(...) (void)0
#define LOG_FUNCTION_END(...) (void)0
#define LOG_FUNCTION_END_FMT(...) (void)0
#define VL6180X_ErrLog(... ) //OnErrLog() //(void)0

#define VL6180XDevDataGet(dev, field) (dev->Data.field)
#define VL6180XDevDataSet(dev, field, data) (dev->Data.field)=(data)

/** @defgroup api_init Init functions
 *  @brief    API init functions
 *  @ingroup api_hl
 *  @{
 */


/****************** define for i2c configuration *******************************/
#define TEMP_BUF_SIZE   32

/******************************************************************************/
/******************************* 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

/** 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

#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)

/**
 * 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_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

#define Fix7_2_KCPs(x) ((((uint32_t)(x))*1000)>>7)

int VL6180X::VL6180X_WaitDeviceBooted(VL6180XDev_t dev)
{
    uint8_t FreshOutReset=0;
    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;
    uint32_t CalValue;
    uint16_t u16;
    uint32_t XTalkCompRate_KCps;

    LOG_FUNCTION_START("");

    VL6180XDevDataSet(dev, EceFactorM , DEF_ECE_FACTOR_M);
    VL6180XDevDataSet(dev, EceFactorD , DEF_ECE_FACTOR_D);

#ifdef VL6180X_HAVE_UPSCALE_DATA
    VL6180XDevDataSet(dev, UpscaleFactor ,  DEF_UPSCALE);
#endif

#ifdef VL6180X_HAVE_ALS_DATA
    VL6180XDevDataSet(dev, IntegrationPeriod, DEF_INT_PEFRIOD);
    VL6180XDevDataSet(dev, AlsGainCode, DEF_ALS_GAIN);
    VL6180XDevDataSet(dev, AlsScaler, DEF_ALS_SCALER);
#endif
    do {

        /* backup offset initial value from nvm these must be done prior any over call that use 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( dev, SYSRANGE_RANGE_IGNORE_THRESHOLD, &CalValue);
        if( status ) {
            VL6180X_ErrLog("Part2PartAmbNVM rd fail");
            break;
        }
        if( (CalValue&0xFFFF0000) == 0 ) {
            CalValue=0x00CE03F8;
        }
        VL6180XDevDataSet(dev, Part2PartAmbNVM, CalValue);

        status = VL6180X_RdWord(dev, SYSRANGE_CROSSTALK_COMPENSATION_RATE ,&u16);
        if( status) {
            VL6180X_ErrLog("SYSRANGE_CROSSTALK_COMPENSATION_RATE rd fail ");
            break;
        }
        XTalkCompRate_KCps = Fix7_2_KCPs(u16);
        VL6180XDevDataSet(dev, XTalkCompRate_KCps , XTalkCompRate_KCps );
        /* Read or wait for fresh out of reset  */
        status = VL6180X_RdByte(dev,SYSTEM_FRESH_OUT_OF_RESET, &FreshOutReset);
        if( status )  {
            VL6180X_ErrLog("SYSTEM_FRESH_OUT_OF_RESET rd fail");
            break;
        }
        if( FreshOutReset!= 1 || dmax_status )
            status = CALIBRATION_WARNING;

    } while(0);

    LOG_FUNCTION_END(status);
    return status;
}

int VL6180X::VL6180X_StaticInit(VL6180XDev_t dev)
{
    int status=0, init_status;
    LOG_FUNCTION_START("");

    if( _GetUpscale(dev) == 1 )
        init_status=VL6180X_RangeStaticInit(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;
}
#if 1
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;

}
#endif
int VL6180X::VL6180X_Prepare(VL6180XDev_t dev)
{
    int status;
    LOG_FUNCTION_START("");

    do {
        status=VL6180X_StaticInit(dev);
        if( status<0) 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, 0xFFFF);
        if( status ) break;
#endif
    } 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_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_AlsSetThresholds(VL6180XDev_t dev, uint16_t low, uint16_t high)
{
    int status;

    LOG_FUNCTION_START("%d %d", (int )low, (int)high);

    status = VL6180X_WrWord(dev, SYSALS_THRESH_LOW, low);
    if(!status ) {
        status = VL6180X_WrWord(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_RangeGetResult(VL6180XDev_t dev, uint32_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_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, SYSTEM_MODE_GPIO1, 0x10); /* Enables polling for New Sample ready when measurement completes */
    VL6180X_WrByte( dev, READOUT_AVERAGING_SAMPLE_PERIOD, 0x30); /* Set the averaging sample period (compromise between lower noise and increased execution time) */
    VL6180X_WrByte( dev, SYSALS_ANALOGUE_GAIN, 0x46); /* Sets the light and dark gain (upper nibble). Dark gain should not be changed.*/
    VL6180X_WrByte( dev, SYSRANGE_VHV_REPEAT_RATE, 0xFF); /* sets the # of range measurements after which auto calibration of system is performed */
    VL6180X_WrByte( dev, SYSALS_INTEGRATION_PERIOD, 0x63); /* Set ALS integration time to 100ms */
    VL6180X_WrByte( dev, SYSRANGE_VHV_RECALIBRATE, 0x01); /* perform a single temperature calibration of the ranging sensor */

    /* Optional: Public registers - See data sheet for more detail */
    VL6180X_WrByte( dev, SYSRANGE_INTERMEASUREMENT_PERIOD, 0x09); /* Set default ranging inter-measurement period to 100ms */
    VL6180X_WrByte( dev, SYSALS_INTERMEASUREMENT_PERIOD, 0x31); /* Set default ALS inter-measurement period to 500ms */
    VL6180X_WrByte( dev, SYSTEM_INTERRUPT_CONFIG_GPIO, 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

#else
#define VL6180X_UpscaleRegInit(...) -1
#endif

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;
        }
    } while(0);
    LOG_FUNCTION_END(status);
    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_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;
}


#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 */

/******************************************************************************/
/******************************************************************************/

/****************** Write and read functions from I2C *************************/

int VL6180X::VL6180X_WrByte(VL6180XDev_t dev, uint16_t index, uint8_t data)
{
    int  status;

    status=VL6180X_I2CWrite(dev->I2cAddr, index, &data,(uint8_t)1);
    return status;
}

int VL6180X::VL6180X_WrWord(VL6180XDev_t dev, uint16_t index, uint16_t data)
{
    int  status;

    status=VL6180X_I2CWrite(dev->I2cAddr, index, (uint8_t *)&data,(uint8_t)2);
    return status;
}

int VL6180X::VL6180X_WrDWord(VL6180XDev_t dev, uint16_t index, uint32_t data)
{
    int  status;

    status=VL6180X_I2CWrite(dev->I2cAddr, index, (uint8_t *)&data,(uint8_t)4);
    return status;
}

int VL6180X::VL6180X_RdByte(VL6180XDev_t dev, uint16_t index, uint8_t *data)
{
    int  status;

    uint8_t buffer=0;
    status=VL6180X_I2CRead(dev->I2cAddr, index, &buffer,1);
    if(!status) {
        *data=buffer;
    }
    return status;
}

int VL6180X::VL6180X_RdWord(VL6180XDev_t dev, uint16_t index, uint16_t *data)
{
    int  status;

    uint8_t buffer[2];
    buffer[0]=buffer[1]=0;
    status=VL6180X_I2CRead(dev->I2cAddr, index, buffer, 2);
    if(!status) {
        memcpy(data, buffer, 2);
    }
    return status;
}

int VL6180X::VL6180X_RdDWord(VL6180XDev_t dev, uint16_t index, uint32_t *data)
{
    int status;
    uint8_t buffer[4];
    buffer[0]=buffer[1]=buffer[2]=buffer[3]=0;
    status=VL6180X_I2CRead(dev->I2cAddr, index, buffer,4);
    if(!status) {
        memcpy(data, buffer, 4);
    }
    return status;
}

int VL6180X::VL6180X_UpdateByte(VL6180XDev_t dev, uint16_t index, uint8_t AndData, uint8_t OrData)
{
    int  status;
    uint8_t buffer=0;

    status=VL6180X_I2CWrite(dev->I2cAddr, index, (uint8_t *)&buffer,(uint8_t)0);
    if(!status) {
        /* read data direct onto buffer */
        status=VL6180X_I2CRead(dev->I2cAddr, index, &buffer,1);
        if(!status) {
            buffer=(buffer & AndData)|OrData;
            status=VL6180X_I2CWrite(dev->I2cAddr, index, &buffer, (uint8_t)1);
        }
    }
    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;
    uint16_t WriteDeviceAddr=0;

    /* First, prepare 8 bits device address in 7bits i2ci format */
    WriteDeviceAddr=DeviceAddr*2;
    if(NumByteToWrite >= TEMP_BUF_SIZE) return -2;

    /* then prepare 16 bits register address in BE format. Then, send data and STOP condition */
    tmp[0] = *(((uint8_t*)&myRegisterAddr)+1);
    tmp[1] = (uint8_t)RegisterAddr;

    if(NumByteToWrite>1) { /* swap data endianess */
        for(i=0; i<NumByteToWrite; i++) {
            tmp[NumByteToWrite+sizeof(RegisterAddr)-1-i]=pBuffer[i];
        }
    } else {
        memcpy(tmp+sizeof(RegisterAddr), pBuffer, NumByteToWrite);
    }
    ret = _i2c.write(WriteDeviceAddr, (const char*)tmp, NumByteToWrite+sizeof(RegisterAddr), false);
    if(ret)
        return -1;
    return 0;
}

int VL6180X::VL6180X_I2CRead(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToRead)
{
    int ret,i;
    uint8_t tmp[TEMP_BUF_SIZE];
    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 = _i2c.write(ReadDeviceAddr, (const char*)&myRegisterAddrBE, sizeof(RegisterAddr), true);
    if(!ret) {
        ReadDeviceAddr|=0x001;
        /* Read data, with STOP condition  */
        ret = _i2c.read(ReadDeviceAddr, (char*)tmp, NumByteToRead, false);
    }
    if(ret)
        return -1;

    if(NumByteToRead>1) { /* swap data endianess */
        for(i=0; i<NumByteToRead; i++) {
            pBuffer[i] = tmp[NumByteToRead-1-i];
        }
    } else {
        memcpy(pBuffer, tmp, NumByteToRead);
    }
    return 0;
}

/******************************************************************************/
int VL6180X::read_id(uint8_t *id)
{
    return VL6180X_RdByte(_device, IDENTIFICATION_MODEL_ID, id);
}

int VL6180X::start_measurement(operating_mode_t operating_mode)
{

    switch (operating_mode) {
        case(range_single_shot_polling):
            return VL6180X_WrByte(_device, SYSRANGE_START, MODE_START_STOP|MODE_SINGLESHOT);
        case(als_single_shot_polling):
            return VL6180X_WrByte(_device, SYSALS_START, MODE_START_STOP|MODE_SINGLESHOT);
        default:
            return INVALID_PARAMS;
    }
}

int VL6180X::stop_measurement(operating_mode_t operating_mode)
{
    switch(operating_mode) {
        case(range_single_shot_polling):
            return VL6180X_RangeSetSystemMode(_device, MODE_SINGLESHOT);
        case(als_single_shot_polling):
            return VL6180X_AlsSetSystemMode(_device, MODE_SINGLESHOT);
        default:
            return INVALID_PARAMS;
    }
}

int VL6180X::IsPresent()
{
    int status;
    uint8_t id;

    status=read_id(&id);
    if(status)
        VL6180X_ErrLog("Failed to read ID device. _device not present!\n\r");
    return status;
}

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/