Add VL53L0X time-of-flight distance sensor to Heltec V3 Sensor

Author: mikenz

src/helpers/sensors/EnvironmentSensorManager.cpp

@@ -59,6 +59,12 @@ static Adafruit_INA219 INA219(TELEM_INA219_ADDRESS);
 static Adafruit_MLX90614 MLX90614;
 #endif
 
+#if ENV_INCLUDE_VL53L0X
+#define TELEM_VL53L0X_ADDRESS 0x29      // VL53L0X time-of-flight distance sensor I2C address
+#include <Adafruit_VL53L0X.h>
+static Adafruit_VL53L0X VL53L0X;
+#endif
+
 #if ENV_INCLUDE_GPS && RAK_BOARD
 static uint32_t gpsResetPin = 0;
 static bool i2cGPSFlag = false;
@@ -169,6 +175,16 @@ bool EnvironmentSensorManager::begin() {
   }
   #endif
 
+  #if ENV_INCLUDE_VL53L0X
+  if (VL53L0X.begin(TELEM_VL53L0X_ADDRESS, false, TELEM_WIRE)) {
+    MESH_DEBUG_PRINTLN("Found VL53L0X at address: %02X", TELEM_VL53L0X_ADDRESS);
+    VL53L0X_initialized = true;
+  } else {
+    VL53L0X_initialized = false;
+    MESH_DEBUG_PRINTLN("VL53L0X was not found at I2C address %02X", TELEM_VL53L0X_ADDRESS);
+  }
+  #endif
+
   return true;
 }
 
@@ -256,6 +272,18 @@ bool EnvironmentSensorManager::querySensors(uint8_t requester_permissions, Cayen
     }
     #endif
 
+    #if ENV_INCLUDE_VL53L0X
+    if (VL53L0X_initialized) {
+      VL53L0X_RangingMeasurementData_t measure;
+      VL53L0X.rangingTest(&measure, false); // pass in 'true' to get debug data
+      if (measure.RangeStatus != 4) { // phase failures
+        telemetry.addDistance(TELEM_CHANNEL_SELF, measure.RangeMilliMeter / 1000.0f); // convert mm to m
+      } else {
+        telemetry.addDistance(TELEM_CHANNEL_SELF, 0.0f); // no valid measurement
+      }
+    }
+    #endif
+
   }
 
   return true;

src/helpers/sensors/EnvironmentSensorManager.h

@@ -16,6 +16,7 @@ class EnvironmentSensorManager : public SensorManager {
   bool SHTC3_initialized = false;
   bool LPS22HB_initialized = false;
   bool MLX90614_initialized = false;
+  bool VL53L0X_initialized = false;
 
   bool gps_detected = false;
   bool gps_active = false;

variants/heltec_v3/platformio.ini

@@ -158,6 +158,7 @@ build_flags =
   -D ENV_PIN_SDA=33
   -D ENV_PIN_SCL=34
   -D ENV_INCLUDE_MLX90614=1
+  -D ENV_INCLUDE_VL53L0X=1
   -D DISPLAY_CLASS=SSD1306Display
 ;  -D MESH_PACKET_LOGGING=1
 ;  -D MESH_DEBUG=1
@@ -168,6 +169,7 @@ lib_deps =
   ${Heltec_lora32_v3.lib_deps}
   ${esp32_ota.lib_deps}
   adafruit/Adafruit MLX90614 Library @ ^2.1.5
+  adafruit/Adafruit_VL53L0X @ ^1.2.4
 
 [env:Heltec_WSL3_repeater]
 extends = Heltec_lora32_v3