This is embedded C++ firmware, written for the ESP32 (Arduino) platform. Its primary function is to process ranging data from two VL53L8CX Time-of-Flight (ToF) sensors for rudimentary proximity based collision avoidance.
This code serves as a PoC for the sensor integration.
Note: The motor interface currently relies on a basic serial CLI protocol for the motor controller. This is non-optimal for a final flight system (e.g., it does not use standard protocols like MAVLink or a iNav's API) but was the fastest solution to validate the sensor and control logic for the demo.
The project focused on building a full stack embedded solution for omnidirectional depth perception under severe size constraints
- sensor SPI Management: Custom low level ULD firmware polls two VL53L8CX sensors simultaneously over the shared ESP32 SPI bus (using dedicated Chip Selects) to achieve a consistent ~15 Hz dual-sensor ranging rate.
- Real time 3D point cloud: High speed conversion of the raw 8x8 distance matrices (128 total points) into a unified global XYZ coordinate system using pre calibrated transformation matrices.
- Sector based proximity control: Implemented a low latency control loop that analyzes Left/Right depth sectors and translates proximity directly into a proportional motor speed decrease via Serial CLI.
- High bandwidth telemetry: The ESP32 operates as a WiFi AP, streaming the full, raw XYZ point cloud data over a dedicated TCP socket (Port 8080) for real time external visualization and debugging.
System parameters and hardware pins must be defined in main.cpp.
| Parameter | Default Value | Purpose |
|---|---|---|
| WIFI_SSID | "esp32" | WiFi AP Name |
| WIFI_PASSWORD | "12345678xd" | WiFi AP Password |
| SERVER_PORT | 8080 | TCP Listener Port |
| SENSOR1_CS_PIN | 4 | SPI Chip Select for TOP Sensor |
| SENSOR2_CS_PIN | 5 | SPI Chip Select for BOTTOM Sensor |
| RX1_PIN/TX1_PIN | 16/17 | Motor Controller UART Pins (Serial1) |
SensorProcessor calculates command values which are then passed to MotorController. Speed values are clamped and mapped from the range 150mm → MAX_SPEED_ADJ to 1000mm → MIN_SPEED_ADJ.
| Motor Index | Controlled By | Region |
|---|---|---|
| 0 | Sensor 1 (TOP) | Left FOV |
| 1 | Sensor 1 (TOP) | Right FOV |
| 2 | Sensor 2 (BOTTOM) | Left FOV |
| 3 | Sensor 2 (BOTTOM) | Right FOV |
Note: Motor speeds are defined in MotorController.h. 1000 is the motor off value.
- Requires the ESP32 platform in your Arduino environment
- SPI is initialized at 1MHz (
SPI_MODE3) - Startup checks ensure both sensors are responding before the control loop starts. If initialization fails, the ESP32 restarts (
ESP.restart()) - Monitor Serial output at
115200 Baudfor system status and detailed logs
- Visualization Software: Add the dedicated Python visualization software to the repository. This tool is necessary for interpreting the streamed XYZ point cloud data over TCP/IP.
- Obstacle Avoidance: Implement a robust, dedicated obstacle avoidance algorithm suitable for integration with a flight controller. The current implementation is a PoC demonstration of the sensor pipeline, but the ultimate goal is full deployment onto the drone platform.
This project was built together with Cedric and Arthur.
- Cedric and Arthur were responsible for the hardware and electrical engineering setup, including the design of the custom 3D printed sensor mounts as well as the soldering
- Cedric also provided significant help in developing the low level SPI reading code (as well as the debugging)
- The entire project was tested using Cedric's drone