Sharing this given that a previous topic thread "TOF400F Sensor Help" was closed. Am new to IOT development and recently purchased an RPi Pico W 2 and TOF400F LiDAR Sensor for an easy first project.
I bought this sensor from Amazon: Amazon.com: HiLetgo TOF400F VL53L1 Time-of-Flight (TOF) Distance Measurement Sensor Built-in MCU Algorithm with UART IIC I2C MODBUS Modes Measuring Distance 400CM : Electronics
which I imagine is far cheaper on AliExpress, but was time crunched to get it.
It came with no documentation and was intending to communicate with it over I2C bus, but kept seeing multiple addresses returned (and random addresses each time) when scanning the bus. The previous thread on this topic suggested that the default mode is serial/UART and since I didn't have a USB-to-TTL device to change it up with, I decided to just go with UART for reading it.
I didn't find any good example micropython code for using this sensor, so wrote a simple API class based on what I could gleam from the documentation.
HiLetGo didn't seem to have documentation, but guess it's the same as this one:
which suggests that reading this sensor is fairly straightforward. In long range mode, it will deliver new 16bit data reading every 200ms at bytes indices 4 & 5.
Here is my API class for anyone looking to do similar or improve it;
NOTE: get_distance will return reading in meters, you can change the divisor in the code if you want mm or cm, etc.
e.g. main.py
from tof400f import TOF400F
import time
lidar = TOF400F()
time.sleep_ms(2000)
while True:
distance = lidar.get_distance()
if distance > 0:
print(f"Sensor Value: {distance:.2f} m")
time.sleep_ms(100)
the API class; accepts pin indices for uart serial tx/rx; defaults to long-range; if you want high precision (shorter range) you'll have to write a command in initialization per documentation to switch it.
from machine import Pin, UART
class TOF400F:
def __init__(self, tx: int=0, rx: int=1, uart_id: int=0, baudrate: int=115200):
self._tx_pin = Pin(tx, Pin.OUT, 0)
self._rx_pin = Pin(rx, Pin.IN, 0)
# Initilize Serial Hardware Interfaces
self._lidar_uart = UART(uart_id, baudrate) # init with given baudrate
self._lidar_uart.init(
baudrate,
bits=8,
parity=None,
stop=1,
tx=self._tx_pin,
rx=self._rx_pin) # init with given parameters
def _calculate_crc(self, data):
"""Calculate CRC-16/MODBUS for the data."""
crc = 0xFFFF
for byte in data:
crc ^= byte # XOR the byte with the lower 8 bits of the CRC
for _ in range(8):
if crc & 0x0001: # Check if the LSB is 1
crc >>= 1
crc ^= 0xA001 # Polynomial for CRC-16/MODBUS
else:
crc >>= 1
return crc & 0xFFFF
def _parse_distance(self, data):
"""Parse the distance from the byte string data frame."""
if len(data) < 7:
raise ValueError("Incomplete data frame")
# Extract relevant bytes
slave_addr = data[0]
function_num = data[1]
num_data_bytes = data[2]
data_high = data[3]
data_low = data[4]
# Combine high and low bytes for distance
distance_mm = (data_high << 8) | data_low
# Calculate and verify CRC
received_crc = (data[6] << 8) | data[5]
calculated_crc = self._calculate_crc(data[:5])
if received_crc != calculated_crc:
raise ValueError("CRC mismatch")
return distance_mm
def get_distance(self):
"""Read data and return distance value in meters"""
_distance = 0.0
if (self._lidar_uart.any() > 0):
data = self._lidar_uart.read(7)
try:
_distance = self._parse_distance(data) / 1000.0
except Exception as e:
print(f"Error in TOF400F: {e}")
return min(4.0, _distance)