File content as of revision 1:dae4cb24beec:

/**
  ******************************************************************************
  * @file    custom_ranging_sensor.c
  * @author  IMG SW Application Team
  * @brief   This file provides BSP Ranging Sensors interface for custom boards
  ******************************************************************************
  * @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 "custom_ranging_sensor.h"
#include "vl53lx_platform.h"
#include "spi_interface.h"
#include "PinNames.h"

void wait_ms(int ms);

void *CUSTOM_RANGING_CompObj[CUSTOM_RANGING_INSTANCES_NBR] = {0};
static RANGING_SENSOR_Drv_t *CUSTOM_RANGING_Drv[CUSTOM_RANGING_INSTANCES_NBR];
static RANGING_SENSOR_Capabilities_t RANGING_SENSOR_Cap[CUSTOM_RANGING_INSTANCES_NBR];

#if (USE_CUSTOM_RANGING_VL53L3CX == 1U)
static int32_t VL53L3CX_Probe(uint32_t Instance);
#endif

/**
  * @brief Initializes the ranging sensor.
  * @param Instance    Ranging sensor instance.
  * @retval BSP status
  */
int32_t CUSTOM_RANGING_SENSOR_Init(uint32_t Instance)
{
  int32_t ret;
  
  printf("CUSTOM_RANGING_SENSOR_Init %d  >= %d \n",Instance,CUSTOM_RANGING_INSTANCES_NBR);

  if (Instance >= CUSTOM_RANGING_INSTANCES_NBR)
  {
    ret = BSP_ERROR_WRONG_PARAM;
  }
  else
  {
    CUSTOM_RANGING_SENSOR_SetPowerMode(Instance, CUSTOM_RANGING_POWERMODE_OFF);
    CUSTOM_RANGING_SENSOR_SetPowerMode(Instance, CUSTOM_RANGING_POWERMODE_ON);

    switch (Instance)
    {
#if (USE_CUSTOM_RANGING_VL53L3CX == 1U)
      case CUSTOM_VL53L3CX:
          if (VL53L3CX_Probe(Instance) != BSP_ERROR_NONE)
          {
            ret = BSP_ERROR_NO_INIT;
          }
          else
          {
            ret = BSP_ERROR_NONE;
          }
          break;
#endif
      default:
          ret = BSP_ERROR_UNKNOWN_COMPONENT;
          break;
    }
  }

  return ret;
}

/**
  * @brief Deinitializes the ranging sensor.
  * @param Instance    Ranging sensor instance.
  * @retval BSP status
  */
int32_t CUSTOM_RANGING_SENSOR_DeInit(uint32_t Instance)
{
  int32_t ret;

  if(Instance >= CUSTOM_RANGING_INSTANCES_NBR)
  {
    ret = BSP_ERROR_WRONG_PARAM;
  }
  else if (CUSTOM_RANGING_Drv[Instance]->DeInit(CUSTOM_RANGING_CompObj[Instance]) < 0)
  {
    ret = BSP_ERROR_COMPONENT_FAILURE;
  }
  else
  {
    ret = BSP_ERROR_NONE;
  }

  return ret;
}

/**
  * @brief Read the ranging sensor device ID.
  * @param Instance    Ranging sensor instance.
  * @param pId    Pointer to the device ID.
  * @retval BSP status
  */
int32_t CUSTOM_RANGING_SENSOR_ReadID(uint32_t Instance, uint32_t *pId)
{
  int32_t ret;

  if (Instance >= CUSTOM_RANGING_INSTANCES_NBR)
  {
    ret = BSP_ERROR_WRONG_PARAM;
  }
  else if (CUSTOM_RANGING_Drv[Instance]->ReadID(CUSTOM_RANGING_CompObj[Instance], pId) < 0)
  {
    ret = BSP_ERROR_COMPONENT_FAILURE;
  }
  else
  {
    ret = BSP_ERROR_NONE;
  }

  return ret;
}

/**
  * @brief Get the ranging sensor capabilities.
  * @param Instance    Ranging sensor instance.
  * @param pCapabilities    Pointer to the ranging sensor capabilities.
  * @note This function should be called after the init.
  * @retval BSP status
  */
int32_t CUSTOM_RANGING_SENSOR_GetCapabilities(uint32_t Instance, RANGING_SENSOR_Capabilities_t *pCapabilities)
{
    int32_t ret;

    if (Instance >= CUSTOM_RANGING_INSTANCES_NBR)
    {
      ret = BSP_ERROR_WRONG_PARAM;
    }
    else if (CUSTOM_RANGING_Drv[Instance]->GetCapabilities(CUSTOM_RANGING_CompObj[Instance], pCapabilities) < 0)
    {
      ret = BSP_ERROR_COMPONENT_FAILURE;
    }
    else
    {
      ret = BSP_ERROR_NONE;
    }

    return ret;
}

/**
  * @brief Set the ranging configuration profile.
  * @param Instance    Ranging sensor instance.
  * @param pConfig    Pointer to the new configuration profile to be applied.
  * @retval BSP status
  */
int32_t CUSTOM_RANGING_SENSOR_ConfigProfile(uint32_t Instance, RANGING_SENSOR_ProfileConfig_t *pConfig)
{
  int32_t ret;

  if (Instance >= CUSTOM_RANGING_INSTANCES_NBR)
  {
    ret = BSP_ERROR_WRONG_PARAM;
  }
  else if (CUSTOM_RANGING_Drv[Instance]->ConfigProfile(CUSTOM_RANGING_CompObj[Instance], pConfig) < 0)
  {
    ret = BSP_ERROR_COMPONENT_FAILURE;
  }
  else
  {
    ret = BSP_ERROR_NONE;
  }

  return ret;
}

/**
  * @brief Configure the Region of Interest of the ranging sensor.
  * @param Instance    Ranging sensor instance.
  * @param pConfig    Pointer to the ROI configuration struct.
  * @note Should be called only if the device supports CustomROI.
  * @retval BSP status
  */
int32_t CUSTOM_RANGING_SENSOR_ConfigROI(uint32_t Instance, RANGING_SENSOR_ROIConfig_t *pConfig)
{
  int32_t ret;

  if (Instance >= CUSTOM_RANGING_INSTANCES_NBR)
  {
    ret = BSP_ERROR_WRONG_PARAM;
  }
  else if (RANGING_SENSOR_Cap[Instance].CustomROI == 0)
  {
    ret = BSP_ERROR_FEATURE_NOT_SUPPORTED;
  }
  else if (CUSTOM_RANGING_Drv[Instance]->ConfigROI(CUSTOM_RANGING_CompObj[Instance], pConfig) < 0)
  {
    ret = BSP_ERROR_COMPONENT_FAILURE;
  }
  else
  {
    ret = BSP_ERROR_NONE;
  }

  return ret;
}

/**
  * @brief Configure the IT event generation parameters.
  * @param Instance    Ranging sensor instance.
  * @param pConfig    Pointer to the IT configuration struct.
  * @note The threshold modes can be used only if supported by the device (check the capabilities)
  * @retval BSP status
  */
int32_t CUSTOM_RANGING_SENSOR_ConfigIT(uint32_t Instance, RANGING_SENSOR_ITConfig_t *pConfig)
{
  int32_t ret;

  if (Instance >= CUSTOM_RANGING_INSTANCES_NBR)
  {
    ret = BSP_ERROR_WRONG_PARAM;
  }
  else if (CUSTOM_RANGING_Drv[Instance]->ConfigIT(CUSTOM_RANGING_CompObj[Instance], pConfig) < 0)
  {
    ret = BSP_ERROR_COMPONENT_FAILURE;
  }
  else
  {
    ret = BSP_ERROR_NONE;
  }

  return ret;
}

/**
  * @brief Get the last distance measurement information.
  * @param Instance    Ranging sensor instance.
  * @param pResult    Pointer to the result struct.
  * @retval BSP status
  */
int32_t CUSTOM_RANGING_SENSOR_GetDistance(uint32_t Instance, RANGING_SENSOR_Result_t *pResult)
{
  int32_t ret;

  if (Instance >= CUSTOM_RANGING_INSTANCES_NBR)
  {
    ret = BSP_ERROR_WRONG_PARAM;
  }
  else if (CUSTOM_RANGING_Drv[Instance]->GetDistance(CUSTOM_RANGING_CompObj[Instance], pResult) < 0)
  {
    ret = BSP_ERROR_COMPONENT_FAILURE;
  }
  else
  {
    ret = BSP_ERROR_NONE;
  }

  return ret;
}

/**
  * @brief Start ranging.
  * @param Instance    Ranging sensor instance.
  * @param Mode        The desired ranging mode.
  * @retval BSP status
  */
int32_t CUSTOM_RANGING_SENSOR_Start(uint32_t Instance, uint8_t Mode)
{
  int32_t ret;

  if (Instance >= CUSTOM_RANGING_INSTANCES_NBR)
  {
    ret = BSP_ERROR_WRONG_PARAM;
  }
  else if (CUSTOM_RANGING_Drv[Instance]->Start(CUSTOM_RANGING_CompObj[Instance], Mode) < 0)
  {
    ret = BSP_ERROR_COMPONENT_FAILURE;
  }
  else
  {
    ret = BSP_ERROR_NONE;
  }

  return ret;
}

/**
  * @brief Stop ranging.
  * @param Instance    Ranging sensor instance.
  * @retval BSP status
  */
int32_t CUSTOM_RANGING_SENSOR_Stop(uint32_t Instance)
{
  int32_t ret;

  if (Instance >= CUSTOM_RANGING_INSTANCES_NBR)
  {
    ret = BSP_ERROR_WRONG_PARAM;
  }
  else if (CUSTOM_RANGING_Drv[Instance]->Stop(CUSTOM_RANGING_CompObj[Instance]) < 0)
  {
    ret = BSP_ERROR_COMPONENT_FAILURE;
  }
  else
  {
    ret = BSP_ERROR_NONE;
  }

  return ret;
}

/**
  * @brief Set The I2C address of the device.
  * @param Instance    Ranging sensor instance.
  * @param Address     New I2C address.
  * @retval BSP status
  */
int32_t CUSTOM_RANGING_SENSOR_SetAddress(uint32_t Instance, uint16_t Address)
{
  int32_t ret;

  if (Instance >= CUSTOM_RANGING_INSTANCES_NBR)
  {
    ret = BSP_ERROR_WRONG_PARAM;
  }
  else if (CUSTOM_RANGING_Drv[Instance]->SetAddress(CUSTOM_RANGING_CompObj[Instance], Address) < 0)
  {
    ret = BSP_ERROR_COMPONENT_FAILURE;
  }
  else
  {
    ret = BSP_ERROR_NONE;
  }

  return ret;
}

/**
  * @brief Get the I2C address of the device.
  * @param Instance    Ranging sensor instance.
  * @param pAddress    Pointer to the current I2C address.
  * @retval BSP status
  */
int32_t CUSTOM_RANGING_SENSOR_GetAddress(uint32_t Instance, uint16_t *pAddress)
{
  int32_t ret;

  if (Instance >= CUSTOM_RANGING_INSTANCES_NBR)
  {
    ret = BSP_ERROR_WRONG_PARAM;
  }
  else if (CUSTOM_RANGING_Drv[Instance]->GetAddress(CUSTOM_RANGING_CompObj[Instance], (uint32_t*)pAddress) < 0)
  {
    ret = BSP_ERROR_COMPONENT_FAILURE;
  }
  else
  {
    ret = BSP_ERROR_NONE;
  }

  return ret;
}

/**
  * @brief Set the power mode.
  * @param Instance    Ranging sensor instance.
  * @param PowerMode    New power mode to be entered.
  * @retval BSP status
  */
int32_t CUSTOM_RANGING_SENSOR_SetPowerMode(uint32_t Instance, uint32_t PowerMode)
{
  int32_t ret;

  if (Instance >= CUSTOM_RANGING_INSTANCES_NBR)
  {
    ret = BSP_ERROR_WRONG_PARAM;
  }
  else if (PowerMode == CUSTOM_RANGING_POWERMODE_ON)
  {
       switch (Instance)
        {
#if (USE_CUSTOM_RANGING_VL53L3CX == 1U)
          case CUSTOM_VL53L3CX:
	  	    set_gpio_pin( CUSTOM_VL53L3CX_XSHUT_PIN, 1);
//            HAL_GPIO_WritePin(CUSTOM_VL53L3CX_XSHUT_PORT,
 //               CUSTOM_VL53L3CX_XSHUT_PIN, GPIO_PIN_SET);
 //           HAL_Delay(2);
            wait_ms(2);
            ret = BSP_ERROR_NONE;
            break;
#endif
          default:
              ret = BSP_ERROR_UNKNOWN_COMPONENT;
              break;
    }
  }
  else if (PowerMode == CUSTOM_RANGING_POWERMODE_OFF)
  {
        switch (Instance)
        {
#if (USE_CUSTOM_RANGING_VL53L3CX == 1U)
          case CUSTOM_VL53L3CX:
	        set_gpio_pin(CUSTOM_VL53L3CX_XSHUT_PIN, 0);
     //       HAL_GPIO_WritePin(CUSTOM_VL53L3CX_XSHUT_PORT,
     //           CUSTOM_VL53L3CX_XSHUT_PIN, GPIO_PIN_RESET);
    //        HAL_Delay(2);
	        wait_ms(2);
            ret = BSP_ERROR_NONE;
            break;
#endif
          default:
              ret = BSP_ERROR_UNKNOWN_COMPONENT;
              break;
    }
  }
  else
  {
    ret = BSP_ERROR_WRONG_PARAM;
  }

    return ret;
}

/**
  * @brief Set the power mode.
  * @param Instance    Ranging sensor instance.
  * @param pPowerMode    Pointer to the current power mode.
  * @retval BSP status
  */
int32_t CUSTOM_RANGING_SENSOR_GetPowerMode(uint32_t Instance, uint32_t *pPowerMode)
{
  int32_t ret;

  if (Instance >= CUSTOM_RANGING_INSTANCES_NBR)
  {
    ret = BSP_ERROR_WRONG_PARAM;
  }
  else if (CUSTOM_RANGING_Drv[Instance]->GetPowerMode(CUSTOM_RANGING_CompObj[Instance], pPowerMode) < 0)
  {
    ret = BSP_ERROR_COMPONENT_FAILURE;
  }
  else
  {
    ret = BSP_ERROR_NONE;
  }

  return ret;
}

#if (USE_CUSTOM_RANGING_VL53L3CX == 1U)
/**
  * @brief Register Bus IOs if component ID is OK.
  * @param Instance    Ranging sensor instance.
  * @retval BSP status
  */
static int32_t VL53L3CX_Probe(uint32_t Instance)
{
  int32_t ret;
  VL53L3CX_IO_t              IOCtx;
  uint32_t                   id;
  static VL53L3CX_Object_t   VL53L3CXObj;

  /* Configure the ranging sensor driver */
  IOCtx.Address     = RANGING_SENSOR_VL53L3CX_ADDRESS;
 // IOCtx.Init        = CUSTOM_VL53L3CX_I2C_Init;
 // IOCtx.DeInit      = CUSTOM_VL53L3CX_I2C_DeInit;
 // IOCtx.WriteReg    = CUSTOM_VL53L3CX_I2C_WriteReg;
 // IOCtx.ReadReg     = CUSTOM_VL53L3CX_I2C_ReadReg;
 // IOCtx.GetTick     = BSP_GetTick;
  IOCtx.GetTick = GetTickCountMs; // 1ms ticks

  if (VL53L3CX_RegisterBusIO(&VL53L3CXObj, &IOCtx) != VL53L3CX_OK)
  {
    ret = BSP_ERROR_COMPONENT_FAILURE;
  }
  else if (VL53L3CX_ReadID(&VL53L3CXObj, &id) != VL53L3CX_OK)
  {
    ret = BSP_ERROR_COMPONENT_FAILURE;
  }
  else
  {
    if (id != VL53L3CX_ID)
    {
      ret = BSP_ERROR_UNKNOWN_COMPONENT;
    }
    else
    {
      CUSTOM_RANGING_Drv[Instance] = (RANGING_SENSOR_Drv_t *) &VL53L3CX_RANGING_SENSOR_Driver;
      CUSTOM_RANGING_CompObj[Instance] = &VL53L3CXObj;

      if (CUSTOM_RANGING_Drv[Instance]->Init(CUSTOM_RANGING_CompObj[Instance]) != VL53L3CX_OK)
      {
        ret = BSP_ERROR_COMPONENT_FAILURE;
      }
      else if (CUSTOM_RANGING_Drv[Instance]->GetCapabilities(CUSTOM_RANGING_CompObj[Instance], &RANGING_SENSOR_Cap[Instance]) != VL53L3CX_OK)
      {
        ret = BSP_ERROR_COMPONENT_FAILURE;
      }
      else
      {
        ret = BSP_ERROR_NONE;
      }
    }
  }

  return ret;
}
#endif

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