Real-Time 3D Room and Spatial Mapping with ESP32 and VL53L5CX 8×8 Time-of-Flight (ToF) Sensor

While mmWave radar excels at penetrating non-metallic obstacles to detect human respiratory rhythms, it lacks optical spatial resolution. Conversely, traditional cameras introduce severe privacy concerns in sensitive areas like bedrooms and bathrooms. The STMicroelectronics VL53L5CX bridges this gap perfectly by functioning as an optical 8×8 multizone Time-of-Flight (ToF) LiDAR sensor.

Emitting harmless 940 nm VCSEL infrared light pulses and capturing photon arrivals across a SPAD (Single Photon Avalanche Diode) array, the VL53L5CX generates a real-time 64-pixel depth map up to 4 meters away. When paired with an ESP32, it becomes an embedded, privacy-preserving 3D depth camera capable of obstacle avoidance, gesture tracking, and true 3D room profiling.

Direct Time-of-Flight (dToF) Optical Architecture

Unlike stereoscopic cameras that rely on visual ambient light or ultrasonic sensors that suffer from slow acoustic propagation, the VL53L5CX utilizes direct Time-of-Flight (dToF) photon counting:

• 940 nm Invisible VCSEL Emitter: Fires picosecond-scale laser bursts invisible to the human eye.
• Integrated Diffraction Optics: A microscopic metasurface lens splits the emitted laser beam across a wide 63° diagonal Field of View (FoV).
• SPAD Receiver Matrix: 64 individual sensing zones (arranged in an 8×8 grid, or 4×4 for high-speed operation) count individual photons and calculate flight duration with millimeter precision.
• Multi-Target Per Zone: Detects up to 4 target reflections per zone, allowing it to “see through” dirty glass cover sheets.

Hardware Interfacing and High-Speed I2C Bus Design

The VL53L5CX communicates over I2C at up to 1 MHz (Fast Mode Plus). However, initializing the sensor requires loading a proprietary firmware blob (~84 KB) during startup, which demands careful memory management on microcontrollers like the ESP32:

ESP32 Arduino Firmware Implementation

Using the official SparkFun VL53L5CX Arduino Library (built on top of ST’s Ultra Lite Driver), the following code initializes the sensor in 8×8 resolution at 15 Hz and streams raw distance data across UART in an easy-to-parse matrix format:

#include <Wire.h>
#include <SparkFun_VL53L5CX_Library.h>

SparkFun_VL53L5CX myImager;
VL53L5CX_ResultsData measurementData;

int imageResolution = 64; // 8x8 Grid
int imageWidth = 8;

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);
  Wire.setClock(1000000); // 1 MHz Fast Mode+

  Serial.println("Initializing VL53L5CX ULD...");
  if (!myImager.begin()) {
    Serial.println("Sensor not found! Check wiring and I2C pullups.");
    while (1);
  }

  myImager.setResolution(8 * 8); // Set 8x8 (64 zones)
  myImager.setRangingFrequency(15); // 15 Hz sample rate
  myImager.startRanging();
  Serial.println("VL53L5CX Ranging Started.");
}

void loop() {
  if (myImager.isDataReady()) {
    if (myImager.getRangingData(&measurementData)) {
      // Stream 8x8 matrix as JSON array
      Serial.print("[");
      for (int i = 0; i < imageResolution; i++) {
        Serial.print(measurementData.distance_mm[i]);
        if (i < imageResolution - 1) Serial.print(",");
      }
      Serial.println("]");
    }
  }
  delay(10);
}

Real-Time 3D Heatmap Visualization in Python

To render the 8×8 point cloud into an interactive heat map, you can run a lightweight Python listener on your PC or Home Assistant companion server using matplotlib:

import serial
import json
import numpy as np
import matplotlib.pyplot as plt

ser = serial.Serial('COM5', 115200)
plt.ion()
fig, ax = plt.subplots(figsize=(6, 6))

while True:
    line = ser.readline().decode('utf-8').strip()
    if line.startswith('[') and line.endswith(']'):
        try:
            data = json.loads(line)
            matrix = np.array(data).reshape((8, 8))
            ax.clear()
            im = ax.imshow(matrix, cmap='viridis', vmin=0, vmax=3000)
            ax.set_title("VL53L5CX Real-Time 8x8 Depth Map")
            plt.pause(0.01)
        except Exception:
            pass

Practical Smart Home Applications

• Bed Posture and Sleep Tracking: Mount the sensor directly above a bed to identify whether an occupant is on their back, side, or has exited the bed, without ever capturing privacy-invasive photographic images.
• Directional Doorway People Counting: When installed above a doorway, tracking the spatial progression of depth across the 8 columns reveals whether an individual is entering or exiting a room.
• Touchless 3D Gesture Interface: Swipe up, down, left, or right in the sensor’s field of view to control volume, light brightness, or motorized window shades.