101 lines
3.1 KiB
C++
101 lines
3.1 KiB
C++
#include "vl53l0x.h"
|
|
|
|
|
|
bool checkResult(VL53L0X_Error error, VL53L0XResult* result, VL53L0XPosition position)
|
|
{
|
|
if (error != VL53L0X_ERROR_NONE)
|
|
{
|
|
result->error = error;
|
|
result->position = position;
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
bool VL53L0X::init(uint8_t address, VL53L0XResult* result)
|
|
{
|
|
device.I2cDevAddr = 0x29;
|
|
device.comms_type = 1;
|
|
device.comms_speed_khz = 400;
|
|
device.i2c = &Wire;
|
|
|
|
if (!checkResult(VL53L0X_DataInit(&device), result, VL53L0X_POSITION_DATAINIT))
|
|
return false;
|
|
|
|
|
|
auto setAddress = address & 0x7F;
|
|
|
|
// * 2 to convert from 7 to 8 bits
|
|
if (!checkResult(VL53L0X_SetDeviceAddress(&device, setAddress * 2), result, VL53L0X_POSITION_SETDEVICEADDRESS))
|
|
return false;
|
|
|
|
delay(10);
|
|
device.I2cDevAddr = setAddress;
|
|
|
|
VL53L0X_DeviceInfo_t deviceInfo;
|
|
if (!checkResult(VL53L0X_GetDeviceInfo(&device, &deviceInfo), result, VL53L0X_POSITION_GETDEVICEINFO))
|
|
return false;
|
|
|
|
if (!checkResult(VL53L0X_StaticInit(&device), result, VL53L0X_POSITION_STATICINIT))
|
|
return false;
|
|
|
|
uint32_t refSpadCount;
|
|
uint8_t isApertureSpads;
|
|
|
|
if (!checkResult(VL53L0X_PerformRefSpadManagement(&device, &refSpadCount, &isApertureSpads), result, VL53L0X_POSITION_REFSPAD))
|
|
return false;
|
|
|
|
// TODO expose as API to be run before changing desk positions
|
|
uint8_t vhvSettings;
|
|
uint8_t phaseCal;
|
|
|
|
if (!checkResult(VL53L0X_PerformRefCalibration(&device, &vhvSettings, &phaseCal), result, VL53L0X_POSITION_REFCAL))
|
|
return false;
|
|
|
|
if (!checkResult(VL53L0X_SetDeviceMode(&device, VL53L0X_DEVICEMODE_SINGLE_RANGING), result, VL53L0X_POSITION_DEVICEMODE))
|
|
return false;
|
|
|
|
if (!checkResult(VL53L0X_SetLimitCheckEnable(&device, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1), result, VL53L0X_POSITION_LIMITRANGE))
|
|
return false;
|
|
|
|
if (!checkResult(VL53L0X_SetLimitCheckEnable(&device, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1), result, VL53L0X_POSITION_LIMITRATE))
|
|
return false;
|
|
|
|
if (!checkResult(VL53L0X_SetLimitCheckEnable(&device, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 1), result, VL53L0X_POSITION_TRESHOLD1))
|
|
return false;
|
|
|
|
if (!checkResult(VL53L0X_SetLimitCheckValue(&device, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, (FixPoint1616_t)( 1.5 * 0.023 * 65536)), result, VL53L0X_POSITION_TRESHOLD2))
|
|
return false;
|
|
|
|
result->error = VL53L0X_ERROR_NONE;
|
|
result->position = VL53L0X_POSITION_UNKNOWN;
|
|
return true;
|
|
}
|
|
|
|
|
|
bool VL53L0X::setMeasurementTimingBudget(uint32_t budget_us, VL53L0X_Error* error)
|
|
{
|
|
*error = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&device, budget_us);
|
|
return (*error == VL53L0X_ERROR_NONE);
|
|
}
|
|
|
|
|
|
bool VL53L0X::getSingleRangingMeasurement(VL53L0X_RangingMeasurementData_t *data, VL53L0X_Error* error)
|
|
{
|
|
*error = VL53L0X_PerformSingleRangingMeasurement(&device, data);
|
|
return (*error == VL53L0X_ERROR_NONE);
|
|
}
|
|
|
|
|
|
bool VL53L0X::getSingleRangingMeasurement(uint16_t* distanceMillimeters, VL53L0X_Error* error, uint16_t maxValidHeight)
|
|
{
|
|
VL53L0X_RangingMeasurementData_t data;
|
|
auto result = this->getSingleRangingMeasurement(&data, error);
|
|
|
|
if (result)
|
|
*distanceMillimeters = data.RangeMilliMeter;
|
|
|
|
return result && data.RangeMilliMeter <= maxValidHeight;
|
|
} |