File content as of revision 0:c1910e04fc6c:

/**
  ******************************************************************************
  * @file    vl53l3cx.c
  * @author  IMG SW Application Team
  * @brief   This file provides the VL53L3CX ranging sensor component driver
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "vl53l3cx.h"
#include "vl53lx_platform.h"
#include "vl53lx_api.h"
#include "spi_interface.h"

/** @addtogroup BSP
  * @{
  */

/** @addtogroup Components
  * @{
  */

/** @addtogroup VL53L3CX
  * @brief     This file provides a set of functions needed to drive the
  *            VL53L3CX ranging sensor.
  * @{
  */

/** @defgroup VL53L3CX_Private_TypesDefinitions Private Types Definitions
  * @{
  */

#define V53L3CX_POLL_TIMEOUT  (0xFFFFU)
#define UNUSED(x) (void)(x)

/**
  * @}
  */
/** @defgroup VL53L3CX_Private_Variables Private Variables
  * @{
  */

VL53L3CX_RANGING_SENSOR_Drv_t VL53L3CX_RANGING_SENSOR_Driver =
{
  VL53L3CX_Init,
  VL53L3CX_DeInit,
  VL53L3CX_ReadID,
  VL53L3CX_GetCapabilities,
  VL53L3CX_ConfigProfile,
  VL53L3CX_ConfigROI,
  VL53L3CX_ConfigIT,
  VL53L3CX_GetDistance,
  VL53L3CX_Start,
  VL53L3CX_Stop,
  VL53L3CX_SetAddress,
  VL53L3CX_GetAddress,
  VL53L3CX_SetPowerMode,
  VL53L3CX_GetPowerMode
};

/**
  * @}
  */


/** @defgroup VL53L3CX_Private_Functions_Prototypes Private Functions Prototypes
  * @{
  */
static int32_t vl53l3cx_poll_for_measurement(VL53L3CX_Object_t *pObj, uint32_t Timeout);
static int32_t vl53l3cx_get_result(VL53L3CX_Object_t *pObj, VL53L3CX_Result_t *pResult);

/**
  * @}
  */

/**
  * @brief Initializes the vl53l3cx context object.
  * @param pObj    vl53l3cx context object.
  * @param pIO     BSP IO struct.
  * @retval VL53L3CX status
  */
int32_t VL53L3CX_RegisterBusIO(VL53L3CX_Object_t *pObj, VL53L3CX_IO_t *pIO)
{
  int32_t ret = 0;
 
  if (pObj == NULL)
  {
    ret = VL53L3CX_INVALID_PARAM;
  }
  else
  {
  //  pObj->IO.Init      = pIO->Init;
  //  pObj->IO.DeInit    = pIO->DeInit;
    pObj->IO.Address   = pIO->Address;
 //   pObj->IO.WriteReg  = pIO->WriteReg;
 //   pObj->IO.ReadReg   = pIO->ReadReg;
    pObj->IO.GetTick   = pIO->GetTick;

//    if (pObj->IO.Init != NULL)
 //   {
 //     ret = pObj->IO.Init();
//    }
//    else
//    {
//      ret = VL53L3CX_ERROR;
 //   }
  }



	
  return ret;
}

/**
  * @brief Initializes the vl53l3cx.
  * @param pObj    vl53l3cx context object.
  * @retval VL53L3CX status
  */
int32_t VL53L3CX_Init(VL53L3CX_Object_t *pObj)
{
  int32_t ret;

  if (pObj->IsInitialized != 0U)
  {
    ret =  VL53L3CX_ERROR;
  }
  else if (VL53LX_WaitDeviceBooted(pObj) != VL53LX_ERROR_NONE)
  {
    ret =  VL53L3CX_ERROR;
  }
  else if (VL53LX_DataInit(pObj) != VL53LX_ERROR_NONE)
  {
    ret = VL53L3CX_ERROR;
  }
  else if (VL53LX_PerformRefSpadManagement(pObj) != VL53LX_ERROR_NONE)
  {
    ret = VL53L3CX_ERROR;
  }
  else
  {
    pObj->IsRanging = 0;
    pObj->IsBlocking = 0;
    pObj->IsContinuous = 0;
    pObj->IsAmbientEnabled = 0;
    pObj->IsSignalEnabled = 0;
    pObj->IsInitialized = 1;
    ret = VL53L3CX_OK;
  }

  return ret;
}

/**
  * @brief Deinitializes the vl53l3cx.
  * @param pObj    vl53l3cx context object.
  * @retval VL53L3CX status
  */
int32_t VL53L3CX_DeInit(VL53L3CX_Object_t *pObj)
{
  int32_t ret = VL53L3CX_ERROR;

  if (pObj->IsInitialized == 1U)
  {
    /* De-initialize the vl53l3cx interface */
    if (pObj->IO.DeInit() != 0)
    {
      ret = VL53L3CX_ERROR;
    }
    else
    {
      ret = VL53L3CX_OK;
      pObj->IsInitialized = 0;
    }
  }

  return ret;
}

/**
  * @brief Read the vl53l3cx device ID.
  * @param pObj    vl53l3cx context object.
  * @param pId    Pointer to the device ID.
  * @retval VL53L3CX status
  */
int32_t VL53L3CX_ReadID(VL53L3CX_Object_t *pObj, uint32_t *pId)
{
  int32_t ret;

  if ((pObj == NULL) || (pId == NULL))
  {
    ret = VL53L3CX_INVALID_PARAM;
  }
  else
  {
	 printf("VL53L3CX_ReadID  \n"); 
    *pId = 0;
    ret = VL53LX_RdWord(pObj, VL53L3CX_ID_REG, (uint16_t *) pId);
  }

  return ret;
}

/**
  * @brief Get the vl53l3cx capabilities.
  * @param pObj    vl53l3cx context object.
  * @param pCap    Pointer to the vl53l3cx capabilities.
  * @note This function should be called after the init.
  * @retval VL53L3CX status
  */
int32_t VL53L3CX_GetCapabilities(VL53L3CX_Object_t *pObj, VL53L3CX_Capabilities_t *pCap)
{
  int32_t ret;

  if ((pObj == NULL) || (pCap == NULL))
  {
    ret = VL53L3CX_INVALID_PARAM;
  }
  else
  {
    pCap->NumberOfZones = 1;
    pCap->MaxNumberOfTargetsPerZone = VL53L3CX_NB_TARGET_PER_ZONE;
    pCap->CustomROI = 1;
    pCap->ThresholdDetection = 0;

    ret = VL53L3CX_OK;
  }

  return ret;
}

/**
  * @brief Set the ranging configuration profile.
  * @param pObj    vl53l3cx context object.
  * @param pConfig    Pointer to the new configuration profile to be applied.
  * @note for VL53L3CX the profile corresponds to the distance modes.
  * @retval VL53L3CX status
  */
int32_t VL53L3CX_ConfigProfile(VL53L3CX_Object_t *pObj, VL53L3CX_ProfileConfig_t *pConfig)
{
  int32_t ret;
  uint8_t ranging_profile;

  if ((pObj != NULL) && (pConfig != NULL))
  {
    ranging_profile = pConfig->RangingProfile;
  }
  else
  {
	  printf("VL53L3CX_ConfigProfile pObj \n");
    return VL53L3CX_INVALID_PARAM;
  }

  if ((ranging_profile != VL53L3CX_PROFILE_SHORT) &&
      (ranging_profile != VL53L3CX_PROFILE_MEDIUM) &&
      (ranging_profile != VL53L3CX_PROFILE_LONG))
  {
	  	  printf("VL53L3CX_ConfigProfile ranging_profile \n");
    ret = VL53L3CX_INVALID_PARAM;
  }
  else if (VL53LX_SetDistanceMode(pObj, ranging_profile) != VL53LX_ERROR_NONE)
  {
    ret = VL53L3CX_ERROR;
  }
  else if (VL53LX_SetMeasurementTimingBudgetMicroSeconds(
             pObj, (1000U * pConfig->TimingBudget)) != VL53LX_ERROR_NONE)
  {
    ret = VL53L3CX_ERROR;
  }
  else
  {
    pObj->IsAmbientEnabled = (pConfig->EnableAmbient == 0U) ? 0U : 1U;
    pObj->IsSignalEnabled = (pConfig->EnableSignal == 0U) ? 0U : 1U;	  
	pObj->EnableInterrupt = pConfig->EnableInterrupt;	  
	pObj->pin_gpio1 = pConfig->pin_gpio1;
	pObj->Interrupt_Func = pConfig->Interrupt_Func;	
	
	delete_interrupt_measure_detection_irq(); // delete any existing interrupt
	if ( pObj->EnableInterrupt == 1)	
	     create_interrupt_measure_detection_irq(pObj->pin_gpio1);

    ret = VL53L3CX_OK;
  }
	  	  printf("VL53L3CX_ConfigProfile %d \n",ret);
  return ret;
}

/**
  * @brief Configure the Region of Interest of the vl53l3cx.
  * @param pObj    vl53l3cx context object.
  * @param pROIConfig    Pointer to the ROI configuration struct.
  * @note Minimal ROI size is 4x4 spads.
  * @retval VL53L3CX status
  */
int32_t VL53L3CX_ConfigROI(VL53L3CX_Object_t *pObj, VL53L3CX_ROIConfig_t *pROIConfig)
{
  int32_t ret;
  VL53LX_UserRoi_t roi_settings;

  if ((pObj == NULL) || (pROIConfig == NULL))
  {
    ret = VL53L3CX_INVALID_PARAM;
  }
  else
  {
    roi_settings.BotRightX = pROIConfig->BotRightX;
    roi_settings.BotRightY = pROIConfig->BotRightY;
    roi_settings.TopLeftX = pROIConfig->TopLeftX;
    roi_settings.TopLeftY = pROIConfig->TopLeftY;

    if (VL53LX_SetUserROI(pObj, &roi_settings) != VL53LX_ERROR_NONE)
    {
      ret = VL53L3CX_ERROR;
    }
    else
    {
      ret = VL53L3CX_OK;
    }
  }

  return ret;
}

/**
  * @brief Configure the IT event generation parameters.
  * @param pObj    vl53l3cx context object.
  * @param pITConfig    Pointer to the IT configuration struct.
  * @note The threshold modes can be used only if supported by the device (check the capabilities)
  * @warning This device does not support this feature.
  * @retval VL53L3CX status
  */
int32_t VL53L3CX_ConfigIT(VL53L3CX_Object_t *pObj, VL53L3CX_ITConfig_t *pITConfig)
{
  UNUSED(pObj);
  UNUSED(pITConfig);
  return VL53L3CX_NOT_IMPLEMENTED;
}

/**
  * @brief Get the last distance measurement information.
  * @param pObj    vl53l3cx context object.
  * @param pResult    Pointer to the result struct.
  * @retval VL53L3CX status
  */
int32_t VL53L3CX_GetDistance(VL53L3CX_Object_t *pObj, VL53L3CX_Result_t *pResult)
{
  int32_t ret;

  if ((pObj == NULL) || (pResult == NULL))
  {
    ret = VL53L3CX_INVALID_PARAM;
  }
  else if (pObj->IsRanging == 0U)
  {
    ret = VL53L3CX_ERROR;
  }
  else
  {
    if (pObj->IsBlocking == 1U)
    {
      ret = vl53l3cx_poll_for_measurement(pObj, V53L3CX_POLL_TIMEOUT);
    }
    else
    {
      ret = vl53l3cx_poll_for_measurement(pObj, 0U);
    }
  }

  /* a new measure is available if no error is returned by the poll function */
  if (ret == VL53L3CX_OK)
  {
    /* retrieve measurements and fill result structure */
    if (vl53l3cx_get_result(pObj, pResult) != VL53L3CX_OK)
    {
      ret = VL53L3CX_ERROR;
    }
    else if (pObj->IsContinuous == 1U)
    {
      /* trigger new measurement if device configured in continuous mode */
      ret = (int32_t)VL53LX_ClearInterruptAndStartMeasurement(pObj);
    }
    else
    {
      ret = VL53L3CX_OK;
    }
  }
  return ret;
}

/**
  * @brief Start ranging.
  * @param pObj    vl53l3cx context object.
  * @param Mode        The desired ranging mode.
  * @retval VL53L3CX status
  */
int32_t VL53L3CX_Start(VL53L3CX_Object_t *pObj, uint32_t Mode)
{
  int32_t ret;

  if (pObj == NULL)
  {
	  printf("VL53L3CX_Start NULL \n");
    ret = VL53L3CX_INVALID_PARAM;
  }
  else if (pObj->IsRanging == 1U)
  {
	  	  printf("VL53L3CX_Start IsRanging \n");
    ret = VL53L3CX_ERROR;
  }
  else if (VL53LX_StartMeasurement(pObj) == VL53LX_ERROR_NONE)
  {
    pObj->IsRanging = 1;
    ret = VL53L3CX_OK;

    switch (Mode)
    {
    case VL53L3CX_MODE_BLOCKING_CONTINUOUS:
      pObj->IsContinuous = 1U;
      pObj->IsBlocking = 1U;
      break;

    case VL53L3CX_MODE_BLOCKING_ONESHOT:
      pObj->IsContinuous = 0U;
      pObj->IsBlocking = 1U;
      break;

    case VL53L3CX_MODE_ASYNC_CONTINUOUS:
      pObj->IsContinuous = 1U;
      pObj->IsBlocking = 0U;
      break;

    case VL53L3CX_MODE_ASYNC_ONESHOT:
      pObj->IsContinuous = 0U;
      pObj->IsBlocking = 0U;
      break;

    default:
		  	  printf("VL53L3CX_Start default \n");
      pObj->IsRanging = 0U;
      ret = VL53L3CX_INVALID_PARAM;
      break;
    }
  }
  else
  {
	  		  	  printf("VL53L3CX_Start end err \n");
    ret = VL53L3CX_ERROR;
  }
  return ret;
}

/**
  * @brief Stop ranging.
  * @param pObj    vl53l3cx context object.
  * @retval VL53L3CX status
  */
int32_t VL53L3CX_Stop(VL53L3CX_Object_t *pObj)
{
  int32_t ret;

  if (pObj == NULL)
  {
    ret = VL53L3CX_INVALID_PARAM;
  }
  else if (pObj->IsRanging == 0U)
  {
	  /* ranging not started */
	  ret = VL53L3CX_ERROR;
  }
  else if (VL53LX_StopMeasurement(pObj) == VL53LX_ERROR_NONE)
  {
    pObj->IsRanging = 0U;
    ret = VL53L3CX_OK;
  }
  else
  {
    ret = VL53L3CX_ERROR;
  }

  return ret;
}

/**
  * @brief Set the I2C address of the device.
  * @param pObj    vl53l3cx context object.
  * @param Address     New I2C address.
  * @retval VL53L3CX status
  */
int32_t VL53L3CX_SetAddress(VL53L3CX_Object_t *pObj, uint32_t Address)
{
  int32_t ret;

  if (pObj == NULL)
  {
    ret = VL53L3CX_INVALID_PARAM;
  }
  else if (VL53LX_SetDeviceAddress(pObj, (uint8_t)Address) != VL53LX_ERROR_NONE)
  {
    ret = VL53L3CX_ERROR;
  }
  else
  {
    pObj->IO.Address = (uint16_t) Address;
    ret = VL53L3CX_OK;
  }

  return ret;
}

/**
  * @brief Get the current I2C address.
  * @param pObj    vl53l3cx context object.
  * @param pAddress     Pointer to the current I2C address.
  * @retval VL53L3CX status
  */
int32_t VL53L3CX_GetAddress(VL53L3CX_Object_t *pObj, uint32_t *pAddress)
{
  int32_t ret;

  if ((pObj == NULL) || (pAddress == NULL))
  {
    ret = VL53L3CX_INVALID_PARAM;
  }
  else
  {
    *pAddress = pObj->IO.Address;
    ret = VL53L3CX_OK;
  }

  return ret;
}

/**
  * @brief Set the power mode.
  * @param pObj    vl53l3cx context object.
  * @param PowerMode    New power mode to be entered.
  * @note Not implemented for this device
  * @retval VL53L3CX status
  */
int32_t VL53L3CX_SetPowerMode(VL53L3CX_Object_t *pObj, uint32_t PowerMode)
{
  UNUSED(pObj);
  UNUSED(PowerMode);
  return VL53L3CX_NOT_IMPLEMENTED;
}

/**
  * @brief Get the power mode.
  * @param pObj    vl53l3cx context object.
  * @param pPowerMode    Pointer to the current power mode.
  * @note Not implemented for this device
  * @retval VL53L3CX status
  */
int32_t VL53L3CX_GetPowerMode(VL53L3CX_Object_t *pObj, uint32_t *pPowerMode)
{
  UNUSED(pObj);
  UNUSED(pPowerMode);
  return VL53L3CX_NOT_IMPLEMENTED;
}

/**
  * @brief Perform an offset calibration.
  * @param pObj    vl53l3cx context object.
  * @param CalDistance    Calibration distance in mm.
  * @retval VL53L3CX status
  */
int32_t VL53L3CX_OffsetCalibration(VL53L3CX_Object_t *pObj, uint32_t CalDistance)
{
  int32_t ret;

  VL53LX_OffsetCorrectionModes Mode = VL53LX_OFFSETCORRECTIONMODE_STANDARD;

  if (VL53LX_SetOffsetCorrectionMode(pObj, Mode) != VL53LX_ERROR_NONE)
  {
    ret = VL53L3CX_ERROR;
  }
  else if (VL53LX_PerformOffsetSimpleCalibration(pObj, (int32_t)CalDistance) != VL53LX_ERROR_NONE)
  {
    ret = VL53L3CX_ERROR;
  }
  else
  {
    ret = VL53L3CX_OK;
  }

  return ret;
}

/**
  * @brief Perform a xtalk calibration.
  * @param pObj    vl53l3cx context object.
  * @retval VL53L3CX status
  */
int32_t VL53L3CX_XTalkCalibration(VL53L3CX_Object_t *pObj)
{
  int32_t ret;

  if (VL53LX_PerformXTalkCalibration(pObj) != VL53LX_ERROR_NONE)
  {
    ret = VL53L3CX_ERROR;
  }
  else
  {
    ret = VL53L3CX_OK;
  }

  return ret;
}

/** @defgroup VL53L3CX_Private_Functions Private Functions
  * @{
  */

static int32_t vl53l3cx_poll_for_measurement(VL53L3CX_Object_t *pObj, uint32_t Timeout)
{
  uint32_t TickStart;
  uint8_t NewDataReady = 0;

  if (pObj == NULL)
  {
    return VL53L3CX_INVALID_PARAM;
  }


  TickStart = pObj->IO.GetTick();

  do
  {
    (void)VL53LX_GetMeasurementDataReady(pObj, &NewDataReady);

    if (NewDataReady == 1U)
    {
      return VL53L3CX_OK;
    }
  } while ((pObj->IO.GetTick() - TickStart) < Timeout);
  return VL53L3CX_TIMEOUT;
}

static int32_t vl53l3cx_get_result(VL53L3CX_Object_t *pObj, VL53L3CX_Result_t *pResult)
{
  int32_t ret;
  uint8_t i, j;
  uint16_t spad_count; /* number of active spads for the current measurement */
  float_t ambient_temp, signal_temp; /* temporary variables used for computation */
  VL53LX_MultiRangingData_t data;

  if ((pObj == NULL) || (pResult == NULL))
  {
    ret = VL53L3CX_INVALID_PARAM;
  }
  else if (VL53LX_GetMultiRangingData(pObj, &data) != VL53LX_ERROR_NONE)
  {
    ret = VL53L3CX_ERROR;
  }
  else
  {
    for (i = 0; i < VL53L3CX_MAX_NB_ZONES; i++)
    {
      pResult->NumberOfZones = VL53L3CX_MAX_NB_ZONES;
      /* number of detected targets by the device */
      pResult->ZoneResult[i].NumberOfTargets = data.NumberOfObjectsFound;
		  
      for (j = 0; j < data.NumberOfObjectsFound; j++)
      {
        /* clip the value if negative */
        if (data.RangeData[j].RangeMilliMeter < 0)
        {
          pResult->ZoneResult[i].Distance[j] = 0U;
        }
        else
        {
          pResult->ZoneResult[i].Distance[j] = (uint32_t)data.RangeData[j].RangeMilliMeter;
        }
        
        /* compute and return Ambient value if ambient rate output is enabled :
         * - convert value from FixPoint1616 to Mcps by dividing by 65536
         * - convert value from Mcps to Kcps by multiplying it by 1000
         * - obtain number of active spads by dividing EffectiveSpadRtnCount by 256
         * - convert ambient value from Kcps to Kcps/spad diving by the number of active spads  
         */
        if (pObj->IsAmbientEnabled == 1U)
        {
          ambient_temp = (data.RangeData[j].AmbientRateRtnMegaCps / 65536.0f) * 1000.0f;
          spad_count = data.EffectiveSpadRtnCount >> 8;
          pResult->ZoneResult[i].Ambient[j] = ambient_temp / (float_t)spad_count;
        }
        else
        {
          pResult->ZoneResult[i].Ambient[j] = 0.0f;
        }

        /* compute and return Ambient value if signal rate output is enabled :
         * - convert value from FixPoint1616 to Mcps by dividing by 65536
         * - convert value from Mcps to Kcps by multiplying it by 1000
         * - obtain number of active spads by dividing EffectiveSpadRtnCount by 256
         * - convert ambient value from Kcps to Kcps/spad diving by the number of active spads  
         */
        if (pObj->IsSignalEnabled == 1U)
        {
          signal_temp = (data.RangeData[j].SignalRateRtnMegaCps / 65536.0f) * 1000.0f;
          spad_count = data.EffectiveSpadRtnCount >> 8;
          pResult->ZoneResult[i].Signal[j] = signal_temp / (float_t)spad_count;
        }
        else
        {
          pResult->ZoneResult[i].Signal[j] = 0.0f;
        }

        pResult->ZoneResult[i].Status[j] = data.RangeData[j].RangeStatus;
      }
    }
    
	
    ret = VL53L3CX_OK;
  }

  return ret;
}

/**
  * @}
  */

/**
  * @}
  */

/**
  * @}
  */

/**
  * @}
  */