drivers/distance_sensor/vl53l1x: added multiple region of interest settings (#18073)

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,7 +36,28 @@
 
 #include "vl53l1x.hpp"
 
-#define VL53L1X_SAMPLE_RATE                                20  // ms
+#define VL53L1X_DELAY                          20   //ms
+#define VL53L1X_SAMPLE_RATE                    200  // ms, default
+#define VL53L1X_INTER_MEAS_MS				   200  // ms
+#define VL53L1X_SHORT_RANGE			            1  // sub-2 meter distance mode
+#define VL53L1X_LONG_RANGE			            2  // sub-4 meter distance mode
+#define VL53L1X_RANGE_STATUS_OUT_OF_BOUNDS     13 // region of interest out of bounds error
+#define VL53L1X_RANGE_STATUS_OK                 0 // range status ok
+#define VL53L1X_ROI_FAR_RIGHT                 247 // ROI far right of optical center
+#define VL53L1X_ROI_MID_RIGHT                 215 // ROI middle right of optical center
+#define VL53L1X_ROI_CENTER_LEFT               183 // ROI optical center left
+#define VL53L1X_ROI_MID_LEFT                  167 // ROI middle left of optical center
+#define VL53L1X_ROI_FAR_LEFT                  151 // ROI far left of optical center
+#define VL53L1X_ROI_FAR_RIGHT_LO               10 // ROI far right lo
+#define VL53L1X_ROI_MID_RIGHT_LO               42 // ROI middle right of optical center
+#define VL53L1X_ROI_CENTER_LEFT_LO             74 // ROI optical center left lo
+#define VL53L1X_ROI_MID_LEFT_LO                90 // ROI middle left of optical center
+#define VL53L1X_ROI_FAR_LEFT_LO               106 // ROI far left lo
+#define VL53L1X_ROI_FAR_RIGHT_HI              243 // ROI far right hi
+#define VL53L1X_ROI_MID_RIGHT_HI              211 // ROI middle right of optical center
+#define VL53L1X_ROI_CENTER_LEFT_HI            179 // ROI optical center left hi
+#define VL53L1X_ROI_MID_LEFT_HI               163 // ROI middle left of optical center
+#define VL53L1X_ROI_FAR_LEFT_HI               147 // ROI far left hi
 
 /* ST */
 const uint8_t VL51L1X_DEFAULT_CONFIGURATION[] = {
@@ -140,15 +161,36 @@ static const uint8_t status_rtn[24] = { 255, 255, 255, 5, 2, 4, 1, 7, 3, 0,
 
 /* end ST */
 
+// ROI array assignment
+
+uint8_t roi_center[] = {VL53L1X_ROI_FAR_RIGHT, VL53L1X_ROI_MID_RIGHT, VL53L1X_ROI_CENTER_LEFT, VL53L1X_ROI_MID_LEFT, VL53L1X_ROI_FAR_LEFT, VL53L1X_ROI_FAR_RIGHT_LO, VL53L1X_ROI_MID_RIGHT_LO, VL53L1X_ROI_CENTER_LEFT_LO,  VL53L1X_ROI_MID_LEFT_LO, VL53L1X_ROI_FAR_LEFT_LO, VL53L1X_ROI_FAR_RIGHT_HI, VL53L1X_ROI_MID_RIGHT_HI, VL53L1X_ROI_CENTER_LEFT_HI, VL53L1X_ROI_MID_LEFT_HI, VL53L1X_ROI_FAR_LEFT_HI};
+
 VL53L1X::VL53L1X(const I2CSPIDriverConfig &config) :
 	I2C(config),
 	I2CSPIDriver(config),
 	_px4_rangefinder(get_device_id(), config.rotation)
 {
-	// VL53L1X typical range 0-2 meters with 25 degree field of view
+	// Set distance mode (1 for ~2m ranging, 2 for ~4m ranging
+	_distance_mode = VL53L1X_LONG_RANGE;
+	// VL53L1X typical range 0-4 meters with 27 degree field of view
 	_px4_rangefinder.set_min_distance(0.f);
-	_px4_rangefinder.set_max_distance(2.f);
+
-	_px4_rangefinder.set_fov(math::radians(25.f));
+	if (_distance_mode == VL53L1X_SHORT_RANGE) {
+		_px4_rangefinder.set_max_distance(2.f);
+
+	} else {
+		_px4_rangefinder.set_max_distance(4.f);
+	}
+
+	_px4_rangefinder.set_fov(math::radians(27.f));
+
+	// Zone limits
+	_zone_limit = sizeof(roi_center) / sizeof(uint8_t);
+
+	// Zone index
+	_zone_index = 0;
+	// Allow 3 retries as the device typically misses the first measure attempts.
+	I2C::_retries = 3;
 
 	_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_VL53L1X);
 }
@@ -163,7 +205,7 @@ VL53L1X::~VL53L1X()
 int VL53L1X::collect()
 {
 	uint8_t ret = 0;
-	uint8_t rangeStatus;
+	uint8_t rangeStatus = VL53L1X_RANGE_STATUS_OK;
 	uint16_t distance_mm = 0;
 
 	perf_begin(_sample_perf);
@@ -172,7 +214,7 @@ int VL53L1X::collect()
 
 	ret = VL53L1X_GetRangeStatus(&rangeStatus);
 
-	if ((ret != PX4_OK) | (rangeStatus != PX4_OK)) {
+	if ((ret != PX4_OK) | (rangeStatus == VL53L1X_RANGE_STATUS_OUT_OF_BOUNDS)) {
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
 		return PX4_ERROR;
@@ -220,20 +262,24 @@ int VL53L1X::probe()
 void VL53L1X::RunImpl()
 {
 	uint8_t dataReady = 0;
-
 	VL53L1X_CheckForDataReady(&dataReady);
 
 	if (dataReady == 1) {
 		collect();
 	}
 
-	ScheduleDelayed(VL53L1X_SAMPLE_RATE);
+	ScheduleDelayed(VL53L1X_DELAY);
+
+	// zone modulus increment
+	_zone_index = (_zone_index + 1) % _zone_limit;
+
+	// Set the ROI center based on zone incrementation
+	VL53L1X_SetROICenter(roi_center[_zone_index]);
 }
 
 int VL53L1X::init()
 {
 	int ret = PX4_OK;
-
 	ret = device::I2C::init();
 
 	if (ret != PX4_OK) {
@@ -241,9 +287,16 @@ int VL53L1X::init()
 		return PX4_ERROR;
 	}
 
+	// Spad width (x) & height (y)
+
+	uint8_t x = 4;
+	uint8_t y = 4;
+
 	ret |= VL53L1X_SensorInit();
-	ret |= VL53L1X_ConfigBig(2, VL53L1X_SAMPLE_RATE);
+	ret |= VL53L1X_ConfigBig(_distance_mode, VL53L1X_SAMPLE_RATE);
-	ret |= VL53L1X_SetInterMeasurementInMs(VL53L1X_SAMPLE_RATE);
+	ret |= VL53L1X_SetROI(x, y);
+	ret |= VL53L1X_SetROICenter(roi_center[_zone_index]);
+	ret |= VL53L1X_SetInterMeasurementInMs(VL53L1X_INTER_MEAS_MS);
 	ret |= VL53L1X_StartRanging();
 
 	if (ret != PX4_OK) {
@@ -478,6 +531,18 @@ int8_t VL53L1X::VL53L1X_StopRanging()
 	return status;
 }
 
+int8_t VL53L1X::VL53L1X_SetROI(uint16_t x, uint16_t y)
+{
+	// set ROI size x and y
+	return VL53L1_WrByte(ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, (y - 1) << 4 | (x - 1));
+}
+
+int8_t VL53L1X::VL53L1X_SetROICenter(uint8_t zone)
+{
+	// Set ROI spad center
+	return VL53L1_WrByte(VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD, zone);
+}
+
 /**
  * @brief This function programs the Intermeasurement period in ms\n
  * Intermeasurement period must be >/= timing budget.

src/drivers/distance_sensor/vl53l1x/vl53l1x.hpp

@@ -94,6 +94,8 @@
 
 /* Configuration Constants */
 #define VL53L1X_BASEADDR                                0x29
+#define VL53L1X_SHORT_RANGE                                 1  // sub-2 meter distance mode
+#define VL53L1X_LONG_RANGE                                  2  // sub-4 meter distance mode
 
 class VL53L1X : public device::I2C, public I2CSPIDriver<VL53L1X>
 {
@@ -117,6 +119,15 @@ public:
 	 */
 	void RunImpl();
 
+	// Distance mode member variable
+	uint16_t _distance_mode{VL53L1X_LONG_RANGE};
+
+	// Zone index member variable
+	uint8_t _zone_index{0};
+
+	// Zone limit member variable
+	uint8_t _zone_limit{0};
+
 private:
 	int probe() override;
 
@@ -143,7 +154,8 @@ private:
 	// Remove if config store
 	int8_t VL53L1X_ConfigBig(uint16_t DM, uint16_t TimingBudgetInMs);
 	int8_t VL53L1X_SetInterMeasurementInMs(uint32_t InterMeasurementInMs);
-
+	int8_t VL53L1X_SetROICenter(uint8_t data);
+	int8_t VL53L1X_SetROI(uint16_t x, uint16_t y);
 	PX4Rangefinder    _px4_rangefinder;
 
 	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")};