ToF Sensor for MicroPython

main.py

from machine import UART, Pin
import utime
from binascii import hexlify

# Configuration settings
UART_ID = 0       # UART instance ID
BAUD_RATE = 115200 # Baud rate for UART communication
TX_PIN = 16       # UART TX pin
RX_PIN = 17       # UART RX pin
SAMPLE_RATE_HZ = 4 # Sampling rate in Hz
NUM_SAMPLES = 1   # Number of samples to average (1 means no averaging)
USE_MM = False    # False for cm, True for mm
DISTANCE_OFFSET = 0  # Distance offset in mm or cm

# Initialize the UART for the LIDAR module
lidar_uart = UART(UART_ID, baudrate=BAUD_RATE, tx=Pin(TX_PIN), rx=Pin(RX_PIN))


def send_packet(packet):
    """Sends a packet of data to the LIDAR module.

    Args:
        packet (list): List of bytes to send.
    """
    lidar_uart.write(bytes(packet))
    utime.sleep_ms(100)


def save_settings():
    """Saves the current LIDAR settings to non-volatile memory."""
    send_packet([0x5A, 0x04, 0x11, 0x6F])
    utime.sleep_ms(1000)


def set_sample_rate(rate_hz):
    """Sets the LIDAR sample rate.

    Args:
        rate_hz (int): Sample rate in Hz (1-30).
    """
    rate_bytes = int(rate_hz).to_bytes(2, 'big')
    packet = [0x5A, 0x06, 0x03, rate_bytes[1], rate_bytes[0], 0x00, 0x00]
    send_packet(packet)


def read_with_timeout(uart, byte_count=9, timeout_ms=100):
    """Reads a specified number of bytes from UART with a timeout.

    Args:
        uart (UART): UART object.
        byte_count (int): Number of bytes to read.
        timeout_ms (int): Timeout in milliseconds.

    Returns:
        bytearray: Received bytes or an empty array if timed out.
    """
    start_time = utime.ticks_ms()
    while utime.ticks_diff(utime.ticks_ms(), start_time) < timeout_ms:
        if uart.any() > 0:
            return uart.read(byte_count)
    return bytearray()


def get_version():
    """Requests and prints the LIDAR firmware version.

    Returns:
        bool: True if the version was retrieved successfully, False otherwise.
    """
    version_request = [0x5A, 0x04, 0x14, 0x00]
    lidar_uart.write(bytes(version_request))
    response = read_with_timeout(lidar_uart, 30)

    if response and response[0] == 0x5A:
        print(response[0:].decode('utf-8'))
        return True

    print("Failed to retrieve version")
    return False


def get_lidar_data():
    """Fetches distance, strength, and temperature data from the LIDAR.

    Returns:
        tuple: (distance, strength, temperature) or (-1, -1, -1) if invalid.
    """
    response = read_with_timeout(lidar_uart)

    if len(response) >= 9 and response[0] == 0x59 and response[1] == 0x59:
        distance = response[2] + (response[3] << 8)
        strength = response[4] + (response[5] << 8)
        temperature = ((response[6] + (response[7] << 8)) / 8) - 256
        return distance + DISTANCE_OFFSET, strength, temperature

    return -1, -1, -1


def set_measurement_mode(use_mm):
    """Sets the LIDAR measurement mode to mm or cm.

    Args:
        use_mm (bool): True for mm, False for cm.
    """
    if use_mm:
        send_packet([0x5A, 0x05, 0x05, 0x06, 0x6A])
    else:
        send_packet([0x5A, 0x05, 0x05, 0x01, 0x65])


try:
    while not get_version():
        pass

    set_measurement_mode(USE_MM)
    set_sample_rate(SAMPLE_RATE_HZ)
    save_settings()

    while True:
        total_distance = 0
        valid_samples = 0

        for _ in range(NUM_SAMPLES):
            distance, strength, _ = get_lidar_data()
            if distance >= 0 and strength >= 100:
                total_distance += distance
                valid_samples += 1

        if valid_samples > 0:
            print(total_distance / valid_samples)

except KeyboardInterrupt:
    print("Program terminated")