adafruit_lidarlite.py

# SPDX-FileCopyrightText: 2018 ladyada for Adafruit Industries
# SPDX-FileCopyrightText: 2022 johnrbnsn
#
# SPDX-License-Identifier: MIT

"""
`adafruit_lidarlite`
====================================================

A CircuitPython & Python library for Garmin LIDAR Lite sensors over I2C

* Author(s): ladyada, johnrbnsn

Implementation Notes
--------------------

**Hardware:**


**Software and Dependencies:**

* Adafruit CircuitPython firmware for the supported boards:
  https://github.com/adafruit/circuitpython/releases

* Adafruit's Bus Device library: https://github.com/adafruit/Adafruit_CircuitPython_BusDevice

"""

# imports
import time
from adafruit_bus_device.i2c_device import I2CDevice
from digitalio import Direction
from micropython import const

try:
    from typing import Optional
    from microcontroller import Pin
    from busio import I2C
except ImportError:
    pass

__version__ = "0.0.0+auto.0"
__repo__ = "https://github.com/adafruit/Adafruit_CircuitPython_LIDARLite.git"


_ADDR_DEFAULT = const(0x62)
_REG_ACQ_COMMAND = const(0x00)
_REG_DIST_MEAS_V3 = const(0x8F)
_REG_DIST_MEAS_V3HP = const(0x0F)
_REG_SIG_COUNT_VAL = const(0x02)
_REG_ACQ_CONFIG_REG = const(0x04)
_REG_THRESHOLD_BYPASS = const(0x1C)
_REG_STATUS = const(0x01)
_REG_UNIT_ID_HIGH = const(0x16)
_REG_UNIT_ID_LOW = const(0x17)
_REG_SIGNAL_STRENGTH = const(0x0E)
_REG_HEALTH_STATUS_V3HP = const(0x48)
_REG_POWER_CONTROL = const(0x65)
_REG_I2C_CONFIG = const(0x1E)
_REG_TEST_COMMAND = const(0x40)
_REG_CORR_DATA = const(0x52)

_CMD_RESET = const(0x00)
_CMD_DISTANCENOBIAS = const(0x03)
_CMD_DISTANCEWITHBIAS = const(0x04)
_CMD_DISTANCE_V3HP = const(0x03)
_NUM_DIST_BYTES = 2  # How many bytes is the returned distance measurement?

TYPE_V3 = "V3"
TYPE_V3HP = "V3HP"

CONFIG_DEFAULT = 0
CONFIG_SHORTFAST = 1
CONFIG_DEFAULTFAST = 2
CONFIG_MAXRANGE = 3
CONFIG_HIGHSENSITIVE = 4
CONFIG_LOWSENSITIVE = 5

"""Status Registers"""
# v3
STATUS_BUSY = 0x01
STATUS_REF_OVERFLOW = 0x02
STATUS_SIGNAL_OVERFLOW = 0x04
STATUS_NO_PEAK = 0x08
STATUS_SECOND_RETURN = 0x10
STATUS_HEALTHY = 0x20
STATUS_SYS_ERROR = 0x40

# v3 HP
STATUS_BUSY_V3HP = 0x01
STATUS_SIGNAL_OVERFLOW_V3HP = 0x02

# The various configuration register values, from arduino library
_LIDAR_CONFIGS = (
    (0x80, 0x08, 0x00),  # default
    (0x1D, 0x08, 0x00),  # short range, high speed
    (0x80, 0x00, 0x00),  # default range, higher speed short range
    (0xFF, 0x08, 0x00),  # maximum range
    (0x80, 0x08, 0x80),  # high sensitivity & error
    (0x80, 0x08, 0xB0),
)  # low sensitivity & error


class LIDARLite:
    """
    A driver for the Garmin LIDAR Lite laser distance sensor.

    Initialize the hardware for the LIDAR over I2C. You can pass in an
    optional reset_pin for when you call reset(). There are a few common
    configurations Garmin suggests: CONFIG_DEFAULT, CONFIG_SHORTFAST,
    CONFIG_DEFAULTFAST, CONFIG_MAXRANGE, CONFIG_HIGHSENSITIVE, and
    CONFIG_LOWSENSITIVE. For the I2C address, the default is 0x62 but if you
    pass a different number in, we'll try to change the address so multiple
    LIDARs can be connected. (Note all but one need to be in reset for this
    to work!)

    :param i2c_bus: The `busio.I2C` object to use. This is the only
                    required parameter.
    :param int address: (optional) The I2C address of the device to set
                        after initialization.
    """

    def __init__(
        self,
        i2c_bus: I2C,
        *,
        reset_pin: Optional[Pin] = None,
        configuration: Optional[int] = CONFIG_DEFAULT,
        address: Optional[int] = _ADDR_DEFAULT,
        sensor_type: Optional[str] = TYPE_V3,
    ) -> None:
        self.i2c_device = I2CDevice(i2c_bus, address)
        self._buf = bytearray(2)
        self._bias_count = 0
        self._reset = reset_pin
        time.sleep(0.5)
        self.configure(configuration)
        self._status = self.status
        self._sensor_type = sensor_type

    def reset(self) -> None:
        """Hardware reset (if pin passed into init) or software reset. Will take
        100 readings in order to 'flush' measurement unit, otherwise data is off."""
        # Optional hardware reset pin
        if self._reset is not None:
            self._reset.direction = Direction.OUTPUT
            self._reset.value = True
            self._reset.value = False
            time.sleep(0.01)
            self._reset.value = True
        else:
            try:
                self._write_reg(_REG_ACQ_COMMAND, _CMD_RESET)
            except OSError:
                print("OSError")
        time.sleep(1)
        # take 100 readings to 'flush' out sensor!
        for _ in range(100):
            try:
                self.read_distance_v3(True)
            except RuntimeError:
                print("RuntimeError")

    def configure(self, config: int) -> None:
        """Set the LIDAR desired style of measurement. There are a few common
        configurations Garmin suggests: CONFIG_DEFAULT, CONFIG_SHORTFAST,
        CONFIG_DEFAULTFAST, CONFIG_MAXRANGE, CONFIG_HIGHSENSITIVE, and
        CONFIG_LOWSENSITIVE."""
        settings = _LIDAR_CONFIGS[config]
        self._write_reg(_REG_SIG_COUNT_VAL, settings[0])
        self._write_reg(_REG_ACQ_CONFIG_REG, settings[1])
        self._write_reg(_REG_THRESHOLD_BYPASS, settings[2])

    def read_distance_v3(self, bias: Optional[bool] = False) -> int:
        """Perform a distance reading with or without 'bias'. It's recommended
        to take a bias measurement every 100 non-bias readings (they're slower)"""
        if bias:
            self._write_reg(_REG_ACQ_COMMAND, _CMD_DISTANCEWITHBIAS)
        else:
            self._write_reg(_REG_ACQ_COMMAND, _CMD_DISTANCENOBIAS)
        dist = self._read_reg(_REG_DIST_MEAS_V3, _NUM_DIST_BYTES)
        if self._status & (STATUS_NO_PEAK | STATUS_SECOND_RETURN):
            if self._status & STATUS_NO_PEAK:
                raise RuntimeError("Measurement failure STATUS_NO_PEAK")
            if self._status & STATUS_SECOND_RETURN:
                raise RuntimeError("Measurement failure STATUS_NO_PEAK")
            raise RuntimeError("Some other runtime error")

        if (self._status & STATUS_SYS_ERROR) or (not self._status & STATUS_HEALTHY):
            raise RuntimeError("System failure")
        return dist[0] << 8 | dist[1]

    def read_distance_v3hp(self) -> int:
        """Perform a distance measurement for the v3 HP sensor"""
        # Any non-zero value written to _REG_ACQ_COMMAND will start a reading on v3HP, no bias vs.
        #   non-bias
        self._write_reg(_REG_ACQ_COMMAND, _CMD_DISTANCEWITHBIAS)
        dist = self._read_reg(_REG_DIST_MEAS_V3HP, _NUM_DIST_BYTES)
        return dist[0] << 8 | dist[1]

    @property
    def correlation_data(self) -> int:
        """Reads correlation data"""
        # TODO: How to translate correlation data property?
        corr_data = self._read_reg(_REG_CORR_DATA, 2)
        return corr_data[0] << 8 | corr_data[1]

    @property
    def test_command(self) -> int:
        """Reads the test command"""
        return self._read_reg(_REG_TEST_COMMAND, 1)[0]

    @property
    def i2c_config(self) -> int:
        """Reads the I2C config"""
        return self._read_reg(_REG_I2C_CONFIG, 1)[0]

    @property
    def power_control(self) -> int:
        """Reads the power control register"""
        return self._read_reg(_REG_POWER_CONTROL, 1)[0]

    @property
    def health_status(self) -> int:
        """Reads health status for v3HP (not available on v3, will return -1)"""
        if self._sensor_type == TYPE_V3HP:
            return self._read_reg(_REG_HEALTH_STATUS_V3HP, 1)[0]

        return -1

    @property
    def signal_strength(self) -> int:
        """Reads the signal strength of the last measurement"""
        return self._read_reg(_REG_SIGNAL_STRENGTH, 1)[0]

    @property
    def unit_id(self) -> int:
        """Reads the serial number of the unit"""
        high_byte = self._read_reg(_REG_UNIT_ID_HIGH, 1)
        low_byte = self._read_reg(_REG_UNIT_ID_LOW, 1)

        return high_byte[0] << 8 | low_byte[0]

    @property
    def distance(self) -> int:  # pylint: disable=R1710
        """The measured distance in cm. Will take a bias reading every 100 calls"""
        self._bias_count -= 1

        if self._bias_count < 0:
            self._bias_count = 100  # every 100 reads, check bias
        if self._sensor_type == TYPE_V3:
            return self.read_distance_v3(self._bias_count <= 0)
        if self._sensor_type == TYPE_V3HP:
            return self.read_distance_v3hp()

        # If no sensor type has been identified, return a negative distance as an error
        return -1.0

    @property
    def status(self) -> int:
        """The status byte, check datasheet for bitmask"""
        buf = bytearray([_REG_STATUS])
        with self.i2c_device as i2c:
            i2c.write_then_readinto(buf, buf)
        return buf[0]

    def _write_reg(self, reg: int, value: int) -> None:
        self._buf[0] = reg
        self._buf[1] = value
        with self.i2c_device as i2c:
            # print("Writing: ", [hex(i) for i in self._buf])
            i2c.write(self._buf)
        time.sleep(0.001)  # there's a delay in arduino library

    def _read_reg(self, reg: int, num: int) -> bytearray:
        while True:
            self._status = self.status
            if not self._status & STATUS_BUSY:
                break
        # no longer busy
        self._buf[0] = reg
        with self.i2c_device as i2c:
            i2c.write_then_readinto(self._buf, self._buf, out_end=1, in_end=num)
        # print("Read from ", hex(reg), [hex(i) for i in self._buf])
        return self._buf