diff --git a/components/constants/include/constants/module.h b/components/constants/include/constants/module.h
index a532571..67a724e 100644
--- a/components/constants/include/constants/module.h
+++ b/components/constants/include/constants/module.h
@@ -42,6 +42,9 @@ inline std::unordered_map<int, int> MODULE_TO_NUM_CHANNELS_MAP {
 #define OLED_SDA 17
 #define OLED_SCL 18
 
+#define DISTANCE_SDA 17
+#define DISTANCE_SCL 18
+
 inline std::unordered_map<uint8_t, uint8_t> CHANNEL_TO_0_DEG_MAP{{0, 9} };
 inline std::unordered_map<uint8_t, uint8_t> CHANNEL_TO_90_DEG_MAP{{0, 7} };
 inline std::unordered_map<uint8_t, uint8_t> CHANNEL_TO_180_DEG_MAP{{0, 8} };
diff --git a/components/flatbuffers/SensorMessageBuilder.cpp b/components/flatbuffers/SensorMessageBuilder.cpp
index a9e4f66..9558513 100644
--- a/components/flatbuffers/SensorMessageBuilder.cpp
+++ b/components/flatbuffers/SensorMessageBuilder.cpp
@@ -30,6 +30,18 @@ SerializedMessage SensorMessageBuilder::build_sensor_message(std::vector<sensor_
                     auto text_offset = builder_.CreateString(t.text);
                     values_vec.push_back(Messaging::CreateCurrentText(builder_, text_offset).Union());
                     sensor_values_vec.push_back(Messaging::SensorValue_CurrentText);
+                },
+                [&](distance d) {
+                    values_vec.push_back(Messaging::CreateDistance(builder_, d.distance).Union());
+                    sensor_values_vec.push_back(Messaging::SensorValue_Distance);
+                },
+                [&](temperature t) {
+                    values_vec.push_back(Messaging::CreateTemperature(builder_, t.temperature).Union());
+                    sensor_values_vec.push_back(Messaging::SensorValue_Temperature);
+                },
+                [&](position p) {
+                    values_vec.push_back(Messaging::CreatePosition(builder_, p.heading, p.pitch, p.roll).Union());
+                    sensor_values_vec.push_back(Messaging::SensorValue_Temperature);
                 }
             },
         v);
diff --git a/components/flatbuffers/include/SensorMessageBuilder.h b/components/flatbuffers/include/SensorMessageBuilder.h
index 782ff55..d2113ce 100644
--- a/components/flatbuffers/include/SensorMessageBuilder.h
+++ b/components/flatbuffers/include/SensorMessageBuilder.h
@@ -19,7 +19,21 @@ struct target_text {
     std::string text;
 };
 
-typedef std::variant<target_angle, current_angle, target_text> sensor_value;
+struct distance {
+    float distance;
+};
+
+struct temperature {
+    float temperature;
+};
+
+struct position {
+    float heading;
+    float pitch;
+    float roll;
+};
+
+typedef std::variant<target_angle, current_angle, target_text, distance, temperature, position> sensor_value;
 
 class SensorMessageBuilder {
   public:
diff --git a/components/flatbuffers/include/flatbuffers_generated/SensorMessage_generated.h b/components/flatbuffers/include/flatbuffers_generated/SensorMessage_generated.h
index be795f5..b298fb4 100644
--- a/components/flatbuffers/include/flatbuffers_generated/SensorMessage_generated.h
+++ b/components/flatbuffers/include/flatbuffers_generated/SensorMessage_generated.h
@@ -24,6 +24,15 @@ struct CurrentTextBuilder;
 struct CurrentAngle;
 struct CurrentAngleBuilder;
 
+struct Distance;
+struct DistanceBuilder;
+
+struct Temperature;
+struct TemperatureBuilder;
+
+struct Position;
+struct PositionBuilder;
+
 struct SensorMessage;
 struct SensorMessageBuilder;
 
@@ -32,33 +41,42 @@ enum SensorValue : uint8_t {
   SensorValue_TargetAngle = 1,
   SensorValue_CurrentAngle = 2,
   SensorValue_CurrentText = 3,
+  SensorValue_Distance = 4,
+  SensorValue_Temperature = 5,
+  SensorValue_Position = 6,
   SensorValue_MIN = SensorValue_NONE,
-  SensorValue_MAX = SensorValue_CurrentText
+  SensorValue_MAX = SensorValue_Position
 };
 
-inline const SensorValue (&EnumValuesSensorValue())[4] {
+inline const SensorValue (&EnumValuesSensorValue())[7] {
   static const SensorValue values[] = {
     SensorValue_NONE,
     SensorValue_TargetAngle,
     SensorValue_CurrentAngle,
-    SensorValue_CurrentText
+    SensorValue_CurrentText,
+    SensorValue_Distance,
+    SensorValue_Temperature,
+    SensorValue_Position
   };
   return values;
 }
 
 inline const char * const *EnumNamesSensorValue() {
-  static const char * const names[5] = {
+  static const char * const names[8] = {
     "NONE",
     "TargetAngle",
     "CurrentAngle",
     "CurrentText",
+    "Distance",
+    "Temperature",
+    "Position",
     nullptr
   };
   return names;
 }
 
 inline const char *EnumNameSensorValue(SensorValue e) {
-  if (::flatbuffers::IsOutRange(e, SensorValue_NONE, SensorValue_CurrentText)) return "";
+  if (::flatbuffers::IsOutRange(e, SensorValue_NONE, SensorValue_Position)) return "";
   const size_t index = static_cast<size_t>(e);
   return EnumNamesSensorValue()[index];
 }
@@ -79,6 +97,18 @@ template<> struct SensorValueTraits<Messaging::CurrentText> {
   static const SensorValue enum_value = SensorValue_CurrentText;
 };
 
+template<> struct SensorValueTraits<Messaging::Distance> {
+  static const SensorValue enum_value = SensorValue_Distance;
+};
+
+template<> struct SensorValueTraits<Messaging::Temperature> {
+  static const SensorValue enum_value = SensorValue_Temperature;
+};
+
+template<> struct SensorValueTraits<Messaging::Position> {
+  static const SensorValue enum_value = SensorValue_Position;
+};
+
 bool VerifySensorValue(::flatbuffers::Verifier &verifier, const void *obj, SensorValue type);
 bool VerifySensorValueVector(::flatbuffers::Verifier &verifier, const ::flatbuffers::Vector<::flatbuffers::Offset<void>> *values, const ::flatbuffers::Vector<uint8_t> *types);
 
@@ -215,6 +245,149 @@ inline ::flatbuffers::Offset<CurrentAngle> CreateCurrentAngle(
   return builder_.Finish();
 }
 
+struct Distance FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
+  typedef DistanceBuilder Builder;
+  enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
+    VT_VALUE = 4
+  };
+  float value() const {
+    return GetField<float>(VT_VALUE, 0.0f);
+  }
+  bool Verify(::flatbuffers::Verifier &verifier) const {
+    return VerifyTableStart(verifier) &&
+           VerifyField<float>(verifier, VT_VALUE, 4) &&
+           verifier.EndTable();
+  }
+};
+
+struct DistanceBuilder {
+  typedef Distance Table;
+  ::flatbuffers::FlatBufferBuilder &fbb_;
+  ::flatbuffers::uoffset_t start_;
+  void add_value(float value) {
+    fbb_.AddElement<float>(Distance::VT_VALUE, value, 0.0f);
+  }
+  explicit DistanceBuilder(::flatbuffers::FlatBufferBuilder &_fbb)
+        : fbb_(_fbb) {
+    start_ = fbb_.StartTable();
+  }
+  ::flatbuffers::Offset<Distance> Finish() {
+    const auto end = fbb_.EndTable(start_);
+    auto o = ::flatbuffers::Offset<Distance>(end);
+    return o;
+  }
+};
+
+inline ::flatbuffers::Offset<Distance> CreateDistance(
+    ::flatbuffers::FlatBufferBuilder &_fbb,
+    float value = 0.0f) {
+  DistanceBuilder builder_(_fbb);
+  builder_.add_value(value);
+  return builder_.Finish();
+}
+
+struct Temperature FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
+  typedef TemperatureBuilder Builder;
+  enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
+    VT_VALUE = 4
+  };
+  float value() const {
+    return GetField<float>(VT_VALUE, 0.0f);
+  }
+  bool Verify(::flatbuffers::Verifier &verifier) const {
+    return VerifyTableStart(verifier) &&
+           VerifyField<float>(verifier, VT_VALUE, 4) &&
+           verifier.EndTable();
+  }
+};
+
+struct TemperatureBuilder {
+  typedef Temperature Table;
+  ::flatbuffers::FlatBufferBuilder &fbb_;
+  ::flatbuffers::uoffset_t start_;
+  void add_value(float value) {
+    fbb_.AddElement<float>(Temperature::VT_VALUE, value, 0.0f);
+  }
+  explicit TemperatureBuilder(::flatbuffers::FlatBufferBuilder &_fbb)
+        : fbb_(_fbb) {
+    start_ = fbb_.StartTable();
+  }
+  ::flatbuffers::Offset<Temperature> Finish() {
+    const auto end = fbb_.EndTable(start_);
+    auto o = ::flatbuffers::Offset<Temperature>(end);
+    return o;
+  }
+};
+
+inline ::flatbuffers::Offset<Temperature> CreateTemperature(
+    ::flatbuffers::FlatBufferBuilder &_fbb,
+    float value = 0.0f) {
+  TemperatureBuilder builder_(_fbb);
+  builder_.add_value(value);
+  return builder_.Finish();
+}
+
+struct Position FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
+  typedef PositionBuilder Builder;
+  enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
+    VT_HEADING = 4,
+    VT_PITCH = 6,
+    VT_ROLL = 8
+  };
+  float heading() const {
+    return GetField<float>(VT_HEADING, 0.0f);
+  }
+  float pitch() const {
+    return GetField<float>(VT_PITCH, 0.0f);
+  }
+  float roll() const {
+    return GetField<float>(VT_ROLL, 0.0f);
+  }
+  bool Verify(::flatbuffers::Verifier &verifier) const {
+    return VerifyTableStart(verifier) &&
+           VerifyField<float>(verifier, VT_HEADING, 4) &&
+           VerifyField<float>(verifier, VT_PITCH, 4) &&
+           VerifyField<float>(verifier, VT_ROLL, 4) &&
+           verifier.EndTable();
+  }
+};
+
+struct PositionBuilder {
+  typedef Position Table;
+  ::flatbuffers::FlatBufferBuilder &fbb_;
+  ::flatbuffers::uoffset_t start_;
+  void add_heading(float heading) {
+    fbb_.AddElement<float>(Position::VT_HEADING, heading, 0.0f);
+  }
+  void add_pitch(float pitch) {
+    fbb_.AddElement<float>(Position::VT_PITCH, pitch, 0.0f);
+  }
+  void add_roll(float roll) {
+    fbb_.AddElement<float>(Position::VT_ROLL, roll, 0.0f);
+  }
+  explicit PositionBuilder(::flatbuffers::FlatBufferBuilder &_fbb)
+        : fbb_(_fbb) {
+    start_ = fbb_.StartTable();
+  }
+  ::flatbuffers::Offset<Position> Finish() {
+    const auto end = fbb_.EndTable(start_);
+    auto o = ::flatbuffers::Offset<Position>(end);
+    return o;
+  }
+};
+
+inline ::flatbuffers::Offset<Position> CreatePosition(
+    ::flatbuffers::FlatBufferBuilder &_fbb,
+    float heading = 0.0f,
+    float pitch = 0.0f,
+    float roll = 0.0f) {
+  PositionBuilder builder_(_fbb);
+  builder_.add_roll(roll);
+  builder_.add_pitch(pitch);
+  builder_.add_heading(heading);
+  return builder_.Finish();
+}
+
 struct SensorMessage FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
   typedef SensorMessageBuilder Builder;
   enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
@@ -298,6 +471,18 @@ inline bool VerifySensorValue(::flatbuffers::Verifier &verifier, const void *obj
       auto ptr = reinterpret_cast<const Messaging::CurrentText *>(obj);
       return verifier.VerifyTable(ptr);
     }
+    case SensorValue_Distance: {
+      auto ptr = reinterpret_cast<const Messaging::Distance *>(obj);
+      return verifier.VerifyTable(ptr);
+    }
+    case SensorValue_Temperature: {
+      auto ptr = reinterpret_cast<const Messaging::Temperature *>(obj);
+      return verifier.VerifyTable(ptr);
+    }
+    case SensorValue_Position: {
+      auto ptr = reinterpret_cast<const Messaging::Position *>(obj);
+      return verifier.VerifyTable(ptr);
+    }
     default: return true;
   }
 }
diff --git a/components/oled/Oled.cpp b/components/oled/Oled.cpp
index 7c1f26e..dfe7d89 100644
--- a/components/oled/Oled.cpp
+++ b/components/oled/Oled.cpp
@@ -1,8 +1,7 @@
-
 #include <string>
 
 #include "driver/gpio.h"
-#include "driver/i2c.h"
+#include <driver/i2c_master.h>
 #include "esp_err.h"
 #include "esp_log.h"
 #include "Oled.h"
@@ -18,141 +17,141 @@
 #define DISPLAY_HEIGHT 64
 #define PAGE_SIZE 128
 #define PAGE_COUNT 8
-#define I2C_PORT I2C_NUM_0
+#define I2C_TIMEOUT_MS 100
 
 void Oled::init(uint8_t sda_pin, uint8_t scl_pin) {
-    i2c_config_t i2c_config = {
-        .mode = I2C_MODE_MASTER,
-        .sda_io_num = sda_pin,
-        .scl_io_num = scl_pin,
-        .sda_pullup_en = GPIO_PULLUP_ENABLE,
-        .scl_pullup_en = GPIO_PULLUP_ENABLE,
-        .master = {
-            .clk_speed = 1000000
+    i2c_master_bus_config_t bus_config = {
+        .i2c_port = I2C_NUM_0,
+        .sda_io_num = (gpio_num_t)sda_pin,
+        .scl_io_num = (gpio_num_t)scl_pin,
+        .clk_source = I2C_CLK_SRC_DEFAULT,
+        .glitch_ignore_cnt = 7,
+        .intr_priority = 0,
+        .trans_queue_depth = 0,
+        .flags = {
+            .enable_internal_pullup = true,
         },
-        .clk_flags = 0,
     };
-    i2c_param_config(I2C_PORT, &i2c_config);
+    ESP_ERROR_CHECK(i2c_new_master_bus(&bus_config, &bus_handle));
 
-    i2c_driver_install(I2C_PORT, I2C_MODE_MASTER, 0, 0, 0);
+    i2c_device_config_t dev_config = {
+        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
+        .device_address = OLED_I2C_ADDRESS,
+        .scl_speed_hz = 400000,
+        .scl_wait_us = 0,
+        .flags = {
+            .disable_ack_check = false,
+        },
+    };
+    ESP_ERROR_CHECK(i2c_master_bus_add_device(bus_handle, &dev_config, &dev_handle));
 
-	i2c_cmd_handle_t cmd = i2c_cmd_link_create();
-	i2c_master_start(cmd);
-	i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);
-	i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_CMD_STREAM, true);
-	i2c_master_write_byte(cmd, OLED_CMD_SET_CHARGE_PUMP, true);
-	i2c_master_write_byte(cmd, 0x14, true);
-	i2c_master_write_byte(cmd, OLED_CMD_SET_SEGMENT_REMAP, true);
-	i2c_master_write_byte(cmd, OLED_CMD_SET_COM_SCAN_MODE, true);
-	i2c_master_write_byte(cmd, OLED_CMD_DISPLAY_ON, true);
-	i2c_master_stop(cmd);
+    // Full SSD1306 initialisation sequence
+    uint8_t init_cmd[] = {
+        OLED_CONTROL_BYTE_CMD_STREAM,
+        OLED_CMD_DISPLAY_OFF,               // display off during init
+        OLED_CMD_SET_DISPLAY_CLK_DIV, 0x80, // clock divide ratio / oscillator frequency
+        OLED_CMD_SET_MUX_RATIO, 0x3F,       // 1/64 duty (64 rows)
+        OLED_CMD_SET_DISPLAY_OFFSET, 0x00,  // no vertical shift
+        OLED_CMD_SET_DISPLAY_START_LINE,    // start line = 0
+        OLED_CMD_SET_CHARGE_PUMP, 0x14,     // enable charge pump
+        OLED_CMD_SET_MEMORY_ADDR_MODE, 0x02,// page addressing mode
+        OLED_CMD_SET_SEGMENT_REMAP,         // mirror horizontally (0xA1)
+        OLED_CMD_SET_COM_SCAN_MODE,         // mirror vertically   (0xC8)
+        OLED_CMD_SET_COM_PIN_MAP, 0x12,     // alt COM pin config for 128x64
+        OLED_CMD_SET_CONTRAST, 0x7F,        // mid contrast
+        OLED_CMD_SET_PRECHARGE, 0xF1,       // pre-charge period
+        OLED_CMD_SET_VCOMH_DESELCT, 0x30,  // VCOMH deselect level
+        OLED_CMD_DISPLAY_RAM,               // display follows RAM
+        OLED_CMD_DISPLAY_NORMAL,            // non-inverted
+        OLED_CMD_DISPLAY_ON,                // turn display on
+    };
 
-	if (ESP_OK != i2c_master_cmd_begin(I2C_PORT, cmd, 10 / portTICK_PERIOD_MS)) {
-		ESP_LOGE(TAG, "configuration failed");
-	}
+    if (ESP_OK != i2c_master_transmit(dev_handle, init_cmd, sizeof(init_cmd), I2C_TIMEOUT_MS)) {
+        ESP_LOGE(TAG, "configuration failed");
+    }
 
-	i2c_cmd_link_delete(cmd);
-
-	clear_display();
+    clear_display();
 }
 
 void Oled::clear_display() {
-    ESP_LOGI(TAG, "clear_display");
-	i2c_cmd_handle_t cmd;
 
-	uint8_t page[PAGE_SIZE] = {0};
-	for (uint8_t i = 0; i < PAGE_COUNT; i++) {
-		cmd = i2c_cmd_link_create();
-		i2c_master_start(cmd);
-		i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);
-		i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_CMD_SINGLE, true);
-		i2c_master_write_byte(cmd, 0xB0 | i, true);
+    // Data packet: control byte + PAGE_SIZE zero bytes
+    uint8_t data_buf[1 + PAGE_SIZE] = { OLED_CONTROL_BYTE_DATA_STREAM };
+    // rest is already zero-initialised
 
-		i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_DATA_STREAM, true);
-		i2c_master_write(cmd, page, PAGE_SIZE, true);
-		i2c_master_stop(cmd);
-		i2c_master_cmd_begin(I2C_PORT, cmd, 10 / portTICK_PERIOD_MS);
-		i2c_cmd_link_delete(cmd);
-	}
+    for (uint8_t i = 0; i < PAGE_COUNT; i++) {
+        // Set page address and reset column to 0
+        uint8_t page_cmd[] = {
+            OLED_CONTROL_BYTE_CMD_STREAM,
+            (uint8_t)(0xB0 | i), // page address
+            0x00,                // column low nibble = 0
+            0x10,                // column high nibble = 0
+        };
+        i2c_master_transmit(dev_handle, page_cmd, sizeof(page_cmd), I2C_TIMEOUT_MS);
+        i2c_master_transmit(dev_handle, data_buf, sizeof(data_buf), I2C_TIMEOUT_MS);
+    }
 }
 
 void Oled::set_contrast(uint8_t contrast) {
-	i2c_cmd_handle_t cmd;
-	cmd = i2c_cmd_link_create();
-	i2c_master_start(cmd);
-	i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);
-	i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_CMD_STREAM, true);
-	i2c_master_write_byte(cmd, OLED_CMD_SET_CONTRAST, true);
-	i2c_master_write_byte(cmd, contrast, true);
-	i2c_master_stop(cmd);
-	i2c_master_cmd_begin(I2C_NUM_0, cmd, 10 / portTICK_PERIOD_MS);
-	i2c_cmd_link_delete(cmd);
+    uint8_t cmd[] = {
+        OLED_CONTROL_BYTE_CMD_STREAM,
+        OLED_CMD_SET_CONTRAST,
+        contrast,
+    };
+    i2c_master_transmit(dev_handle, cmd, sizeof(cmd), I2C_TIMEOUT_MS);
 }
 
 void Oled::display_text(const std::string& text) {
-    ESP_LOGI(TAG, "display_text %s", text.c_str());
-	i2c_cmd_handle_t cmd;
 
-	uint8_t cur_page = 0;
-	uint8_t col = 0;
-	auto col_width = DISPLAY_WIDTH / 8;  // using an 8x8 font
-	auto num_rows = DISPLAY_HEIGHT / 8;  // using an 8x8 font
-	auto max_size = col_width * num_rows;
+    uint8_t cur_page = 0;
+    uint8_t col = 0;
+    auto col_width = DISPLAY_WIDTH / 8;  // using an 8x8 font
+    auto num_rows  = DISPLAY_HEIGHT / 8; // using an 8x8 font
+    auto max_size  = col_width * num_rows;
 
-	cmd = i2c_cmd_link_create();
-	i2c_master_start(cmd);
-	i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);
+    // Reset cursor to top-left
+    uint8_t reset_cmd[] = {
+        OLED_CONTROL_BYTE_CMD_STREAM,
+        0x00,           // reset column low nibble
+        0x10,           // reset column high nibble
+        (uint8_t)(0xB0 | cur_page), // reset page
+    };
+    i2c_master_transmit(dev_handle, reset_cmd, sizeof(reset_cmd), I2C_TIMEOUT_MS);
 
-	i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_CMD_STREAM, true);
-	i2c_master_write_byte(cmd, 0x00, true); // reset column
-	i2c_master_write_byte(cmd, 0x10, true);
-	i2c_master_write_byte(cmd, 0xB0 | cur_page, true); // reset page
+    for (uint8_t i = 0; i < text.size() && i < max_size; i++) {
+        if (text[i] == '\n' || col >= col_width) {
+            // new line
+            cur_page++;
+            col = 0;
+            uint8_t newline_cmd[] = {
+                OLED_CONTROL_BYTE_CMD_STREAM,
+                0x00,
+                0x10,
+                (uint8_t)(0xB0 | cur_page),
+            };
+            i2c_master_transmit(dev_handle, newline_cmd, sizeof(newline_cmd), I2C_TIMEOUT_MS);
+        } else {
+            uint8_t rotated[8];
+            rotate8x8((uint8_t*)font8x8_basic[(uint8_t)text[i]], rotated);
 
-	i2c_master_stop(cmd);
-	i2c_master_cmd_begin(I2C_NUM_0, cmd, 10 / portTICK_PERIOD_MS);
-	i2c_cmd_link_delete(cmd);
-
-	for (uint8_t i = 0; i < text.size() && i < max_size; i++) {
-	    ESP_LOGI(TAG, "%d: %c", i, text[i]);
-		if (text[i] == '\n' || col >= col_width) {
-		    ESP_LOGI(TAG, "new line");
-			cmd = i2c_cmd_link_create();
-			i2c_master_start(cmd);
-			i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);
-
-			i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_CMD_STREAM, true);
-			i2c_master_write_byte(cmd, 0x00, true); // reset column
-			i2c_master_write_byte(cmd, 0x10, true);
-			i2c_master_write_byte(cmd, 0xB0 | ++cur_page, true); // increment page
-
-			i2c_master_stop(cmd);
-			i2c_master_cmd_begin(I2C_NUM_0, cmd, 10/portTICK_PERIOD_MS);
-			i2c_cmd_link_delete(cmd);
-			col = 0;
-		} else {
-			cmd = i2c_cmd_link_create();
-			i2c_master_start(cmd);
-			i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);
-
-			i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_DATA_STREAM, true);
-			uint8_t rotated[8];
-			rotate8x8((uint8_t*)font8x8_basic[(uint8_t)text[i]], rotated);
-			i2c_master_write(cmd, rotated, 8, true);
-
-			i2c_master_stop(cmd);
-			i2c_master_cmd_begin(I2C_NUM_0, cmd, 10/portTICK_PERIOD_MS);
-			i2c_cmd_link_delete(cmd);
-			col++;
-		}
-	}
+            // Prepend data-stream control byte
+            uint8_t data_buf[9];
+            data_buf[0] = OLED_CONTROL_BYTE_DATA_STREAM;
+            for (int b = 0; b < 8; b++) {
+                data_buf[1 + b] = rotated[b];
+            }
+            i2c_master_transmit(dev_handle, data_buf, sizeof(data_buf), I2C_TIMEOUT_MS);
+            col++;
+        }
+    }
 }
 
 void Oled::rotate8x8(uint8_t in[8], uint8_t out[8]) {
     for (int x = 0; x < 8; x++) {
         out[x] = 0;
         for (int y = 0; y < 8; y++) {
-            if (in[y] & (1 << x)) {
-                out[x] |= (1 << (7 - y));
+            if (in[7 - y] & (1 << x)) {
+                out[x] |= (1 << y);  // bit 0 = top row, iterate rows bottom-to-top to correct orientation
             }
         }
     }
diff --git a/components/oled/include/Oled.h b/components/oled/include/Oled.h
index 854b812..c7248e7 100644
--- a/components/oled/include/Oled.h
+++ b/components/oled/include/Oled.h
@@ -1,6 +1,7 @@
 
 #include <cstdint>
 #include <string>
+#include "driver/i2c_master.h"
 
 #ifndef OLED_H
 #define OLED_H
@@ -18,6 +19,8 @@ private:
     void init(uint8_t sda_pin, uint8_t scl_pin);
     void rotate8x8(uint8_t in[8], uint8_t out[8]);
 
+    i2c_master_bus_handle_t bus_handle = nullptr;
+    i2c_master_dev_handle_t dev_handle = nullptr;
 };
 
 #endif // OLED_H
diff --git a/components/vl53l0x/CMakeLists.txt b/components/vl53l0x/CMakeLists.txt
new file mode 100644
index 0000000..e8c86f0
--- /dev/null
+++ b/components/vl53l0x/CMakeLists.txt
@@ -0,0 +1,5 @@
+idf_component_register(
+			SRCS "vl53l0x.c"
+			INCLUDE_DIRS "include"
+			REQUIRES "driver" "esp_timer"
+)
diff --git a/components/vl53l0x/Kconfig b/components/vl53l0x/Kconfig
new file mode 100644
index 0000000..fdb78d7
--- /dev/null
+++ b/components/vl53l0x/Kconfig
@@ -0,0 +1,9 @@
+menu "VL53L0X"
+
+	config VL53L0X_DEBUG
+	bool "Low level debug"
+	default n
+	help
+		Low level debug
+
+endmenu
diff --git a/components/vl53l0x/LICENSE b/components/vl53l0x/LICENSE
new file mode 100644
index 0000000..9cecc1d
--- /dev/null
+++ b/components/vl53l0x/LICENSE
@@ -0,0 +1,674 @@
+                    GNU GENERAL PUBLIC LICENSE
+                       Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+                            Preamble
+
+  The GNU General Public License is a free, copyleft license for
+software and other kinds of works.
+
+  The licenses for most software and other practical works are designed
+to take away your freedom to share and change the works.  By contrast,
+the GNU General Public License is intended to guarantee your freedom to
+share and change all versions of a program--to make sure it remains free
+software for all its users.  We, the Free Software Foundation, use the
+GNU General Public License for most of our software; it applies also to
+any other work released this way by its authors.  You can apply it to
+your programs, too.
+
+  When we speak of free software, we are referring to freedom, not
+price.  Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+them if you wish), that you receive source code or can get it if you
+want it, that you can change the software or use pieces of it in new
+free programs, and that you know you can do these things.
+
+  To protect your rights, we need to prevent others from denying you
+these rights or asking you to surrender the rights.  Therefore, you have
+certain responsibilities if you distribute copies of the software, or if
+you modify it: responsibilities to respect the freedom of others.
+
+  For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must pass on to the recipients the same
+freedoms that you received.  You must make sure that they, too, receive
+or can get the source code.  And you must show them these terms so they
+know their rights.
+
+  Developers that use the GNU GPL protect your rights with two steps:
+(1) assert copyright on the software, and (2) offer you this License
+giving you legal permission to copy, distribute and/or modify it.
+
+  For the developers' and authors' protection, the GPL clearly explains
+that there is no warranty for this free software.  For both users' and
+authors' sake, the GPL requires that modified versions be marked as
+changed, so that their problems will not be attributed erroneously to
+authors of previous versions.
+
+  Some devices are designed to deny users access to install or run
+modified versions of the software inside them, although the manufacturer
+can do so.  This is fundamentally incompatible with the aim of
+protecting users' freedom to change the software.  The systematic
+pattern of such abuse occurs in the area of products for individuals to
+use, which is precisely where it is most unacceptable.  Therefore, we
+have designed this version of the GPL to prohibit the practice for those
+products.  If such problems arise substantially in other domains, we
+stand ready to extend this provision to those domains in future versions
+of the GPL, as needed to protect the freedom of users.
+
+  Finally, every program is threatened constantly by software patents.
+States should not allow patents to restrict development and use of
+software on general-purpose computers, but in those that do, we wish to
+avoid the special danger that patents applied to a free program could
+make it effectively proprietary.  To prevent this, the GPL assures that
+patents cannot be used to render the program non-free.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.
+
+                       TERMS AND CONDITIONS
+
+  0. Definitions.
+
+  "This License" refers to version 3 of the GNU General Public License.
+
+  "Copyright" also means copyright-like laws that apply to other kinds of
+works, such as semiconductor masks.
+
+  "The Program" refers to any copyrightable work licensed under this
+License.  Each licensee is addressed as "you".  "Licensees" and
+"recipients" may be individuals or organizations.
+
+  To "modify" a work means to copy from or adapt all or part of the work
+in a fashion requiring copyright permission, other than the making of an
+exact copy.  The resulting work is called a "modified version" of the
+earlier work or a work "based on" the earlier work.
+
+  A "covered work" means either the unmodified Program or a work based
+on the Program.
+
+  To "propagate" a work means to do anything with it that, without
+permission, would make you directly or secondarily liable for
+infringement under applicable copyright law, except executing it on a
+computer or modifying a private copy.  Propagation includes copying,
+distribution (with or without modification), making available to the
+public, and in some countries other activities as well.
+
+  To "convey" a work means any kind of propagation that enables other
+parties to make or receive copies.  Mere interaction with a user through
+a computer network, with no transfer of a copy, is not conveying.
+
+  An interactive user interface displays "Appropriate Legal Notices"
+to the extent that it includes a convenient and prominently visible
+feature that (1) displays an appropriate copyright notice, and (2)
+tells the user that there is no warranty for the work (except to the
+extent that warranties are provided), that licensees may convey the
+work under this License, and how to view a copy of this License.  If
+the interface presents a list of user commands or options, such as a
+menu, a prominent item in the list meets this criterion.
+
+  1. Source Code.
+
+  The "source code" for a work means the preferred form of the work
+for making modifications to it.  "Object code" means any non-source
+form of a work.
+
+  A "Standard Interface" means an interface that either is an official
+standard defined by a recognized standards body, or, in the case of
+interfaces specified for a particular programming language, one that
+is widely used among developers working in that language.
+
+  The "System Libraries" of an executable work include anything, other
+than the work as a whole, that (a) is included in the normal form of
+packaging a Major Component, but which is not part of that Major
+Component, and (b) serves only to enable use of the work with that
+Major Component, or to implement a Standard Interface for which an
+implementation is available to the public in source code form.  A
+"Major Component", in this context, means a major essential component
+(kernel, window system, and so on) of the specific operating system
+(if any) on which the executable work runs, or a compiler used to
+produce the work, or an object code interpreter used to run it.
+
+  The "Corresponding Source" for a work in object code form means all
+the source code needed to generate, install, and (for an executable
+work) run the object code and to modify the work, including scripts to
+control those activities.  However, it does not include the work's
+System Libraries, or general-purpose tools or generally available free
+programs which are used unmodified in performing those activities but
+which are not part of the work.  For example, Corresponding Source
+includes interface definition files associated with source files for
+the work, and the source code for shared libraries and dynamically
+linked subprograms that the work is specifically designed to require,
+such as by intimate data communication or control flow between those
+subprograms and other parts of the work.
+
+  The Corresponding Source need not include anything that users
+can regenerate automatically from other parts of the Corresponding
+Source.
+
+  The Corresponding Source for a work in source code form is that
+same work.
+
+  2. Basic Permissions.
+
+  All rights granted under this License are granted for the term of
+copyright on the Program, and are irrevocable provided the stated
+conditions are met.  This License explicitly affirms your unlimited
+permission to run the unmodified Program.  The output from running a
+covered work is covered by this License only if the output, given its
+content, constitutes a covered work.  This License acknowledges your
+rights of fair use or other equivalent, as provided by copyright law.
+
+  You may make, run and propagate covered works that you do not
+convey, without conditions so long as your license otherwise remains
+in force.  You may convey covered works to others for the sole purpose
+of having them make modifications exclusively for you, or provide you
+with facilities for running those works, provided that you comply with
+the terms of this License in conveying all material for which you do
+not control copyright.  Those thus making or running the covered works
+for you must do so exclusively on your behalf, under your direction
+and control, on terms that prohibit them from making any copies of
+your copyrighted material outside their relationship with you.
+
+  Conveying under any other circumstances is permitted solely under
+the conditions stated below.  Sublicensing is not allowed; section 10
+makes it unnecessary.
+
+  3. Protecting Users' Legal Rights From Anti-Circumvention Law.
+
+  No covered work shall be deemed part of an effective technological
+measure under any applicable law fulfilling obligations under article
+11 of the WIPO copyright treaty adopted on 20 December 1996, or
+similar laws prohibiting or restricting circumvention of such
+measures.
+
+  When you convey a covered work, you waive any legal power to forbid
+circumvention of technological measures to the extent such circumvention
+is effected by exercising rights under this License with respect to
+the covered work, and you disclaim any intention to limit operation or
+modification of the work as a means of enforcing, against the work's
+users, your or third parties' legal rights to forbid circumvention of
+technological measures.
+
+  4. Conveying Verbatim Copies.
+
+  You may convey verbatim copies of the Program's source code as you
+receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice;
+keep intact all notices stating that this License and any
+non-permissive terms added in accord with section 7 apply to the code;
+keep intact all notices of the absence of any warranty; and give all
+recipients a copy of this License along with the Program.
+
+  You may charge any price or no price for each copy that you convey,
+and you may offer support or warranty protection for a fee.
+
+  5. Conveying Modified Source Versions.
+
+  You may convey a work based on the Program, or the modifications to
+produce it from the Program, in the form of source code under the
+terms of section 4, provided that you also meet all of these conditions:
+
+    a) The work must carry prominent notices stating that you modified
+    it, and giving a relevant date.
+
+    b) The work must carry prominent notices stating that it is
+    released under this License and any conditions added under section
+    7.  This requirement modifies the requirement in section 4 to
+    "keep intact all notices".
+
+    c) You must license the entire work, as a whole, under this
+    License to anyone who comes into possession of a copy.  This
+    License will therefore apply, along with any applicable section 7
+    additional terms, to the whole of the work, and all its parts,
+    regardless of how they are packaged.  This License gives no
+    permission to license the work in any other way, but it does not
+    invalidate such permission if you have separately received it.
+
+    d) If the work has interactive user interfaces, each must display
+    Appropriate Legal Notices; however, if the Program has interactive
+    interfaces that do not display Appropriate Legal Notices, your
+    work need not make them do so.
+
+  A compilation of a covered work with other separate and independent
+works, which are not by their nature extensions of the covered work,
+and which are not combined with it such as to form a larger program,
+in or on a volume of a storage or distribution medium, is called an
+"aggregate" if the compilation and its resulting copyright are not
+used to limit the access or legal rights of the compilation's users
+beyond what the individual works permit.  Inclusion of a covered work
+in an aggregate does not cause this License to apply to the other
+parts of the aggregate.
+
+  6. Conveying Non-Source Forms.
+
+  You may convey a covered work in object code form under the terms
+of sections 4 and 5, provided that you also convey the
+machine-readable Corresponding Source under the terms of this License,
+in one of these ways:
+
+    a) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by the
+    Corresponding Source fixed on a durable physical medium
+    customarily used for software interchange.
+
+    b) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by a
+    written offer, valid for at least three years and valid for as
+    long as you offer spare parts or customer support for that product
+    model, to give anyone who possesses the object code either (1) a
+    copy of the Corresponding Source for all the software in the
+    product that is covered by this License, on a durable physical
+    medium customarily used for software interchange, for a price no
+    more than your reasonable cost of physically performing this
+    conveying of source, or (2) access to copy the
+    Corresponding Source from a network server at no charge.
+
+    c) Convey individual copies of the object code with a copy of the
+    written offer to provide the Corresponding Source.  This
+    alternative is allowed only occasionally and noncommercially, and
+    only if you received the object code with such an offer, in accord
+    with subsection 6b.
+
+    d) Convey the object code by offering access from a designated
+    place (gratis or for a charge), and offer equivalent access to the
+    Corresponding Source in the same way through the same place at no
+    further charge.  You need not require recipients to copy the
+    Corresponding Source along with the object code.  If the place to
+    copy the object code is a network server, the Corresponding Source
+    may be on a different server (operated by you or a third party)
+    that supports equivalent copying facilities, provided you maintain
+    clear directions next to the object code saying where to find the
+    Corresponding Source.  Regardless of what server hosts the
+    Corresponding Source, you remain obligated to ensure that it is
+    available for as long as needed to satisfy these requirements.
+
+    e) Convey the object code using peer-to-peer transmission, provided
+    you inform other peers where the object code and Corresponding
+    Source of the work are being offered to the general public at no
+    charge under subsection 6d.
+
+  A separable portion of the object code, whose source code is excluded
+from the Corresponding Source as a System Library, need not be
+included in conveying the object code work.
+
+  A "User Product" is either (1) a "consumer product", which means any
+tangible personal property which is normally used for personal, family,
+or household purposes, or (2) anything designed or sold for incorporation
+into a dwelling.  In determining whether a product is a consumer product,
+doubtful cases shall be resolved in favor of coverage.  For a particular
+product received by a particular user, "normally used" refers to a
+typical or common use of that class of product, regardless of the status
+of the particular user or of the way in which the particular user
+actually uses, or expects or is expected to use, the product.  A product
+is a consumer product regardless of whether the product has substantial
+commercial, industrial or non-consumer uses, unless such uses represent
+the only significant mode of use of the product.
+
+  "Installation Information" for a User Product means any methods,
+procedures, authorization keys, or other information required to install
+and execute modified versions of a covered work in that User Product from
+a modified version of its Corresponding Source.  The information must
+suffice to ensure that the continued functioning of the modified object
+code is in no case prevented or interfered with solely because
+modification has been made.
+
+  If you convey an object code work under this section in, or with, or
+specifically for use in, a User Product, and the conveying occurs as
+part of a transaction in which the right of possession and use of the
+User Product is transferred to the recipient in perpetuity or for a
+fixed term (regardless of how the transaction is characterized), the
+Corresponding Source conveyed under this section must be accompanied
+by the Installation Information.  But this requirement does not apply
+if neither you nor any third party retains the ability to install
+modified object code on the User Product (for example, the work has
+been installed in ROM).
+
+  The requirement to provide Installation Information does not include a
+requirement to continue to provide support service, warranty, or updates
+for a work that has been modified or installed by the recipient, or for
+the User Product in which it has been modified or installed.  Access to a
+network may be denied when the modification itself materially and
+adversely affects the operation of the network or violates the rules and
+protocols for communication across the network.
+
+  Corresponding Source conveyed, and Installation Information provided,
+in accord with this section must be in a format that is publicly
+documented (and with an implementation available to the public in
+source code form), and must require no special password or key for
+unpacking, reading or copying.
+
+  7. Additional Terms.
+
+  "Additional permissions" are terms that supplement the terms of this
+License by making exceptions from one or more of its conditions.
+Additional permissions that are applicable to the entire Program shall
+be treated as though they were included in this License, to the extent
+that they are valid under applicable law.  If additional permissions
+apply only to part of the Program, that part may be used separately
+under those permissions, but the entire Program remains governed by
+this License without regard to the additional permissions.
+
+  When you convey a copy of a covered work, you may at your option
+remove any additional permissions from that copy, or from any part of
+it.  (Additional permissions may be written to require their own
+removal in certain cases when you modify the work.)  You may place
+additional permissions on material, added by you to a covered work,
+for which you have or can give appropriate copyright permission.
+
+  Notwithstanding any other provision of this License, for material you
+add to a covered work, you may (if authorized by the copyright holders of
+that material) supplement the terms of this License with terms:
+
+    a) Disclaiming warranty or limiting liability differently from the
+    terms of sections 15 and 16 of this License; or
+
+    b) Requiring preservation of specified reasonable legal notices or
+    author attributions in that material or in the Appropriate Legal
+    Notices displayed by works containing it; or
+
+    c) Prohibiting misrepresentation of the origin of that material, or
+    requiring that modified versions of such material be marked in
+    reasonable ways as different from the original version; or
+
+    d) Limiting the use for publicity purposes of names of licensors or
+    authors of the material; or
+
+    e) Declining to grant rights under trademark law for use of some
+    trade names, trademarks, or service marks; or
+
+    f) Requiring indemnification of licensors and authors of that
+    material by anyone who conveys the material (or modified versions of
+    it) with contractual assumptions of liability to the recipient, for
+    any liability that these contractual assumptions directly impose on
+    those licensors and authors.
+
+  All other non-permissive additional terms are considered "further
+restrictions" within the meaning of section 10.  If the Program as you
+received it, or any part of it, contains a notice stating that it is
+governed by this License along with a term that is a further
+restriction, you may remove that term.  If a license document contains
+a further restriction but permits relicensing or conveying under this
+License, you may add to a covered work material governed by the terms
+of that license document, provided that the further restriction does
+not survive such relicensing or conveying.
+
+  If you add terms to a covered work in accord with this section, you
+must place, in the relevant source files, a statement of the
+additional terms that apply to those files, or a notice indicating
+where to find the applicable terms.
+
+  Additional terms, permissive or non-permissive, may be stated in the
+form of a separately written license, or stated as exceptions;
+the above requirements apply either way.
+
+  8. Termination.
+
+  You may not propagate or modify a covered work except as expressly
+provided under this License.  Any attempt otherwise to propagate or
+modify it is void, and will automatically terminate your rights under
+this License (including any patent licenses granted under the third
+paragraph of section 11).
+
+  However, if you cease all violation of this License, then your
+license from a particular copyright holder is reinstated (a)
+provisionally, unless and until the copyright holder explicitly and
+finally terminates your license, and (b) permanently, if the copyright
+holder fails to notify you of the violation by some reasonable means
+prior to 60 days after the cessation.
+
+  Moreover, your license from a particular copyright holder is
+reinstated permanently if the copyright holder notifies you of the
+violation by some reasonable means, this is the first time you have
+received notice of violation of this License (for any work) from that
+copyright holder, and you cure the violation prior to 30 days after
+your receipt of the notice.
+
+  Termination of your rights under this section does not terminate the
+licenses of parties who have received copies or rights from you under
+this License.  If your rights have been terminated and not permanently
+reinstated, you do not qualify to receive new licenses for the same
+material under section 10.
+
+  9. Acceptance Not Required for Having Copies.
+
+  You are not required to accept this License in order to receive or
+run a copy of the Program.  Ancillary propagation of a covered work
+occurring solely as a consequence of using peer-to-peer transmission
+to receive a copy likewise does not require acceptance.  However,
+nothing other than this License grants you permission to propagate or
+modify any covered work.  These actions infringe copyright if you do
+not accept this License.  Therefore, by modifying or propagating a
+covered work, you indicate your acceptance of this License to do so.
+
+  10. Automatic Licensing of Downstream Recipients.
+
+  Each time you convey a covered work, the recipient automatically
+receives a license from the original licensors, to run, modify and
+propagate that work, subject to this License.  You are not responsible
+for enforcing compliance by third parties with this License.
+
+  An "entity transaction" is a transaction transferring control of an
+organization, or substantially all assets of one, or subdividing an
+organization, or merging organizations.  If propagation of a covered
+work results from an entity transaction, each party to that
+transaction who receives a copy of the work also receives whatever
+licenses to the work the party's predecessor in interest had or could
+give under the previous paragraph, plus a right to possession of the
+Corresponding Source of the work from the predecessor in interest, if
+the predecessor has it or can get it with reasonable efforts.
+
+  You may not impose any further restrictions on the exercise of the
+rights granted or affirmed under this License.  For example, you may
+not impose a license fee, royalty, or other charge for exercise of
+rights granted under this License, and you may not initiate litigation
+(including a cross-claim or counterclaim in a lawsuit) alleging that
+any patent claim is infringed by making, using, selling, offering for
+sale, or importing the Program or any portion of it.
+
+  11. Patents.
+
+  A "contributor" is a copyright holder who authorizes use under this
+License of the Program or a work on which the Program is based.  The
+work thus licensed is called the contributor's "contributor version".
+
+  A contributor's "essential patent claims" are all patent claims
+owned or controlled by the contributor, whether already acquired or
+hereafter acquired, that would be infringed by some manner, permitted
+by this License, of making, using, or selling its contributor version,
+but do not include claims that would be infringed only as a
+consequence of further modification of the contributor version.  For
+purposes of this definition, "control" includes the right to grant
+patent sublicenses in a manner consistent with the requirements of
+this License.
+
+  Each contributor grants you a non-exclusive, worldwide, royalty-free
+patent license under the contributor's essential patent claims, to
+make, use, sell, offer for sale, import and otherwise run, modify and
+propagate the contents of its contributor version.
+
+  In the following three paragraphs, a "patent license" is any express
+agreement or commitment, however denominated, not to enforce a patent
+(such as an express permission to practice a patent or covenant not to
+sue for patent infringement).  To "grant" such a patent license to a
+party means to make such an agreement or commitment not to enforce a
+patent against the party.
+
+  If you convey a covered work, knowingly relying on a patent license,
+and the Corresponding Source of the work is not available for anyone
+to copy, free of charge and under the terms of this License, through a
+publicly available network server or other readily accessible means,
+then you must either (1) cause the Corresponding Source to be so
+available, or (2) arrange to deprive yourself of the benefit of the
+patent license for this particular work, or (3) arrange, in a manner
+consistent with the requirements of this License, to extend the patent
+license to downstream recipients.  "Knowingly relying" means you have
+actual knowledge that, but for the patent license, your conveying the
+covered work in a country, or your recipient's use of the covered work
+in a country, would infringe one or more identifiable patents in that
+country that you have reason to believe are valid.
+
+  If, pursuant to or in connection with a single transaction or
+arrangement, you convey, or propagate by procuring conveyance of, a
+covered work, and grant a patent license to some of the parties
+receiving the covered work authorizing them to use, propagate, modify
+or convey a specific copy of the covered work, then the patent license
+you grant is automatically extended to all recipients of the covered
+work and works based on it.
+
+  A patent license is "discriminatory" if it does not include within
+the scope of its coverage, prohibits the exercise of, or is
+conditioned on the non-exercise of one or more of the rights that are
+specifically granted under this License.  You may not convey a covered
+work if you are a party to an arrangement with a third party that is
+in the business of distributing software, under which you make payment
+to the third party based on the extent of your activity of conveying
+the work, and under which the third party grants, to any of the
+parties who would receive the covered work from you, a discriminatory
+patent license (a) in connection with copies of the covered work
+conveyed by you (or copies made from those copies), or (b) primarily
+for and in connection with specific products or compilations that
+contain the covered work, unless you entered into that arrangement,
+or that patent license was granted, prior to 28 March 2007.
+
+  Nothing in this License shall be construed as excluding or limiting
+any implied license or other defenses to infringement that may
+otherwise be available to you under applicable patent law.
+
+  12. No Surrender of Others' Freedom.
+
+  If conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot convey a
+covered work so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you may
+not convey it at all.  For example, if you agree to terms that obligate you
+to collect a royalty for further conveying from those to whom you convey
+the Program, the only way you could satisfy both those terms and this
+License would be to refrain entirely from conveying the Program.
+
+  13. Use with the GNU Affero General Public License.
+
+  Notwithstanding any other provision of this License, you have
+permission to link or combine any covered work with a work licensed
+under version 3 of the GNU Affero General Public License into a single
+combined work, and to convey the resulting work.  The terms of this
+License will continue to apply to the part which is the covered work,
+but the special requirements of the GNU Affero General Public License,
+section 13, concerning interaction through a network will apply to the
+combination as such.
+
+  14. Revised Versions of this License.
+
+  The Free Software Foundation may publish revised and/or new versions of
+the GNU General Public License from time to time.  Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+  Each version is given a distinguishing version number.  If the
+Program specifies that a certain numbered version of the GNU General
+Public License "or any later version" applies to it, you have the
+option of following the terms and conditions either of that numbered
+version or of any later version published by the Free Software
+Foundation.  If the Program does not specify a version number of the
+GNU General Public License, you may choose any version ever published
+by the Free Software Foundation.
+
+  If the Program specifies that a proxy can decide which future
+versions of the GNU General Public License can be used, that proxy's
+public statement of acceptance of a version permanently authorizes you
+to choose that version for the Program.
+
+  Later license versions may give you additional or different
+permissions.  However, no additional obligations are imposed on any
+author or copyright holder as a result of your choosing to follow a
+later version.
+
+  15. Disclaimer of Warranty.
+
+  THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
+OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
+IS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
+ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. Limitation of Liability.
+
+  IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
+THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
+USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
+DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
+PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
+EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGES.
+
+  17. Interpretation of Sections 15 and 16.
+
+  If the disclaimer of warranty and limitation of liability provided
+above cannot be given local legal effect according to their terms,
+reviewing courts shall apply local law that most closely approximates
+an absolute waiver of all civil liability in connection with the
+Program, unless a warranty or assumption of liability accompanies a
+copy of the Program in return for a fee.
+
+                     END OF TERMS AND CONDITIONS
+
+            How to Apply These Terms to Your New Programs
+
+  If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+  To do so, attach the following notices to the program.  It is safest
+to attach them to the start of each source file to most effectively
+state the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+    {one line to give the program's name and a brief idea of what it does.}
+    Copyright (C) {year}  {name of author}
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+Also add information on how to contact you by electronic and paper mail.
+
+  If the program does terminal interaction, make it output a short
+notice like this when it starts in an interactive mode:
+
+    {project}  Copyright (C) {year}  {fullname}
+    This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+    This is free software, and you are welcome to redistribute it
+    under certain conditions; type `show c' for details.
+
+The hypothetical commands `show w' and `show c' should show the appropriate
+parts of the General Public License.  Of course, your program's commands
+might be different; for a GUI interface, you would use an "about box".
+
+  You should also get your employer (if you work as a programmer) or school,
+if any, to sign a "copyright disclaimer" for the program, if necessary.
+For more information on this, and how to apply and follow the GNU GPL, see
+<http://www.gnu.org/licenses/>.
+
+  The GNU General Public License does not permit incorporating your program
+into proprietary programs.  If your program is a subroutine library, you
+may consider it more useful to permit linking proprietary applications with
+the library.  If this is what you want to do, use the GNU Lesser General
+Public License instead of this License.  But first, please read
+<http://www.gnu.org/philosophy/why-not-lgpl.html>.
diff --git a/components/vl53l0x/README.md b/components/vl53l0x/README.md
new file mode 100644
index 0000000..525bbd6
--- /dev/null
+++ b/components/vl53l0x/README.md
@@ -0,0 +1,360 @@
+# ESP32-VL53L0X
+
+VL53L0X laser ranging sensor library for ESP-IDF (ESP32, ESP32-S2, ESP32-C3, etc.)
+
+## Overview
+
+This library provides a simple interface to the ST VL53L0X Time-of-Flight (ToF) ranging sensor using ESP-IDF v5.2+ with the new I2C master driver API.
+This is a fork from revk's library for the same, but tweaked for the new I2C Master driver API, and tweaked to handle multiple devices, the original
+code wouldn't actually do that. 
+
+## Features
+
+- Support for ESP-IDF v5.2 and newer
+- Uses the new I2C master bus/device driver API
+- Single and continuous ranging modes
+- Multiple sensor support on same I2C bus
+- Configurable timing budgets and VCSEL periods
+- XSHUT pin control for sensor management
+
+## Requirements
+
+- **ESP-IDF v5.2 or newer** - This library uses the new I2C master driver API introduced in ESP-IDF v5.2
+- VL53L0X sensor module
+
+## Installation
+
+Add this component to your ESP-IDF project:
+
+```bash
+cd components
+git clone https://github.com/bigredelf/ESP32-VL53L0X.git
+```
+
+Or add as a git submodule:
+
+```bash
+git submodule add https://github.com/bigredelf/ESP32-VL53L0X.git components/ESP32-VL53L0X
+```
+
+## Quick Start
+
+### Single Sensor Example
+
+```c
+#include <stdio.h>
+#include "vl53l0x.h"
+#include "esp_log.h"
+
+#define I2C_PORT    0
+#define I2C_SDA     21
+#define I2C_SCL     22
+#define XSHUT_PIN   -1  // Not used
+
+void app_main(void)
+{
+    // Initialize the sensor
+    vl53l0x_t *sensor = vl53l0x_config(I2C_PORT, I2C_SCL, I2C_SDA, XSHUT_PIN, 0x29, 1);
+    if (!sensor) {
+        ESP_LOGE("VL53L0X", "Failed to configure sensor");
+        return;
+    }
+
+    const char *err = vl53l0x_init(sensor);
+    if (err) {
+        ESP_LOGE("VL53L0X", "Init failed: %s", err);
+        vl53l0x_end(sensor);
+        return;
+    }
+
+    // Start continuous ranging
+    vl53l0x_startContinuous(sensor, 0);
+
+    while (1) {
+        uint16_t range = vl53l0x_readRangeContinuousMillimeters(sensor);
+        if (range == 65535) {
+            ESP_LOGW("VL53L0X", "Out of range or timeout");
+        } else {
+            ESP_LOGI("VL53L0X", "Range: %d mm", range);
+        }
+        vTaskDelay(pdMS_TO_TICKS(100));
+    }
+}
+```
+
+### Multiple Sensors Example
+
+For multiple sensors on the same I2C bus, use XSHUT pins to enable them one at a time and assign unique addresses:
+
+```c
+#include "vl53l0x.h"
+#include <driver/i2c_master.h>
+
+#define I2C_PORT    0
+#define I2C_SDA     21
+#define I2C_SCL     22
+#define XSHUT1      4
+#define XSHUT2      5
+
+void app_main(void)
+{
+    // Create shared I2C bus
+    i2c_master_bus_config_t bus_config = {
+        .clk_source = I2C_CLK_SRC_DEFAULT,
+        .i2c_port = I2C_PORT,
+        .scl_io_num = I2C_SCL,
+        .sda_io_num = I2C_SDA,
+        .glitch_ignore_cnt = 7,
+        .flags.enable_internal_pullup = true,
+    };
+    
+    i2c_master_bus_handle_t bus_handle;
+    i2c_new_master_bus(&bus_config, &bus_handle);
+
+    // Initialize first sensor with default address
+    vl53l0x_t *sensor1 = vl53l0x_config_with_bus(bus_handle, XSHUT1, 0x29, 1);
+    vl53l0x_init(sensor1);
+    vl53l0x_setAddress(sensor1, 0x30);  // Change address
+
+    // Initialize second sensor
+    vl53l0x_t *sensor2 = vl53l0x_config_with_bus(bus_handle, XSHUT2, 0x29, 1);
+    vl53l0x_init(sensor2);
+    vl53l0x_setAddress(sensor2, 0x31);  // Different address
+
+    // Start continuous ranging on both
+    vl53l0x_startContinuous(sensor1, 0);
+    vl53l0x_startContinuous(sensor2, 0);
+
+    while (1) {
+        uint16_t range1 = vl53l0x_readRangeContinuousMillimeters(sensor1);
+        uint16_t range2 = vl53l0x_readRangeContinuousMillimeters(sensor2);
+        ESP_LOGI("VL53L0X", "Sensor1: %d mm, Sensor2: %d mm", range1, range2);
+        vTaskDelay(pdMS_TO_TICKS(100));
+    }
+}
+```
+
+## API Reference
+
+### Configuration Functions
+
+#### `vl53l0x_config()`
+Create and configure a VL53L0X sensor with its own I2C bus.
+
+```c
+vl53l0x_t *vl53l0x_config(int8_t port, int8_t scl, int8_t sda, 
+                          int8_t xshut, uint8_t address, uint8_t io_2v8);
+```
+
+**Parameters:**
+- `port` - I2C port number (0 or 1)
+- `scl` - SCL GPIO pin number
+- `sda` - SDA GPIO pin number
+- `xshut` - XSHUT GPIO pin number (-1 if not used)
+- `address` - I2C address (default: 0x29)
+- `io_2v8` - Set to 1 for 2.8V I/O, 0 for 1.8V
+
+**Returns:** Pointer to vl53l0x_t structure, or NULL on failure
+
+#### `vl53l0x_config_with_bus()`
+Create a VL53L0X sensor using an existing I2C bus handle (for multiple sensors).
+
+```c
+vl53l0x_t *vl53l0x_config_with_bus(i2c_master_bus_handle_t bus_handle, 
+                                   int8_t xshut, uint8_t address, uint8_t io_2v8);
+```
+
+**Parameters:**
+- `bus_handle` - Existing I2C master bus handle
+- `xshut` - XSHUT GPIO pin number (-1 if not used)
+- `address` - I2C address (default: 0x29)
+- `io_2v8` - Set to 1 for 2.8V I/O, 0 for 1.8V
+
+**Returns:** Pointer to vl53l0x_t structure, or NULL on failure
+
+#### `vl53l0x_init()`
+Initialize the sensor with default settings.
+
+```c
+const char *vl53l0x_init(vl53l0x_t *v);
+```
+
+**Returns:** NULL on success, error string on failure
+
+#### `vl53l0x_end()`
+Clean up and free resources.
+
+```c
+void vl53l0x_end(vl53l0x_t *v);
+```
+
+### Ranging Functions
+
+#### `vl53l0x_readRangeSingleMillimeters()`
+Perform a single ranging measurement.
+
+```c
+uint16_t vl53l0x_readRangeSingleMillimeters(vl53l0x_t *v);
+```
+
+**Returns:** Range in millimeters, or 65535 on error/timeout
+
+#### `vl53l0x_startContinuous()`
+Start continuous ranging mode.
+
+```c
+void vl53l0x_startContinuous(vl53l0x_t *v, uint32_t period_ms);
+```
+
+**Parameters:**
+- `period_ms` - Inter-measurement period in milliseconds (0 for back-to-back mode)
+
+#### `vl53l0x_readRangeContinuousMillimeters()`
+Read range in continuous mode.
+
+```c
+uint16_t vl53l0x_readRangeContinuousMillimeters(vl53l0x_t *v);
+```
+
+**Returns:** Range in millimeters, or 65535 on error/timeout
+
+#### `vl53l0x_stopContinuous()`
+Stop continuous ranging mode.
+
+```c
+void vl53l0x_stopContinuous(vl53l0x_t *v);
+```
+
+### Configuration Functions
+
+#### `vl53l0x_setAddress()`
+Change the I2C address of the sensor.
+
+```c
+void vl53l0x_setAddress(vl53l0x_t *v, uint8_t new_addr);
+```
+
+#### `vl53l0x_setTimeout()`
+Set I/O timeout in milliseconds.
+
+```c
+void vl53l0x_setTimeout(vl53l0x_t *v, uint16_t timeout_ms);
+```
+
+#### `vl53l0x_setMeasurementTimingBudget()`
+Set measurement timing budget in microseconds (20000 to 1000000).
+
+```c
+const char *vl53l0x_setMeasurementTimingBudget(vl53l0x_t *v, uint32_t budget_us);
+```
+
+#### `vl53l0x_setSignalRateLimit()`
+Set signal rate limit in MCPS (0.0 to 511.99).
+
+```c
+const char *vl53l0x_setSignalRateLimit(vl53l0x_t *v, float limit_Mcps);
+```
+
+#### `vl53l0x_setVcselPulsePeriod()`
+Set VCSEL pulse period.
+
+```c
+const char *vl53l0x_setVcselPulsePeriod(vl53l0x_t *v, 
+                                        vl53l0x_vcselPeriodType type, 
+                                        uint8_t period_pclks);
+```
+
+**Parameters:**
+- `type` - `VcselPeriodPreRange` or `VcselPeriodFinalRange`
+- `period_pclks` - Period in PCLKs (pre-range: 12-18 even, final-range: 8-14 even)
+
+### Status Functions
+
+#### `vl53l0x_timeoutOccurred()`
+Check if a timeout occurred during the last operation.
+
+```c
+int vl53l0x_timeoutOccurred(vl53l0x_t *v);
+```
+
+**Returns:** 1 if timeout occurred, 0 otherwise
+
+#### `vl53l0x_i2cFail()`
+Check if an I2C communication failure occurred.
+
+```c
+int vl53l0x_i2cFail(vl53l0x_t *v);
+```
+
+**Returns:** 1 if I2C failure occurred, 0 otherwise
+
+## Hardware Connections
+
+| VL53L0X Pin | ESP32 Pin | Description |
+|-------------|-----------|-------------|
+| VDD | 3.3V | Power supply (2.6V - 3.5V) |
+| GND | GND | Ground |
+| SCL | GPIO 22 | I2C clock (configurable) |
+| SDA | GPIO 21 | I2C data (configurable) |
+| XSHUT | GPIO (optional) | Shutdown control (active low) |
+| GPIO1 | Not connected | Interrupt output (optional) |
+
+**Note:** The VL53L0X operates at 2.8V I/O by default in this library. If your module has level shifters, set `io_2v8 = 1`.
+
+## Migration from Legacy I2C Driver
+
+If you're migrating from the old ESP-IDF I2C driver (pre-v5.2), note these key changes:
+
+### Old API (ESP-IDF < v5.2)
+```c
+vl53l0x_t *sensor = vl53l0x_config(0, 22, 21, -1, 0x29, 1);
+```
+
+### New API (ESP-IDF >= v5.2)
+```c
+// Same function call - internal implementation updated
+vl53l0x_t *sensor = vl53l0x_config(0, 22, 21, -1, 0x29, 1);
+
+// Or use the new bus-based API for multiple sensors
+i2c_master_bus_handle_t bus_handle;
+// ... create bus ...
+vl53l0x_t *sensor = vl53l0x_config_with_bus(bus_handle, -1, 0x29, 1);
+```
+
+The high-level API remains the same, but internally uses the new I2C master driver.
+
+## Examples
+
+See the `examples/` directory for complete working examples:
+
+- **single_sensor** - Basic single sensor ranging example
+- **multiple_sensors** - Multiple sensors on same I2C bus with address management
+
+## Troubleshooting
+
+### Sensor not responding
+- Check power supply (2.6V - 3.5V)
+- Verify I2C connections (SDA, SCL)
+- Ensure pull-up resistors are present (usually on module)
+- Check I2C address (default 0x29)
+
+### Timeout errors
+- Increase timeout: `vl53l0x_setTimeout(sensor, 500);`
+- Check for I2C bus conflicts
+- Verify sensor is powered and initialized
+
+### Range readings are unstable
+- Adjust measurement timing budget
+- Change VCSEL pulse period
+- Ensure target surface is suitable (non-reflective works best)
+
+## Credits
+
+Based on the original work by:
+- Copyright © 2019 Adrian Kennard, Andrews & Arnold Ltd
+- Based on [Pololu VL53L0X Arduino library](https://github.com/pololu/vl53l0x-arduino)
+- Updated for ESP-IDF v5.2+ I2C master driver API
+
+## License
+
+GPL 3.0 - See LICENSE file for details
diff --git a/components/vl53l0x/component.mk b/components/vl53l0x/component.mk
new file mode 100644
index 0000000..0b9d758
--- /dev/null
+++ b/components/vl53l0x/component.mk
@@ -0,0 +1,5 @@
+#
+# "main" pseudo-component makefile.
+#
+# (Uses default behaviour of compiling all source files in directory, adding 'include' to include path.)
+
diff --git a/components/vl53l0x/examples/multiple_sensors/CMakeLists.txt b/components/vl53l0x/examples/multiple_sensors/CMakeLists.txt
new file mode 100644
index 0000000..8eb79cd
--- /dev/null
+++ b/components/vl53l0x/examples/multiple_sensors/CMakeLists.txt
@@ -0,0 +1,6 @@
+# The following lines of boilerplate have to be in your project's
+# CMakeLists in this exact order for cmake to work correctly
+cmake_minimum_required(VERSION 3.16)
+
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+project(vl53l0x_multiple_sensors)
diff --git a/components/vl53l0x/examples/multiple_sensors/main/CMakeLists.txt b/components/vl53l0x/examples/multiple_sensors/main/CMakeLists.txt
new file mode 100644
index 0000000..2616d1f
--- /dev/null
+++ b/components/vl53l0x/examples/multiple_sensors/main/CMakeLists.txt
@@ -0,0 +1,2 @@
+idf_component_register(SRCS "multiple_sensors_example.c"
+                    INCLUDE_DIRS ".")
diff --git a/components/vl53l0x/examples/multiple_sensors/main/multiple_sensors_example.c b/components/vl53l0x/examples/multiple_sensors/main/multiple_sensors_example.c
new file mode 100644
index 0000000..0796865
--- /dev/null
+++ b/components/vl53l0x/examples/multiple_sensors/main/multiple_sensors_example.c
@@ -0,0 +1,180 @@
+/* VL53L0X Multiple Sensors Example
+ * 
+ * This example demonstrates how to use multiple VL53L0X sensors on the same I2C bus
+ * using the new API (vl53l0x_config_with_bus)
+ * 
+ * Key points:
+ * - All sensors share the same I2C bus
+ * - Each sensor needs a unique I2C address
+ * - XSHUT pins are used to change addresses at initialization
+ * 
+ * Hardware connections:
+ * Sensor 1:
+ * - VL53L0X SCL/SDA -> Shared I2C bus (GPIO 22/21)
+ * - VL53L0X XSHUT -> GPIO 23
+ * 
+ * Sensor 2:
+ * - VL53L0X SCL/SDA -> Shared I2C bus (GPIO 22/21)
+ * - VL53L0X XSHUT -> GPIO 25
+ * 
+ * Sensor 3:
+ * - VL53L0X SCL/SDA -> Shared I2C bus (GPIO 22/21)
+ * - VL53L0X XSHUT -> GPIO 26
+ */
+
+#include <stdio.h>
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "esp_log.h"
+#include "driver/i2c_master.h"
+#include "driver/gpio.h"
+#include "vl53l0x.h"
+
+static const char *TAG = "VL53L0X_MULTI";
+
+// I2C Configuration
+#define I2C_MASTER_NUM      0
+#define I2C_MASTER_SCL_IO   22
+#define I2C_MASTER_SDA_IO   21
+
+// Sensor configuration
+#define NUM_SENSORS 3
+
+// XSHUT pins for each sensor (must be different)
+#define SENSOR1_XSHUT 23
+#define SENSOR2_XSHUT 25
+#define SENSOR3_XSHUT 26
+
+// New I2C addresses (must be unique, default is 0x29)
+#define SENSOR1_ADDR 0x30
+#define SENSOR2_ADDR 0x31
+#define SENSOR3_ADDR 0x32
+
+void app_main(void)
+{
+    ESP_LOGI(TAG, "VL53L0X Multiple Sensors Example");
+    
+    // Step 1: Create a single shared I2C bus
+    i2c_master_bus_config_t bus_config = {
+        .clk_source = I2C_CLK_SRC_DEFAULT,
+        .i2c_port = I2C_MASTER_NUM,
+        .scl_io_num = I2C_MASTER_SCL_IO,
+        .sda_io_num = I2C_MASTER_SDA_IO,
+        .glitch_ignore_cnt = 7,
+        .flags.enable_internal_pullup = true,
+    };
+    
+    i2c_master_bus_handle_t bus_handle;
+    esp_err_t ret = i2c_new_master_bus(&bus_config, &bus_handle);
+    if (ret != ESP_OK) {
+        ESP_LOGE(TAG, "Failed to create I2C bus: %s", esp_err_to_name(ret));
+        return;
+    }
+    
+    ESP_LOGI(TAG, "I2C bus created successfully");
+    
+    // Step 2: Initialize sensors one at a time with unique addresses
+    // We use XSHUT pins to keep sensors in reset while configuring others
+    vl53l0x_t *sensors[NUM_SENSORS];
+    int8_t xshut_pins[NUM_SENSORS] = {SENSOR1_XSHUT, SENSOR2_XSHUT, SENSOR3_XSHUT};
+    uint8_t addresses[NUM_SENSORS] = {SENSOR1_ADDR, SENSOR2_ADDR, SENSOR3_ADDR};
+    
+    // First, hold all sensors in reset
+    for (int i = 0; i < NUM_SENSORS; i++) {
+        gpio_reset_pin(xshut_pins[i]);
+        gpio_set_direction(xshut_pins[i], GPIO_MODE_OUTPUT);
+        gpio_set_level(xshut_pins[i], 0);  // Keep in reset
+        vTaskDelay(pdMS_TO_TICKS(10));
+    }
+    
+    ESP_LOGI(TAG, "All sensors held in reset");
+    vTaskDelay(pdMS_TO_TICKS(100));
+    
+    // Initialize each sensor one by one
+    for (int i = 0; i < NUM_SENSORS; i++) {
+        ESP_LOGI(TAG, "\nInitializing sensor %d...", i + 1);
+        
+        // Create sensor with shared bus and default address (0x29)
+        sensors[i] = vl53l0x_config_with_bus(
+            bus_handle,
+            xshut_pins[i],
+            0x29,  // All start with default address
+            1      // Use 2.8V I/O mode
+        );
+        
+        if (!sensors[i]) {
+            ESP_LOGE(TAG, "Failed to configure sensor %d", i + 1);
+            continue;
+        }
+        
+        // Release this sensor from reset
+        gpio_set_level(xshut_pins[i], 1);
+        vTaskDelay(pdMS_TO_TICKS(10));  // Wait for sensor to boot
+        
+        // Initialize the sensor
+        const char *err = vl53l0x_init(sensors[i]);
+        if (err) {
+            ESP_LOGE(TAG, "Failed to initialize sensor %d: %s", i + 1, err);
+            vl53l0x_end(sensors[i]);
+            sensors[i] = NULL;
+            continue;
+        }
+        
+        // Change to unique I2C address
+        vl53l0x_setAddress(sensors[i], addresses[i]);
+        ESP_LOGI(TAG, "Sensor %d initialized with address 0x%02X", i + 1, addresses[i]);
+        
+        // Optional: Configure timing budget
+        vl53l0x_setMeasurementTimingBudget(sensors[i], 40000);  // 40ms
+    }
+    
+    ESP_LOGI(TAG, "\nAll sensors initialized!\n");
+    
+    // Step 3: Start continuous measurements on all sensors
+    for (int i = 0; i < NUM_SENSORS; i++) {
+        if (sensors[i]) {
+            vl53l0x_startContinuous(sensors[i], 100);  // 100ms interval
+        }
+    }
+    
+    // Step 4: Read from all sensors in a loop
+    ESP_LOGI(TAG, "Starting continuous measurements from all sensors...");
+    
+    for (int iteration = 0; iteration < 30; iteration++) {
+        ESP_LOGI(TAG, "\n--- Measurement %d ---", iteration + 1);
+        
+        for (int i = 0; i < NUM_SENSORS; i++) {
+            if (!sensors[i]) {
+                ESP_LOGW(TAG, "Sensor %d: Not available", i + 1);
+                continue;
+            }
+            
+            uint16_t range_mm = vl53l0x_readRangeContinuousMillimeters(sensors[i]);
+            
+            if (vl53l0x_timeoutOccurred(sensors[i])) {
+                ESP_LOGW(TAG, "Sensor %d: Timeout", i + 1);
+            } else if (vl53l0x_i2cFail(sensors[i])) {
+                ESP_LOGE(TAG, "Sensor %d: I2C error", i + 1);
+            } else {
+                ESP_LOGI(TAG, "Sensor %d: %d mm", i + 1, range_mm);
+            }
+        }
+        
+        vTaskDelay(pdMS_TO_TICKS(150));
+    }
+    
+    // Step 5: Stop and cleanup
+    ESP_LOGI(TAG, "\nStopping measurements and cleaning up...");
+    
+    for (int i = 0; i < NUM_SENSORS; i++) {
+        if (sensors[i]) {
+            vl53l0x_stopContinuous(sensors[i]);
+            vl53l0x_end(sensors[i]);  // Does NOT delete the shared bus
+        }
+    }
+    
+    // Delete the shared bus only after all sensors are done
+    i2c_del_master_bus(bus_handle);
+    
+    ESP_LOGI(TAG, "Example finished");
+}
diff --git a/components/vl53l0x/examples/single_sensor/CMakeLists.txt b/components/vl53l0x/examples/single_sensor/CMakeLists.txt
new file mode 100644
index 0000000..d205ea0
--- /dev/null
+++ b/components/vl53l0x/examples/single_sensor/CMakeLists.txt
@@ -0,0 +1,6 @@
+# The following lines of boilerplate have to be in your project's
+# CMakeLists in this exact order for cmake to work correctly
+cmake_minimum_required(VERSION 3.16)
+
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+project(vl53l0x_single_sensor)
diff --git a/components/vl53l0x/examples/single_sensor/main/CMakeLists.txt b/components/vl53l0x/examples/single_sensor/main/CMakeLists.txt
new file mode 100644
index 0000000..a973a5e
--- /dev/null
+++ b/components/vl53l0x/examples/single_sensor/main/CMakeLists.txt
@@ -0,0 +1,2 @@
+idf_component_register(SRCS "single_sensor_example.c"
+                    INCLUDE_DIRS ".")
diff --git a/components/vl53l0x/examples/single_sensor/main/single_sensor_example.c b/components/vl53l0x/examples/single_sensor/main/single_sensor_example.c
new file mode 100644
index 0000000..2db0855
--- /dev/null
+++ b/components/vl53l0x/examples/single_sensor/main/single_sensor_example.c
@@ -0,0 +1,127 @@
+/* VL53L0X Single Sensor Example
+ * 
+ * This example demonstrates basic usage of a single VL53L0X sensor
+ * using the legacy API (vl53l0x_config)
+ * 
+ * Hardware connections:
+ * - VL53L0X VCC -> 3.3V or 5V (depending on module)
+ * - VL53L0X GND -> GND
+ * - VL53L0X SCL -> GPIO 22 (or your chosen SCL pin)
+ * - VL53L0X SDA -> GPIO 21 (or your chosen SDA pin)
+ * - VL53L0X XSHUT -> GPIO 23 (optional, for power control)
+ */
+
+#include <stdio.h>
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "esp_log.h"
+#include "driver/gpio.h"
+#include "vl53l0x.h"
+
+static const char *TAG = "VL53L0X_SINGLE";
+
+// I2C Configuration
+#define I2C_MASTER_NUM      0
+#define I2C_MASTER_SCL_IO   22
+#define I2C_MASTER_SDA_IO   21
+#define VL53L0X_XSHUT_PIN   23  // Set to -1 if not using XSHUT
+#define VL53L0X_ADDRESS     0x29  // Default I2C address
+
+void app_main(void)
+{
+    ESP_LOGI(TAG, "VL53L0X Single Sensor Example");
+    
+    // Initialize VL53L0X sensor with legacy API
+    // This creates and manages its own I2C bus
+    vl53l0x_t *sensor = vl53l0x_config(
+        I2C_MASTER_NUM,
+        I2C_MASTER_SCL_IO,
+        I2C_MASTER_SDA_IO,
+        VL53L0X_XSHUT_PIN,
+        VL53L0X_ADDRESS,
+        1  // Use 2.8V I/O mode (set to 0 for 1.8V)
+    );
+    
+    if (!sensor) {
+        ESP_LOGE(TAG, "Failed to configure VL53L0X sensor");
+        return;
+    }
+    
+    ESP_LOGI(TAG, "VL53L0X configured successfully");
+    
+    // Initialize the sensor
+    const char *err = vl53l0x_init(sensor);
+    if (err) {
+        ESP_LOGE(TAG, "VL53L0X initialization failed: %s", err);
+        vl53l0x_end(sensor);
+        return;
+    }
+    
+    ESP_LOGI(TAG, "VL53L0X initialized successfully");
+    
+    // Optional: Set timing budget (default is ~33ms)
+    // Higher budget = more accurate but slower
+    err = vl53l0x_setMeasurementTimingBudget(sensor, 50000);  // 50ms
+    if (err) {
+        ESP_LOGW(TAG, "Failed to set timing budget: %s", err);
+    }
+    
+    // Example 1: Single shot measurements
+    ESP_LOGI(TAG, "\n=== Single Shot Mode ===");
+    for (int i = 0; i < 5; i++) {
+        uint16_t range_mm = vl53l0x_readRangeSingleMillimeters(sensor);
+        
+        if (vl53l0x_timeoutOccurred(sensor)) {
+            ESP_LOGW(TAG, "Measurement timeout!");
+        } else {
+            ESP_LOGI(TAG, "Range: %d mm", range_mm);
+        }
+        
+        vTaskDelay(pdMS_TO_TICKS(500));
+    }
+    
+    // Example 2: Continuous measurements
+    ESP_LOGI(TAG, "\n=== Continuous Mode ===");
+    ESP_LOGI(TAG, "Starting continuous ranging (200ms interval)...");
+    
+    vl53l0x_startContinuous(sensor, 200);  // 200ms between measurements
+    
+    for (int i = 0; i < 20; i++) {
+        uint16_t range_mm = vl53l0x_readRangeContinuousMillimeters(sensor);
+        
+        if (vl53l0x_timeoutOccurred(sensor)) {
+            ESP_LOGW(TAG, "Measurement timeout!");
+        } else {
+            ESP_LOGI(TAG, "Range: %d mm", range_mm);
+        }
+        
+        // Check for I2C errors
+        if (vl53l0x_i2cFail(sensor)) {
+            ESP_LOGE(TAG, "I2C communication error!");
+            break;
+        }
+        
+        vTaskDelay(pdMS_TO_TICKS(250));  // Slightly longer than measurement interval
+    }
+    
+    vl53l0x_stopContinuous(sensor);
+    ESP_LOGI(TAG, "Continuous ranging stopped");
+    
+    // Example 3: High-speed continuous mode (back-to-back)
+    ESP_LOGI(TAG, "\n=== High-Speed Mode ===");
+    ESP_LOGI(TAG, "Starting back-to-back continuous ranging...");
+    
+    vl53l0x_startContinuous(sensor, 0);  // 0 = back-to-back mode (fastest)
+    
+    for (int i = 0; i < 10; i++) {
+        uint16_t range_mm = vl53l0x_readRangeContinuousMillimeters(sensor);
+        ESP_LOGI(TAG, "Range: %d mm", range_mm);
+        vTaskDelay(pdMS_TO_TICKS(50));  // Small delay
+    }
+    
+    vl53l0x_stopContinuous(sensor);
+    
+    // Cleanup
+    vl53l0x_end(sensor);
+    ESP_LOGI(TAG, "Example finished");
+}
diff --git a/components/vl53l0x/include/vl53l0x.h b/components/vl53l0x/include/vl53l0x.h
new file mode 100644
index 0000000..71c87b4
--- /dev/null
+++ b/components/vl53l0x/include/vl53l0x.h
@@ -0,0 +1,80 @@
+// VL53L0X control
+// Copyright © 2019 Adrian Kennard, Andrews & Arnold Ltd. See LICENCE file for details. GPL 3.0
+// Updated for ESP-IDF v5.2+ I2C Master Driver API
+
+#ifndef VL53L0X_H
+#define VL53L0X_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <string.h>
+#include <unistd.h>
+#include <malloc.h>
+#include <driver/i2c_master.h>
+
+typedef struct vl53l0x_s vl53l0x_t;
+
+typedef enum
+{ VcselPeriodPreRange, VcselPeriodFinalRange } vl53l0x_vcselPeriodType;
+
+// Functions returning const char * are OK for NULL, else error string
+
+// Legacy API: Set up I2C bus and create the vl53l0x structure
+// This will create and manage its own I2C bus (single device use case)
+vl53l0x_t *vl53l0x_config (int8_t port, int8_t scl, int8_t sda, int8_t xshut, uint8_t address, uint8_t io_2v8);
+
+// New API: Create vl53l0x device using an existing I2C bus handle
+// Use this when you have multiple devices on the same I2C bus
+// bus_handle: existing I2C master bus (from i2c_new_master_bus)
+// Returns NULL on failure
+vl53l0x_t *vl53l0x_config_with_bus (i2c_master_bus_handle_t bus_handle, int8_t xshut, uint8_t address, uint8_t io_2v8);
+
+// Initialise the VL53L0X
+const char *vl53l0x_init (vl53l0x_t *);
+
+// End I2C and free the structure
+// If using vl53l0x_config_with_bus, this will NOT delete the bus (caller manages it)
+void vl53l0x_end (vl53l0x_t *);
+
+void vl53l0x_setAddress (vl53l0x_t *, uint8_t new_addr);
+uint8_t vl53l0x_getAddress (vl53l0x_t * v);
+
+void vl53l0x_writeReg8Bit (vl53l0x_t *, uint8_t reg, uint8_t value);
+void vl53l0x_writeReg16Bit (vl53l0x_t *, uint8_t reg, uint16_t value);
+void vl53l0x_writeReg32Bit (vl53l0x_t *, uint8_t reg, uint32_t value);
+uint8_t vl53l0x_readReg8Bit (vl53l0x_t *, uint8_t reg);
+uint16_t vl53l0x_readReg16Bit (vl53l0x_t *, uint8_t reg);
+uint32_t vl53l0x_readReg32Bit (vl53l0x_t *, uint8_t reg);
+
+void vl53l0x_writeMulti (vl53l0x_t *, uint8_t reg, uint8_t const *src, uint8_t count);
+void vl53l0x_readMulti (vl53l0x_t *, uint8_t reg, uint8_t * dst, uint8_t count);
+
+const char *vl53l0x_setSignalRateLimit (vl53l0x_t *, float limit_Mcps);
+float vl53l0x_getSignalRateLimit (vl53l0x_t *);
+
+const char *vl53l0x_setMeasurementTimingBudget (vl53l0x_t *, uint32_t budget_us);
+uint32_t vl53l0x_getMeasurementTimingBudget (vl53l0x_t *);
+
+const char *vl53l0x_setVcselPulsePeriod (vl53l0x_t *, vl53l0x_vcselPeriodType type, uint8_t period_pclks);
+uint8_t vl53l0x_getVcselPulsePeriod (vl53l0x_t *, vl53l0x_vcselPeriodType type);
+
+void vl53l0x_startContinuous (vl53l0x_t *, uint32_t period_ms);
+void vl53l0x_stopContinuous (vl53l0x_t *);
+uint16_t vl53l0x_readRangeContinuousMillimeters (vl53l0x_t *);
+uint16_t vl53l0x_readRangeSingleMillimeters (vl53l0x_t *);
+
+void vl53l0x_setTimeout (vl53l0x_t *, uint16_t timeout);
+uint16_t vl53l0x_getTimeout (vl53l0x_t *);
+int vl53l0x_timeoutOccurred (vl53l0x_t *);
+int vl53l0x_i2cFail (vl53l0x_t *);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/components/vl53l0x/vl53l0x.c b/components/vl53l0x/vl53l0x.c
new file mode 100644
index 0000000..35957d7
--- /dev/null
+++ b/components/vl53l0x/vl53l0x.c
@@ -0,0 +1,1106 @@
+// VL53L0X control
+// Copyright © 2019 Adrian Kennard, Andrews & Arnold Ltd. See LICENCE file for details. GPL 3.0
+// Updated for ESP-IDF v5.2+ I2C Master Driver API
+// Based on https://github.com/pololu/vl53l0x-arduino
+static const char __attribute__((unused)) TAG[] = "ranger";
+
+#include "vl53l0x.h"
+#include "esp_timer.h"
+#include "esp_log.h"
+#include <driver/i2c_master.h>
+#include <driver/gpio.h>
+#include <string.h>
+
+#define TIMEOUT_MS 100 // I2C command timeout in milliseconds
+#define I2C_BUSSPEED 100000 // I2C Bus Speed
+
+#ifdef	CONFIG_VL53L0X_DEBUG
+#define VL53L0X_LOG   ESP_LOGI        // Set to allow I2C logging
+#endif
+
+#ifndef VL53L0X_LOG
+#define VL53L0X_LOG(tag,...) err=err;
+#endif
+
+enum
+{
+   SYSRANGE_START = 0x00,
+
+   SYSTEM_THRESH_HIGH = 0x0C,
+   SYSTEM_THRESH_LOW = 0x0E,
+
+   SYSTEM_SEQUENCE_CONFIG = 0x01,
+   SYSTEM_RANGE_CONFIG = 0x09,
+   SYSTEM_INTERMEASUREMENT_PERIOD = 0x04,
+
+   SYSTEM_INTERRUPT_CONFIG_GPIO = 0x0A,
+
+   GPIO_HV_MUX_ACTIVE_HIGH = 0x84,
+
+   SYSTEM_INTERRUPT_CLEAR = 0x0B,
+
+   RESULT_INTERRUPT_STATUS = 0x13,
+   RESULT_RANGE_STATUS = 0x14,
+
+   RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN = 0xBC,
+   RESULT_CORE_RANGING_TOTAL_EVENTS_RTN = 0xC0,
+   RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF = 0xD0,
+   RESULT_CORE_RANGING_TOTAL_EVENTS_REF = 0xD4,
+   RESULT_PEAK_SIGNAL_RATE_REF = 0xB6,
+
+   ALGO_PART_TO_PART_RANGE_OFFSET_MM = 0x28,
+
+   MSRC_CONFIG_CONTROL = 0x60,
+
+   PRE_RANGE_CONFIG_MIN_SNR = 0x27,
+   PRE_RANGE_CONFIG_VALID_PHASE_LOW = 0x56,
+   PRE_RANGE_CONFIG_VALID_PHASE_HIGH = 0x57,
+   PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT = 0x64,
+
+   FINAL_RANGE_CONFIG_MIN_SNR = 0x67,
+   FINAL_RANGE_CONFIG_VALID_PHASE_LOW = 0x47,
+   FINAL_RANGE_CONFIG_VALID_PHASE_HIGH = 0x48,
+   FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT = 0x44,
+
+   PRE_RANGE_CONFIG_SIGMA_THRESH_HI = 0x61,
+   PRE_RANGE_CONFIG_SIGMA_THRESH_LO = 0x62,
+
+   PRE_RANGE_CONFIG_VCSEL_PERIOD = 0x50,
+   PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x51,
+   PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x52,
+
+   SYSTEM_HISTOGRAM_BIN = 0x81,
+   HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT = 0x33,
+   HISTOGRAM_CONFIG_READOUT_CTRL = 0x55,
+
+   FINAL_RANGE_CONFIG_VCSEL_PERIOD = 0x70,
+   FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x71,
+   FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x72,
+   CROSSTALK_COMPENSATION_PEAK_RATE_MCPS = 0x20,
+
+   MSRC_CONFIG_TIMEOUT_MACROP = 0x46,
+
+   I2C_SLAVE_DEVICE_ADDRESS = 0x8A,
+
+   SOFT_RESET_GO2_SOFT_RESET_N = 0xBF,
+   IDENTIFICATION_MODEL_ID = 0xC0,
+   IDENTIFICATION_REVISION_ID = 0xC2,
+
+   OSC_CALIBRATE_VAL = 0xF8,
+
+   GLOBAL_CONFIG_VCSEL_WIDTH = 0x32,
+   GLOBAL_CONFIG_SPAD_ENABLES_REF_0 = 0xB0,
+   GLOBAL_CONFIG_SPAD_ENABLES_REF_1 = 0xB1,
+   GLOBAL_CONFIG_SPAD_ENABLES_REF_2 = 0xB2,
+   GLOBAL_CONFIG_SPAD_ENABLES_REF_3 = 0xB3,
+   GLOBAL_CONFIG_SPAD_ENABLES_REF_4 = 0xB4,
+   GLOBAL_CONFIG_SPAD_ENABLES_REF_5 = 0xB5,
+
+   GLOBAL_CONFIG_REF_EN_START_SELECT = 0xB6,
+   DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD = 0x4E,
+   DYNAMIC_SPAD_REF_EN_START_OFFSET = 0x4F,
+   POWER_MANAGEMENT_GO1_POWER_FORCE = 0x80,
+
+   VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV = 0x89,
+
+   ALGO_PHASECAL_LIM = 0x30,
+   ALGO_PHASECAL_CONFIG_TIMEOUT = 0x30,
+};
+
+struct vl53l0x_s
+{
+   i2c_master_bus_handle_t bus_handle;
+   i2c_master_dev_handle_t dev_handle;
+   uint8_t address;
+   int8_t xshut;
+   uint16_t io_timeout;
+   uint8_t io_2v8:1;
+   uint8_t did_timeout:1;
+   uint8_t i2c_fail:1;
+   uint8_t owns_bus:1;  // true if this instance created the bus
+};
+
+typedef struct
+{
+   uint8_t tcc:1;
+   uint8_t msrc:1;
+   uint8_t dss:1;
+   uint8_t pre_range:1;
+   uint8_t final_range:1;
+} SequenceStepEnables;
+
+typedef struct
+{
+   uint16_t pre_range_vcsel_period_pclks,
+     final_range_vcsel_period_pclks;
+   uint16_t msrc_dss_tcc_mclks,
+     pre_range_mclks,
+     final_range_mclks;
+   uint32_t msrc_dss_tcc_us,
+     pre_range_us,
+     final_range_us;
+}
+SequenceStepTimeouts;
+
+static uint8_t stop_variable;
+static uint16_t timeout_start_ms;
+static uint32_t measurement_timing_budget_us;
+#define millis() (esp_timer_get_time()/1000LL)
+
+// Record the current time to check an upcoming timeout against
+#define startTimeout() (timeout_start_ms = millis())
+
+// Check if timeout is enabled (set to nonzero value) and has expired
+#define checkTimeoutExpired() (v->io_timeout > 0 && ((uint16_t)(millis() - timeout_start_ms)) > v->io_timeout)
+
+// Encode VCSEL pulse period register value from period in PCLKs
+// based on VL53L0X_encode_vcsel_period()
+#define encodeVcselPeriod(period_pclks) (((period_pclks) >> 1) - 1)
+
+void
+vl53l0x_writeReg8Bit (vl53l0x_t * v, uint8_t reg, uint8_t val)
+{
+   uint8_t write_buf[2] = {reg, val};
+   esp_err_t err = i2c_master_transmit(v->dev_handle, write_buf, 2, TIMEOUT_MS);
+   if (err != ESP_OK)
+      v->i2c_fail = 1;
+   VL53L0X_LOG (TAG, "W %02X=%02X %s", reg, val, esp_err_to_name (err));
+}
+
+void
+vl53l0x_writeReg16Bit (vl53l0x_t * v, uint8_t reg, uint16_t val)
+{
+   uint8_t write_buf[3] = {reg, val >> 8, val & 0xFF};
+   esp_err_t err = i2c_master_transmit(v->dev_handle, write_buf, 3, TIMEOUT_MS);
+   if (err != ESP_OK)
+      v->i2c_fail = 1;
+   VL53L0X_LOG (TAG, "W %02X=%04X %s", reg, val, esp_err_to_name (err));
+}
+
+void
+vl53l0x_writeReg32Bit (vl53l0x_t * v, uint8_t reg, uint32_t val)
+{
+   uint8_t write_buf[5] = {reg, val >> 24, val >> 16, val >> 8, val & 0xFF};
+   esp_err_t err = i2c_master_transmit(v->dev_handle, write_buf, 5, TIMEOUT_MS);
+   if (err != ESP_OK)
+      v->i2c_fail = 1;
+   VL53L0X_LOG (TAG, "W %02X=%08X %s", reg, val, esp_err_to_name (err));
+}
+
+uint8_t
+vl53l0x_readReg8Bit (vl53l0x_t * v, uint8_t reg)
+{
+   uint8_t buf[1] = {0};
+   esp_err_t err = i2c_master_transmit_receive(v->dev_handle, &reg, 1, buf, 1, TIMEOUT_MS);
+   if (err != ESP_OK)
+      v->i2c_fail = 1;
+   VL53L0X_LOG (TAG, "R %02X=%02X %s", reg, buf[0], esp_err_to_name (err));
+   return buf[0];
+}
+
+uint16_t
+vl53l0x_readReg16Bit (vl53l0x_t * v, uint8_t reg)
+{
+   uint8_t buf[2] = {0};
+   esp_err_t err = i2c_master_transmit_receive(v->dev_handle, &reg, 1, buf, 2, TIMEOUT_MS);
+   if (err != ESP_OK)
+      v->i2c_fail = 1;
+   VL53L0X_LOG (TAG, "R %02X=%02X%02X %s", reg, buf[0], buf[1], esp_err_to_name (err));
+   return (buf[0] << 8) + buf[1];
+}
+
+uint32_t
+vl53l0x_readReg32Bit (vl53l0x_t * v, uint8_t reg)
+{
+   uint8_t buf[4] = {0};
+   esp_err_t err = i2c_master_transmit_receive(v->dev_handle, &reg, 1, buf, 4, TIMEOUT_MS);
+   if (err != ESP_OK)
+      v->i2c_fail = 1;
+   VL53L0X_LOG (TAG, "R %02X=%02X%02X%02X%02X %s", reg, buf[0], buf[1], buf[2], buf[3], esp_err_to_name (err));
+   return (buf[0] << 24) + (buf[1] << 16) + (buf[2] << 8) + buf[3];
+}
+
+// Read an arbitrary number of bytes from the sensor, starting at the given register
+void
+vl53l0x_readMulti (vl53l0x_t * v, uint8_t reg, uint8_t * dst, uint8_t count)
+{
+   esp_err_t err = i2c_master_transmit_receive(v->dev_handle, &reg, 1, dst, count, TIMEOUT_MS);
+   if (err != ESP_OK)
+      v->i2c_fail = 1;
+   VL53L0X_LOG (TAG, "R %02X (%d) %s", reg, count, esp_err_to_name (err));
+}
+
+// Write an arbitrary number of bytes from the given array to the sensor, starting at the given register
+void
+vl53l0x_writeMulti (vl53l0x_t * v, uint8_t reg, uint8_t const *src, uint8_t count)
+{
+   uint8_t *write_buf = malloc(count + 1);
+   if (!write_buf) {
+      v->i2c_fail = 1;
+      return;
+   }
+   write_buf[0] = reg;
+   memcpy(write_buf + 1, src, count);
+   esp_err_t err = i2c_master_transmit(v->dev_handle, write_buf, count + 1, TIMEOUT_MS);
+   free(write_buf);
+   if (err != ESP_OK)
+      v->i2c_fail = 1;
+   VL53L0X_LOG (TAG, "W %02X (%d) %s", reg, count, esp_err_to_name (err));
+}
+
+// Decode VCSEL (vertical cavity surface emitting laser) pulse period in PCLKs from register value
+// based on VL53L0X_decode_vcsel_period()
+#define decodeVcselPeriod(reg_val)      (((reg_val) + 1) << 1)
+
+// Calculate macro period in *nanoseconds* from VCSEL period in PCLKs
+// based on VL53L0X_calc_macro_period_ps()
+// PLL_period_ps = 1655; macro_period_vclks = 2304
+#define calcMacroPeriod(vcsel_period_pclks) ((((uint32_t)2304 * (vcsel_period_pclks) * 1655) + 500) / 1000)
+
+// Decode sequence step timeout in MCLKs from register value
+// based on VL53L0X_decode_timeout()
+static uint16_t
+decodeTimeout (uint16_t reg_val)
+{
+   return (uint16_t) ((reg_val & 0x00FF) << (uint16_t) ((reg_val & 0xFF00) >> 8)) + 1;
+}
+
+// Convert sequence step timeout from MCLKs to microseconds with given VCSEL period in PCLKs
+// based on VL53L0X_calc_timeout_us()
+static uint32_t
+timeoutMclksToMicroseconds (uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks)
+{
+   uint32_t macro_period_ns = calcMacroPeriod (vcsel_period_pclks);
+   return ((timeout_period_mclks * macro_period_ns) + (macro_period_ns / 2)) / 1000;
+}
+
+// based on VL53L0X_perform_single_ref_calibration()
+static const char *
+performSingleRefCalibration (vl53l0x_t * v, uint8_t vhv_init_byte)
+{
+   vl53l0x_writeReg8Bit (v, SYSRANGE_START, 0x01 | vhv_init_byte);
+   startTimeout ();
+   while ((vl53l0x_readReg8Bit (v, RESULT_INTERRUPT_STATUS) & 0x07) == 0)
+   {
+      if (checkTimeoutExpired ())
+         return "CAL Timeout";
+   }
+   vl53l0x_writeReg8Bit (v, SYSTEM_INTERRUPT_CLEAR, 0x01);
+   vl53l0x_writeReg8Bit (v, SYSRANGE_START, 0x00);
+   return NULL;
+}
+
+// Encode sequence step timeout register value from timeout in MCLKs
+// based on VL53L0X_encode_timeout()
+static uint16_t
+encodeTimeout (uint16_t timeout_mclks)
+{
+   uint32_t ls_byte = 0;
+   uint16_t ms_byte = 0;
+
+   if (timeout_mclks > 0)
+   {
+      ls_byte = timeout_mclks - 1;
+
+      while ((ls_byte & 0xFFFFFF00) > 0)
+      {
+         ls_byte >>= 1;
+         ms_byte++;
+      }
+
+      return (ms_byte << 8) | (ls_byte & 0xFF);
+   } else
+   {
+      return 0;
+   }
+}
+
+// Get the VCSEL pulse period in PCLKs for the given period type.
+// based on VL53L0X_get_vcsel_pulse_period()
+static uint8_t
+getVcselPulsePeriod (vl53l0x_t * v, vl53l0x_vcselPeriodType type)
+{
+   if (type == VcselPeriodPreRange)
+   {
+      return decodeVcselPeriod (vl53l0x_readReg8Bit (v, PRE_RANGE_CONFIG_VCSEL_PERIOD));
+   } else if (type == VcselPeriodFinalRange)
+   {
+      return decodeVcselPeriod (vl53l0x_readReg8Bit (v, FINAL_RANGE_CONFIG_VCSEL_PERIOD));
+   } else
+   {
+      return 255;
+   }
+}
+
+// Get sequence step enables
+// based on VL53L0X_GetSequenceStepEnables()
+static void
+getSequenceStepEnables (vl53l0x_t * v, SequenceStepEnables * enables)
+{
+   uint8_t sequence_config = vl53l0x_readReg8Bit (v, SYSTEM_SEQUENCE_CONFIG);
+
+   enables->tcc = (sequence_config >> 4) & 0x1;
+   enables->dss = (sequence_config >> 3) & 0x1;
+   enables->msrc = (sequence_config >> 2) & 0x1;
+   enables->pre_range = (sequence_config >> 6) & 0x1;
+   enables->final_range = (sequence_config >> 7) & 0x1;
+}
+
+// Get sequence step timeouts
+// based on get_sequence_step_timeout()
+static void
+getSequenceStepTimeouts (vl53l0x_t * v, SequenceStepEnables const *enables, SequenceStepTimeouts * timeouts)
+{
+   timeouts->pre_range_vcsel_period_pclks = getVcselPulsePeriod (v, VcselPeriodPreRange);
+
+   timeouts->msrc_dss_tcc_mclks = vl53l0x_readReg8Bit (v, MSRC_CONFIG_TIMEOUT_MACROP) + 1;
+   timeouts->msrc_dss_tcc_us = timeoutMclksToMicroseconds (timeouts->msrc_dss_tcc_mclks, timeouts->pre_range_vcsel_period_pclks);
+
+   timeouts->pre_range_mclks = decodeTimeout (vl53l0x_readReg16Bit (v, PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI));
+   timeouts->pre_range_us = timeoutMclksToMicroseconds (timeouts->pre_range_mclks, timeouts->pre_range_vcsel_period_pclks);
+
+   timeouts->final_range_vcsel_period_pclks = getVcselPulsePeriod (v, VcselPeriodFinalRange);
+
+   timeouts->final_range_mclks = decodeTimeout (vl53l0x_readReg16Bit (v, FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI));
+
+   if (enables->pre_range)
+   {
+      timeouts->final_range_mclks -= timeouts->pre_range_mclks;
+   }
+
+   timeouts->final_range_us = timeoutMclksToMicroseconds (timeouts->final_range_mclks, timeouts->final_range_vcsel_period_pclks);
+}
+
+// Convert sequence step timeout from microseconds to MCLKs with given VCSEL period in PCLKs
+// based on VL53L0X_calc_timeout_mclks()
+static uint32_t
+timeoutMicrosecondsToMclks (uint32_t timeout_period_us, uint8_t vcsel_period_pclks)
+{
+   uint32_t macro_period_ns = calcMacroPeriod (vcsel_period_pclks);
+
+   return (((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns);
+}
+
+const char *
+vl53l0x_setSignalRateLimit (vl53l0x_t * v, float limit_Mcps)
+{
+   if (limit_Mcps < 0 || limit_Mcps > 511.99)
+      return "Bad rate";
+   // Q9.7 fixed point format (9 integer bits, 7 fractional bits)
+   vl53l0x_writeReg16Bit (v, FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, limit_Mcps * (1 << 7));
+   return NULL;
+}
+
+// Get reference SPAD (single photon avalanche diode) count and type
+// based on VL53L0X_get_info_from_device()
+const char *
+vl53l0x_getSpadInfo (vl53l0x_t * v, uint8_t * count, int *type_is_aperture)
+{
+   uint8_t tmp;
+
+   vl53l0x_writeReg8Bit (v, 0x80, 0x01);
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+   vl53l0x_writeReg8Bit (v, 0x00, 0x00);
+
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x06);
+   vl53l0x_writeReg8Bit (v, 0x83, vl53l0x_readReg8Bit (v, 0x83) | 0x04);
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x07);
+   vl53l0x_writeReg8Bit (v, 0x81, 0x01);
+
+   vl53l0x_writeReg8Bit (v, 0x80, 0x01);
+
+   vl53l0x_writeReg8Bit (v, 0x94, 0x6b);
+   vl53l0x_writeReg8Bit (v, 0x83, 0x00);
+   startTimeout ();
+   while (vl53l0x_readReg8Bit (v, 0x83) == 0x00)
+   {
+      if (checkTimeoutExpired ())
+         return "SPAD Timeout";
+   }
+   vl53l0x_writeReg8Bit (v, 0x83, 0x01);
+   tmp = vl53l0x_readReg8Bit (v, 0x92);
+
+   *count = tmp & 0x7f;
+   *type_is_aperture = (tmp >> 7) & 0x01;
+
+   vl53l0x_writeReg8Bit (v, 0x81, 0x00);
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x06);
+   vl53l0x_writeReg8Bit (v, 0x83, vl53l0x_readReg8Bit (v, 0x83) & ~0x04);
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+   vl53l0x_writeReg8Bit (v, 0x00, 0x01);
+
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x80, 0x00);
+
+   return NULL;
+}
+
+// Get the measurement timing budget in microseconds
+// based on VL53L0X_get_measurement_timing_budget_micro_seconds()
+uint32_t
+vl53l0x_getMeasurementTimingBudget (vl53l0x_t * v)
+{
+   SequenceStepEnables enables;
+   SequenceStepTimeouts timeouts;
+
+   uint16_t const StartOverhead = 1910;
+   uint16_t const EndOverhead = 960;
+   uint16_t const MsrcOverhead = 660;
+   uint16_t const TccOverhead = 590;
+   uint16_t const DssOverhead = 690;
+   uint16_t const PreRangeOverhead = 660;
+   uint16_t const FinalRangeOverhead = 550;
+
+   uint32_t budget_us = StartOverhead + EndOverhead;
+
+   getSequenceStepEnables (v, &enables);
+   getSequenceStepTimeouts (v, &enables, &timeouts);
+
+   if (enables.tcc)
+   {
+      budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
+   }
+
+   if (enables.dss)
+   {
+      budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
+   } else if (enables.msrc)
+   {
+      budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
+   }
+
+   if (enables.pre_range)
+   {
+      budget_us += (timeouts.pre_range_us + PreRangeOverhead);
+   }
+
+   if (enables.final_range)
+   {
+      budget_us += (timeouts.final_range_us + FinalRangeOverhead);
+   }
+
+   measurement_timing_budget_us = budget_us;
+   return budget_us;
+}
+
+// Set the measurement timing budget in microseconds
+// based on VL53L0X_set_measurement_timing_budget_micro_seconds()
+const char *
+vl53l0x_setMeasurementTimingBudget (vl53l0x_t * v, uint32_t budget_us)
+{
+   SequenceStepEnables enables;
+   SequenceStepTimeouts timeouts;
+
+   uint16_t const StartOverhead = 1320;
+   uint16_t const EndOverhead = 960;
+   uint16_t const MsrcOverhead = 660;
+   uint16_t const TccOverhead = 590;
+   uint16_t const DssOverhead = 690;
+   uint16_t const PreRangeOverhead = 660;
+   uint16_t const FinalRangeOverhead = 550;
+
+   uint32_t const MinTimingBudget = 20000;
+
+   if (budget_us < MinTimingBudget)
+      return "Low budget";
+
+   uint32_t used_budget_us = StartOverhead + EndOverhead;
+
+   getSequenceStepEnables (v, &enables);
+   getSequenceStepTimeouts (v, &enables, &timeouts);
+
+   if (enables.tcc)
+      used_budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
+
+   if (enables.dss)
+      used_budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
+   else if (enables.msrc)
+      used_budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
+
+   if (enables.pre_range)
+      used_budget_us += (timeouts.pre_range_us + PreRangeOverhead);
+
+   if (enables.final_range)
+   {
+      used_budget_us += FinalRangeOverhead;
+
+      if (used_budget_us > budget_us)
+         return "High budget";
+
+      uint32_t final_range_timeout_us = budget_us - used_budget_us;
+
+      uint16_t
+         final_range_timeout_mclks = timeoutMicrosecondsToMclks (final_range_timeout_us, timeouts.final_range_vcsel_period_pclks);
+
+      if (enables.pre_range)
+         final_range_timeout_mclks += timeouts.pre_range_mclks;
+
+      vl53l0x_writeReg16Bit (v, FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, encodeTimeout (final_range_timeout_mclks));
+
+      measurement_timing_budget_us = budget_us;
+   }
+   return NULL;
+}
+
+// Create vl53l0x device using an existing I2C bus handle
+vl53l0x_t *
+vl53l0x_config_with_bus (i2c_master_bus_handle_t bus_handle, int8_t xshut, uint8_t address, uint8_t io_2v8)
+{
+   if (!bus_handle)
+      return NULL;
+
+   if (xshut >= 0 && !GPIO_IS_VALID_OUTPUT_GPIO (xshut))
+      return NULL;
+
+   vl53l0x_t *v = malloc (sizeof (*v));
+   if (!v)
+      return NULL;
+
+   memset (v, 0, sizeof (*v));
+
+   // Configure the I2C device on the existing bus
+   i2c_device_config_t dev_cfg = {
+      .dev_addr_length = I2C_ADDR_BIT_LEN_7,
+      .device_address = address,
+      .scl_speed_hz = I2C_BUSSPEED,
+   };
+
+   esp_err_t err = i2c_master_bus_add_device(bus_handle, &dev_cfg, &v->dev_handle);
+   if (err != ESP_OK) {
+      free(v);
+      return NULL;
+   }
+
+   v->bus_handle = bus_handle;
+   v->xshut = xshut;
+   v->io_2v8 = io_2v8;
+   v->address = address;
+   v->io_timeout = 100;
+   v->owns_bus = 0;  // Caller manages the bus
+
+   if (xshut >= 0)
+   {
+      gpio_reset_pin (xshut);
+      gpio_set_level (xshut, 0);        // Off
+      gpio_set_drive_capability (xshut, GPIO_DRIVE_CAP_3);
+      gpio_set_direction (xshut, GPIO_MODE_OUTPUT);
+   }
+
+   return v;
+}
+
+// Legacy API: Set up I2C bus and create the vl53l0x structure
+vl53l0x_t *
+vl53l0x_config (int8_t port, int8_t scl, int8_t sda, int8_t xshut, uint8_t address, uint8_t io_2v8)
+{
+   if (port < 0 || scl < 0 || sda < 0 || scl == sda)
+      return NULL;
+   if (!GPIO_IS_VALID_OUTPUT_GPIO (scl) || !GPIO_IS_VALID_OUTPUT_GPIO (sda))
+      return NULL;
+   if (xshut >= 0 && !GPIO_IS_VALID_OUTPUT_GPIO (xshut))
+      return NULL;
+
+   // Create new I2C master bus
+   i2c_master_bus_config_t bus_config = {
+      .clk_source = I2C_CLK_SRC_DEFAULT,
+      .i2c_port = port,
+      .scl_io_num = scl,
+      .sda_io_num = sda,
+      .glitch_ignore_cnt = 7,
+      .flags.enable_internal_pullup = true,
+   };
+
+   i2c_master_bus_handle_t bus_handle;
+   esp_err_t err = i2c_new_master_bus(&bus_config, &bus_handle);
+   if (err != ESP_OK)
+      return NULL;
+
+   // Create device on the bus
+   vl53l0x_t *v = vl53l0x_config_with_bus(bus_handle, xshut, address, io_2v8);
+   if (!v) {
+      i2c_del_master_bus(bus_handle);
+      return NULL;
+   }
+
+   v->owns_bus = 1;  // This instance owns the bus
+   return v;
+}
+
+// Initialize sensor
+const char *
+vl53l0x_init (vl53l0x_t * v)
+{
+   const char *err;
+   // Set up the VL53L0X
+   if (v->xshut >= 0)
+   {
+      gpio_set_level (v->xshut, 0);     // Off
+      usleep (100000);
+      gpio_set_level (v->xshut, 1);     // On
+      usleep (10000);
+   }
+
+   // sensor uses 1V8 mode for I/O by default; switch to 2V8 mode if necessary
+   if (v->io_2v8)
+      vl53l0x_writeReg8Bit (v, VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, vl53l0x_readReg8Bit (v, VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV) | 0x01);
+
+   // "Set I2C standard mode"
+   vl53l0x_writeReg8Bit (v, 0x88, 0x00);
+
+   vl53l0x_writeReg8Bit (v, 0x80, 0x01);
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+   vl53l0x_writeReg8Bit (v, 0x00, 0x00);
+   stop_variable = vl53l0x_readReg8Bit (v, 0x91);
+   vl53l0x_writeReg8Bit (v, 0x00, 0x01);
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x80, 0x00);
+
+   // disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
+   vl53l0x_writeReg8Bit (v, MSRC_CONFIG_CONTROL, vl53l0x_readReg8Bit (v, MSRC_CONFIG_CONTROL) | 0x12);
+
+   // set final range signal rate limit to 0.25 MCPS (million counts per second)
+   if ((err = vl53l0x_setSignalRateLimit (v, 0.25)))
+      return err;
+
+   vl53l0x_writeReg8Bit (v, SYSTEM_SEQUENCE_CONFIG, 0xFF);
+
+   // VL53L0X_DataInit() end
+
+   // VL53L0X_StaticInit() begin
+
+   uint8_t spad_count;
+   int spad_type_is_aperture;
+   if ((err = vl53l0x_getSpadInfo (v, &spad_count, &spad_type_is_aperture)))
+      return err;
+
+   // The SPAD map (RefGoodSpadMap) is read by VL53L0X_get_info_from_device() in
+   // the API, but the same data seems to be more easily readable from
+   // GLOBAL_CONFIG_SPAD_ENABLES_REF_0 through _6, so read it from there
+   uint8_t ref_spad_map[6];
+   vl53l0x_readMulti (v, GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6);
+
+   // -- VL53L0X_set_reference_spads() begin (assume NVM values are valid)
+
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+   vl53l0x_writeReg8Bit (v, DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
+   vl53l0x_writeReg8Bit (v, DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x00);
+   vl53l0x_writeReg8Bit (v, GLOBAL_CONFIG_REF_EN_START_SELECT, 0xB4);
+
+   uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0;
+   uint8_t spads_enabled = 0;
+
+   for (uint8_t i = 0; i < 48; i++)
+   {
+      if (i < first_spad_to_enable || spads_enabled == spad_count)
+      {
+         ref_spad_map[i / 8] &= ~(1 << (i % 8));
+      } else if ((ref_spad_map[i / 8] >> (i % 8)) & 0x1)
+      {
+         spads_enabled++;
+      }
+   }
+
+   vl53l0x_writeMulti (v, GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6);
+
+   // -- VL53L0X_set_reference_spads() end
+
+   // -- VL53L0X_load_tuning_settings() begin
+
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+   vl53l0x_writeReg8Bit (v, 0x00, 0x00);
+
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x09, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x10, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x11, 0x00);
+
+   vl53l0x_writeReg8Bit (v, 0x24, 0x01);
+   vl53l0x_writeReg8Bit (v, 0x25, 0xFF);
+   vl53l0x_writeReg8Bit (v, 0x75, 0x00);
+
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+   vl53l0x_writeReg8Bit (v, 0x4E, 0x2C);
+   vl53l0x_writeReg8Bit (v, 0x48, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x30, 0x20);
+
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x30, 0x09);
+   vl53l0x_writeReg8Bit (v, 0x54, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x31, 0x04);
+   vl53l0x_writeReg8Bit (v, 0x32, 0x03);
+   vl53l0x_writeReg8Bit (v, 0x40, 0x83);
+   vl53l0x_writeReg8Bit (v, 0x46, 0x25);
+   vl53l0x_writeReg8Bit (v, 0x60, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x27, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x50, 0x06);
+   vl53l0x_writeReg8Bit (v, 0x51, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x52, 0x96);
+   vl53l0x_writeReg8Bit (v, 0x56, 0x08);
+   vl53l0x_writeReg8Bit (v, 0x57, 0x30);
+   vl53l0x_writeReg8Bit (v, 0x61, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x62, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x64, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x65, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x66, 0xA0);
+
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+   vl53l0x_writeReg8Bit (v, 0x22, 0x32);
+   vl53l0x_writeReg8Bit (v, 0x47, 0x14);
+   vl53l0x_writeReg8Bit (v, 0x49, 0xFF);
+   vl53l0x_writeReg8Bit (v, 0x4A, 0x00);
+
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x7A, 0x0A);
+   vl53l0x_writeReg8Bit (v, 0x7B, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x78, 0x21);
+
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+   vl53l0x_writeReg8Bit (v, 0x23, 0x34);
+   vl53l0x_writeReg8Bit (v, 0x42, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x44, 0xFF);
+   vl53l0x_writeReg8Bit (v, 0x45, 0x26);
+   vl53l0x_writeReg8Bit (v, 0x46, 0x05);
+   vl53l0x_writeReg8Bit (v, 0x40, 0x40);
+   vl53l0x_writeReg8Bit (v, 0x0E, 0x06);
+   vl53l0x_writeReg8Bit (v, 0x20, 0x1A);
+   vl53l0x_writeReg8Bit (v, 0x43, 0x40);
+
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x34, 0x03);
+   vl53l0x_writeReg8Bit (v, 0x35, 0x44);
+
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+   vl53l0x_writeReg8Bit (v, 0x31, 0x04);
+   vl53l0x_writeReg8Bit (v, 0x4B, 0x09);
+   vl53l0x_writeReg8Bit (v, 0x4C, 0x05);
+   vl53l0x_writeReg8Bit (v, 0x4D, 0x04);
+
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x44, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x45, 0x20);
+   vl53l0x_writeReg8Bit (v, 0x47, 0x08);
+   vl53l0x_writeReg8Bit (v, 0x48, 0x28);
+   vl53l0x_writeReg8Bit (v, 0x67, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x70, 0x04);
+   vl53l0x_writeReg8Bit (v, 0x71, 0x01);
+   vl53l0x_writeReg8Bit (v, 0x72, 0xFE);
+   vl53l0x_writeReg8Bit (v, 0x76, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x77, 0x00);
+
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+   vl53l0x_writeReg8Bit (v, 0x0D, 0x01);
+
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x80, 0x01);
+   vl53l0x_writeReg8Bit (v, 0x01, 0xF8);
+
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+   vl53l0x_writeReg8Bit (v, 0x8E, 0x01);
+   vl53l0x_writeReg8Bit (v, 0x00, 0x01);
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x80, 0x00);
+
+   // -- VL53L0X_load_tuning_settings() end
+
+   // "Set interrupt config to new sample ready"
+   vl53l0x_writeReg8Bit (v, SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04);
+   vl53l0x_writeReg8Bit (v, GPIO_HV_MUX_ACTIVE_HIGH, vl53l0x_readReg8Bit (v, GPIO_HV_MUX_ACTIVE_HIGH) & ~0x10);
+   vl53l0x_writeReg8Bit (v, SYSTEM_INTERRUPT_CLEAR, 0x01);
+
+   measurement_timing_budget_us = vl53l0x_getMeasurementTimingBudget (v);
+
+   // "Disable MSRC and TCC by default"
+   vl53l0x_writeReg8Bit (v, SYSTEM_SEQUENCE_CONFIG, 0xE8);
+
+   // "Recalculate timing budget"
+   if ((err = vl53l0x_setMeasurementTimingBudget (v, measurement_timing_budget_us)))
+      return err;
+
+   // VL53L0X_StaticInit() end
+
+   // VL53L0X_PerformRefCalibration() begin
+
+   vl53l0x_writeReg8Bit (v, SYSTEM_SEQUENCE_CONFIG, 0x01);
+   if ((err = performSingleRefCalibration (v, 0x40)))
+      return err;
+
+   vl53l0x_writeReg8Bit (v, SYSTEM_SEQUENCE_CONFIG, 0x02);
+   if ((err = performSingleRefCalibration (v, 0x00)))
+      return err;
+
+   // "restore the previous Sequence Config"
+   vl53l0x_writeReg8Bit (v, SYSTEM_SEQUENCE_CONFIG, 0xE8);
+
+   // VL53L0X_PerformRefCalibration() end
+   if (vl53l0x_i2cFail (v))
+      return "I2C fail";
+
+   return NULL;
+}
+
+void
+vl53l0x_end (vl53l0x_t * v)
+{
+   if (!v)
+      return;
+
+   if (v->dev_handle) {
+      i2c_master_bus_rm_device(v->dev_handle);
+   }
+
+   if (v->owns_bus && v->bus_handle) {
+      i2c_del_master_bus(v->bus_handle);
+   }
+
+   free (v);
+}
+
+void
+vl53l0x_setAddress (vl53l0x_t * v, uint8_t new_addr)
+{
+   vl53l0x_writeReg8Bit (v, I2C_SLAVE_DEVICE_ADDRESS, new_addr & 0x7F);
+   v->address = new_addr;
+      // Remove the old device handle and create a new one with the new address
+   if (v->dev_handle) {
+      i2c_master_bus_rm_device(v->dev_handle);
+
+      // Create new device handle with updated address
+      i2c_device_config_t dev_cfg = {
+         .dev_addr_length = I2C_ADDR_BIT_LEN_7,
+         .device_address = new_addr,
+         .scl_speed_hz = I2C_BUSSPEED,  // Match your current speed setting
+      };
+
+      i2c_master_bus_add_device(v->bus_handle, &dev_cfg, &v->dev_handle);
+   }
+}
+
+uint8_t
+vl53l0x_getAddress (vl53l0x_t * v)
+{
+   return v->address;
+}
+
+void
+vl53l0x_setTimeout (vl53l0x_t * v, uint16_t new_timeout)
+{
+   v->io_timeout = new_timeout;
+}
+
+uint16_t
+vl53l0x_getTimeout (vl53l0x_t * v)
+{
+   return v->io_timeout;
+}
+
+int
+vl53l0x_timeoutOccurred (vl53l0x_t * v)
+{
+   int tmp = v->did_timeout;
+   v->did_timeout = 0;
+   return tmp;
+}
+
+int
+vl53l0x_i2cFail (vl53l0x_t * v)
+{
+   int tmp = v->i2c_fail;
+   v->i2c_fail = 0;
+   return tmp;
+}
+
+float
+vl53l0x_getSignalRateLimit (vl53l0x_t * v)
+{
+   return (float) vl53l0x_readReg16Bit (v, FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT) / (1 << 7);
+}
+
+// Set the VCSEL pulse period for the given period type
+const char *
+vl53l0x_setVcselPulsePeriod (vl53l0x_t * v, vl53l0x_vcselPeriodType type, uint8_t period_pclks)
+{
+   uint8_t vcsel_period_reg = encodeVcselPeriod (period_pclks);
+
+   SequenceStepEnables enables;
+   SequenceStepTimeouts timeouts;
+
+   getSequenceStepEnables (v, &enables);
+   getSequenceStepTimeouts (v, &enables, &timeouts);
+
+   if (type == VcselPeriodPreRange)
+   {
+      switch (period_pclks)
+      {
+      case 12:
+         vl53l0x_writeReg8Bit (v, PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18);
+         break;
+      case 14:
+         vl53l0x_writeReg8Bit (v, PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30);
+         break;
+      case 16:
+         vl53l0x_writeReg8Bit (v, PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40);
+         break;
+      case 18:
+         vl53l0x_writeReg8Bit (v, PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50);
+         break;
+      default:
+         return "Invalid period";
+      }
+      vl53l0x_writeReg8Bit (v, PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
+      vl53l0x_writeReg8Bit (v, PRE_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
+
+      uint16_t new_pre_range_timeout_mclks = timeoutMicrosecondsToMclks (timeouts.pre_range_us, period_pclks);
+      vl53l0x_writeReg16Bit (v, PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, encodeTimeout (new_pre_range_timeout_mclks));
+
+      uint16_t new_msrc_timeout_mclks = timeoutMicrosecondsToMclks (timeouts.msrc_dss_tcc_us, period_pclks);
+      vl53l0x_writeReg8Bit (v, MSRC_CONFIG_TIMEOUT_MACROP, (new_msrc_timeout_mclks > 256) ? 255 : (new_msrc_timeout_mclks - 1));
+   } else if (type == VcselPeriodFinalRange)
+   {
+      switch (period_pclks)
+      {
+      case 8:
+         vl53l0x_writeReg8Bit (v, FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10);
+         vl53l0x_writeReg8Bit (v, FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
+         vl53l0x_writeReg8Bit (v, GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
+         vl53l0x_writeReg8Bit (v, ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
+         vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+         vl53l0x_writeReg8Bit (v, ALGO_PHASECAL_LIM, 0x30);
+         vl53l0x_writeReg8Bit (v, 0xFF, 0x00);
+         break;
+      case 10:
+         vl53l0x_writeReg8Bit (v, FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28);
+         vl53l0x_writeReg8Bit (v, FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
+         vl53l0x_writeReg8Bit (v, GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
+         vl53l0x_writeReg8Bit (v, ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
+         vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+         vl53l0x_writeReg8Bit (v, ALGO_PHASECAL_LIM, 0x20);
+         vl53l0x_writeReg8Bit (v, 0xFF, 0x00);
+         break;
+      case 12:
+         vl53l0x_writeReg8Bit (v, FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38);
+         vl53l0x_writeReg8Bit (v, FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
+         vl53l0x_writeReg8Bit (v, GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
+         vl53l0x_writeReg8Bit (v, ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
+         vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+         vl53l0x_writeReg8Bit (v, ALGO_PHASECAL_LIM, 0x20);
+         vl53l0x_writeReg8Bit (v, 0xFF, 0x00);
+         break;
+      case 14:
+         vl53l0x_writeReg8Bit (v, FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48);
+         vl53l0x_writeReg8Bit (v, FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
+         vl53l0x_writeReg8Bit (v, GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
+         vl53l0x_writeReg8Bit (v, ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
+         vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+         vl53l0x_writeReg8Bit (v, ALGO_PHASECAL_LIM, 0x20);
+         vl53l0x_writeReg8Bit (v, 0xFF, 0x00);
+         break;
+      default:
+         return "Invalid period";
+      }
+
+      vl53l0x_writeReg8Bit (v, FINAL_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
+
+      uint16_t new_final_range_timeout_mclks = timeoutMicrosecondsToMclks (timeouts.final_range_us, period_pclks);
+
+      if (enables.pre_range)
+         new_final_range_timeout_mclks += timeouts.pre_range_mclks;
+
+      vl53l0x_writeReg16Bit (v, FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, encodeTimeout (new_final_range_timeout_mclks));
+   } else
+      return "Invalid type";
+
+   const char *err;
+   if ((err = vl53l0x_setMeasurementTimingBudget (v, measurement_timing_budget_us)))
+      return err;
+
+   uint8_t sequence_config = vl53l0x_readReg8Bit (v, SYSTEM_SEQUENCE_CONFIG);
+   vl53l0x_writeReg8Bit (v, SYSTEM_SEQUENCE_CONFIG, 0x02);
+   performSingleRefCalibration (v, 0x0);
+   vl53l0x_writeReg8Bit (v, SYSTEM_SEQUENCE_CONFIG, sequence_config);
+
+   return NULL;
+}
+
+void
+vl53l0x_startContinuous (vl53l0x_t * v, uint32_t period_ms)
+{
+   vl53l0x_writeReg8Bit (v, 0x80, 0x01);
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+   vl53l0x_writeReg8Bit (v, 0x00, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x91, stop_variable);
+   vl53l0x_writeReg8Bit (v, 0x00, 0x01);
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x80, 0x00);
+
+   if (period_ms != 0)
+   {
+      uint16_t osc_calibrate_val = vl53l0x_readReg16Bit (v, OSC_CALIBRATE_VAL);
+
+      if (osc_calibrate_val != 0)
+      {
+         period_ms *= osc_calibrate_val;
+      }
+
+      vl53l0x_writeReg32Bit (v, SYSTEM_INTERMEASUREMENT_PERIOD, period_ms);
+      vl53l0x_writeReg8Bit (v, SYSRANGE_START, 0x04);
+   } else
+   {
+      vl53l0x_writeReg8Bit (v, SYSRANGE_START, 0x02);
+   }
+}
+
+void
+vl53l0x_stopContinuous (vl53l0x_t * v)
+{
+   vl53l0x_writeReg8Bit (v, SYSRANGE_START, 0x01);
+
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+   vl53l0x_writeReg8Bit (v, 0x00, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x91, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x00, 0x01);
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x00);
+}
+
+uint16_t
+vl53l0x_readRangeContinuousMillimeters (vl53l0x_t * v)
+{
+   startTimeout ();
+   while ((vl53l0x_readReg8Bit (v, RESULT_INTERRUPT_STATUS) & 0x07) == 0)
+   {
+      if (checkTimeoutExpired ())
+      {
+         v->did_timeout = 1;
+         return 65535;
+      }
+   }
+   uint16_t range = vl53l0x_readReg16Bit (v, RESULT_RANGE_STATUS + 10);
+   vl53l0x_writeReg8Bit (v, SYSTEM_INTERRUPT_CLEAR, 0x01);
+   return range;
+}
+
+uint16_t
+vl53l0x_readRangeSingleMillimeters (vl53l0x_t * v)
+{
+   vl53l0x_writeReg8Bit (v, 0x80, 0x01);
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x01);
+   vl53l0x_writeReg8Bit (v, 0x00, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x91, stop_variable);
+   vl53l0x_writeReg8Bit (v, 0x00, 0x01);
+   vl53l0x_writeReg8Bit (v, 0xFF, 0x00);
+   vl53l0x_writeReg8Bit (v, 0x80, 0x00);
+
+   vl53l0x_writeReg8Bit (v, SYSRANGE_START, 0x01);
+
+   startTimeout ();
+   while (vl53l0x_readReg8Bit (v, SYSRANGE_START) & 0x01)
+   {
+      if (checkTimeoutExpired ())
+      {
+         v->did_timeout = 1;
+         return 65535;
+      }
+   }
+
+   return vl53l0x_readRangeContinuousMillimeters (v);
+}
diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt
index cc3ce65..f285319 100644
--- a/main/CMakeLists.txt
+++ b/main/CMakeLists.txt
@@ -17,7 +17,7 @@ else()
 endif()
 
 idf_component_register(SRCS ${ALL_SRCS}
-                       PRIV_REQUIRES esp_psram spi_flash nvs_flash esp_event rpc constants config rmt esp_driver_gptimer dataLink flatbuffers esp_driver_ledc oled app_update
+                       PRIV_REQUIRES esp_psram spi_flash nvs_flash esp_event rpc constants config rmt esp_driver_gptimer dataLink flatbuffers esp_driver_ledc oled app_update vl53l0x
                        INCLUDE_DIRS "include")
 
 if(DEFINED SRC_BOARD AND SRC_BOARD)
diff --git a/main/LoopManager.cpp b/main/LoopManager.cpp
index 9f4bde6..4bec166 100644
--- a/main/LoopManager.cpp
+++ b/main/LoopManager.cpp
@@ -8,6 +8,7 @@
 #include "SensorMessageBuilder.h"
 #include "TopologyMessageBuilder.h"
 #include "esp_wifi.h"
+#include "freertos/projdefs.h"
 
 #define ACTUATOR_CMD_TAG 5
 #define TOPOLOGY_CMD_TAG 6
@@ -16,16 +17,22 @@
 
 #define METADATA_PERIOD_MS 1000
 #define SENSOR_DATA_PERIOD_MS 1000
+#define SLEEP_WITH_NO_ACTUATOR_MS 2000
 
 [[noreturn]] void LoopManager::control_loop() const {
     uint8_t buffer[512];
     while (true) {
+        if (m_actuator == nullptr) {
+            vTaskDelay(pdMS_TO_TICKS(SLEEP_WITH_NO_ACTUATOR_MS));
+            continue;
+        }
+
         if (m_messaging_interface->recv(buffer, 512,
                                     ACTUATOR_CMD_TAG)) {
                                         m_actuator->actuate(buffer);
                                         send_sensor_reading(false);
                                     }
-            }
+    }
 }
 
 [[noreturn]] void LoopManager::sensor_loop(char *args) {
@@ -72,10 +79,20 @@
 
 void LoopManager::send_sensor_reading(bool durable) const {
     Flatbuffers::SensorMessageBuilder smb{};
-    // todo: get data from sensor
+    std::vector<Flatbuffers::sensor_value> sensor_values;
+
     if (m_actuator) {
         auto data = m_actuator->get_sensor_data();
-        const auto [ptr, size] = smb.build_sensor_message(data);
+        sensor_values.insert(sensor_values.end(), data.begin(), data.end());
+    }
+
+    if (m_sensor) {
+        auto data = m_sensor->get_sensor_data();
+        sensor_values.insert(sensor_values.end(), data.begin(), data.end());
+    }
+
+    if (sensor_values.size() > 0) {
+        const auto [ptr, size] = smb.build_sensor_message(sensor_values);
         m_messaging_interface->send((uint8_t *)ptr, size, PC_ADDR, SENSOR_TAG, durable);
     }
 }
diff --git a/main/control/DistanceSensor.cpp b/main/control/DistanceSensor.cpp
new file mode 100644
index 0000000..dcd18f5
--- /dev/null
+++ b/main/control/DistanceSensor.cpp
@@ -0,0 +1,70 @@
+
+#include "SensorMessageBuilder.h"
+#include "constants/module.h"
+#include "control/DistanceSensor.h"
+#include "esp_log.h"
+
+#define I2C_PORT 1
+#define XSHUT_PIN -1
+#define TAG "DistanceSensor"
+
+DistanceSensor::DistanceSensor() {
+    i2c_master_bus_config_t bus_config = {
+        .i2c_port = I2C_PORT,
+        .sda_io_num = (gpio_num_t)DISTANCE_SDA,
+        .scl_io_num = (gpio_num_t)DISTANCE_SCL,
+        .clk_source = I2C_CLK_SRC_DEFAULT,
+        .glitch_ignore_cnt = 7,
+        .flags = {
+            .enable_internal_pullup = true,
+        }
+    };
+
+    i2c_master_bus_handle_t bus_handle;
+    i2c_new_master_bus(&bus_config, &bus_handle);
+
+    // Initialize first sensor with default address
+    m_sensor= vl53l0x_config_with_bus(bus_handle, XSHUT_PIN, 0x29, 1);
+    vl53l0x_init(m_sensor);
+    vl53l0x_setAddress(m_sensor, 0x30);  // Change address
+
+    // Start continuous ranging on both
+    vl53l0x_startContinuous(m_sensor, 0);
+
+
+    // m_sensor = vl53l0x_config(I2C_PORT, DISTANCE_SCL, DISTANCE_SDA, XSHUT_PIN, 0x29, 1);
+    // if (!m_sensor) {
+    //     ESP_LOGE(TAG, "Failed to configure sensor");
+    //     return;
+    // }
+
+    // const char *err = vl53l0x_init(m_sensor);
+    // if (err) {
+    //     ESP_LOGE(TAG, "Init failed: %s", err);
+    //     vl53l0x_end(m_sensor);
+    //     m_sensor = nullptr;
+    //     return;
+    // }
+
+    // // Start continuous ranging
+    // vl53l0x_startContinuous(m_sensor, 0);
+}
+
+DistanceSensor::~DistanceSensor() {
+    if (m_sensor == nullptr) {
+        return;
+    }
+
+    vl53l0x_end(m_sensor);
+}
+
+std::vector<Flatbuffers::sensor_value> DistanceSensor::get_sensor_data() {
+    uint16_t range = vl53l0x_readRangeContinuousMillimeters(m_sensor);
+    if (range == 65535) {
+        ESP_LOGW(TAG, "Out of range or timeout");
+    } else {
+        ESP_LOGI(TAG, "Range: %d mm", range);
+        return {Flatbuffers::distance{(float)range}};
+    }
+    return {{}};
+}
diff --git a/main/control/OledActuator.cpp b/main/control/OledActuator.cpp
index 86e0d0b..dcb82ce 100644
--- a/main/control/OledActuator.cpp
+++ b/main/control/OledActuator.cpp
@@ -19,6 +19,7 @@ void OledActuator::actuate(uint8_t *cmd) {
     const auto new_str = text_control_message->message()->str();
 
     if (m_display_str != new_str) {
+
         m_display_str = new_str;
 
         m_oled->clear_display();
diff --git a/main/control/SensorFactory.cpp b/main/control/SensorFactory.cpp
new file mode 100644
index 0000000..f5f64f9
--- /dev/null
+++ b/main/control/SensorFactory.cpp
@@ -0,0 +1,15 @@
+
+#include <memory>
+
+#include "control/SensorFactory.h"
+#include "control/DistanceSensor.h"
+#include "flatbuffers_generated/RobotModule_generated.h"
+
+std::unique_ptr<ISensor> SensorFactory::create_sensor(const ModuleType type) {
+  switch (type) {
+  case ModuleType_DISTANCE_SENSOR:
+    return std::make_unique<DistanceSensor>();
+  default:
+    return nullptr;
+  }
+}
diff --git a/main/include/LoopManager.h b/main/include/LoopManager.h
index 5dfbab3..3563973 100644
--- a/main/include/LoopManager.h
+++ b/main/include/LoopManager.h
@@ -11,13 +11,15 @@
 
 #include "MessagingInterface.h"
 #include "control/ActuatorFactory.h"
+#include "control/SensorFactory.h"
 #include "control/IActuator.h"
 
 class LoopManager {
 public:
     LoopManager() : m_config_manager(ConfigManager::get_instance()),
         m_messaging_interface(std::make_unique<MessagingInterface>()),
-       m_actuator(ActuatorFactory::create_actuator(m_config_manager.get_module_type())) {
+       m_actuator(ActuatorFactory::create_actuator(m_config_manager.get_module_type())),
+       m_sensor(SensorFactory::create_sensor(m_config_manager.get_module_type())) {
            xTaskCreate(reinterpret_cast<TaskFunction_t>(LoopManager::metadata_tx_loop),
                    "metadata_tx", 3096, this, 3, nullptr);
            xTaskCreate(reinterpret_cast<TaskFunction_t>(LoopManager::sensor_loop),
@@ -43,6 +45,7 @@ private:
     ConfigManager& m_config_manager;
     std::unique_ptr<MessagingInterface> m_messaging_interface;
     std::unique_ptr<IActuator> m_actuator;
+    std::unique_ptr<ISensor> m_sensor;
 
     void send_sensor_reading(bool durable) const;
 };
diff --git a/main/include/control/DistanceSensor.h b/main/include/control/DistanceSensor.h
new file mode 100644
index 0000000..c6c7952
--- /dev/null
+++ b/main/include/control/DistanceSensor.h
@@ -0,0 +1,21 @@
+
+#ifndef DCMOTORACTUATOR_H
+#define DCMOTORACTUATOR_H
+
+#include "control/ISensor.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "vl53l0x.h"
+
+class DistanceSensor final : public ISensor {
+  public:
+    DistanceSensor();
+    ~DistanceSensor() override;
+    std::vector<Flatbuffers::sensor_value> get_sensor_data() override;
+
+  private:
+    vl53l0x_t *m_sensor = nullptr;
+
+};
+
+#endif //SERVO1ACTUATOR_H
diff --git a/main/include/control/ISensor.h b/main/include/control/ISensor.h
index a43af33..03011b1 100644
--- a/main/include/control/ISensor.h
+++ b/main/include/control/ISensor.h
@@ -5,10 +5,13 @@
 #ifndef ISENSOR_H
 #define ISENSOR_H
 
+#include <vector>
+#include "SensorMessageBuilder.h"
+
 class ISensor {
 public:
     virtual ~ISensor() {}
-    virtual void get_reading() = 0; // todo: return a flatbuffer object
+    virtual std::vector<Flatbuffers::sensor_value> get_sensor_data() = 0;
 };
 
 #endif //ISENSOR_H
diff --git a/main/include/control/SensorFactory.h b/main/include/control/SensorFactory.h
index cb8cedc..9eec1cd 100644
--- a/main/include/control/SensorFactory.h
+++ b/main/include/control/SensorFactory.h
@@ -5,6 +5,9 @@
 #ifndef SENSORFACTORY_H
 #define SENSORFACTORY_H
 
+#include "flatbuffers_generated/RobotModule_generated.h"
+#include "control/ISensor.h"
+
 class SensorFactory {
 public:
     static std::unique_ptr<ISensor> create_sensor(ModuleType type);
diff --git a/main/main.cpp b/main/main.cpp
index c0414c4..4a7bc39 100644
--- a/main/main.cpp
+++ b/main/main.cpp
@@ -1,3 +1,4 @@
+#include "flatbuffers_generated/RobotModule_generated.h"
 #if !defined(RMT_TEST) || (defined(RMT_TEST) && RMT_TEST == 0)
 // #include <cstdio>
 // #include <memory>
diff --git a/pinout.md b/pinout.md
index e232aba..54953ee 100644
--- a/pinout.md
+++ b/pinout.md
@@ -18,5 +18,5 @@
 |    14   | Ch. 3 RMT RX                  |
 |    15   | DC Motor Encoder A            |
 |    16   | DC Motor Encoder B            |
-|    17   | OLED SDA                      |
-|    18   | OLED SCL                      |
+|    17   | OLED SDA, Distance SDA        |
+|    18   | OLED SCL, Distance SCL        |
diff --git a/sdkconfig b/sdkconfig
index a32fd0f..07826de 100644
--- a/sdkconfig
+++ b/sdkconfig
@@ -1936,6 +1936,12 @@ CONFIG_VFS_SEMIHOSTFS_MAX_MOUNT_POINTS=1
 CONFIG_VFS_INITIALIZE_DEV_NULL=y
 # end of Virtual file system
 
+#
+# VL53L0X
+#
+# CONFIG_VL53L0X_DEBUG is not set
+# end of VL53L0X
+
 #
 # mDNS
 #