From 448598cbf8fa96c6a2741f8a09005e66a43da840 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Oliver=20St=C3=A4bler?= <oliver.staebler@bytesatwork.ch>
Date: Tue, 19 Apr 2022 11:46:38 +0200
Subject: [PATCH] vl53l1x: Add functions for setting 'region of interest'
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Implement functions to get and set 'region of interest' similar to its
reference implementation in C:
https://github.com/pololu/vl53l1x-arduino/commit/a6de3966784aa07680bb35a1f9ac369959efe19f

Signed-off-by: Oliver Stäbler <oliver.staebler@bytesatwork.ch>
---
 vl53l1x/registers.go |  2 ++
 vl53l1x/vl53l1x.go   | 33 +++++++++++++++++++++++++++++++++
 2 files changed, 35 insertions(+)

diff --git a/vl53l1x/registers.go b/vl53l1x/registers.go
index 7d47087..8b937a0 100644
--- a/vl53l1x/registers.go
+++ b/vl53l1x/registers.go
@@ -46,6 +46,8 @@ const (
 	SD_CONFIG_INITIAL_PHASE_SD1                             = 0x007B
 	SYSTEM_GROUPED_PARAMETER_HOLD_1                         = 0x007C
 	SD_CONFIG_QUANTIFIER                                    = 0x007E
+	ROI_CONFIG_USER_ROI_CENTRE_SPAD                         = 0x007F
+	ROI_CONFIG_USER_ROI_REQUESTED_GLOBAL_XY_SIZE            = 0x0080
 	SYSTEM_SEQUENCE_CONFIG                                  = 0x0081
 	SYSTEM_GROUPED_PARAMETER_HOLD                           = 0x0082
 	SYSTEM_INTERRUPT_CLEAR                                  = 0x0086
diff --git a/vl53l1x/vl53l1x.go b/vl53l1x/vl53l1x.go
index f09ce70..5fc982c 100644
--- a/vl53l1x/vl53l1x.go
+++ b/vl53l1x/vl53l1x.go
@@ -10,6 +10,7 @@
 package vl53l1x // import "tinygo.org/x/drivers/vl53l1x"
 
 import (
+	"errors"
 	"time"
 
 	"tinygo.org/x/drivers"
@@ -417,6 +418,38 @@ func (d *Device) StopContinuous() {
 	d.writeReg(PHASECAL_CONFIG_OVERRIDE, 0x00)
 }
 
+// SetROI sets the 'region of interest' for x and y coordinates. Valid ranges are from 4/4 to 16/16.
+func (d *Device) SetROI(x, y uint8) error {
+	if !validROIRange(x, y) {
+		return errors.New("ROI value out of range")
+	}
+
+	if x > 10 || y > 10 {
+		d.writeReg(ROI_CONFIG_USER_ROI_CENTRE_SPAD, 199)
+	}
+
+	d.writeReg(ROI_CONFIG_USER_ROI_REQUESTED_GLOBAL_XY_SIZE, (y-1)<<4|(x-1))
+	return nil
+}
+
+// GetROI returns the currently configured 'region of interest' for x and y coordinates.
+func (d *Device) GetROI() (x, y uint8, err error) {
+	reg := d.readReg(ROI_CONFIG_USER_ROI_REQUESTED_GLOBAL_XY_SIZE)
+
+	x = (reg & 0x0f) + 1
+	y = ((reg & 0xf0) >> 4) + 1
+
+	if !validROIRange(x, y) {
+		err = errors.New("ROI value out of range")
+	}
+
+	return
+}
+
+func validROIRange(x, y uint8) bool {
+	return x >= 4 && x <= 16 && y >= 4 && y <= 16
+}
+
 // writeReg sends a single byte to the specified register address
 func (d *Device) writeReg(reg uint16, value uint8) {
 	msb := byte((reg >> 8) & 0xFF)