Diff: TI_VL53L0X.cpp

Revision:

0:9d485cd4147c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TI_VL53L0X.cpp	Tue Jun 05 15:52:04 2018 +0000
@@ -0,0 +1,336 @@
+#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);
+  }
+}
+