Diff: porting/vl53lx_platform.c

Revision:

5:89031b2f5316

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/porting/vl53lx_platform.c	Wed Jul 14 12:45:49 2021 +0100
@@ -0,0 +1,375 @@
+
+/* 
+* This file is part of VL53L1 Platform 
+* 
+* Copyright (c) 2016, STMicroelectronics - All Rights Reserved 
+* 
+* License terms: BSD 3-clause "New" or "Revised" License. 
+* 
+* 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 the copyright holder 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. 
+* 
+*/
+
+unsigned int i2creadCount = 0;
+unsigned int i2cwriteCount = 0;
+unsigned char SPI2C_Buffer[256];
+
+#include <mbed_wait_api.h>
+#include "vl53lx_platform.h"
+#ifndef SMALL_FOOTPRINT
+#endif
+#include "vl53lx_platform_log.h"
+#include "vl53lx_api.h"
+#include "spi_interface.h"
+#include <string.h>
+#include <time.h>
+#include <math.h>
+
+
+extern void wait_ms(int ms);
+
+
+
+#define I2C_TIME_OUT_BASE   10
+#define I2C_TIME_OUT_BYTE   1
+
+#ifdef VL53L1_LOG_ENABLE
+#define trace_print(level, ...) VL53L1_trace_print_module_function(VL53L1_TRACE_MODULE_PLATFORM, level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
+#define trace_i2c(...) VL53L1_trace_print_module_function(VL53L1_TRACE_MODULE_NONE, VL53L1_TRACE_LEVEL_NONE, VL53L1_TRACE_FUNCTION_I2C, ##__VA_ARGS__)
+#endif
+
+/* when not customized by application define dummy one */
+#ifndef VL53L1_GetI2cBus
+/** This macro can be overloaded by user to enforce i2c sharing in RTOS context
+ */
+#   define VL53L1_GetI2cBus(...) (void)0
+#endif
+
+#ifndef VL53L1_PutI2cBus
+/** This macro can be overloaded by user to enforce i2c sharing in RTOS context
+ */
+#   define VL53L1_PutI2cBus(...) (void)0
+#endif
+
+uint8_t _I2CBuffer[256];
+
+
+VL53LX_Error VL53LX_WriteMulti(VL53LX_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count)
+{
+   int  status;
+//   printf("VL53LX_WriteMulti %d %d %d \n",Dev->IO.Address,index,count);
+   status = v53l1x_i2c_write_if(pdata,Dev->IO.Address, index,count);
+//   printf("VL53LX_WriteMulti %d %d %d %d\n",Dev->IO.Address,index,count,status);
+   return status;
+}
+
+
+VL53LX_Error VL53LX_ReadMulti(VL53LX_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count)
+{
+    int status;
+
+    status = v53l1x_i2c_read_if(pdata,Dev->IO.Address, index,count);
+
+    return status;
+}
+
+
+VL53LX_Error VL53LX_WrByte(VL53LX_DEV Dev, uint16_t index, uint8_t data)
+{
+   int  status=0;
+//		printf("VL53LX_WrByte arrived %d %d  %d\n",index,data,Dev->IO.Address);
+   status = v53l1x_i2c_write_if(&data,Dev->IO.Address, index,1);
+ //  		printf("VL53LX_WrByte end %d \n",status);
+   return status;
+}
+
+
+VL53LX_Error VL53LX_WrWord(VL53LX_DEV Dev, uint16_t index, uint16_t data)
+{
+   int  status;
+   uint8_t buffer[2];
+
+   buffer[0] = data >> 8;
+   buffer[1] = data & 0x00FF;
+   status = v53l1x_i2c_write_if((uint8_t *)buffer,Dev->IO.Address, index,2);
+   return status;
+}
+
+
+VL53LX_Error VL53LX_WrDWord(VL53LX_DEV Dev, uint16_t index, uint32_t data)
+{
+   int  status;
+   uint8_t buffer[4];
+
+    buffer[0] = (data >> 24) & 0xFF;
+    buffer[1] = (data >> 16) & 0xFF;
+    buffer[2] = (data >>  8) & 0xFF;
+    buffer[3] = (data >>  0) & 0xFF;
+    status = v53l1x_i2c_write_if((uint8_t *)buffer,Dev->IO.Address, index,4);
+   return status;
+}
+
+VL53LX_Error VL53LX_UpdateByte(VL53LX_DEV Dev, uint16_t index, uint8_t AndData, uint8_t OrData)
+{
+   int  status;
+   uint8_t buffer = 0;
+
+   /* read data direct onto buffer */
+   status = v53l1x_i2c_read_if(&buffer,Dev->IO.Address, index,1);
+   if (!status)
+   {
+      buffer = (buffer & AndData) | OrData;
+      status = v53l1x_i2c_write_if(&buffer,Dev->IO.Address, index,1);
+   }
+   return status;
+}
+
+VL53LX_Error VL53LX_RdByte(VL53LX_DEV Dev, uint16_t index, uint8_t *data)
+{
+   int  status;
+
+   status = v53l1x_i2c_read_if(data,Dev->IO.Address, index,1);  //is this correct
+ // printf("VL53L1_RdByte %d %d %d\n",Dev->IO.Address, status,*data);
+   if(status)
+     return -1;
+
+   return 0;
+}
+
+
+VL53LX_Error VL53LX_RdWord(VL53LX_DEV Dev, uint16_t index, uint16_t *data)
+{
+   int  status;
+   uint8_t buffer[2] = {0,0};
+   
+//  printf("VL53LX_RdWord %d %d %d %d\n",index,Dev->IO.Address, status,*data);   
+   status = v53l1x_i2c_read_if(buffer,Dev->IO.Address, index,2);  //is this correct
+   if (!status)
+   {
+       *data = (buffer[0] << 8) + buffer[1];
+   }
+//     printf("VL53LX_RdWord %d %d %d %d\n",index,Dev->IO.Address, status,*data);   
+   return status;
+
+}
+
+
+
+VL53LX_Error VL53LX_RdDWord(VL53LX_DEV Dev, uint16_t index, uint32_t *data)
+{
+   int status;
+   uint8_t buffer[4] = {0,0,0,0};
+
+   status = v53l1x_i2c_read_if(buffer,Dev->IO.Address, index,4);
+   if(!status)
+   {
+       *data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
+   }
+   return status;
+
+}
+
+
+VL53LX_Error VL53LX_GetTickCount(VL53LX_DEV Dev, uint32_t *ptick_count_ms)
+{
+
+    /* Returns current tick count in [ms] */
+
+    VL53LX_Error status  = VL53LX_ERROR_NONE;
+
+    GetTickCount( ptick_count_ms); 
+
+#ifdef VL53L1_LOG_ENABLE
+    trace_print(
+        VL53L1_TRACE_LEVEL_DEBUG,
+        "VL53L1_GetTickCount() = %5u ms;\n",
+    *ptick_count_ms);
+#endif
+
+    return status;
+}
+
+
+#define trace_print(level, ...) \
+    _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_PLATFORM, \
+    level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
+
+#define trace_i2c(...) \
+    _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_NONE, \
+    VL53L1_TRACE_LEVEL_NONE, VL53L1_TRACE_FUNCTION_I2C, ##__VA_ARGS__)
+
+
+VL53LX_Error VL53LX_GetTimerFrequency(int32_t *ptimer_freq_hz)
+{
+    *ptimer_freq_hz = 0;
+    
+    trace_print(VL53L1_TRACE_LEVEL_INFO, "VL53L1_GetTimerFrequency: Freq : %dHz\n", *ptimer_freq_hz);
+    return VL53LX_ERROR_NONE;
+}
+
+
+
+VL53LX_Error VL53LX_WaitMs(VL53LX_DEV dev, int32_t wait_time){
+  //  (void)pdev;
+    wait_ms(wait_time);
+    return VL53LX_ERROR_NONE;
+}
+
+
+
+VL53LX_Error VL53LX_WaitUs(VL53LX_DEV dev, int32_t wait_time){
+ //   (void)pdev;
+    wait_us(wait_time);
+    return VL53LX_ERROR_NONE;
+}
+
+VL53LX_Error VL53LX_WaitValueMaskEx(
+    VL53LX_DEV Dev,
+    uint32_t      timeout_ms,
+    uint16_t      index,
+    uint8_t       value,
+    uint8_t       mask,
+    uint32_t      poll_delay_ms)
+{
+
+    /*
+     * Platform implementation of WaitValueMaskEx V2WReg script command
+     *
+     * WaitValueMaskEx(
+     *          duration_ms,
+     *          index,
+     *          value,
+     *          mask,
+     *          poll_delay_ms);
+     */
+
+    VL53LX_Error status         = VL53LX_ERROR_NONE;
+    uint32_t     start_time_ms = 0;
+    uint32_t     current_time_ms = 0;
+    uint32_t     polling_time_ms = 0;
+    uint8_t      byte_value      = 0;
+    uint8_t      found           = 0;
+#ifdef VL53L1_LOG_ENABLE
+    uint8_t      trace_functions = VL53L1_TRACE_FUNCTION_NONE;
+#endif
+
+    char   register_name[VL53LX_MAX_STRING_LENGTH];
+
+    /* look up register name */
+#ifdef PAL_EXTENDED
+    VL53LX_get_register_name(
+            index,
+            register_name);
+#else
+ //   VL53L1_COPYSTRING(register_name, "");
+#endif
+
+    /* Output to I2C logger for FMT/DFT  */
+
+    /*trace_i2c("WaitValueMaskEx(%5d, 0x%04X, 0x%02X, 0x%02X, %5d);\n",
+                 timeout_ms, index, value, mask, poll_delay_ms); */
+    trace_i2c("WaitValueMaskEx(%5d, %s, 0x%02X, 0x%02X, %5d);\n",
+                 timeout_ms, register_name, value, mask, poll_delay_ms);
+
+    /* calculate time limit in absolute time */
+
+     VL53LX_GetTickCount(Dev,&start_time_ms);
+
+     
+     wait_ms(10);
+
+    /* remember current trace functions and temporarily disable
+     * function logging
+     */
+
+#ifdef VL53L1_LOG_ENABLE
+    trace_functions = VL53L1_get_trace_functions();
+    VL53L1_set_trace_functions(VL53L1_TRACE_FUNCTION_NONE);
+#endif
+
+    /* wait until value is found, timeout reached on error occurred */
+    while ((status == VL53LX_ERROR_NONE) &&
+           (polling_time_ms < timeout_ms) &&
+           (found == 0)) {
+
+        if (status == VL53LX_ERROR_NONE)
+            status = VL53LX_RdByte(
+                            Dev,
+                            index,
+                            &byte_value);
+
+        if ((byte_value & mask) == value)
+            found = 1;
+        if (status)
+        {
+            printf("VL53L1_WaitValueMaskEx 1 %d %d\n",Dev->IO.Address,status);
+        }
+
+        if (status == VL53LX_ERROR_NONE  &&
+            found == 0 &&
+            poll_delay_ms > 0)
+            status = VL53LX_WaitMs(
+                    Dev,
+                    poll_delay_ms);
+        /* Update polling time (Compare difference rather than absolute to
+        negate 32bit wrap around issue) */
+            if (status)
+            {
+                printf("VL53L1_WaitValueMaskEx 2 %d\n",status);
+            }
+        VL53LX_GetTickCount(Dev,&current_time_ms);
+        polling_time_ms = current_time_ms - start_time_ms;
+
+    }
+ //   printf("polling_time_ms %d \n",polling_time_ms);
+#ifdef VL53L1_LOG_ENABLE
+    /* Restore function logging */
+    VL53L1_set_trace_functions(trace_functions);
+#endif
+
+    if (found == 0 && status == VL53LX_ERROR_NONE)
+        status = VL53LX_ERROR_TIME_OUT;
+        
+    return status;
+}
+
+
+
+int32_t GetTickCountMs()
+{
+
+    /* Returns current tick count in [ms] */
+ uint32_t ptick_count_ms;
+ //   int32_t status  = us_ticker_read();
+	int32_t status = GetTickCount(&ptick_count_ms);
+    return  ptick_count_ms;
+}
+
+
+
+
+
+