TI_VL53L0X.cpp

00001 #include "TI_VL53L0X.h"
00002 #include "mbed.h"
00003 
00004 TI_VL53L0X::TI_VL53L0X() {
00005   status = VL53L0X_ERROR_NONE;
00006   pMyDevice = &myDevice;
00007 }
00008 
00009 /**
00010  * APIの状態を取得する
00011  */
00012 void TI_VL53L0X::printPalState(VL53L0X_State state) {
00013   char buf[VL53L0X_MAX_STRING_LENGTH];
00014   VL53L0X_GetPalStateString(state, buf);
00015   // printf("API State: %i : %s\n", state, buf);
00016 }
00017 
00018 void TI_VL53L0X::printPalError(VL53L0X_Error status) {
00019   char buf[VL53L0X_MAX_STRING_LENGTH];
00020   VL53L0X_GetPalErrorString(status, buf);
00021   // printf("API Status: %i : %s\n", status, buf);
00022 }
00023 
00024 /**
00025  * 範囲ステータスをprint
00026  */
00027 void TI_VL53L0X::printRangeStatus(
00028     VL53L0X_RangingMeasurementData_t *pRangingMeasurementData) {
00029   char buf[VL53L0X_MAX_STRING_LENGTH];
00030   uint8_t rangeStatus = pRangingMeasurementData->RangeStatus;
00031 
00032   VL53L0X_GetRangeStatusString(rangeStatus, buf);
00033   // printf("Range Status: %i : %s\n", rangeStatus, buf);
00034 }
00035 
00036 /**
00037  * 測定データ準備完了まで待つ
00038  */
00039 VL53L0X_Error TI_VL53L0X::waitMeasurementDataReady(VL53L0X_DEV dev) {
00040   VL53L0X_Error status = VL53L0X_ERROR_NONE;
00041   uint8_t newDatReady = 0;
00042   uint32_t loopNb;
00043 
00044   // 完了するまで待つ
00045   //タイムアウトを使用してデッドロックを回避する
00046   if (status == VL53L0X_ERROR_NONE) {
00047     loopNb = 0;
00048 
00049     do {
00050       status = VL53L0X_GetMeasurementDataReady(dev, &newDatReady);
00051       if ((newDatReady == 0x01) || status != VL53L0X_ERROR_NONE) {
00052         break;
00053       }
00054 
00055       loopNb = loopNb + 1;
00056       VL53L0X_PollingDelay(dev);
00057     } while (loopNb < VL53L0X_DEFAULT_MAX_LOOP);
00058 
00059     if (loopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
00060       status = VL53L0X_ERROR_TIME_OUT;
00061     }
00062   }
00063 
00064   return status;
00065 }
00066 
00067 /**
00068  * 停止完了を待つ
00069  */
00070 VL53L0X_Error TI_VL53L0X::waitStopCompleted(VL53L0X_DEV dev) {
00071   VL53L0X_Error status = VL53L0X_ERROR_NONE;
00072   uint32_t stopCompleted = 0;
00073   uint32_t loopNb;
00074 
00075   // 完了するまで待つ
00076   //タイムアウトを使用してデッドロックを回避する
00077   if (status == VL53L0X_ERROR_NONE) {
00078     loopNb = 0;
00079 
00080     do {
00081       status = VL53L0X_GetStopCompletedStatus(dev, &stopCompleted);
00082 
00083       if ((stopCompleted == 0x00) || status != VL53L0X_ERROR_NONE) {
00084         break;
00085       }
00086 
00087       loopNb = loopNb + 1;
00088       VL53L0X_PollingDelay(dev);
00089     } while (loopNb < VL53L0X_DEFAULT_MAX_LOOP);
00090 
00091     if (loopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
00092       status = VL53L0X_ERROR_TIME_OUT;
00093     }
00094   }
00095 
00096   return status;
00097 }
00098 
00099 void TI_VL53L0X::setup() {
00100   VL53L0X_Version_t version;
00101   VL53L0X_Version_t *pVersion = &version;
00102   VL53L0X_DeviceInfo_t deviceInfo;
00103 
00104   int32_t status_int;
00105 
00106   // Initialize Comms
00107   pMyDevice->I2cDevAddr = 0x52;
00108   pMyDevice->comms_type = 1;
00109   pMyDevice->comms_speed_khz = 400;
00110 
00111   printf("VL53L0X init\r\n");
00112 
00113   if (status == VL53L0X_ERROR_NONE) {
00114     status_int = VL53L0X_GetVersion(pVersion);
00115 
00116     if (status_int != 0) {
00117       status = VL53L0X_ERROR_CONTROL_INTERFACE;
00118     }
00119   }
00120 
00121   // printf("VL53L0X API Version: %d.%d.%d (revision %d)\r\n", pVersion->major,
00122   // pVersion->minor, pVersion->build, pVersion->revision);
00123 
00124   int addr = VL53L0X_scan();
00125   printf("VL53L0X Device found at: %i\r\n", addr);
00126 
00127   // VL53L0X_DataInit（）関数が1回呼び出され、デバイスの初期化が実行されます。
00128   // デバイスがリセット解除された後、一度だけ呼び出されます。
00129   if (status == VL53L0X_ERROR_NONE) {
00130     printf("VL53L0X Data init\n");
00131     uint16_t oscCalibrateVal = 0;
00132     status = VL53L0X_RdWord(&myDevice, VL53L0X_REG_OSC_CALIBRATE_VAL,
00133                             &oscCalibrateVal);
00134     printf("%i\n", oscCalibrateVal);
00135     status = VL53L0X_DataInit(&myDevice);
00136     printPalError(status);
00137   }
00138 
00139   if (status == VL53L0X_ERROR_NONE) {
00140     status = VL53L0X_GetDeviceInfo(&myDevice, &deviceInfo);
00141 
00142     if (status == VL53L0X_ERROR_NONE) {
00143       printf("VL53L0X Get Device Info:\n");
00144       printf("Device Name : %s\n", deviceInfo.Name);
00145       printf("Device Type : %s\n", deviceInfo.Type);
00146       printf("Device ID : %s\n", deviceInfo.ProductId);
00147       printf("Product Revision Major : %d\n", deviceInfo.ProductRevisionMajor);
00148       printf("Product Revision Minor : %d\n", deviceInfo.ProductRevisionMinor);
00149 
00150       if ((deviceInfo.ProductRevisionMinor != 1) &&
00151           (deviceInfo.ProductRevisionMinor != 1)) {
00152         printf("Error expected cut 1.1 but found cut %d.%d\n",
00153                deviceInfo.ProductRevisionMajor,
00154                deviceInfo.ProductRevisionMinor);
00155         status = VL53L0X_ERROR_NOT_SUPPORTED;
00156       }
00157     }
00158     printPalError(status);
00159   }
00160 
00161   // VL53L0X_StaticInit（）関数を使用すると、特定の用途に固有のデバイス設定をロードできます
00162   if (status == VL53L0X_ERROR_NONE) {
00163     // printf("VL53L0X Static init\n");
00164     status = VL53L0X_StaticInit(pMyDevice);
00165     TI_VL53L0X::printPalError(status);
00166   }
00167 }
00168 
00169 /**
00170  * 1回だけ実行
00171  */
00172 void TI_VL53L0X::calibration() {
00173 
00174   status = VL53L0X_ERROR_NONE;
00175   uint32_t refSpadCount;
00176   uint8_t isApertureSpads;
00177   uint8_t vhvSettings;
00178   uint8_t phaseCal;
00179 
00180   FixPoint1616_t calDistanceMilliMeter;
00181   int32_t pOffsetMicroMeter;
00182 
00183   // 基準SPADの較正
00184   // 直接 TOF 法に用いられる SPAD（single photon avalanche diode）では，
00185   // 入射した単一フォトンにより発生したキャリヤーをアバランシ増倍を用いて電気的パルスに変換して，
00186   // その到来時刻を TDC（time to digital converter）により計測する
00187 
00188   // VL53L0X_StaticInitのあとに実行可能、VL53L0X_PerformRefCalibrationの前に実行必須
00189   if (status == VL53L0X_ERROR_NONE) {
00190     // printf("VL53L0X Spad Management\n");
00191     status = VL53L0X_PerformRefSpadManagement(
00192         pMyDevice, &refSpadCount, &isApertureSpads); // Device Initialization
00193     TI_VL53L0X::printPalError(status);
00194   }
00195 
00196   // 温度キャリブレーション。VL53L0X_PerformRefSpadManagementのあとに実行可能
00197   if (status == VL53L0X_ERROR_NONE) {
00198     // printf("VL53L0X Calibration\n");
00199     status = VL53L0X_PerformRefCalibration(pMyDevice, &vhvSettings, &phaseCal);
00200     TI_VL53L0X::printPalError(status);
00201   }
00202 
00203   /*
00204   // オフセットキャリブレーション。VL53L0X_PerformRefCalibrationのあとに実行可能
00205   if (status == VL53L0X_ERROR_NONE) {
00206     status = VL53L0X_PerformOffsetCalibration(pMyDevice, calDistanceMilliMeter,&pOffsetMicroMeter);
00207     TI_VL53L0X::printPalError(status);
00208   }
00209   */
00210 
00211   /*
00212   if (status == VL53L0X_ERROR_NONE) {
00213     FixPoint1616_t xTalkCalDistance,
00214     FixPoint1616_t pXTalkCompensationRateMegaCps
00215 
00216     status = VL53L0X_PerformXTalkCalibration(pMyDevice, &xTalkCalDistance, &pXTalkCompensationRateMegaCps);
00217     TI_VL53L0X::printPalError(status);
00218   }
00219   */
00220 
00221   // クロストーク補正
00222   if (status == VL53L0X_ERROR_NONE) {
00223 
00224     // printf("VL53L0X set device Mode\n");
00225     status = VL53L0X_SetXTalkCompensationRateMegaCps(pMyDevice, 0);
00226 
00227     // 下記は実装されてない
00228     // status = VL53L0X_SetXTalkCompensationEnable(pMyDevice, 1);
00229 
00230     TI_VL53L0X::printPalError(status);
00231   }
00232 
00233   // デバイスモード
00234   // VL53L0X_SetDeviceMode（）は、次のいずれかのモードを選択します。
00235   // シングルレンジング
00236   // 継続的な距離
00237   // 継続的なタイムレンジ
00238   // VL53L0X_GetDeviceMode（）は、実際にどのモードがプログラムされているかを知るために使用します。
00239   // これらのモードは、VL53L0Xのデータシートに記載されています。
00240   if (status == VL53L0X_ERROR_NONE) {
00241 
00242     // printf("VL53L0X set device Mode\n");
00243     status = VL53L0X_SetDeviceMode(
00244         pMyDevice,
00245         VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in single ranging mode
00246     TI_VL53L0X::printPalError(status);
00247   }
00248 }
00249 
00250 void TI_VL53L0X::startMeasurement() {
00251   if (status == VL53L0X_ERROR_NONE) {
00252     printf("VL53L0X start Measurement\n");
00253     status = VL53L0X_StartMeasurement(pMyDevice);
00254     TI_VL53L0X::printPalError(status);
00255   }
00256 }
00257 
00258 /**
00259  * 計測開始
00260  */
00261 int TI_VL53L0X::getMeasurement() {
00262   int averageRange = 9999;
00263 
00264   VL53L0X_RangingMeasurementData_t rangingMeasurementData;
00265   VL53L0X_RangingMeasurementData_t *pRangingMeasurementData =
00266       &rangingMeasurementData;
00267 
00268   if (status == VL53L0X_ERROR_NONE) {
00269     uint32_t measurement;
00270     // uint32_t tryCount = 32;
00271     uint32_t tryCount = 3;
00272 
00273     uint16_t *pResults = (uint16_t *)malloc(sizeof(uint16_t) * tryCount);
00274 
00275     // 計測���始
00276     for (measurement = 0; measurement < tryCount; measurement++) {
00277 
00278       status = TI_VL53L0X::waitMeasurementDataReady(pMyDevice);
00279 
00280       if (status == VL53L0X_ERROR_NONE) {
00281         status = VL53L0X_GetRangingMeasurementData(pMyDevice,
00282                                                    pRangingMeasurementData);
00283 
00284         *(pResults + measurement) = pRangingMeasurementData->RangeMilliMeter;
00285         // printf("VL53L0X In loop measurement %lu: %d\n", measurement,
00286         // pRangingMeasurementData->RangeMilliMeter);
00287 
00288         // 割り込みをクリアする
00289         VL53L0X_ClearInterruptMask(
00290             pMyDevice, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
00291         VL53L0X_PollingDelay(pMyDevice);
00292       } else {
00293         break;
00294       }
00295     }
00296 
00297     if (status == VL53L0X_ERROR_NONE) {
00298 
00299       int total = 0;
00300       // 距離を計測
00301       for (measurement = 0; measurement < tryCount; measurement++) {
00302         // printf("VL53L0X measurement %lu: %d\n", measurement, *(pResults +
00303         // measurement));
00304 
00305         total = total + *(pResults + measurement);
00306       }
00307 
00308       averageRange = total / tryCount;
00309     }
00310 
00311     free(pResults);
00312   }
00313 
00314   // printf("VL53L0X measurement average %d\n", averageRange);
00315 
00316   return averageRange;
00317 }
00318 
00319 void TI_VL53L0X::stopMeasurement() {
00320   if (status == VL53L0X_ERROR_NONE) {
00321     // printf("VL53L0X stop measurement\n");
00322     status = VL53L0X_StopMeasurement(pMyDevice);
00323   }
00324 
00325   if (status == VL53L0X_ERROR_NONE) {
00326     printf("VL53L0X wait stop completed\n");
00327     // 停止完了を待つ
00328     status = TI_VL53L0X::waitStopCompleted(pMyDevice);
00329   }
00330 
00331   if (status == VL53L0X_ERROR_NONE) {
00332     status = VL53L0X_ClearInterruptMask(
00333         pMyDevice, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
00334   }
00335 }
00336