adam_prochazka/ProfilometerProbe
1
0
Fork 0
mirror of https://github.com/aprochazka/ProfilometerProbe.git synced 2025-06-30 17:47:20 +02:00
Code Issues Packages Projects Releases Wiki Activity

Add distance sensor, README update in next commit

Browse Source
This commit is contained in:
Adam Prochazka
2023-04-16 23:22:34 +02:00
parent 7ace5ef5a2
commit 11003a1eab
24 changed files with 3208 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

855
Firmware/Core/Src/VL53L1X_api.c Normal file
View File

@ -0,0 +1,855 @@
/*
Copyright (c) 2017, STMicroelectronics - All Rights Reserved
This file : part of VL53L1 Core and : dual licensed,
either 'STMicroelectronics
Proprietary license'
or 'BSD 3-clause "New" or "Revised" License' , at your option.
*******************************************************************************
'STMicroelectronics Proprietary license'
*******************************************************************************
License terms: STMicroelectronics Proprietary in accordance with licensing
terms at www.st.com/sla0081
STMicroelectronics confidential
Reproduction and Communication of this document : strictly prohibited unless
specifically authorized in writing by STMicroelectronics.
*******************************************************************************
Alternatively, VL53L1 Core may be distributed under the terms of
'BSD 3-clause "New" or "Revised" License', in which case the following
provisions apply instead of the ones mentioned above :
*******************************************************************************
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.
*******************************************************************************
*/
/**
* @file vl53l1x_api.c
* @brief Functions implementation
*/
#include "VL53L1X_api.h"
#include <string.h>
#if 0
uint8_t VL51L1X_NVM_CONFIGURATION[] = {
0x00, /* 0x00 : not user-modifiable */
0x29, /* 0x01 : 7 bits I2C address (default=0x29), use SetI2CAddress(). Warning: after changing the register value to a new I2C address, the device will only answer to the new address */
0x00, /* 0x02 : not user-modifiable */
0x00, /* 0x03 : not user-modifiable */
0x00, /* 0x04 : not user-modifiable */
0x00, /* 0x05 : not user-modifiable */
0x00, /* 0x06 : not user-modifiable */
0x00, /* 0x07 : not user-modifiable */
0x00, /* 0x08 : not user-modifiable */
0x50, /* 0x09 : not user-modifiable */
0x00, /* 0x0A : not user-modifiable */
0x00, /* 0x0B : not user-modifiable */
0x00, /* 0x0C : not user-modifiable */
0x00, /* 0x0D : not user-modifiable */
0x0a, /* 0x0E : not user-modifiable */
0x00, /* 0x0F : not user-modifiable */
0x00, /* 0x10 : not user-modifiable */
0x00, /* 0x11 : not user-modifiable */
0x00, /* 0x12 : not user-modifiable */
0x00, /* 0x13 : not user-modifiable */
0x00, /* 0x14 : not user-modifiable */
0x00, /* 0x15 : not user-modifiable */
0x00, /* 0x16 : Xtalk calibration value MSB (7.9 format in kcps), use SetXtalk() */
0x00, /* 0x17 : Xtalk calibration value LSB */
0x00, /* 0x18 : not user-modifiable */
0x00, /* 0x19 : not user-modifiable */
0x00, /* 0x1a : not user-modifiable */
0x00, /* 0x1b : not user-modifiable */
0x00, /* 0x1e : Part to Part offset x4 MSB (in mm), use SetOffset() */
0x50, /* 0x1f : Part to Part offset x4 LSB */
0x00, /* 0x20 : not user-modifiable */
0x00, /* 0x21 : not user-modifiable */
0x00, /* 0x22 : not user-modifiable */
0x00, /* 0x23 : not user-modifiable */
}
#endif
const uint8_t VL51L1X_DEFAULT_CONFIGURATION[] = {
0x00, /* 0x2d : set bit 2 and 5 to 1 for fast plus mode (1MHz I2C), else don't touch */
0x00, /* 0x2e : bit 0 if I2C pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */
0x00, /* 0x2f : bit 0 if GPIO pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */
0x01, /* 0x30 : set bit 4 to 0 for active high interrupt and 1 for active low (bits 3:0 must be 0x1), use SetInterruptPolarity() */
0x02, /* 0x31 : bit 1 = interrupt depending on the polarity, use CheckForDataReady() */
0x00, /* 0x32 : not user-modifiable */
0x02, /* 0x33 : not user-modifiable */
0x08, /* 0x34 : not user-modifiable */
0x00, /* 0x35 : not user-modifiable */
0x08, /* 0x36 : not user-modifiable */
0x10, /* 0x37 : not user-modifiable */
0x01, /* 0x38 : not user-modifiable */
0x01, /* 0x39 : not user-modifiable */
0x00, /* 0x3a : not user-modifiable */
0x00, /* 0x3b : not user-modifiable */
0x00, /* 0x3c : not user-modifiable */
0x00, /* 0x3d : not user-modifiable */
0xff, /* 0x3e : not user-modifiable */
0x00, /* 0x3f : not user-modifiable */
0x0F, /* 0x40 : not user-modifiable */
0x00, /* 0x41 : not user-modifiable */
0x00, /* 0x42 : not user-modifiable */
0x00, /* 0x43 : not user-modifiable */
0x00, /* 0x44 : not user-modifiable */
0x00, /* 0x45 : not user-modifiable */
0x20, /* 0x46 : interrupt configuration 0->level low detection, 1-> level high, 2-> Out of window, 3->In window, 0x20-> New sample ready , TBC */
0x0b, /* 0x47 : not user-modifiable */
0x00, /* 0x48 : not user-modifiable */
0x00, /* 0x49 : not user-modifiable */
0x02, /* 0x4a : not user-modifiable */
0x0a, /* 0x4b : not user-modifiable */
0x21, /* 0x4c : not user-modifiable */
0x00, /* 0x4d : not user-modifiable */
0x00, /* 0x4e : not user-modifiable */
0x05, /* 0x4f : not user-modifiable */
0x00, /* 0x50 : not user-modifiable */
0x00, /* 0x51 : not user-modifiable */
0x00, /* 0x52 : not user-modifiable */
0x00, /* 0x53 : not user-modifiable */
0xc8, /* 0x54 : not user-modifiable */
0x00, /* 0x55 : not user-modifiable */
0x00, /* 0x56 : not user-modifiable */
0x38, /* 0x57 : not user-modifiable */
0xff, /* 0x58 : not user-modifiable */
0x01, /* 0x59 : not user-modifiable */
0x00, /* 0x5a : not user-modifiable */
0x08, /* 0x5b : not user-modifiable */
0x00, /* 0x5c : not user-modifiable */
0x00, /* 0x5d : not user-modifiable */
0x01, /* 0x5e : not user-modifiable */
0xcc, /* 0x5f : not user-modifiable */
0x0f, /* 0x60 : not user-modifiable */
0x01, /* 0x61 : not user-modifiable */
0xf1, /* 0x62 : not user-modifiable */
0x0d, /* 0x63 : not user-modifiable */
0x01, /* 0x64 : Sigma threshold MSB (mm in 14.2 format for MSB+LSB), use SetSigmaThreshold(), default value 90 mm */
0x68, /* 0x65 : Sigma threshold LSB */
0x00, /* 0x66 : Min count Rate MSB (MCPS in 9.7 format for MSB+LSB), use SetSignalThreshold() */
0x80, /* 0x67 : Min count Rate LSB */
0x08, /* 0x68 : not user-modifiable */
0xb8, /* 0x69 : not user-modifiable */
0x00, /* 0x6a : not user-modifiable */
0x00, /* 0x6b : not user-modifiable */
0x00, /* 0x6c : Intermeasurement period MSB, 32 bits register, use SetIntermeasurementInMs() */
0x00, /* 0x6d : Intermeasurement period */
0x0f, /* 0x6e : Intermeasurement period */
0x89, /* 0x6f : Intermeasurement period LSB */
0x00, /* 0x70 : not user-modifiable */
0x00, /* 0x71 : not user-modifiable */
0x00, /* 0x72 : distance threshold high MSB (in mm, MSB+LSB), use SetD:tanceThreshold() */
0x00, /* 0x73 : distance threshold high LSB */
0x00, /* 0x74 : distance threshold low MSB ( in mm, MSB+LSB), use SetD:tanceThreshold() */
0x00, /* 0x75 : distance threshold low LSB */
0x00, /* 0x76 : not user-modifiable */
0x01, /* 0x77 : not user-modifiable */
0x0f, /* 0x78 : not user-modifiable */
0x0d, /* 0x79 : not user-modifiable */
0x0e, /* 0x7a : not user-modifiable */
0x0e, /* 0x7b : not user-modifiable */
0x00, /* 0x7c : not user-modifiable */
0x00, /* 0x7d : not user-modifiable */
0x02, /* 0x7e : not user-modifiable */
0xc7, /* 0x7f : ROI center, use SetROI() */
0xff, /* 0x80 : XY ROI (X=Width, Y=Height), use SetROI() */
0x9B, /* 0x81 : not user-modifiable */
0x00, /* 0x82 : not user-modifiable */
0x00, /* 0x83 : not user-modifiable */
0x00, /* 0x84 : not user-modifiable */
0x01, /* 0x85 : not user-modifiable */
0x00, /* 0x86 : clear interrupt, use ClearInterrupt() */
0x00 /* 0x87 : start ranging, use StartRanging() or StopRanging(), If you want an automatic start after VL53L1X_init() call, put 0x40 in location 0x87 */
};
static const uint8_t status_rtn[24] = { 255, 255, 255, 5, 2, 4, 1, 7, 3, 0,
255, 255, 9, 13, 255, 255, 255, 255, 10, 6,
255, 255, 11, 12
};
VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion)
{
VL53L1X_ERROR Status = 0;
pVersion->major = VL53L1X_IMPLEMENTATION_VER_MAJOR;
pVersion->minor = VL53L1X_IMPLEMENTATION_VER_MINOR;
pVersion->build = VL53L1X_IMPLEMENTATION_VER_SUB;
pVersion->revision = VL53L1X_IMPLEMENTATION_VER_REVISION;
return Status;
}
VL53L1X_ERROR VL53L1X_SetI2CAddress(uint16_t dev, uint8_t new_address)
{
VL53L1X_ERROR status = 0;
status |= VL53L1_WrByte(dev, VL53L1_I2C_SLAVE__DEVICE_ADDRESS, new_address >> 1);
return status;
}
VL53L1X_ERROR VL53L1X_SensorInit(uint16_t dev)
{
VL53L1X_ERROR status = 0;
uint8_t Addr = 0x00, tmp;
for (Addr = 0x2D; Addr <= 0x87; Addr++){
status |= VL53L1_WrByte(dev, Addr, VL51L1X_DEFAULT_CONFIGURATION[Addr - 0x2D]);
}
status |= VL53L1X_StartRanging(dev);
tmp = 0;
while(tmp==0){
status |= VL53L1X_CheckForDataReady(dev, &tmp);
}
status |= VL53L1X_ClearInterrupt(dev);
status |= VL53L1X_StopRanging(dev);
status |= VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */
status |= VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */
return status;
}
VL53L1X_ERROR VL53L1X_ClearInterrupt(uint16_t dev)
{
VL53L1X_ERROR status = 0;
status |= VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CLEAR, 0x01);
return status;
}
VL53L1X_ERROR VL53L1X_SetInterruptPolarity(uint16_t dev, uint8_t NewPolarity)
{
uint8_t Temp;
VL53L1X_ERROR status = 0;
status |= VL53L1_RdByte(dev, GPIO_HV_MUX__CTRL, &Temp);
Temp = Temp & 0xEF;
status |= VL53L1_WrByte(dev, GPIO_HV_MUX__CTRL, Temp | (!(NewPolarity & 1)) << 4);
return status;
}
VL53L1X_ERROR VL53L1X_GetInterruptPolarity(uint16_t dev, uint8_t *pInterruptPolarity)
{
uint8_t Temp;
VL53L1X_ERROR status = 0;
status |= VL53L1_RdByte(dev, GPIO_HV_MUX__CTRL, &Temp);
Temp = Temp & 0x10;
*pInterruptPolarity = !(Temp>>4);
return status;
}
VL53L1X_ERROR VL53L1X_StartRanging(uint16_t dev)
{
VL53L1X_ERROR status = 0;
status |= VL53L1_WrByte(dev, SYSTEM__MODE_START, 0x40); /* Enable VL53L1X */
return status;
}
VL53L1X_ERROR VL53L1X_StopRanging(uint16_t dev)
{
VL53L1X_ERROR status = 0;
status |= VL53L1_WrByte(dev, SYSTEM__MODE_START, 0x00); /* Disable VL53L1X */
return status;
}
VL53L1X_ERROR VL53L1X_CheckForDataReady(uint16_t dev, uint8_t *isDataReady)
{
uint8_t Temp;
uint8_t IntPol;
VL53L1X_ERROR status = 0;
status |= VL53L1X_GetInterruptPolarity(dev, &IntPol);
status |= VL53L1_RdByte(dev, GPIO__TIO_HV_STATUS, &Temp);
/* Read in the register to check if a new value is available */
if (status == 0){
if ((Temp & 1) == IntPol)
*isDataReady = 1;
else
*isDataReady = 0;
}
return status;
}
VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(uint16_t dev, uint16_t TimingBudgetInMs)
{
uint16_t DM;
VL53L1X_ERROR status=0;
status |= VL53L1X_GetDistanceMode(dev, &DM);
if (DM == 0)
return 1;
else if (DM == 1) { /* Short DistanceMode */
switch (TimingBudgetInMs) {
case 15: /* only available in short distance mode */
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
0x01D);
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
0x0027);
break;
case 20:
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
0x0051);
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
0x006E);
break;
case 33:
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
0x00D6);
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
0x006E);
break;
case 50:
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
0x1AE);
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
0x01E8);
break;
case 100:
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
0x02E1);
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
0x0388);
break;
case 200:
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
0x03E1);
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
0x0496);
break;
case 500:
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
0x0591);
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
0x05C1);
break;
default:
status = 1;
break;
}
} else {
switch (TimingBudgetInMs) {
case 20:
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
0x001E);
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
0x0022);
break;
case 33:
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
0x0060);
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
0x006E);
break;
case 50:
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
0x00AD);
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
0x00C6);
break;
case 100:
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
0x01CC);
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
0x01EA);
break;
case 200:
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
0x02D9);
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
0x02F8);
break;
case 500:
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
0x048F);
VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
0x04A4);
break;
default:
status = 1;
break;
}
}
return status;
}
VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(uint16_t dev, uint16_t *pTimingBudget)
{
uint16_t Temp;
VL53L1X_ERROR status = 0;
status |= VL53L1_RdWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, &Temp);
switch (Temp) {
case 0x001D :
*pTimingBudget = 15;
break;
case 0x0051 :
case 0x001E :
*pTimingBudget = 20;
break;
case 0x00D6 :
case 0x0060 :
*pTimingBudget = 33;
break;
case 0x1AE :
case 0x00AD :
*pTimingBudget = 50;
break;
case 0x02E1 :
case 0x01CC :
*pTimingBudget = 100;
break;
case 0x03E1 :
case 0x02D9 :
*pTimingBudget = 200;
break;
case 0x0591 :
case 0x048F :
*pTimingBudget = 500;
break;
default:
status = 1;
*pTimingBudget = 0;
}
return status;
}
VL53L1X_ERROR VL53L1X_SetDistanceMode(uint16_t dev, uint16_t DM)
{
uint16_t TB;
VL53L1X_ERROR status = 0;
status |= VL53L1X_GetTimingBudgetInMs(dev, &TB);
if (status != 0)
return 1;
switch (DM) {
case 1:
status = VL53L1_WrByte(dev, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x14);
status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_A, 0x07);
status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_B, 0x05);
status = VL53L1_WrByte(dev, RANGE_CONFIG__VALID_PHASE_HIGH, 0x38);
status = VL53L1_WrWord(dev, SD_CONFIG__WOI_SD0, 0x0705);
status = VL53L1_WrWord(dev, SD_CONFIG__INITIAL_PHASE_SD0, 0x0606);
break;
case 2:
status = VL53L1_WrByte(dev, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x0A);
status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F);
status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D);
status = VL53L1_WrByte(dev, RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8);
status = VL53L1_WrWord(dev, SD_CONFIG__WOI_SD0, 0x0F0D);
status = VL53L1_WrWord(dev, SD_CONFIG__INITIAL_PHASE_SD0, 0x0E0E);
break;
default:
status = 1;
break;
}
if (status == 0)
status |= VL53L1X_SetTimingBudgetInMs(dev, TB);
return status;
}
VL53L1X_ERROR VL53L1X_GetDistanceMode(uint16_t dev, uint16_t *DM)
{
uint8_t TempDM, status=0;
status |= VL53L1_RdByte(dev,PHASECAL_CONFIG__TIMEOUT_MACROP, &TempDM);
if (TempDM == 0x14)
*DM=1;
if(TempDM == 0x0A)
*DM=2;
return status;
}
VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(uint16_t dev, uint32_t InterMeasMs)
{
uint16_t ClockPLL;
VL53L1X_ERROR status = 0;
status |= VL53L1_RdWord(dev, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL);
ClockPLL = ClockPLL&0x3FF;
VL53L1_WrDWord(dev, VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD,
(uint32_t)(ClockPLL * InterMeasMs * 1.075));
return status;
}
VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(uint16_t dev, uint16_t *pIM)
{
uint16_t ClockPLL;
VL53L1X_ERROR status = 0;
uint32_t tmp;
status |= VL53L1_RdDWord(dev,VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, &tmp);
*pIM = (uint16_t)tmp;
status |= VL53L1_RdWord(dev, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL);
ClockPLL = ClockPLL&0x3FF;
*pIM= (uint16_t)(*pIM/(ClockPLL*1.065));
return status;
}
VL53L1X_ERROR VL53L1X_BootState(uint16_t dev, uint8_t *state)
{
VL53L1X_ERROR status = 0;
uint8_t tmp = 0;
status |= VL53L1_RdByte(dev,VL53L1_FIRMWARE__SYSTEM_STATUS, &tmp);
*state = tmp;
return status;
}
VL53L1X_ERROR VL53L1X_GetSensorId(uint16_t dev, uint16_t *sensorId)
{
VL53L1X_ERROR status = 0;
uint16_t tmp = 0;
status |= VL53L1_RdWord(dev, VL53L1_IDENTIFICATION__MODEL_ID, &tmp);
*sensorId = tmp;
return status;
}
VL53L1X_ERROR VL53L1X_GetDistance(uint16_t dev, uint16_t *distance)
{
VL53L1X_ERROR status = 0;
uint16_t tmp;
status |= (VL53L1_RdWord(dev,
VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, &tmp));
*distance = tmp;
return status;
}
VL53L1X_ERROR VL53L1X_GetSignalPerSpad(uint16_t dev, uint16_t *signalRate)
{
VL53L1X_ERROR status = 0;
uint16_t SpNb=1, signal;
status |= VL53L1_RdWord(dev,
VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &signal);
status |= VL53L1_RdWord(dev,
VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb);
*signalRate = (uint16_t) (200.0*signal/SpNb);
return status;
}
VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(uint16_t dev, uint16_t *ambPerSp)
{
VL53L1X_ERROR status = 0;
uint16_t AmbientRate, SpNb = 1;
status |= VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &AmbientRate);
status |= VL53L1_RdWord(dev, VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb);
*ambPerSp=(uint16_t) (200.0 * AmbientRate / SpNb);
return status;
}
VL53L1X_ERROR VL53L1X_GetSignalRate(uint16_t dev, uint16_t *signal)
{
VL53L1X_ERROR status = 0;
uint16_t tmp;
status |= VL53L1_RdWord(dev,
VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &tmp);
*signal = tmp*8;
return status;
}
VL53L1X_ERROR VL53L1X_GetSpadNb(uint16_t dev, uint16_t *spNb)
{
VL53L1X_ERROR status = 0;
uint16_t tmp;
status |= VL53L1_RdWord(dev,
VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &tmp);
*spNb = tmp >> 8;
return status;
}
VL53L1X_ERROR VL53L1X_GetAmbientRate(uint16_t dev, uint16_t *ambRate)
{
VL53L1X_ERROR status = 0;
uint16_t tmp;
status |= VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &tmp);
*ambRate = tmp*8;
return status;
}
VL53L1X_ERROR VL53L1X_GetRangeStatus(uint16_t dev, uint8_t *rangeStatus)
{
VL53L1X_ERROR status = 0;
uint8_t RgSt;
*rangeStatus = 255;
status |= VL53L1_RdByte(dev, VL53L1_RESULT__RANGE_STATUS, &RgSt);
RgSt = RgSt & 0x1F;
if (RgSt < 24)
*rangeStatus = status_rtn[RgSt];
return status;
}
VL53L1X_ERROR VL53L1X_GetResult(uint16_t dev, VL53L1X_Result_t *pResult)
{
VL53L1X_ERROR status = 0;
uint8_t Temp[17];
uint8_t RgSt = 255;
status |= VL53L1_ReadMulti(dev, VL53L1_RESULT__RANGE_STATUS, Temp, 17);
RgSt = Temp[0] & 0x1F;
if (RgSt < 24)
RgSt = status_rtn[RgSt];
pResult->Status = RgSt;
pResult->Ambient = (Temp[7] << 8 | Temp[8]) * 8;
pResult->NumSPADs = Temp[3];
pResult->SigPerSPAD = (Temp[15] << 8 | Temp[16]) * 8;
pResult->Distance = Temp[13] << 8 | Temp[14];
return status;
}
VL53L1X_ERROR VL53L1X_SetOffset(uint16_t dev, int16_t OffsetValue)
{
VL53L1X_ERROR status = 0;
int16_t Temp;
Temp = (OffsetValue*4);
status |= VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM,
(uint16_t)Temp);
status |= VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0);
status |= VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0);
return status;
}
VL53L1X_ERROR VL53L1X_GetOffset(uint16_t dev, int16_t *offset)
{
VL53L1X_ERROR status = 0;
uint16_t Temp;
status |= VL53L1_RdWord(dev,ALGO__PART_TO_PART_RANGE_OFFSET_MM, &Temp);
Temp = Temp<<3;
Temp = Temp>>5;
*offset = (int16_t)(Temp);
if(*offset > 1024)
{
*offset = *offset - 2048;
}
return status;
}
VL53L1X_ERROR VL53L1X_SetXtalk(uint16_t dev, uint16_t XtalkValue)
{
/* XTalkValue in count per second to avoid float type */
VL53L1X_ERROR status = 0;
status |= VL53L1_WrWord(dev,
ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS,
0x0000);
status |= VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS,
0x0000);
status |= VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS,
(XtalkValue<<9)/1000); /* * << 9 (7.9 format) and /1000 to convert cps to kpcs */
return status;
}
VL53L1X_ERROR VL53L1X_GetXtalk(uint16_t dev, uint16_t *xtalk )
{
VL53L1X_ERROR status = 0;
status |= VL53L1_RdWord(dev,ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, xtalk);
*xtalk = (uint16_t)((*xtalk*1000)>>9); /* * 1000 to convert kcps to cps and >> 9 (7.9 format) */
return status;
}
VL53L1X_ERROR VL53L1X_SetDistanceThreshold(uint16_t dev, uint16_t ThreshLow,
uint16_t ThreshHigh, uint8_t Window,
uint8_t IntOnNoTarget)
{
VL53L1X_ERROR status = 0;
uint8_t Temp = 0;
status |= VL53L1_RdByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, &Temp);
Temp = Temp & 0x47;
if (IntOnNoTarget == 0) {
status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO,
(Temp | (Window & 0x07)));
} else {
status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO,
((Temp | (Window & 0x07)) | 0x40));
}
status |= VL53L1_WrWord(dev, SYSTEM__THRESH_HIGH, ThreshHigh);
status |= VL53L1_WrWord(dev, SYSTEM__THRESH_LOW, ThreshLow);
return status;
}
VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(uint16_t dev, uint16_t *window)
{
VL53L1X_ERROR status = 0;
uint8_t tmp;
status |= VL53L1_RdByte(dev,SYSTEM__INTERRUPT_CONFIG_GPIO, &tmp);
*window = (uint16_t)(tmp & 0x7);
return status;
}
VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(uint16_t dev, uint16_t *low)
{
VL53L1X_ERROR status = 0;
uint16_t tmp;
status |= VL53L1_RdWord(dev,SYSTEM__THRESH_LOW, &tmp);
*low = tmp;
return status;
}
VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(uint16_t dev, uint16_t *high)
{
VL53L1X_ERROR status = 0;
uint16_t tmp;
status |= VL53L1_RdWord(dev,SYSTEM__THRESH_HIGH, &tmp);
*high = tmp;
return status;
}
VL53L1X_ERROR VL53L1X_SetROICenter(uint16_t dev, uint8_t ROICenter)
{
VL53L1X_ERROR status = 0;
status |= VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, ROICenter);
return status;
}
VL53L1X_ERROR VL53L1X_GetROICenter(uint16_t dev, uint8_t *ROICenter)
{
VL53L1X_ERROR status = 0;
uint8_t tmp;
status |= VL53L1_RdByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, &tmp);
*ROICenter = tmp;
return status;
}
VL53L1X_ERROR VL53L1X_SetROI(uint16_t dev, uint16_t X, uint16_t Y)
{
uint8_t OpticalCenter;
VL53L1X_ERROR status = 0;
status |=VL53L1_RdByte(dev, VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD, &OpticalCenter);
if (X > 16)
X = 16;
if (Y > 16)
Y = 16;
if (X > 10 || Y > 10){
OpticalCenter = 199;
}
status |= VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, OpticalCenter);
status |= VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE,
(Y - 1) << 4 | (X - 1));
return status;
}
VL53L1X_ERROR VL53L1X_GetROI_XY(uint16_t dev, uint16_t *ROI_X, uint16_t *ROI_Y)
{
VL53L1X_ERROR status = 0;
uint8_t tmp;
status = VL53L1_RdByte(dev,ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, &tmp);
*ROI_X |= ((uint16_t)tmp & 0x0F) + 1;
*ROI_Y |= (((uint16_t)tmp & 0xF0) >> 4) + 1;
return status;
}
VL53L1X_ERROR VL53L1X_SetSignalThreshold(uint16_t dev, uint16_t Signal)
{
VL53L1X_ERROR status = 0;
status |= VL53L1_WrWord(dev,RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,Signal>>3);
return status;
}
VL53L1X_ERROR VL53L1X_GetSignalThreshold(uint16_t dev, uint16_t *signal)
{
VL53L1X_ERROR status = 0;
uint16_t tmp;
status |= VL53L1_RdWord(dev,
RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, &tmp);
*signal = tmp <<3;
return status;
}
VL53L1X_ERROR VL53L1X_SetSigmaThreshold(uint16_t dev, uint16_t Sigma)
{
VL53L1X_ERROR status = 0;
if(Sigma>(0xFFFF>>2)){
return 1;
}
/* 16 bits register 14.2 format */
status |= VL53L1_WrWord(dev,RANGE_CONFIG__SIGMA_THRESH,Sigma<<2);
return status;
}
VL53L1X_ERROR VL53L1X_GetSigmaThreshold(uint16_t dev, uint16_t *sigma)
{
VL53L1X_ERROR status = 0;
uint16_t tmp;
status |= VL53L1_RdWord(dev,RANGE_CONFIG__SIGMA_THRESH, &tmp);
*sigma = tmp >> 2;
return status;
}
VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(uint16_t dev)
{
VL53L1X_ERROR status = 0;
uint8_t tmp=0;
status |= VL53L1_WrByte(dev,VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,0x81); /* full VHV */
status |= VL53L1_WrByte(dev,0x0B,0x92);
status |= VL53L1X_StartRanging(dev);
while(tmp==0){
status |= VL53L1X_CheckForDataReady(dev, &tmp);
}
tmp = 0;
status |= VL53L1X_ClearInterrupt(dev);
status |= VL53L1X_StopRanging(dev);
status |= VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */
status |= VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */
return status;
}

138
Firmware/Core/Src/VL53L1X_calibration.c Normal file
View File

@ -0,0 +1,138 @@
/*
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
*
* This file : part of VL53L1 Core and : dual licensed,
* either 'STMicroelectronics
* Proprietary license'
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
*
********************************************************************************
*
* 'STMicroelectronics Proprietary license'
*
********************************************************************************
*
* License terms: STMicroelectronics Proprietary in accordance with licensing
* terms at www.st.com/sla0081
*
* STMicroelectronics confidential
* Reproduction and Communication of this document : strictly prohibited unless
* specifically authorized in writing by STMicroelectronics.
*
*
********************************************************************************
*
* Alternatively, VL53L1 Core may be distributed under the terms of
* 'BSD 3-clause "New" or "Revised" License', in which case the following
* provisions apply instead of the ones mentioned above :
*
********************************************************************************
*
* 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.
*
*
********************************************************************************
*
*/
/**
* @file vl53l1x_calibration.c
* @brief Calibration functions implementation
*/
#include "VL53L1X_api.h"
#include "VL53L1X_calibration.h"
#define ALGO__PART_TO_PART_RANGE_OFFSET_MM 0x001E
#define MM_CONFIG__INNER_OFFSET_MM 0x0020
#define MM_CONFIG__OUTER_OFFSET_MM 0x0022
int8_t VL53L1X_CalibrateOffset(uint16_t dev, uint16_t TargetDistInMm, int16_t *offset)
{
uint8_t i, tmp;
int16_t AverageDistance = 0;
uint16_t distance;
VL53L1X_ERROR status = 0;
status |= VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, 0x0);
status |= VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0);
status |= VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0);
status |= VL53L1X_StartRanging(dev); /* Enable VL53L1X sensor */
for (i = 0; i < 50; i++) {
tmp = 0;
while (tmp == 0){
status |= VL53L1X_CheckForDataReady(dev, &tmp);
}
status |= VL53L1X_GetDistance(dev, &distance);
status |= VL53L1X_ClearInterrupt(dev);
AverageDistance = AverageDistance + distance;
}
status |= VL53L1X_StopRanging(dev);
AverageDistance = AverageDistance / 50;
*offset = TargetDistInMm - AverageDistance;
status |= VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, *offset*4);
return status;
}
int8_t VL53L1X_CalibrateXtalk(uint16_t dev, uint16_t TargetDistInMm, uint16_t *xtalk)
{
uint8_t i, tmp;
float AverageSignalRate = 0;
float AverageDistance = 0;
float AverageSpadNb = 0;
uint16_t distance = 0, spadNum;
uint16_t sr;
VL53L1X_ERROR status = 0;
uint32_t calXtalk;
status |= VL53L1_WrWord(dev, 0x0016,0);
status |= VL53L1X_StartRanging(dev);
for (i = 0; i < 50; i++) {
tmp = 0;
while (tmp == 0){
status |= VL53L1X_CheckForDataReady(dev, &tmp);
}
status |= VL53L1X_GetSignalRate(dev, &sr);
status |= VL53L1X_GetDistance(dev, &distance);
status |= VL53L1X_ClearInterrupt(dev);
AverageDistance = AverageDistance + distance;
status = VL53L1X_GetSpadNb(dev, &spadNum);
AverageSpadNb = AverageSpadNb + spadNum;
AverageSignalRate =
AverageSignalRate + sr;
}
status |= VL53L1X_StopRanging(dev);
AverageDistance = AverageDistance / 50;
AverageSpadNb = AverageSpadNb / 50;
AverageSignalRate = AverageSignalRate / 50;
/* Calculate Xtalk value */
calXtalk = (uint16_t)(512*(AverageSignalRate*(1-(AverageDistance/TargetDistInMm)))/AverageSpadNb);
if(calXtalk > 0xffff)
calXtalk = 0xffff;
*xtalk = (uint16_t)((calXtalk*1000)>>9);
status |= VL53L1_WrWord(dev, 0x0016, (uint16_t)calXtalk);
return status;
}

67
Firmware/Core/Src/main.c
View File

@ -40,7 +40,6 @@
/* Private variables ---------------------------------------------------------*/
I2C_HandleTypeDef hi2c1;
SPI_HandleTypeDef hspi1;
DMA_HandleTypeDef hdma_spi1_rx;
DMA_HandleTypeDef hdma_spi1_tx;
@ -49,6 +48,11 @@ UART_HandleTypeDef huart2;
PCD_HandleTypeDef hpcd_USB_FS;
uint16_t dev=0x52;
int status=0;
volatile int IntCount;
#define isInterrupt 1 /* If isInterrupt = 1 then device working in interrupt mode, else device working in polling mode */
/* USER CODE BEGIN PV */
//DAC_HandleTypeDef hdac1;
@ -82,6 +86,16 @@ static void MX_USB_PCD_Init(void);
int main(void)
{
/* USER CODE BEGIN 1 */
//uint8_t byteData, sensorState=0;
//uint16_t wordData;
//uint8_t ToFSensor = 1; // 0=Left, 1=Center(default), 2=Right
uint16_t Distance = 0;
uint16_t SignalRate;
uint16_t AmbientRate;
uint16_t SpadNum;
uint8_t RangeStatus;
//uint8_t dataReady;
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
@ -109,9 +123,28 @@ int main(void)
MX_I2C1_Init();
MX_USB_PCD_Init();
/* USER CODE BEGIN 2 */
HAL_PWREx_EnableVddUSB();
HAL_Delay(100);
//ToFSensor = 1; // Select ToFSensor: 0=Left, 1=Center, 2=Right
status = VL53L1X_SensorInit(dev);
status = VL53L1X_SetDistanceMode(dev, 2);
status = VL53L1X_SetTimingBudgetInMs(dev, 100);
status = VL53L1X_SetInterMeasurementInMs(dev, 100);
status = VL53L1X_StartRanging(dev);
HAL_Delay(1000);
status = VL53L1X_GetRangeStatus(dev, &RangeStatus);
status = VL53L1X_GetDistance(dev, &Distance);
status = VL53L1X_GetSignalRate(dev, &SignalRate);
status = VL53L1X_GetAmbientRate(dev, &AmbientRate);
status = VL53L1X_GetSpadNb(dev, &SpadNum);
status = VL53L1X_ClearInterrupt(dev);
LED_On();
HAL_PWREx_EnableVddUSB();
HAL_Delay(1);
tud_init(BOARD_DEVICE_RHPORT_NUM);
@ -127,6 +160,7 @@ int main(void)
uint8_t cdc_buff[CDC_BUFF_SIZE+CDC_FRAME_SIZE];
for(int i = 0; i < (CDC_BUFF_SIZE+CDC_FRAME_SIZE); i++) cdc_buff[i] = 0x00;
//Distance_Sensor_Init(&hi2c1);
Cam_Init(&hi2c1, &hspi1);
@ -146,6 +180,10 @@ int main(void)
last_sent_idx = 0;
//for(int i = 0; i < CDC_BUFF_SIZE; i++) cdc_buff[i] = 0x00;
VL53L1X_GetDistance(dev, &Distance);
HAL_Delay(2);
VL53L1X_ClearInterrupt(dev);
CS_Off();
CS_On();
@ -155,6 +193,8 @@ int main(void)
image_size = Cam_FIFO_length(&hspi1);
Cam_Start_Burst_Read(&hspi1);
//getDistance(&hi2c1);
LED_On();
continue;
@ -198,6 +238,29 @@ int main(void)
}
while(last_sent_idx < buff_stop_idx);
if(buff_stop_idx >= (int)image_size){
HAL_Delay(100);
tud_task();
uint8_t distance_buff[12];
distance_buff[0] = 0xff;
distance_buff[1] = 0xff;
distance_buff[2] = 0xff;
distance_buff[3] = 0x69;
distance_buff[4] = (uint8_t)(Distance >> 8);
distance_buff[5] = (Distance & 0xff);
distance_buff[6] = 0xff;
distance_buff[7] = 0x69;
distance_buff[8] = 0xff;
distance_buff[9] = 0xff;
distance_buff[10] = '\n';
distance_buff[11] = '\r';
tud_cdc_write(&distance_buff[0], 12);
HAL_Delay(1);
tud_cdc_write_flush();
HAL_Delay(1);
tud_task();
}
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */

78
Firmware/Core/Src/syscalls.c Normal file
View File

@ -0,0 +1,78 @@
/*
* This file is create on December 8 2017 from the original syscalls.c to allow using the _write function in AC6
*
* 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.
*
*/
/*
*******************************************************************************
**
** File : syscalls.c
**
** Abstract : System Workbench Minimal System calls file
**
** For more information about which c-functions
** need which of these lowlevel functions
** please consult the Newlib libc-manual
**
** Environment : System Workbench for MCU
**
** Distribution: The file is distributed <20>as is,<2C> without any warranty
** of any kind.
**
** (c)Copyright System Workbench for MCU.
** You may use this file as-is or modify it according to the needs of your
** project. Distribution of this file (unmodified or modified) is not
** permitted. System Workbench for MCU permit registered System Workbench(R) users the
** rights to distribute the assembled, compiled & linked contents of this
** file as part of an application binary file, provided that it is built
** using the System Workbench for MCU toolchain.
**
*****************************************************************************
*/
/*
extern int fputc(int ch) __attribute__((weak));
int _write(int file, char *ptr, int len)
{
int DataIdx;
for (DataIdx = 0; DataIdx < len; DataIdx++)
{
fputc(*ptr++);
}
return len;
}
*/

458
Firmware/Core/Src/vl53l1_platform.c Normal file
View File

@ -0,0 +1,458 @@
/*
* 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.
*
*/
#include "vl53l1_platform.h"
#include "vl53l1_platform_log.h"
//#include "vl53l1_api.h"
#include "vl53l1_platform_user_config.h"
#include "stm32xxx_hal.h"
#include "main.h"
#include <string.h>
#include <time.h>
#include <math.h>
#include "vl53l1_error_codes.h"
//#include "X-NUCLEO-53L1A1.h"
#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
#ifndef HAL_I2C_MODULE_ENABLED
#warning "HAL I2C module must be enable "
#endif
//#define VL53L0X_pI2cHandle (&hi2c1)
/* 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];
int _I2CWrite(uint16_t Dev, uint8_t *pdata, uint32_t count) {
int status;
//int i2c_time_out = I2C_TIME_OUT_BASE+ count* I2C_TIME_OUT_BYTE;
status = HAL_I2C_Master_Transmit(&hi2c1, Dev, pdata, count, HAL_MAX_DELAY);
if (status) {
//VL6180x_ErrLog("I2C error 0x%x %d len", dev->I2cAddr, len);
//XNUCLEO6180XA1_I2C1_Init(&hi2c1);
}
return status;
}
int _I2CRead(uint16_t Dev, uint8_t *pdata, uint32_t count) {
int status;
//int i2c_time_out = I2C_TIME_OUT_BASE+ count* I2C_TIME_OUT_BYTE;
status = HAL_I2C_Master_Receive(&hi2c1, Dev|1, pdata, count, HAL_MAX_DELAY);
if (status) {
//VL6180x_ErrLog("I2C error 0x%x %d len", dev->I2cAddr, len);
//XNUCLEO6180XA1_I2C1_Init(&hi2c1);
}
return status;
}
int8_t VL53L1_WriteMulti(uint16_t Dev, uint16_t index, uint8_t *pdata, uint32_t count) {
int status_int;
VL53L1_Error Status = VL53L1_ERROR_NONE;
if (count > sizeof(_I2CBuffer) - 1) {
return VL53L1_ERROR_INVALID_PARAMS;
}
_I2CBuffer[0] = index>>8;
_I2CBuffer[1] = index&0xFF;
memcpy(&_I2CBuffer[2], pdata, count);
VL53L1_GetI2cBus();
status_int = _I2CWrite(Dev, _I2CBuffer, count + 2);
if (status_int != 0) {
Status = VL53L1_ERROR_CONTROL_INTERFACE;
}
VL53L1_PutI2cBus();
return Status;
}
// the ranging_sensor_comms.dll will take care of the page selection
VL53L1_Error VL53L1_ReadMulti(uint16_t Dev, uint16_t index, uint8_t *pdata, uint32_t count) {
VL53L1_Error Status = VL53L1_ERROR_NONE;
int32_t status_int;
_I2CBuffer[0] = index>>8;
_I2CBuffer[1] = index&0xFF;
VL53L1_GetI2cBus();
status_int = _I2CWrite(Dev, _I2CBuffer, 2);
if (status_int != 0) {
Status = VL53L1_ERROR_CONTROL_INTERFACE;
goto done;
}
status_int = _I2CRead(Dev, pdata, count);
if (status_int != 0) {
Status = VL53L1_ERROR_CONTROL_INTERFACE;
}
done:
VL53L1_PutI2cBus();
return Status;
}
VL53L1_Error VL53L1_WrByte(uint16_t Dev, uint16_t index, uint8_t data) {
VL53L1_Error Status = VL53L1_ERROR_NONE;
int32_t status_int;
_I2CBuffer[0] = index>>8;
_I2CBuffer[1] = index&0xFF;
_I2CBuffer[2] = data;
VL53L1_GetI2cBus();
status_int = _I2CWrite(Dev, _I2CBuffer, 3);
if (status_int != 0) {
Status = VL53L1_ERROR_CONTROL_INTERFACE;
}
VL53L1_PutI2cBus();
return Status;
}
VL53L1_Error VL53L1_WrWord(uint16_t Dev, uint16_t index, uint16_t data) {
VL53L1_Error Status = VL53L1_ERROR_NONE;
int32_t status_int;
_I2CBuffer[0] = index>>8;
_I2CBuffer[1] = index&0xFF;
_I2CBuffer[2] = data >> 8;
_I2CBuffer[3] = data & 0x00FF;
VL53L1_GetI2cBus();
status_int = _I2CWrite(Dev, _I2CBuffer, 4);
if (status_int != 0) {
Status = VL53L1_ERROR_CONTROL_INTERFACE;
}
VL53L1_PutI2cBus();
return Status;
}
VL53L1_Error VL53L1_WrDWord(uint16_t Dev, uint16_t index, uint32_t data) {
VL53L1_Error Status = VL53L1_ERROR_NONE;
int32_t status_int;
_I2CBuffer[0] = index>>8;
_I2CBuffer[1] = index&0xFF;
_I2CBuffer[2] = (data >> 24) & 0xFF;
_I2CBuffer[3] = (data >> 16) & 0xFF;
_I2CBuffer[4] = (data >> 8) & 0xFF;
_I2CBuffer[5] = (data >> 0 ) & 0xFF;
VL53L1_GetI2cBus();
status_int = _I2CWrite(Dev, _I2CBuffer, 6);
if (status_int != 0) {
Status = VL53L1_ERROR_CONTROL_INTERFACE;
}
VL53L1_PutI2cBus();
return Status;
}
VL53L1_Error VL53L1_UpdateByte(uint16_t Dev, uint16_t index, uint8_t AndData, uint8_t OrData) {
VL53L1_Error Status = VL53L1_ERROR_NONE;
uint8_t data;
Status = VL53L1_RdByte(Dev, index, &data);
if (Status) {
goto done;
}
data = (data & AndData) | OrData;
Status = VL53L1_WrByte(Dev, index, data);
done:
return Status;
}
VL53L1_Error VL53L1_RdByte(uint16_t Dev, uint16_t index, uint8_t *data) {
VL53L1_Error Status = VL53L1_ERROR_NONE;
int32_t status_int;
_I2CBuffer[0] = index>>8;
_I2CBuffer[1] = index&0xFF;
VL53L1_GetI2cBus();
status_int = _I2CWrite(Dev, _I2CBuffer, 2);
if( status_int ){
Status = VL53L1_ERROR_CONTROL_INTERFACE;
goto done;
}
status_int = _I2CRead(Dev, data, 1);
if (status_int != 0) {
Status = VL53L1_ERROR_CONTROL_INTERFACE;
}
done:
VL53L1_PutI2cBus();
return Status;
}
int8_t VL53L1_RdWord(uint16_t Dev, uint16_t index, uint16_t *data) {
VL53L1_Error Status = VL53L1_ERROR_NONE;
int32_t status_int;
_I2CBuffer[0] = index>>8;
_I2CBuffer[1] = index&0xFF;
VL53L1_GetI2cBus();
status_int = _I2CWrite(Dev, _I2CBuffer, 2);
if( status_int ){
Status = VL53L1_ERROR_CONTROL_INTERFACE;
goto done;
}
status_int = _I2CRead(Dev, _I2CBuffer, 2);
if (status_int != 0) {
Status = VL53L1_ERROR_CONTROL_INTERFACE;
goto done;
}
*data = ((uint16_t)_I2CBuffer[0]<<8) + (uint16_t)_I2CBuffer[1];
done:
VL53L1_PutI2cBus();
return Status;
}
int8_t VL53L1_RdDWord(uint16_t Dev, uint16_t index, uint32_t *data) {
VL53L1_Error Status = VL53L1_ERROR_NONE;
int32_t status_int;
_I2CBuffer[0] = index>>8;
_I2CBuffer[1] = index&0xFF;
VL53L1_GetI2cBus();
status_int = _I2CWrite(Dev, _I2CBuffer, 2);
if (status_int != 0) {
Status = VL53L1_ERROR_CONTROL_INTERFACE;
goto done;
}
status_int = _I2CRead(Dev, _I2CBuffer, 4);
if (status_int != 0) {
Status = VL53L1_ERROR_CONTROL_INTERFACE;
goto done;
}
*data = ((uint32_t)_I2CBuffer[0]<<24) + ((uint32_t)_I2CBuffer[1]<<16) + ((uint32_t)_I2CBuffer[2]<<8) + (uint32_t)_I2CBuffer[3];
done:
VL53L1_PutI2cBus();
return Status;
}
VL53L1_Error VL53L1_GetTickCount(
uint32_t *ptick_count_ms)
{
/* Returns current tick count in [ms] */
VL53L1_Error status = VL53L1_ERROR_NONE;
//*ptick_count_ms = timeGetTime();
*ptick_count_ms = 0;
#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__)
VL53L1_Error VL53L1_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 VL53L1_ERROR_NONE;
}
VL53L1_Error VL53L1_WaitMs(uint16_t dev, int32_t wait_ms){
(void)dev;
HAL_Delay(wait_ms);
return VL53L1_ERROR_NONE;
}
VL53L1_Error VL53L1_WaitUs(uint16_t dev, int32_t wait_us){
(void)dev;
HAL_Delay(wait_us/1000);
return VL53L1_ERROR_NONE;
}
VL53L1_Error VL53L1_WaitValueMaskEx(
uint16_t 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);
*/
VL53L1_Error status = VL53L1_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[VL53L1_MAX_STRING_LENGTH];
/* look up register name */
#ifdef PAL_EXTENDED
VL53L1_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 */
VL53L1_GetTickCount(&start_time_ms);
/* 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 == VL53L1_ERROR_NONE) &&
(polling_time_ms < timeout_ms) &&
(found == 0)) {
if (status == VL53L1_ERROR_NONE)
status = VL53L1_RdByte(
dev,
index,
&byte_value);
if ((byte_value & mask) == value)
found = 1;
if (status == VL53L1_ERROR_NONE &&
found == 0 &&
poll_delay_ms > 0)
status = VL53L1_WaitMs(
dev,
poll_delay_ms);
/* Update polling time (Compare difference rather than absolute to
negate 32bit wrap around issue) */
VL53L1_GetTickCount(&current_time_ms);
polling_time_ms = current_time_ms - start_time_ms;
}
#ifdef VL53L1_LOG_ENABLE
/* Restore function logging */
VL53L1_set_trace_functions(trace_functions);
#endif
if (found == 0 && status == VL53L1_ERROR_NONE)
status = VL53L1_ERROR_TIME_OUT;
return status;
}