File content as of revision 0:9d485cd4147c:

#include "TI_VL53L0X.h"
#include "mbed.h"

TI_VL53L0X::TI_VL53L0X() {
  status = VL53L0X_ERROR_NONE;
  pMyDevice = &myDevice;
}

/**
 * APIの状態を取得する
 */
void TI_VL53L0X::printPalState(VL53L0X_State state) {
  char buf[VL53L0X_MAX_STRING_LENGTH];
  VL53L0X_GetPalStateString(state, buf);
  // printf("API State: %i : %s\n", state, buf);
}

void TI_VL53L0X::printPalError(VL53L0X_Error status) {
  char buf[VL53L0X_MAX_STRING_LENGTH];
  VL53L0X_GetPalErrorString(status, buf);
  // printf("API Status: %i : %s\n", status, buf);
}

/**
 * 範囲ステータスをprint
 */
void TI_VL53L0X::printRangeStatus(
    VL53L0X_RangingMeasurementData_t *pRangingMeasurementData) {
  char buf[VL53L0X_MAX_STRING_LENGTH];
  uint8_t rangeStatus = pRangingMeasurementData->RangeStatus;

  VL53L0X_GetRangeStatusString(rangeStatus, buf);
  // printf("Range Status: %i : %s\n", rangeStatus, buf);
}

/**
 * 測定データ準備完了まで待つ
 */
VL53L0X_Error TI_VL53L0X::waitMeasurementDataReady(VL53L0X_DEV dev) {
  VL53L0X_Error status = VL53L0X_ERROR_NONE;
  uint8_t newDatReady = 0;
  uint32_t loopNb;

  // 完了するまで待つ
  //タイムアウトを使用してデッドロックを回避する
  if (status == VL53L0X_ERROR_NONE) {
    loopNb = 0;

    do {
      status = VL53L0X_GetMeasurementDataReady(dev, &newDatReady);
      if ((newDatReady == 0x01) || status != VL53L0X_ERROR_NONE) {
        break;
      }

      loopNb = loopNb + 1;
      VL53L0X_PollingDelay(dev);
    } while (loopNb < VL53L0X_DEFAULT_MAX_LOOP);

    if (loopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
      status = VL53L0X_ERROR_TIME_OUT;
    }
  }

  return status;
}

/**
 * 停止完了を待つ
 */
VL53L0X_Error TI_VL53L0X::waitStopCompleted(VL53L0X_DEV dev) {
  VL53L0X_Error status = VL53L0X_ERROR_NONE;
  uint32_t stopCompleted = 0;
  uint32_t loopNb;

  // 完了するまで待つ
  //タイムアウトを使用してデッドロックを回避する
  if (status == VL53L0X_ERROR_NONE) {
    loopNb = 0;

    do {
      status = VL53L0X_GetStopCompletedStatus(dev, &stopCompleted);

      if ((stopCompleted == 0x00) || status != VL53L0X_ERROR_NONE) {
        break;
      }

      loopNb = loopNb + 1;
      VL53L0X_PollingDelay(dev);
    } while (loopNb < VL53L0X_DEFAULT_MAX_LOOP);

    if (loopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
      status = VL53L0X_ERROR_TIME_OUT;
    }
  }

  return status;
}

void TI_VL53L0X::setup() {
  VL53L0X_Version_t version;
  VL53L0X_Version_t *pVersion = &version;
  VL53L0X_DeviceInfo_t deviceInfo;

  int32_t status_int;

  // Initialize Comms
  pMyDevice->I2cDevAddr = 0x52;
  pMyDevice->comms_type = 1;
  pMyDevice->comms_speed_khz = 400;

  printf("VL53L0X init\r\n");

  if (status == VL53L0X_ERROR_NONE) {
    status_int = VL53L0X_GetVersion(pVersion);

    if (status_int != 0) {
      status = VL53L0X_ERROR_CONTROL_INTERFACE;
    }
  }

  // printf("VL53L0X API Version: %d.%d.%d (revision %d)\r\n", pVersion->major,
  // pVersion->minor, pVersion->build, pVersion->revision);

  int addr = VL53L0X_scan();
  printf("VL53L0X Device found at: %i\r\n", addr);

  // VL53L0X_DataInit（）関数が1回呼び出され、デバイスの初期化が実行されます。
  // デバイスがリセット解除された後、一度だけ呼び出されます。
  if (status == VL53L0X_ERROR_NONE) {
    printf("VL53L0X Data init\n");
    uint16_t oscCalibrateVal = 0;
    status = VL53L0X_RdWord(&myDevice, VL53L0X_REG_OSC_CALIBRATE_VAL,
                            &oscCalibrateVal);
    printf("%i\n", oscCalibrateVal);
    status = VL53L0X_DataInit(&myDevice);
    printPalError(status);
  }

  if (status == VL53L0X_ERROR_NONE) {
    status = VL53L0X_GetDeviceInfo(&myDevice, &deviceInfo);

    if (status == VL53L0X_ERROR_NONE) {
      printf("VL53L0X Get Device Info:\n");
      printf("Device Name : %s\n", deviceInfo.Name);
      printf("Device Type : %s\n", deviceInfo.Type);
      printf("Device ID : %s\n", deviceInfo.ProductId);
      printf("Product Revision Major : %d\n", deviceInfo.ProductRevisionMajor);
      printf("Product Revision Minor : %d\n", deviceInfo.ProductRevisionMinor);

      if ((deviceInfo.ProductRevisionMinor != 1) &&
          (deviceInfo.ProductRevisionMinor != 1)) {
        printf("Error expected cut 1.1 but found cut %d.%d\n",
               deviceInfo.ProductRevisionMajor,
               deviceInfo.ProductRevisionMinor);
        status = VL53L0X_ERROR_NOT_SUPPORTED;
      }
    }
    printPalError(status);
  }

  // VL53L0X_StaticInit（）関数を使用すると、特定の用途に固有のデバイス設定をロードできます
  if (status == VL53L0X_ERROR_NONE) {
    // printf("VL53L0X Static init\n");
    status = VL53L0X_StaticInit(pMyDevice);
    TI_VL53L0X::printPalError(status);
  }
}

/**
 * 1回だけ実行
 */
void TI_VL53L0X::calibration() {

  status = VL53L0X_ERROR_NONE;
  uint32_t refSpadCount;
  uint8_t isApertureSpads;
  uint8_t vhvSettings;
  uint8_t phaseCal;

  FixPoint1616_t calDistanceMilliMeter;
  int32_t pOffsetMicroMeter;

  // 基準SPADの較正
  // 直接 TOF 法に用いられる SPAD（single photon avalanche diode）では，
  // 入射した単一フォトンにより発生したキャリヤーをアバランシ増倍を用いて電気的パルスに変換して，
  // その到来時刻を TDC（time to digital converter）により計測する

  // VL53L0X_StaticInitのあとに実行可能、VL53L0X_PerformRefCalibrationの前に実行必須
  if (status == VL53L0X_ERROR_NONE) {
    // printf("VL53L0X Spad Management\n");
    status = VL53L0X_PerformRefSpadManagement(
        pMyDevice, &refSpadCount, &isApertureSpads); // Device Initialization
    TI_VL53L0X::printPalError(status);
  }

  // 温度キャリブレーション。VL53L0X_PerformRefSpadManagementのあとに実行可能
  if (status == VL53L0X_ERROR_NONE) {
    // printf("VL53L0X Calibration\n");
    status = VL53L0X_PerformRefCalibration(pMyDevice, &vhvSettings, &phaseCal);
    TI_VL53L0X::printPalError(status);
  }

  /*
  // オフセットキャリブレーション。VL53L0X_PerformRefCalibrationのあとに実行可能
  if (status == VL53L0X_ERROR_NONE) {
    status = VL53L0X_PerformOffsetCalibration(pMyDevice, calDistanceMilliMeter,&pOffsetMicroMeter);
    TI_VL53L0X::printPalError(status);
  }
  */

  /*
  if (status == VL53L0X_ERROR_NONE) {
    FixPoint1616_t xTalkCalDistance,
  	FixPoint1616_t pXTalkCompensationRateMegaCps

    status = VL53L0X_PerformXTalkCalibration(pMyDevice, &xTalkCalDistance, &pXTalkCompensationRateMegaCps);
    TI_VL53L0X::printPalError(status);
  }
  */

  // クロストーク補正
  if (status == VL53L0X_ERROR_NONE) {

    // printf("VL53L0X set device Mode\n");
    status = VL53L0X_SetXTalkCompensationRateMegaCps(pMyDevice, 0);

    // 下記は実装されてない
    // status = VL53L0X_SetXTalkCompensationEnable(pMyDevice, 1);

    TI_VL53L0X::printPalError(status);
  }

  // デバイスモード
  // VL53L0X_SetDeviceMode（）は、次のいずれかのモードを選択します。
  // シングルレンジング
  // 継続的な距離
  // 継続的なタイムレンジ
  // VL53L0X_GetDeviceMode（）は、実際にどのモードがプログラムされているかを知るために使用します。
  // これらのモードは、VL53L0Xのデータシートに記載されています。
  if (status == VL53L0X_ERROR_NONE) {

    // printf("VL53L0X set device Mode\n");
    status = VL53L0X_SetDeviceMode(
        pMyDevice,
        VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in single ranging mode
    TI_VL53L0X::printPalError(status);
  }
}

void TI_VL53L0X::startMeasurement() {
  if (status == VL53L0X_ERROR_NONE) {
    printf("VL53L0X start Measurement\n");
    status = VL53L0X_StartMeasurement(pMyDevice);
    TI_VL53L0X::printPalError(status);
  }
}

/**
 * 計測開始
 */
int TI_VL53L0X::getMeasurement() {
  int averageRange = 9999;

  VL53L0X_RangingMeasurementData_t rangingMeasurementData;
  VL53L0X_RangingMeasurementData_t *pRangingMeasurementData =
      &rangingMeasurementData;

  if (status == VL53L0X_ERROR_NONE) {
    uint32_t measurement;
    // uint32_t tryCount = 32;
    uint32_t tryCount = 3;

    uint16_t *pResults = (uint16_t *)malloc(sizeof(uint16_t) * tryCount);

    // 計測���始
    for (measurement = 0; measurement < tryCount; measurement++) {

      status = TI_VL53L0X::waitMeasurementDataReady(pMyDevice);

      if (status == VL53L0X_ERROR_NONE) {
        status = VL53L0X_GetRangingMeasurementData(pMyDevice,
                                                   pRangingMeasurementData);

        *(pResults + measurement) = pRangingMeasurementData->RangeMilliMeter;
        // printf("VL53L0X In loop measurement %lu: %d\n", measurement,
        // pRangingMeasurementData->RangeMilliMeter);

        // 割り込みをクリアする
        VL53L0X_ClearInterruptMask(
            pMyDevice, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
        VL53L0X_PollingDelay(pMyDevice);
      } else {
        break;
      }
    }

    if (status == VL53L0X_ERROR_NONE) {

      int total = 0;
      // 距離を計測
      for (measurement = 0; measurement < tryCount; measurement++) {
        // printf("VL53L0X measurement %lu: %d\n", measurement, *(pResults +
        // measurement));

        total = total + *(pResults + measurement);
      }

      averageRange = total / tryCount;
    }

    free(pResults);
  }

  // printf("VL53L0X measurement average %d\n", averageRange);

  return averageRange;
}

void TI_VL53L0X::stopMeasurement() {
  if (status == VL53L0X_ERROR_NONE) {
    // printf("VL53L0X stop measurement\n");
    status = VL53L0X_StopMeasurement(pMyDevice);
  }

  if (status == VL53L0X_ERROR_NONE) {
    printf("VL53L0X wait stop completed\n");
    // 停止完了を待つ
    status = TI_VL53L0X::waitStopCompleted(pMyDevice);
  }

  if (status == VL53L0X_ERROR_NONE) {
    status = VL53L0X_ClearInterruptMask(
        pMyDevice, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
  }
}