vl53_manager.cpp

#include "vl53_manager.h"

static VL53Sensor sensors[MAX_VL53_SENSORS];
static int sensorCount = 0;

static void selectChannel(uint8_t channel) {
  Wire.beginTransmission(TCA9548A_ADDR);
  Wire.write(1 << channel);
  Wire.endTransmission();
  delay(2);
}

void vl53Init() {
  Wire.begin();
  Wire.setClock(400000);
  sensorCount = 0;
}

bool vl53AddSensor(uint8_t channel) {
  if (sensorCount >= MAX_VL53_SENSORS) return false;

  selectChannel(channel);
  if (!sensors[sensorCount].tof.begin()) {
    Serial.printf("VL53 not found on channel %d\n", channel);
    return false;
  }

  sensors[sensorCount].tof.setResolution(64);
  sensors[sensorCount].tof.startRanging();

  sensors[sensorCount].channel = channel;
  sensors[sensorCount].active = true;
  sensorCount++;

  Serial.printf("VL53 initialized on channel %d\n", channel);
  return true;
}

bool vl53Read(uint8_t idx) {
  if (idx >= sensorCount || !sensors[idx].active) return false;

  selectChannel(sensors[idx].channel);
  if (!sensors[idx].tof.isDataReady()) return false;

  return sensors[idx].tof.getRangingData(&sensors[idx].results);
}

int vl53GetCount() {
  return sensorCount;
}

VL53Sensor* vl53GetSensor(uint8_t idx) {
  if (idx >= sensorCount) return nullptr;
  return &sensors[idx];
}