Diff: 53l3a2_ranging_sensor.c

Revision:

0:c1910e04fc6c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/53l3a2_ranging_sensor.c	Wed Jul 21 14:19:31 2021 +0000
@@ -0,0 +1,627 @@
+/**
+  ******************************************************************************
+  * @file    53l3a2_ranging_sensor.c
+  * @author  IMG SW Application Team
+  * @brief   This file includes the driver for Ranging Sensor modules mounted on
+  *          X-NUCLEO 53L3A2 expansion board.
+  ******************************************************************************
+  * @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 "53l3a2_ranging_sensor.h"
+#include "vl53lx_platform.h"
+extern void wait_ms(int ms);
+
+
+/* Common Error codes */
+#define BSP_ERROR_NONE                         0
+#define BSP_ERROR_NO_INIT                     -1
+#define BSP_ERROR_WRONG_PARAM                 -2
+#define BSP_ERROR_BUSY                        -3
+#define BSP_ERROR_PERIPH_FAILURE              -4
+#define BSP_ERROR_COMPONENT_FAILURE           -5
+#define BSP_ERROR_UNKNOWN_FAILURE             -6
+#define BSP_ERROR_UNKNOWN_COMPONENT           -7
+#define BSP_ERROR_BUS_FAILURE                 -8
+#define BSP_ERROR_CLOCK_FAILURE               -9
+#define BSP_ERROR_MSP_FAILURE                 -10
+#define BSP_ERROR_FEATURE_NOT_SUPPORTED       -11
+
+
+
+#define RANGING_SENSOR_INSTANCES_NBR    (3U)
+
+/** @addtogroup BSP
+  * @{
+  */
+
+/** @addtogroup XNUCLEO_53L3A2
+  * @{
+  */
+
+/** @addtogroup XNUCLEO_53L3A2_RANGING_SENSOR
+  * @{
+  */
+
+/** @addtogroup XNUCLEO_53L3A2_RANGING_SENSOR_Exported_Variables
+  * @{
+  */
+void *VL53L3A2_RANGING_SENSOR_CompObj[RANGING_SENSOR_INSTANCES_NBR] = {0};
+/**
+  * @}
+  */
+
+/** @defgroup XNUCLEO_53L3A2_RANGING_SENSOR_Private_Variables Private Variables
+  * @{
+  */
+static RANGING_SENSOR_Drv_t *VL53L3A2_RANGING_SENSOR_Drv = NULL;
+static RANGING_SENSOR_Capabilities_t VL53L3A2_RANGING_SENSOR_Cap;
+/**
+  * @}
+  */
+
+/** @defgroup XNUCLEO_53L3A2_RANGING_SENSOR_Private_Functions_Prototypes Private Functions Prototypes
+  * @{
+  */
+static int32_t VL53L3CX_Probe(uint32_t Instance);
+/**
+  * @}
+  */
+
+/**
+  * @brief Initialize the ranging sensor.
+  * @param Instance    Ranging sensor instance.
+  * @retval BSP status
+  */
+int32_t VL53L3A2_RANGING_SENSOR_Init(uint32_t Instance)
+{
+  int32_t ret;
+printf("VL53L3A2_RANGING_SENSOR_Init start \n");
+
+  if (Instance >= RANGING_SENSOR_INSTANCES_NBR)
+  {
+    ret = BSP_ERROR_WRONG_PARAM;
+  }
+  else if (VL53L3A2_Init() != 0)
+  {
+	  printf("VL53L3A2_Init failed\n");
+    ret = BSP_ERROR_PERIPH_FAILURE;
+  }
+  else
+  {
+	wait_ms(100);
+    (void)VL53L3A2_RANGING_SENSOR_SetPowerMode(Instance, RANGING_SENSOR_POWERMODE_OFF);
+
+ 	wait_ms(100);
+    (void)VL53L3A2_RANGING_SENSOR_SetPowerMode(Instance, RANGING_SENSOR_POWERMODE_ON);
+;
+	wait_ms(100);
+    ret = VL53L3CX_Probe(Instance);
+  }
+printf("VL53L3A2_RANGING_SENSOR_Init end \n");
+  return ret;
+}
+
+/**
+  * @brief De-initialize the ranging sensor.
+  * @param Instance    Ranging sensor instance.
+  * @retval BSP status
+  */
+int32_t VL53L3A2_RANGING_SENSOR_DeInit(uint32_t Instance)
+{
+  int32_t ret;
+
+  if (Instance >= RANGING_SENSOR_INSTANCES_NBR)
+  {
+    ret = BSP_ERROR_WRONG_PARAM;
+  }
+  else if (VL53L3A2_DeInit() != 0)
+  {
+    ret = BSP_ERROR_PERIPH_FAILURE;
+  }
+  else if (VL53L3A2_RANGING_SENSOR_Drv->DeInit(VL53L3A2_RANGING_SENSOR_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 VL53L3A2_RANGING_SENSOR_ReadID(uint32_t Instance, uint32_t *pId)
+{
+  int32_t ret;
+
+  if (Instance >= RANGING_SENSOR_INSTANCES_NBR)
+  {
+    ret = BSP_ERROR_WRONG_PARAM;
+  }
+  else if (VL53L3A2_RANGING_SENSOR_Drv->ReadID(VL53L3A2_RANGING_SENSOR_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 VL53L3A2_RANGING_SENSOR_GetCapabilities(uint32_t Instance, RANGING_SENSOR_Capabilities_t *pCapabilities)
+{
+  int32_t ret;
+
+  if (Instance >= RANGING_SENSOR_INSTANCES_NBR)
+  {
+    ret = BSP_ERROR_WRONG_PARAM;
+  }
+  else if (VL53L3A2_RANGING_SENSOR_Drv->GetCapabilities(VL53L3A2_RANGING_SENSOR_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 VL53L3A2_RANGING_SENSOR_ConfigProfile(uint32_t Instance, RANGING_SENSOR_ProfileConfig_t *pConfig)
+{
+  int32_t ret;
+
+  if (Instance >= RANGING_SENSOR_INSTANCES_NBR)
+  {
+    ret = BSP_ERROR_WRONG_PARAM;
+  }
+  else if (VL53L3A2_RANGING_SENSOR_Drv->ConfigProfile(VL53L3A2_RANGING_SENSOR_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.
+  * @retval BSP status
+  */
+int32_t VL53L3A2_RANGING_SENSOR_ConfigROI(uint32_t Instance, RANGING_SENSOR_ROIConfig_t *pConfig)
+{
+  int32_t ret;
+
+  if (Instance >= RANGING_SENSOR_INSTANCES_NBR)
+  {
+    ret = BSP_ERROR_WRONG_PARAM;
+  }
+  else if (VL53L3A2_RANGING_SENSOR_Cap.CustomROI == 0U)
+  {
+    ret = BSP_ERROR_FEATURE_NOT_SUPPORTED;
+  }
+  else if (VL53L3A2_RANGING_SENSOR_Drv->ConfigROI(VL53L3A2_RANGING_SENSOR_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 VL53L3A2_RANGING_SENSOR_ConfigIT(uint32_t Instance, RANGING_SENSOR_ITConfig_t *pConfig)
+{
+  int32_t ret;
+
+  if (Instance >= RANGING_SENSOR_INSTANCES_NBR)
+  {
+    ret = BSP_ERROR_WRONG_PARAM;
+  }
+  else if (VL53L3A2_RANGING_SENSOR_Drv->ConfigIT(VL53L3A2_RANGING_SENSOR_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 VL53L3A2_RANGING_SENSOR_GetDistance(uint32_t Instance, RANGING_SENSOR_Result_t *pResult)
+{
+  int32_t ret;
+
+  if (Instance >= RANGING_SENSOR_INSTANCES_NBR)
+  {
+    ret = BSP_ERROR_WRONG_PARAM;
+  }
+  else if (VL53L3A2_RANGING_SENSOR_Drv->GetDistance(VL53L3A2_RANGING_SENSOR_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_SENSOR_Mode_t
+  * @retval BSP status
+  */
+int32_t VL53L3A2_RANGING_SENSOR_Start(uint32_t Instance, uint32_t Mode)
+{
+  int32_t ret;
+
+  if (Instance >= RANGING_SENSOR_INSTANCES_NBR)
+  {
+    ret = BSP_ERROR_WRONG_PARAM;
+  }
+  else if (VL53L3A2_RANGING_SENSOR_Drv->Start(VL53L3A2_RANGING_SENSOR_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 VL53L3A2_RANGING_SENSOR_Stop(uint32_t Instance)
+{
+  int32_t ret;
+
+  if (Instance >= RANGING_SENSOR_INSTANCES_NBR)
+  {
+    ret = BSP_ERROR_WRONG_PARAM;
+  }
+  else if (VL53L3A2_RANGING_SENSOR_Drv->Stop(VL53L3A2_RANGING_SENSOR_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 VL53L3A2_RANGING_SENSOR_SetAddress(uint32_t Instance, uint32_t Address)
+{
+  int32_t ret;
+
+  if (Instance >= RANGING_SENSOR_INSTANCES_NBR)
+  {
+    ret = BSP_ERROR_WRONG_PARAM;
+  }
+  else if (VL53L3A2_RANGING_SENSOR_Drv->SetAddress(VL53L3A2_RANGING_SENSOR_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 VL53L3A2_RANGING_SENSOR_GetAddress(uint32_t Instance, uint32_t *pAddress)
+{
+  int32_t ret;
+
+  if (Instance >= RANGING_SENSOR_INSTANCES_NBR)
+  {
+    ret = BSP_ERROR_WRONG_PARAM;
+  }
+  else if (VL53L3A2_RANGING_SENSOR_Drv->GetAddress(VL53L3A2_RANGING_SENSOR_CompObj[Instance], 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 VL53L3A2_RANGING_SENSOR_SetPowerMode(uint32_t Instance, uint32_t PowerMode)
+{
+  int32_t ret;
+
+  if (Instance >= RANGING_SENSOR_INSTANCES_NBR)
+  {
+    ret = BSP_ERROR_WRONG_PARAM;
+  }
+  else if (PowerMode == RANGING_SENSOR_POWERMODE_ON)
+  {
+    if (VL53L3A2_ResetId((uint8_t)Instance, 1) < 0)
+    {
+      ret = BSP_ERROR_PERIPH_FAILURE;
+    }
+    else
+    {
+//      HAL_Delay(2);
+	  wait_ms(100);
+      ret = BSP_ERROR_NONE;
+    }
+  }
+  else if (PowerMode == RANGING_SENSOR_POWERMODE_OFF)
+  {
+    if (VL53L3A2_ResetId((uint8_t)Instance, 0) < 0)
+    {
+      ret = BSP_ERROR_PERIPH_FAILURE;
+    }
+    else
+    {
+//      HAL_Delay(2);
+	  wait_ms(100);
+      ret = BSP_ERROR_NONE;
+    }
+  }
+  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 VL53L3A2_RANGING_SENSOR_GetPowerMode(uint32_t Instance, uint32_t *pPowerMode)
+{
+  int32_t ret;
+
+  if (Instance >= RANGING_SENSOR_INSTANCES_NBR)
+  {
+    ret = BSP_ERROR_WRONG_PARAM;
+  }
+  else if (VL53L3A2_RANGING_SENSOR_Drv->GetPowerMode(VL53L3A2_RANGING_SENSOR_CompObj[Instance], pPowerMode) < 0)
+  {
+    ret = BSP_ERROR_COMPONENT_FAILURE;
+  }
+  else
+  {
+    ret = BSP_ERROR_NONE;
+  }
+
+  return ret;
+}
+
+/**
+  * @brief Perform an offset calibration.
+  * @param Instance    Ranging sensor instance.
+  * @param CalDistance   Calibration distance in mm.
+  * @retval BSP status
+  */
+int32_t VL53L3A2_RANGING_SENSOR_OffsetCalibration(uint32_t Instance, uint32_t CalDistance)
+{
+  int32_t ret;
+
+  if (Instance >= RANGING_SENSOR_INSTANCES_NBR)
+  {
+    ret = BSP_ERROR_WRONG_PARAM;
+  }
+  else if (VL53L3CX_OffsetCalibration(VL53L3A2_RANGING_SENSOR_CompObj[Instance], CalDistance) < 0)
+  {
+    ret = BSP_ERROR_COMPONENT_FAILURE;
+  }
+  else
+  {
+    ret = BSP_ERROR_NONE;
+  }
+
+  return ret;
+}
+
+/**
+  * @brief Perform a xtalk calibration.
+  * @param Instance    Ranging sensor instance.
+  * @retval BSP status
+  */
+int32_t VL53L3A2_RANGING_SENSOR_XTalkCalibration(uint32_t Instance)
+{
+  int32_t ret;
+
+  if (Instance >= RANGING_SENSOR_INSTANCES_NBR)
+  {
+    ret = BSP_ERROR_WRONG_PARAM;
+  }
+  else if (VL53L3CX_XTalkCalibration(VL53L3A2_RANGING_SENSOR_CompObj[Instance]) < 0)
+  {
+    ret = BSP_ERROR_COMPONENT_FAILURE;
+  }
+  else
+  {
+    ret = BSP_ERROR_NONE;
+  }
+
+  return ret;
+}
+
+/** @defgroup XNUCLEO_53L3A2_RANGING_SENSOR_Private_Functions Private Functions
+  * @{
+  */
+
+/**
+  * @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[RANGING_SENSOR_INSTANCES_NBR];
+
+  /* Configure the ranging sensor driver */
+      IOCtx.Address     = RANGING_SENSOR_VL53L3CX_ADDRESS; //default address
+ //// IOCtx.Init        = VL53L3A2_I2C_Init;
+ //// IOCtx.DeInit      = VL53L3A2_I2C_DeInit;
+////  IOCtx.WriteReg    = VL53L3A2_I2C_WriteReg;
+////  IOCtx.ReadReg     = VL53L3A2_I2C_ReadReg;/
+////  IOCtx.GetTick     = VL53L3A2_GetTick;
+      IOCtx.GetTick = GetTickCountMs; // 1ms ticks
+   
+//printf("VL53L3CX_Probe  start %d %d\n",Instance,VL53L3CXObj[Instance].IO.Address);
+
+  uint32_t i2c_addr=0x52;
+  VL53L3CXObj[Instance].IO.Address = i2c_addr;
+  
+
+  if (VL53L3CX_RegisterBusIO(&(VL53L3CXObj[Instance]), &IOCtx) != VL53L3CX_OK)
+  {
+    ret = BSP_ERROR_COMPONENT_FAILURE;
+  }
+  else 
+ if (VL53L3CX_ReadID(&(VL53L3CXObj[Instance]), &id) != VL53L3CX_OK)
+  {
+	  printf("VL53L3CX_ReadID failed\n");
+    ret = BSP_ERROR_COMPONENT_FAILURE;
+  }
+  else
+  {
+    if (id != VL53L3CX_ID)
+    {
+			  printf("VL53L3CX_ID failed\n");
+      ret = BSP_ERROR_UNKNOWN_COMPONENT;
+    }
+    else
+    {	
+		
+      VL53L3A2_RANGING_SENSOR_Drv = (RANGING_SENSOR_Drv_t *) &VL53L3CX_RANGING_SENSOR_Driver;
+      VL53L3A2_RANGING_SENSOR_CompObj[Instance] = &(VL53L3CXObj[Instance]);
+      if (VL53L3A2_RANGING_SENSOR_Drv->Init(VL53L3A2_RANGING_SENSOR_CompObj[Instance]) != VL53L3CX_OK)
+      {
+		 printf("Init failed\n");
+        ret = BSP_ERROR_COMPONENT_FAILURE;
+      }
+      else if (VL53L3A2_RANGING_SENSOR_Drv->GetCapabilities(VL53L3A2_RANGING_SENSOR_CompObj[Instance], &VL53L3A2_RANGING_SENSOR_Cap) != VL53L3CX_OK)
+      {
+		 printf("GetCapabilities failed\n");
+        ret = BSP_ERROR_COMPONENT_FAILURE;
+      }
+      else
+      {
+        ret = BSP_ERROR_NONE;
+      }
+    }	
+		
+  }
+
+  return ret;
+}
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
+