8×8ゾーンToF測距モジュール VL53L5CXを試す

スイッチサイエンスさんで購入した8×8ゾーンToF測距モジュールを試してみました。
８ｘ８または４ｘ４のエリアで最大4メートルの距離を計測することができます。
計測速度は最大15Hz(4x4なら60Hz)です。
マイコンボードとはI2Cで接続し、中央のIO4は特に使用しません。
データシート

サンプルコードを元に、

• 距離範囲外等の測定エラー時の挙動を追加
• 128×64ドットOLEDに表示

をしています。

ソースコード

#include <Wire.h>
#include <SparkFun_VL53L5CX_Library.h>
//https://www.pololu.com/file/0J1878/vl53l5cx.pdf

#define LGFX_USE_V1
#include <LovyanGFX.hpp>

class LGFX : public lgfx::LGFX_Device {
  lgfx::Panel_SSD1306 _panel;
  lgfx::Bus_I2C _bus;

public:
  LGFX() {
    auto busConfig = _bus.config();
    busConfig.i2c_port = 0;
    busConfig.pin_sda = SDA;
    busConfig.pin_scl = SCL;
    busConfig.freq_write = 400000;
    busConfig.freq_read = 400000;
    busConfig.i2c_addr = 0x3C;
    _bus.config(busConfig);

    _panel.setBus(&_bus);

    auto panelConfig = _panel.config();
    panelConfig.memory_width = 128;
    panelConfig.memory_height = 64;
    panelConfig.panel_width = 128;
    panelConfig.panel_height = 64;
    panelConfig.offset_x = 0;
    panelConfig.offset_y = 0;
    panelConfig.pin_rst = -1;
    _panel.config(panelConfig);

    setPanel(&_panel);
  }
};

SparkFun_VL53L5CX myImager;
VL53L5CX_ResultsData measurementData;
LGFX lcd;

int imageResolution = 0;
int imageWidth = 0;

const int16_t minimumDisplayDistanceMm = 100;
const int16_t maximumDisplayDistanceMm = 3000;

uint8_t distanceToBrightness(int16_t distanceMm) {
  if (distanceMm >= maximumDisplayDistanceMm) {
    return 0;
  }

  if (distanceMm <= minimumDisplayDistanceMm) {
    return 255;
  }

  return map(
      distanceMm,
      minimumDisplayDistanceMm,
      maximumDisplayDistanceMm,
      255,
      0);
}

void drawDistanceMap() {
  const int cellWidth = lcd.width() / imageWidth;
  const int cellHeight = lcd.height() / imageWidth;

  for (int y = 0; y < imageWidth; ++y) {
    for (int x = 0; x < imageWidth; ++x) {
      uint8_t idx = (imageWidth - 1 - x) + (imageWidth - 1 - y) * imageWidth;
      uint8_t status = measurementData.target_status[idx];
      uint16_t color =0;
      // https://community.st.com/imaging-sensors-49/vl53l5-l7cx-target-status-validity-136986#:~:text=VL53L5%2FL7CX%20target%20status%20validity%20*%20VL53L5CX.%20*,edge%20of%20your%20target.%20*%20%2D%20john.
      //if (status == 5 || status == 6 || status == 9) {   // 正常な判定
      if (status == 5 || status == 6) {   // 正常な判定
        int16_t distanceMm = measurementData.distance_mm[idx];
        uint8_t brightness = distanceToBrightness(distanceMm);
        color = lcd.color565(brightness, brightness, brightness);
      }
      lcd.fillRect(x * cellWidth, y * cellHeight, cellWidth, cellHeight, color);
    }
  }
}

void setup() {
  Serial.begin(115200);
  delay(10);

  Wire.begin();
  Wire.setClock(400000);

  lcd.init();
  lcd.setRotation(2);
  lcd.setTextColor(TFT_WHITE, TFT_BLACK);
  lcd.setTextSize(1);
  for(int y=0;y<30;++y) {
    lcd.fillScreen(TFT_BLACK);
    lcd.drawString("Initializing...", 5, y);
    delay(20);
  }

  delay(1000);
  Serial.println("Initializing sensor board. This can take up to 10s. Please wait.");

  if (myImager.begin() == false) {
    Serial.println(F("Sensor not found - check your wiring. Freezing"));
    lcd.fillScreen(TFT_BLACK);
    lcd.drawString("Sensor not found.", 5, 5);
    while (1) {
      delay(1000);
    }
  }
  lcd.fillScreen(TFT_BLACK);

  myImager.setResolution(VL53L5CX_RESOLUTION_8X8);
  imageResolution = myImager.getResolution();
  imageWidth = sqrt(imageResolution);

  //https://www.google.com/search?q=https%3A%2F%2Fwww.st.com%2Fresource%2Fen%2Fuser_manual%2Fdm00797144-a-guide-to-using-the-vl53l5cx-multizone-timeofflight-ranging-sensor-with-wide-field-of-view-ultra-lite-driver-uld-stmicroelectronics.pdf%0A%E3%82%92%E6%97%A5%E6%9C%AC%E8%AA%9E%E3%81%AB%E3%81%97%E3%81%A6%E3%81%8F%E3%81%A0%E3%81%95%E3%81%84&rlz=1C1PWSB_jaJP977JP977&sourceid=chrome&ie=UTF-8&amc=1&aep=48&cud=0&source=chrome.crn.obic&atvm=2&udm=50&vsrid=CI-S0P3jytOHlQEQARgBIhCmpyT4VryvN_YtTnryyTQnOOC04sne45YDSAFQBQ&lns_mode=cvst&mstk=AUtExfDUcIVSGvqnhnt7dHr7EjNpnduhYDRcHArrvXxq_6yee638XXi9xVYxtDfr0FIW8EYO7dRDtUzjlPOQz7e292E_mQHB6B_Z1AcjufyJLPaifyQcO_8MoI4Y3or_iaF8rfAbdlRg-7vkw9UjMMvre5sIS4BCPSy5DYeqpwohvrybmPK2wummnPKxx253SrWgeAL8gKUmGxCA-uooluDHIyH5T4kHUm6CTfqXgNO553CJcEolj2ndIHW3cXL-HC69ad6rlo0jtZgwHQHMzr8eI23DwH7E_-g2AcQoO5Y0wyTtOEY4IN-djT0qOTN4Lq_O0j7Ae4Xvsi79M-c&csuir=1&mtid=nX2iavz9IZTG0-kPst-zyAQ
  bool response = myImager.setRangingFrequency(15); // VL53L5CX_RESOLUTION_8X8:1 Hz ～ 15 Hz VL53L5CX_RESOLUTION_4X4:1 Hz ～ 60 Hz
  if (response == true) {
    int frequency = myImager.getRangingFrequency();
    if (frequency > 0) {
      Serial.print("Ranging frequency set to ");
      Serial.print(frequency);
      Serial.println(" Hz.");
    }
  } else {
    Serial.println(F("Cannot set ranging frequency requested. Freezing..."));
    lcd.fillScreen(TFT_BLACK);
    lcd.drawString("Cannot set ranging frequency requested.", 5, 5);
    while (1) {
      delay(1000);
    }
  }

  myImager.startRanging();
}

void loop() {
  if (myImager.isDataReady() == true && myImager.getRangingData(&measurementData)) {
    drawDistanceMap();
#if false
    for (int y = 0; y <= imageWidth * (imageWidth - 1); y += imageWidth) {
      for (int x = imageWidth - 1; x >= 0; x--) {
        Serial.print("\t");
        Serial.print(measurementData.distance_mm[x + y]);
      }
      Serial.println();
    }
    Serial.println();
#endif
  }
  delay(100);

  //myImager.stopRanging(); 

}