Error handling and timing budget in VL53L0X class

src/lib/vl53l0x.cpp

@@ -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))
+  if (!checkResult(VL53L0X_StaticInit(&device), result, VL53L0X_POSITION_STATICINIT))
-  //  return false;
-
-  if (!VL53L0X_StaticInit(&device))
     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);
 }

src/lib/vl53l0x.h

@@ -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);
+    bool init(uint8_t address, VL53L0XResult* result);
-
+    bool setMeasurementTimingBudget(uint32_t budget_us, VL53L0X_Error* error);
-    // TODO set timing budget
+    bool getSingleRangingMeasurement(VL53L0X_RangingMeasurementData_t* data, VL53L0X_Error* error);
-
-    bool getSingleRangingMeasurement(VL53L0X_RangingMeasurementData_t* data);
 
  private:
     VL53L0X_Dev_t device;

src/main.cpp

@@ -9,10 +9,10 @@
 #include "./lib/vl53l0x.h"
 
 
-Adafruit_ST7789 display = Adafruit_ST7789(DISPLAY_PORT_CS, DISPLAY_PORT_DC, DISPLAY_PORT_RST);
+auto display = Adafruit_ST7789(DISPLAY_PORT_CS, DISPLAY_PORT_DC, DISPLAY_PORT_RST);
-VL53L0X heightSensor = VL53L0X();
+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);
 
-/*
+  VL53L0XResult result;
-  Wire.begin();
+  if (!heightSensor.init(HEIGHTSENSOR_I2C_ADDRESS, &result))
-  if (!heightSensor.begin())
   {
     // 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()