Error handling and timing budget in VL53L0X class

This commit is contained in:
Mark van Renswoude 2020-01-26 14:10:25 +01:00
parent 271c7de255
commit be0b4a4872
4 changed files with 72 additions and 34 deletions

Binary file not shown.

View File

@ -1,56 +1,76 @@
#include "VL53L0X.h"
#include "vl53l0x.h"
bool VL53L0X::init(uint8_t address)
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 = address;
device.comms_type = 1;
device.comms_speed_khz = 400;
device.i2c = &Wire;
if (!VL53L0X_DataInit(&device))
if (!checkResult(VL53L0X_DataInit(&device), result, VL53L0X_POSITION_DATAINIT))
return false;
//checkResult(VL53L0X_GetDeviceInfo(&device, &deviceInfo)), result, VL53L0X_POSITION_);
//if (!VL53L0X_GetDeviceInfo(&device, &deviceInfo))
// return false;
if (!VL53L0X_StaticInit(&device))
if (!checkResult(VL53L0X_StaticInit(&device), result, VL53L0X_POSITION_STATICINIT))
return false;
uint32_t refSpadCount;
uint8_t isApertureSpads;
if (!VL53L0X_PerformRefSpadManagement(&device, &refSpadCount, &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 (!VL53L0X_PerformRefCalibration(&device, &vhvSettings, &phaseCal))
if (!checkResult(VL53L0X_PerformRefCalibration(&device, &vhvSettings, &phaseCal), result, VL53L0X_POSITION_REFCAL))
return false;
if (!VL53L0X_SetDeviceMode(&device, VL53L0X_DEVICEMODE_SINGLE_RANGING))
if (!checkResult(VL53L0X_SetDeviceMode(&device, VL53L0X_DEVICEMODE_SINGLE_RANGING), result, VL53L0X_POSITION_DEVICEMODE))
return false;
if (!VL53L0X_SetLimitCheckEnable(&device, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1))
if (!checkResult(VL53L0X_SetLimitCheckEnable(&device, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1), result, VL53L0X_POSITION_LIMITRANGE))
return false;
if (!VL53L0X_SetLimitCheckEnable(&device, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1))
if (!checkResult(VL53L0X_SetLimitCheckEnable(&device, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1), result, VL53L0X_POSITION_LIMITRATE))
return false;
if (!VL53L0X_SetLimitCheckEnable(&device, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 1))
if (!checkResult(VL53L0X_SetLimitCheckEnable(&device, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 1), result, VL53L0X_POSITION_TRESHOLD1))
return false;
if (!VL53L0X_SetLimitCheckValue(&device, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, (FixPoint1616_t)( 1.5 * 0.023 * 65536)))
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::getSingleRangingMeasurement(VL53L0X_RangingMeasurementData_t *data)
bool VL53L0X::setMeasurementTimingBudget(uint32_t budget_us, VL53L0X_Error* error)
{
return VL53L0X_PerformSingleRangingMeasurement(&device, data);
*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);
}

View File

@ -14,21 +14,34 @@
#include "Wire.h"
#include "vl53l0x_api.h"
typedef int8_t VL53L0XPosition;
// TODO if begin fails, return a struct with the step and API status code
typedef struct
{
VL53L0X_Error error;
VL53L0XPosition position;
} VL53L0XResult;
#define VL53L0X_BEGIN_SUCCESS 0
#define VL53L0X_BEGIN_SUCCESS 0
#define VL53L0X_POSITION_UNKNOWN ((VL53L0XPosition) 0)
// Positions for the init method
#define VL53L0X_POSITION_DATAINIT ((VL53L0XPosition) 1)
#define VL53L0X_POSITION_STATICINIT ((VL53L0XPosition) 2)
#define VL53L0X_POSITION_REFSPAD ((VL53L0XPosition) 3)
#define VL53L0X_POSITION_REFCAL ((VL53L0XPosition) 4)
#define VL53L0X_POSITION_DEVICEMODE ((VL53L0XPosition) 5)
#define VL53L0X_POSITION_LIMITRANGE ((VL53L0XPosition) 6)
#define VL53L0X_POSITION_LIMITRATE ((VL53L0XPosition) 7)
#define VL53L0X_POSITION_TRESHOLD1 ((VL53L0XPosition) 8)
#define VL53L0X_POSITION_TRESHOLD2 ((VL53L0XPosition) 9)
class VL53L0X
{
public:
bool init(uint8_t address);
// TODO set timing budget
bool getSingleRangingMeasurement(VL53L0X_RangingMeasurementData_t* data);
bool init(uint8_t address, VL53L0XResult* result);
bool setMeasurementTimingBudget(uint32_t budget_us, VL53L0X_Error* error);
bool getSingleRangingMeasurement(VL53L0X_RangingMeasurementData_t* data, VL53L0X_Error* error);
private:
VL53L0X_Dev_t device;

View File

@ -9,10 +9,10 @@
#include "./lib/vl53l0x.h"
Adafruit_ST7789 display = Adafruit_ST7789(DISPLAY_PORT_CS, DISPLAY_PORT_DC, DISPLAY_PORT_RST);
VL53L0X heightSensor = VL53L0X();
auto display = Adafruit_ST7789(DISPLAY_PORT_CS, DISPLAY_PORT_DC, DISPLAY_PORT_RST);
auto heightSensor = VL53L0X();
Menu menu = Menu(&display);
auto menu = Menu(&display);
void setup()
@ -26,19 +26,24 @@ void setup()
// TODO draw "initializing" text
Wire.begin();
heightSensor.init(HEIGHTSENSOR_I2C_ADDRESS);
/*
Wire.begin();
if (!heightSensor.begin())
VL53L0XResult result;
if (!heightSensor.init(HEIGHTSENSOR_I2C_ADDRESS, &result))
{
// TODO draw "height sensor error" text
while(1);
}
VL53L0X_Error error;
if (!heightSensor.setMeasurementTimingBudget(33000, &error))
{
// TODO draw "height sensor budget error" text
while(1);
}
display.fillScreen(ST77XX_BLACK);
*/
}
void loop()