Merge first pololu changes with STSW-IMG007_2.3.3

Dirk007 · Jul 1, 2019 · 5e690d9 · 5e690d9
2 parents 215c70d + 3abc176
commit 5e690d9
Show file tree

Hide file tree

Showing 5 changed files with 260 additions and 43 deletions.
diff --git a/vl53l1_api.c b/vl53l1_api.c
@@ -1,6 +1,7 @@
 
 /*
 * Copyright (c) 2017, STMicroelectronics - All Rights Reserved
+* Modified by Pololu Corporation, 2018
 *
 * This file is part of VL53L1 Core and is dual licensed,
 * either 'STMicroelectronics
@@ -755,6 +756,8 @@ VL53L1_Error VL53L1_SetDeviceAddress(VL53L1_DEV Dev, uint8_t DeviceAddress)
 	return Status;
 }
 
+#define USE_I2C_2V8
+
 VL53L1_Error VL53L1_DataInit(VL53L1_DEV Dev)
 {
 	VL53L1_Error Status = VL53L1_ERROR_NONE;

diff --git a/vl53l1_core.c b/vl53l1_core.c
@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2017, STMicroelectronics - All Rights Reserved
+* Modified by Pololu Corporation, 2018
 *
 * This file is part of VL53L1 Core and is dual licensed,
 * either 'STMicroelectronics
@@ -2297,7 +2298,7 @@ VL53L1_Error VL53L1_low_power_auto_update_DSS(
 
 		/* get the target rate and shift up by 16
 		 * format 9.23 */
-		utemp32a = pdev->stat_cfg.dss_config__target_total_rate_mcps <<
+		utemp32a = (uint32_t)pdev->stat_cfg.dss_config__target_total_rate_mcps <<
 			16;
 
 		/* check for divide by zero */

diff --git a/vl53l1_core_support.c b/vl53l1_core_support.c
@@ -1,5 +1,6 @@
 /*
 * Copyright (c) 2017, STMicroelectronics - All Rights Reserved
+* Modified by Pololu Corporation, 2018
 *
 * This file is part of VL53L1 Core and is dual licensed,
 * either 'STMicroelectronics
@@ -111,7 +112,7 @@ uint32_t VL53L1_calc_pll_period_us(
 
 	LOG_FUNCTION_START("");
 
-	pll_period_us = (0x01 << 30) / fast_osc_frequency;
+	pll_period_us = ((uint32_t)0x01 << 30) / fast_osc_frequency;
 
 #ifdef VL53L1_LOGGING
 	trace_print(VL53L1_TRACE_LEVEL_DEBUG,

diff --git a/vl53l1_platform.c → vl53l1_platform.cpp b/vl53l1_platform.c → vl53l1_platform.cpp
@@ -1,6 +1,11 @@
+/*******************************************************************************
 
-/*
+This file is derived from vl53l1_platform.c in the STSW-IMG007 VL53L1X API.
+
+********************************************************************************
+*
 * Copyright (c) 2017, STMicroelectronics - All Rights Reserved
+* Copyright (c) 2018 Pololu Corporation
 *
 * This file is part of VL53L1 Core and is dual licensed,
 * either 'STMicroelectronics
@@ -61,6 +66,8 @@
 *
 */
 
+#include <Arduino.h>
+#include <Wire.h>
 
 #include "vl53l1_platform.h"
 // #include "vl53l1_platform_log.h"
@@ -112,57 +119,136 @@
 //    return Status;
 // }
 
-VL53L1_Error VL53L1_WriteMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count) {
-    VL53L1_Error Status = VL53L1_ERROR_NONE;
-    return Status;
+VL53L1_Error VL53L1_WriteMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count)
+{
+  while (count > 0)
+  {
+    Wire.beginTransmission(Dev->I2cDevAddr >> 1);
+    Wire.write((index >> 8) & 0xff);
+    Wire.write(index & 0xff);
+
+    uint8_t writing = 0;
+
+    while (count > 0 && Wire.write(*pdata) != 0)
+    {
+      pdata++;
+      writing++;
+      count--;
+    }
+
+    if (writing == 0 || Wire.endTransmission() != 0) { return VL53L1_ERROR_CONTROL_INTERFACE; }
+    index += writing;
+  }
+
+  return VL53L1_ERROR_NONE;
 }
 
-// the ranging_sensor_comms.dll will take care of the page selection
-VL53L1_Error VL53L1_ReadMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count) {
-    VL53L1_Error Status = VL53L1_ERROR_NONE;
-    return Status;
+VL53L1_Error VL53L1_ReadMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count)
+{
+  Wire.beginTransmission(Dev->I2cDevAddr >> 1);
+  Wire.write((index >> 8) & 0xff);
+  Wire.write(index & 0xff);
+  if (Wire.endTransmission() != 0) { return VL53L1_ERROR_CONTROL_INTERFACE; }
+
+  while (count > 0)
+  {
+    uint8_t reading = Wire.requestFrom(Dev->I2cDevAddr >> 1, count);
+
+    if (reading == 0) { return VL53L1_ERROR_CONTROL_INTERFACE; }
+    count -= reading;
+
+    while (reading-- > 0)
+    {
+      *pdata++ = Wire.read();
+    }
+  }
+
+  return VL53L1_ERROR_NONE;
 }
 
-VL53L1_Error VL53L1_WrByte(VL53L1_DEV Dev, uint16_t index, uint8_t data) {
-    VL53L1_Error Status = VL53L1_ERROR_NONE;
-    return Status;
+VL53L1_Error VL53L1_WrByte(VL53L1_DEV Dev, uint16_t index, uint8_t data)
+{
+  Wire.beginTransmission(Dev->I2cDevAddr >> 1);
+  Wire.write((index >> 8) & 0xff);
+  Wire.write(index & 0xff);
+  Wire.write(data);
+  return (Wire.endTransmission() == 0 ? VL53L1_ERROR_NONE : VL53L1_ERROR_CONTROL_INTERFACE);
 }
 
-VL53L1_Error VL53L1_WrWord(VL53L1_DEV Dev, uint16_t index, uint16_t data) {
-    VL53L1_Error Status = VL53L1_ERROR_NONE;
-    return Status;
+VL53L1_Error VL53L1_WrWord(VL53L1_DEV Dev, uint16_t index, uint16_t data)
+{
+  Wire.beginTransmission(Dev->I2cDevAddr >> 1);
+  Wire.write((index >> 8) & 0xff);
+  Wire.write(index & 0xff);
+  Wire.write((data >> 8) & 0xff);
+  Wire.write(data & 0xff);
+  return (Wire.endTransmission() == 0 ? VL53L1_ERROR_NONE : VL53L1_ERROR_CONTROL_INTERFACE);
 }
 
-VL53L1_Error VL53L1_WrDWord(VL53L1_DEV Dev, uint16_t index, uint32_t data) {
-    VL53L1_Error Status = VL53L1_ERROR_NONE;
-    return Status;
+VL53L1_Error VL53L1_WrDWord(VL53L1_DEV Dev, uint16_t index, uint32_t data)
+{
+  Wire.beginTransmission(Dev->I2cDevAddr >> 1);
+  Wire.write((index >> 8) & 0xff);
+  Wire.write(index & 0xff);
+  Wire.write((data >> 24) & 0xff);
+  Wire.write((data >> 16) & 0xff);
+  Wire.write((data >> 8) & 0xff);
+  Wire.write(data & 0xff);
+  return (Wire.endTransmission() == 0 ? VL53L1_ERROR_NONE : VL53L1_ERROR_CONTROL_INTERFACE);
 }
 
-VL53L1_Error VL53L1_UpdateByte(VL53L1_DEV Dev, uint16_t index, uint8_t AndData, uint8_t OrData) {
-    VL53L1_Error Status = VL53L1_ERROR_NONE;
-    return Status;
+VL53L1_Error VL53L1_UpdateByte(VL53L1_DEV Dev, uint16_t index, uint8_t AndData, uint8_t OrData)
+{
+  uint8_t data;
+  VL53L1_Error status = VL53L1_RdByte(Dev, index, &data);
+  if (status != VL53L1_ERROR_NONE) { return status; }
+  data &= AndData;
+  data |= OrData;
+  return VL53L1_WrByte(Dev, index, data);
 }
 
-VL53L1_Error VL53L1_RdByte(VL53L1_DEV Dev, uint16_t index, uint8_t *data) {
-    VL53L1_Error Status = VL53L1_ERROR_NONE;
-    return Status;
+VL53L1_Error VL53L1_RdByte(VL53L1_DEV Dev, uint16_t index, uint8_t *data)
+{
+  Wire.beginTransmission(Dev->I2cDevAddr >> 1);
+  Wire.write((index >> 8) & 0xff);
+  Wire.write(index & 0xff);
+  if (Wire.endTransmission() != 0) { return VL53L1_ERROR_CONTROL_INTERFACE; }
+  if (Wire.requestFrom(Dev->I2cDevAddr >> 1, 1) != 1) { return VL53L1_ERROR_CONTROL_INTERFACE; }
+  *data = Wire.read();
+  return VL53L1_ERROR_NONE;
 }
 
-VL53L1_Error VL53L1_RdWord(VL53L1_DEV Dev, uint16_t index, uint16_t *data) {
-    VL53L1_Error Status = VL53L1_ERROR_NONE;
-    return Status;
+VL53L1_Error VL53L1_RdWord(VL53L1_DEV Dev, uint16_t index, uint16_t *data)
+{
+  Wire.beginTransmission(Dev->I2cDevAddr >> 1);
+  Wire.write((index >> 8) & 0xff);
+  Wire.write(index & 0xff);
+  if (Wire.endTransmission() != 0) { return VL53L1_ERROR_CONTROL_INTERFACE; }
+  if (Wire.requestFrom(Dev->I2cDevAddr >> 1, 2) != 2) { return VL53L1_ERROR_CONTROL_INTERFACE; }
+  *data = (uint16_t)Wire.read() << 8;
+  *data |= Wire.read();
+  return VL53L1_ERROR_NONE;
 }
 
-VL53L1_Error VL53L1_RdDWord(VL53L1_DEV Dev, uint16_t index, uint32_t *data) {
-    VL53L1_Error Status = VL53L1_ERROR_NONE;
-    return Status;
+VL53L1_Error VL53L1_RdDWord(VL53L1_DEV Dev, uint16_t index, uint32_t *data)
+{
+  Wire.beginTransmission(Dev->I2cDevAddr >> 1);
+  Wire.write((index >> 8) & 0xff);
+  Wire.write(index & 0xff);
+  if (Wire.endTransmission() != 0) { return VL53L1_ERROR_CONTROL_INTERFACE; }
+  if (Wire.requestFrom(Dev->I2cDevAddr >> 1, 4) != 4) { return VL53L1_ERROR_CONTROL_INTERFACE; }
+  *data = (uint32_t)Wire.read() << 24;
+  *data |= (uint32_t)Wire.read() << 16;
+  *data |= (uint16_t)Wire.read() << 8;
+  *data |= Wire.read();
+  return VL53L1_ERROR_NONE;
 }
 
 VL53L1_Error VL53L1_GetTickCount(
 	uint32_t *ptick_count_ms)
 {
-	VL53L1_Error status  = VL53L1_ERROR_NONE;
-	return status;
+  *ptick_count_ms = millis();
+	return VL53L1_ERROR_NONE;
 }
 
 //#define trace_print(level, ...) \
@@ -175,18 +261,19 @@ VL53L1_Error VL53L1_GetTickCount(
 
 VL53L1_Error VL53L1_GetTimerFrequency(int32_t *ptimer_freq_hz)
 {
-	VL53L1_Error status  = VL53L1_ERROR_NONE;
-	return status;
+	return VL53L1_ERROR_NOT_IMPLEMENTED;
 }
 
-VL53L1_Error VL53L1_WaitMs(VL53L1_Dev_t *pdev, int32_t wait_ms){
-	VL53L1_Error status  = VL53L1_ERROR_NONE;
-	return status;
+VL53L1_Error VL53L1_WaitMs(VL53L1_Dev_t *pdev, int32_t wait_ms)
+{
+  delay(wait_ms);
+	return VL53L1_ERROR_NONE;
 }
 
-VL53L1_Error VL53L1_WaitUs(VL53L1_Dev_t *pdev, int32_t wait_us){
-	VL53L1_Error status  = VL53L1_ERROR_NONE;
-	return status;
+VL53L1_Error VL53L1_WaitUs(VL53L1_Dev_t *pdev, int32_t wait_us)
+{
+  delayMicroseconds(wait_us);
+	return VL53L1_ERROR_NONE;
 }
 
 VL53L1_Error VL53L1_WaitValueMaskEx(
@@ -197,8 +284,19 @@ VL53L1_Error VL53L1_WaitValueMaskEx(
 	uint8_t       mask,
 	uint32_t      poll_delay_ms)
 {
-	VL53L1_Error status  = VL53L1_ERROR_NONE;
-	return status;
+  uint8_t data;
+  VL53L1_Error status;
+
+  while (timeout_ms > 0)
+  {
+    status = VL53L1_RdByte(pdev, index, &data);
+    if (status != VL53L1_ERROR_NONE) { return status; }
+    if ((data & mask) == value) { return VL53L1_ERROR_NONE; }
+    delay(poll_delay_ms);
+    timeout_ms -= min(poll_delay_ms, timeout_ms);
+  }
+
+  return VL53L1_ERROR_TIME_OUT;
 }