This is a library that makes VL53L0X easy to handle.
# Example
include the mbed library with this snippet
#include "mbed.h" #include "TI_VL53L0X.h" DigitalOut led1(LED1); DigitalIn button(p11); TI_VL53L0X vl53l0x; int main() { led1 = 0; vl53l0x.setup(); vl53l0x.calibration(); while (true) { if (led1) { int averageRange = vl53l0x.getMeasurement(); if (9999 != averageRange) { printf("VL53L0X measurement average %d\n", averageRange); if (averageRange > 300) { led1 = 0; } else { led1 = 1; } } } if (button) { printf("Button Pressed\n\r"); wait(0.7); led1 = !led1; if (led1) { vl53l0x.startMeasurement(); } else { vl53l0x.stopMeasurement(); } } } }
TI_VL53L0X.cpp@0:9d485cd4147c, 2018-06-05 (annotated)
- Committer:
- tichise
- Date:
- Tue Jun 05 15:52:04 2018 +0000
- Revision:
- 0:9d485cd4147c
new;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tichise | 0:9d485cd4147c | 1 | #include "TI_VL53L0X.h" |
tichise | 0:9d485cd4147c | 2 | #include "mbed.h" |
tichise | 0:9d485cd4147c | 3 | |
tichise | 0:9d485cd4147c | 4 | TI_VL53L0X::TI_VL53L0X() { |
tichise | 0:9d485cd4147c | 5 | status = VL53L0X_ERROR_NONE; |
tichise | 0:9d485cd4147c | 6 | pMyDevice = &myDevice; |
tichise | 0:9d485cd4147c | 7 | } |
tichise | 0:9d485cd4147c | 8 | |
tichise | 0:9d485cd4147c | 9 | /** |
tichise | 0:9d485cd4147c | 10 | * APIの状態を取得する |
tichise | 0:9d485cd4147c | 11 | */ |
tichise | 0:9d485cd4147c | 12 | void TI_VL53L0X::printPalState(VL53L0X_State state) { |
tichise | 0:9d485cd4147c | 13 | char buf[VL53L0X_MAX_STRING_LENGTH]; |
tichise | 0:9d485cd4147c | 14 | VL53L0X_GetPalStateString(state, buf); |
tichise | 0:9d485cd4147c | 15 | // printf("API State: %i : %s\n", state, buf); |
tichise | 0:9d485cd4147c | 16 | } |
tichise | 0:9d485cd4147c | 17 | |
tichise | 0:9d485cd4147c | 18 | void TI_VL53L0X::printPalError(VL53L0X_Error status) { |
tichise | 0:9d485cd4147c | 19 | char buf[VL53L0X_MAX_STRING_LENGTH]; |
tichise | 0:9d485cd4147c | 20 | VL53L0X_GetPalErrorString(status, buf); |
tichise | 0:9d485cd4147c | 21 | // printf("API Status: %i : %s\n", status, buf); |
tichise | 0:9d485cd4147c | 22 | } |
tichise | 0:9d485cd4147c | 23 | |
tichise | 0:9d485cd4147c | 24 | /** |
tichise | 0:9d485cd4147c | 25 | * 範囲ステータスをprint |
tichise | 0:9d485cd4147c | 26 | */ |
tichise | 0:9d485cd4147c | 27 | void TI_VL53L0X::printRangeStatus( |
tichise | 0:9d485cd4147c | 28 | VL53L0X_RangingMeasurementData_t *pRangingMeasurementData) { |
tichise | 0:9d485cd4147c | 29 | char buf[VL53L0X_MAX_STRING_LENGTH]; |
tichise | 0:9d485cd4147c | 30 | uint8_t rangeStatus = pRangingMeasurementData->RangeStatus; |
tichise | 0:9d485cd4147c | 31 | |
tichise | 0:9d485cd4147c | 32 | VL53L0X_GetRangeStatusString(rangeStatus, buf); |
tichise | 0:9d485cd4147c | 33 | // printf("Range Status: %i : %s\n", rangeStatus, buf); |
tichise | 0:9d485cd4147c | 34 | } |
tichise | 0:9d485cd4147c | 35 | |
tichise | 0:9d485cd4147c | 36 | /** |
tichise | 0:9d485cd4147c | 37 | * 測定データ準備完了まで待つ |
tichise | 0:9d485cd4147c | 38 | */ |
tichise | 0:9d485cd4147c | 39 | VL53L0X_Error TI_VL53L0X::waitMeasurementDataReady(VL53L0X_DEV dev) { |
tichise | 0:9d485cd4147c | 40 | VL53L0X_Error status = VL53L0X_ERROR_NONE; |
tichise | 0:9d485cd4147c | 41 | uint8_t newDatReady = 0; |
tichise | 0:9d485cd4147c | 42 | uint32_t loopNb; |
tichise | 0:9d485cd4147c | 43 | |
tichise | 0:9d485cd4147c | 44 | // 完了するまで待つ |
tichise | 0:9d485cd4147c | 45 | //タイムアウトを使用してデッドロックを回避する |
tichise | 0:9d485cd4147c | 46 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 47 | loopNb = 0; |
tichise | 0:9d485cd4147c | 48 | |
tichise | 0:9d485cd4147c | 49 | do { |
tichise | 0:9d485cd4147c | 50 | status = VL53L0X_GetMeasurementDataReady(dev, &newDatReady); |
tichise | 0:9d485cd4147c | 51 | if ((newDatReady == 0x01) || status != VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 52 | break; |
tichise | 0:9d485cd4147c | 53 | } |
tichise | 0:9d485cd4147c | 54 | |
tichise | 0:9d485cd4147c | 55 | loopNb = loopNb + 1; |
tichise | 0:9d485cd4147c | 56 | VL53L0X_PollingDelay(dev); |
tichise | 0:9d485cd4147c | 57 | } while (loopNb < VL53L0X_DEFAULT_MAX_LOOP); |
tichise | 0:9d485cd4147c | 58 | |
tichise | 0:9d485cd4147c | 59 | if (loopNb >= VL53L0X_DEFAULT_MAX_LOOP) { |
tichise | 0:9d485cd4147c | 60 | status = VL53L0X_ERROR_TIME_OUT; |
tichise | 0:9d485cd4147c | 61 | } |
tichise | 0:9d485cd4147c | 62 | } |
tichise | 0:9d485cd4147c | 63 | |
tichise | 0:9d485cd4147c | 64 | return status; |
tichise | 0:9d485cd4147c | 65 | } |
tichise | 0:9d485cd4147c | 66 | |
tichise | 0:9d485cd4147c | 67 | /** |
tichise | 0:9d485cd4147c | 68 | * 停止完了を待つ |
tichise | 0:9d485cd4147c | 69 | */ |
tichise | 0:9d485cd4147c | 70 | VL53L0X_Error TI_VL53L0X::waitStopCompleted(VL53L0X_DEV dev) { |
tichise | 0:9d485cd4147c | 71 | VL53L0X_Error status = VL53L0X_ERROR_NONE; |
tichise | 0:9d485cd4147c | 72 | uint32_t stopCompleted = 0; |
tichise | 0:9d485cd4147c | 73 | uint32_t loopNb; |
tichise | 0:9d485cd4147c | 74 | |
tichise | 0:9d485cd4147c | 75 | // 完了するまで待つ |
tichise | 0:9d485cd4147c | 76 | //タイムアウトを使用してデッドロックを回避する |
tichise | 0:9d485cd4147c | 77 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 78 | loopNb = 0; |
tichise | 0:9d485cd4147c | 79 | |
tichise | 0:9d485cd4147c | 80 | do { |
tichise | 0:9d485cd4147c | 81 | status = VL53L0X_GetStopCompletedStatus(dev, &stopCompleted); |
tichise | 0:9d485cd4147c | 82 | |
tichise | 0:9d485cd4147c | 83 | if ((stopCompleted == 0x00) || status != VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 84 | break; |
tichise | 0:9d485cd4147c | 85 | } |
tichise | 0:9d485cd4147c | 86 | |
tichise | 0:9d485cd4147c | 87 | loopNb = loopNb + 1; |
tichise | 0:9d485cd4147c | 88 | VL53L0X_PollingDelay(dev); |
tichise | 0:9d485cd4147c | 89 | } while (loopNb < VL53L0X_DEFAULT_MAX_LOOP); |
tichise | 0:9d485cd4147c | 90 | |
tichise | 0:9d485cd4147c | 91 | if (loopNb >= VL53L0X_DEFAULT_MAX_LOOP) { |
tichise | 0:9d485cd4147c | 92 | status = VL53L0X_ERROR_TIME_OUT; |
tichise | 0:9d485cd4147c | 93 | } |
tichise | 0:9d485cd4147c | 94 | } |
tichise | 0:9d485cd4147c | 95 | |
tichise | 0:9d485cd4147c | 96 | return status; |
tichise | 0:9d485cd4147c | 97 | } |
tichise | 0:9d485cd4147c | 98 | |
tichise | 0:9d485cd4147c | 99 | void TI_VL53L0X::setup() { |
tichise | 0:9d485cd4147c | 100 | VL53L0X_Version_t version; |
tichise | 0:9d485cd4147c | 101 | VL53L0X_Version_t *pVersion = &version; |
tichise | 0:9d485cd4147c | 102 | VL53L0X_DeviceInfo_t deviceInfo; |
tichise | 0:9d485cd4147c | 103 | |
tichise | 0:9d485cd4147c | 104 | int32_t status_int; |
tichise | 0:9d485cd4147c | 105 | |
tichise | 0:9d485cd4147c | 106 | // Initialize Comms |
tichise | 0:9d485cd4147c | 107 | pMyDevice->I2cDevAddr = 0x52; |
tichise | 0:9d485cd4147c | 108 | pMyDevice->comms_type = 1; |
tichise | 0:9d485cd4147c | 109 | pMyDevice->comms_speed_khz = 400; |
tichise | 0:9d485cd4147c | 110 | |
tichise | 0:9d485cd4147c | 111 | printf("VL53L0X init\r\n"); |
tichise | 0:9d485cd4147c | 112 | |
tichise | 0:9d485cd4147c | 113 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 114 | status_int = VL53L0X_GetVersion(pVersion); |
tichise | 0:9d485cd4147c | 115 | |
tichise | 0:9d485cd4147c | 116 | if (status_int != 0) { |
tichise | 0:9d485cd4147c | 117 | status = VL53L0X_ERROR_CONTROL_INTERFACE; |
tichise | 0:9d485cd4147c | 118 | } |
tichise | 0:9d485cd4147c | 119 | } |
tichise | 0:9d485cd4147c | 120 | |
tichise | 0:9d485cd4147c | 121 | // printf("VL53L0X API Version: %d.%d.%d (revision %d)\r\n", pVersion->major, |
tichise | 0:9d485cd4147c | 122 | // pVersion->minor, pVersion->build, pVersion->revision); |
tichise | 0:9d485cd4147c | 123 | |
tichise | 0:9d485cd4147c | 124 | int addr = VL53L0X_scan(); |
tichise | 0:9d485cd4147c | 125 | printf("VL53L0X Device found at: %i\r\n", addr); |
tichise | 0:9d485cd4147c | 126 | |
tichise | 0:9d485cd4147c | 127 | // VL53L0X_DataInit()関数が1回呼び出され、デバイスの初期化が実行されます。 |
tichise | 0:9d485cd4147c | 128 | // デバイスがリセット解除された後、一度だけ呼び出されます。 |
tichise | 0:9d485cd4147c | 129 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 130 | printf("VL53L0X Data init\n"); |
tichise | 0:9d485cd4147c | 131 | uint16_t oscCalibrateVal = 0; |
tichise | 0:9d485cd4147c | 132 | status = VL53L0X_RdWord(&myDevice, VL53L0X_REG_OSC_CALIBRATE_VAL, |
tichise | 0:9d485cd4147c | 133 | &oscCalibrateVal); |
tichise | 0:9d485cd4147c | 134 | printf("%i\n", oscCalibrateVal); |
tichise | 0:9d485cd4147c | 135 | status = VL53L0X_DataInit(&myDevice); |
tichise | 0:9d485cd4147c | 136 | printPalError(status); |
tichise | 0:9d485cd4147c | 137 | } |
tichise | 0:9d485cd4147c | 138 | |
tichise | 0:9d485cd4147c | 139 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 140 | status = VL53L0X_GetDeviceInfo(&myDevice, &deviceInfo); |
tichise | 0:9d485cd4147c | 141 | |
tichise | 0:9d485cd4147c | 142 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 143 | printf("VL53L0X Get Device Info:\n"); |
tichise | 0:9d485cd4147c | 144 | printf("Device Name : %s\n", deviceInfo.Name); |
tichise | 0:9d485cd4147c | 145 | printf("Device Type : %s\n", deviceInfo.Type); |
tichise | 0:9d485cd4147c | 146 | printf("Device ID : %s\n", deviceInfo.ProductId); |
tichise | 0:9d485cd4147c | 147 | printf("Product Revision Major : %d\n", deviceInfo.ProductRevisionMajor); |
tichise | 0:9d485cd4147c | 148 | printf("Product Revision Minor : %d\n", deviceInfo.ProductRevisionMinor); |
tichise | 0:9d485cd4147c | 149 | |
tichise | 0:9d485cd4147c | 150 | if ((deviceInfo.ProductRevisionMinor != 1) && |
tichise | 0:9d485cd4147c | 151 | (deviceInfo.ProductRevisionMinor != 1)) { |
tichise | 0:9d485cd4147c | 152 | printf("Error expected cut 1.1 but found cut %d.%d\n", |
tichise | 0:9d485cd4147c | 153 | deviceInfo.ProductRevisionMajor, |
tichise | 0:9d485cd4147c | 154 | deviceInfo.ProductRevisionMinor); |
tichise | 0:9d485cd4147c | 155 | status = VL53L0X_ERROR_NOT_SUPPORTED; |
tichise | 0:9d485cd4147c | 156 | } |
tichise | 0:9d485cd4147c | 157 | } |
tichise | 0:9d485cd4147c | 158 | printPalError(status); |
tichise | 0:9d485cd4147c | 159 | } |
tichise | 0:9d485cd4147c | 160 | |
tichise | 0:9d485cd4147c | 161 | // VL53L0X_StaticInit()関数を使用すると、特定の用途に固有のデバイス設定をロードできます |
tichise | 0:9d485cd4147c | 162 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 163 | // printf("VL53L0X Static init\n"); |
tichise | 0:9d485cd4147c | 164 | status = VL53L0X_StaticInit(pMyDevice); |
tichise | 0:9d485cd4147c | 165 | TI_VL53L0X::printPalError(status); |
tichise | 0:9d485cd4147c | 166 | } |
tichise | 0:9d485cd4147c | 167 | } |
tichise | 0:9d485cd4147c | 168 | |
tichise | 0:9d485cd4147c | 169 | /** |
tichise | 0:9d485cd4147c | 170 | * 1回だけ実行 |
tichise | 0:9d485cd4147c | 171 | */ |
tichise | 0:9d485cd4147c | 172 | void TI_VL53L0X::calibration() { |
tichise | 0:9d485cd4147c | 173 | |
tichise | 0:9d485cd4147c | 174 | status = VL53L0X_ERROR_NONE; |
tichise | 0:9d485cd4147c | 175 | uint32_t refSpadCount; |
tichise | 0:9d485cd4147c | 176 | uint8_t isApertureSpads; |
tichise | 0:9d485cd4147c | 177 | uint8_t vhvSettings; |
tichise | 0:9d485cd4147c | 178 | uint8_t phaseCal; |
tichise | 0:9d485cd4147c | 179 | |
tichise | 0:9d485cd4147c | 180 | FixPoint1616_t calDistanceMilliMeter; |
tichise | 0:9d485cd4147c | 181 | int32_t pOffsetMicroMeter; |
tichise | 0:9d485cd4147c | 182 | |
tichise | 0:9d485cd4147c | 183 | // 基準SPADの較正 |
tichise | 0:9d485cd4147c | 184 | // 直接 TOF 法に用いられる SPAD(single photon avalanche diode)では, |
tichise | 0:9d485cd4147c | 185 | // 入射した単一フォトンにより発生したキャリヤーをアバランシ増倍を用いて電気的パルスに変換して, |
tichise | 0:9d485cd4147c | 186 | // その到来時刻を TDC(time to digital converter)により計測する |
tichise | 0:9d485cd4147c | 187 | |
tichise | 0:9d485cd4147c | 188 | // VL53L0X_StaticInitのあとに実行可能、VL53L0X_PerformRefCalibrationの前に実行必須 |
tichise | 0:9d485cd4147c | 189 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 190 | // printf("VL53L0X Spad Management\n"); |
tichise | 0:9d485cd4147c | 191 | status = VL53L0X_PerformRefSpadManagement( |
tichise | 0:9d485cd4147c | 192 | pMyDevice, &refSpadCount, &isApertureSpads); // Device Initialization |
tichise | 0:9d485cd4147c | 193 | TI_VL53L0X::printPalError(status); |
tichise | 0:9d485cd4147c | 194 | } |
tichise | 0:9d485cd4147c | 195 | |
tichise | 0:9d485cd4147c | 196 | // 温度キャリブレーション。VL53L0X_PerformRefSpadManagementのあとに実行可能 |
tichise | 0:9d485cd4147c | 197 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 198 | // printf("VL53L0X Calibration\n"); |
tichise | 0:9d485cd4147c | 199 | status = VL53L0X_PerformRefCalibration(pMyDevice, &vhvSettings, &phaseCal); |
tichise | 0:9d485cd4147c | 200 | TI_VL53L0X::printPalError(status); |
tichise | 0:9d485cd4147c | 201 | } |
tichise | 0:9d485cd4147c | 202 | |
tichise | 0:9d485cd4147c | 203 | /* |
tichise | 0:9d485cd4147c | 204 | // オフセットキャリブレーション。VL53L0X_PerformRefCalibrationのあとに実行可能 |
tichise | 0:9d485cd4147c | 205 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 206 | status = VL53L0X_PerformOffsetCalibration(pMyDevice, calDistanceMilliMeter,&pOffsetMicroMeter); |
tichise | 0:9d485cd4147c | 207 | TI_VL53L0X::printPalError(status); |
tichise | 0:9d485cd4147c | 208 | } |
tichise | 0:9d485cd4147c | 209 | */ |
tichise | 0:9d485cd4147c | 210 | |
tichise | 0:9d485cd4147c | 211 | /* |
tichise | 0:9d485cd4147c | 212 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 213 | FixPoint1616_t xTalkCalDistance, |
tichise | 0:9d485cd4147c | 214 | FixPoint1616_t pXTalkCompensationRateMegaCps |
tichise | 0:9d485cd4147c | 215 | |
tichise | 0:9d485cd4147c | 216 | status = VL53L0X_PerformXTalkCalibration(pMyDevice, &xTalkCalDistance, &pXTalkCompensationRateMegaCps); |
tichise | 0:9d485cd4147c | 217 | TI_VL53L0X::printPalError(status); |
tichise | 0:9d485cd4147c | 218 | } |
tichise | 0:9d485cd4147c | 219 | */ |
tichise | 0:9d485cd4147c | 220 | |
tichise | 0:9d485cd4147c | 221 | // クロストーク補正 |
tichise | 0:9d485cd4147c | 222 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 223 | |
tichise | 0:9d485cd4147c | 224 | // printf("VL53L0X set device Mode\n"); |
tichise | 0:9d485cd4147c | 225 | status = VL53L0X_SetXTalkCompensationRateMegaCps(pMyDevice, 0); |
tichise | 0:9d485cd4147c | 226 | |
tichise | 0:9d485cd4147c | 227 | // 下記は実装されてない |
tichise | 0:9d485cd4147c | 228 | // status = VL53L0X_SetXTalkCompensationEnable(pMyDevice, 1); |
tichise | 0:9d485cd4147c | 229 | |
tichise | 0:9d485cd4147c | 230 | TI_VL53L0X::printPalError(status); |
tichise | 0:9d485cd4147c | 231 | } |
tichise | 0:9d485cd4147c | 232 | |
tichise | 0:9d485cd4147c | 233 | // デバイスモード |
tichise | 0:9d485cd4147c | 234 | // VL53L0X_SetDeviceMode()は、次のいずれかのモードを選択します。 |
tichise | 0:9d485cd4147c | 235 | // シングルレンジング |
tichise | 0:9d485cd4147c | 236 | // 継続的な距離 |
tichise | 0:9d485cd4147c | 237 | // 継続的なタイムレンジ |
tichise | 0:9d485cd4147c | 238 | // VL53L0X_GetDeviceMode()は、実際にどのモードがプログラムされているかを知るために使用します。 |
tichise | 0:9d485cd4147c | 239 | // これらのモードは、VL53L0Xのデータシートに記載されています。 |
tichise | 0:9d485cd4147c | 240 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 241 | |
tichise | 0:9d485cd4147c | 242 | // printf("VL53L0X set device Mode\n"); |
tichise | 0:9d485cd4147c | 243 | status = VL53L0X_SetDeviceMode( |
tichise | 0:9d485cd4147c | 244 | pMyDevice, |
tichise | 0:9d485cd4147c | 245 | VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in single ranging mode |
tichise | 0:9d485cd4147c | 246 | TI_VL53L0X::printPalError(status); |
tichise | 0:9d485cd4147c | 247 | } |
tichise | 0:9d485cd4147c | 248 | } |
tichise | 0:9d485cd4147c | 249 | |
tichise | 0:9d485cd4147c | 250 | void TI_VL53L0X::startMeasurement() { |
tichise | 0:9d485cd4147c | 251 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 252 | printf("VL53L0X start Measurement\n"); |
tichise | 0:9d485cd4147c | 253 | status = VL53L0X_StartMeasurement(pMyDevice); |
tichise | 0:9d485cd4147c | 254 | TI_VL53L0X::printPalError(status); |
tichise | 0:9d485cd4147c | 255 | } |
tichise | 0:9d485cd4147c | 256 | } |
tichise | 0:9d485cd4147c | 257 | |
tichise | 0:9d485cd4147c | 258 | /** |
tichise | 0:9d485cd4147c | 259 | * 計測開始 |
tichise | 0:9d485cd4147c | 260 | */ |
tichise | 0:9d485cd4147c | 261 | int TI_VL53L0X::getMeasurement() { |
tichise | 0:9d485cd4147c | 262 | int averageRange = 9999; |
tichise | 0:9d485cd4147c | 263 | |
tichise | 0:9d485cd4147c | 264 | VL53L0X_RangingMeasurementData_t rangingMeasurementData; |
tichise | 0:9d485cd4147c | 265 | VL53L0X_RangingMeasurementData_t *pRangingMeasurementData = |
tichise | 0:9d485cd4147c | 266 | &rangingMeasurementData; |
tichise | 0:9d485cd4147c | 267 | |
tichise | 0:9d485cd4147c | 268 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 269 | uint32_t measurement; |
tichise | 0:9d485cd4147c | 270 | // uint32_t tryCount = 32; |
tichise | 0:9d485cd4147c | 271 | uint32_t tryCount = 3; |
tichise | 0:9d485cd4147c | 272 | |
tichise | 0:9d485cd4147c | 273 | uint16_t *pResults = (uint16_t *)malloc(sizeof(uint16_t) * tryCount); |
tichise | 0:9d485cd4147c | 274 | |
tichise | 0:9d485cd4147c | 275 | // 計測���始 |
tichise | 0:9d485cd4147c | 276 | for (measurement = 0; measurement < tryCount; measurement++) { |
tichise | 0:9d485cd4147c | 277 | |
tichise | 0:9d485cd4147c | 278 | status = TI_VL53L0X::waitMeasurementDataReady(pMyDevice); |
tichise | 0:9d485cd4147c | 279 | |
tichise | 0:9d485cd4147c | 280 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 281 | status = VL53L0X_GetRangingMeasurementData(pMyDevice, |
tichise | 0:9d485cd4147c | 282 | pRangingMeasurementData); |
tichise | 0:9d485cd4147c | 283 | |
tichise | 0:9d485cd4147c | 284 | *(pResults + measurement) = pRangingMeasurementData->RangeMilliMeter; |
tichise | 0:9d485cd4147c | 285 | // printf("VL53L0X In loop measurement %lu: %d\n", measurement, |
tichise | 0:9d485cd4147c | 286 | // pRangingMeasurementData->RangeMilliMeter); |
tichise | 0:9d485cd4147c | 287 | |
tichise | 0:9d485cd4147c | 288 | // 割り込みをクリアする |
tichise | 0:9d485cd4147c | 289 | VL53L0X_ClearInterruptMask( |
tichise | 0:9d485cd4147c | 290 | pMyDevice, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY); |
tichise | 0:9d485cd4147c | 291 | VL53L0X_PollingDelay(pMyDevice); |
tichise | 0:9d485cd4147c | 292 | } else { |
tichise | 0:9d485cd4147c | 293 | break; |
tichise | 0:9d485cd4147c | 294 | } |
tichise | 0:9d485cd4147c | 295 | } |
tichise | 0:9d485cd4147c | 296 | |
tichise | 0:9d485cd4147c | 297 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 298 | |
tichise | 0:9d485cd4147c | 299 | int total = 0; |
tichise | 0:9d485cd4147c | 300 | // 距離を計測 |
tichise | 0:9d485cd4147c | 301 | for (measurement = 0; measurement < tryCount; measurement++) { |
tichise | 0:9d485cd4147c | 302 | // printf("VL53L0X measurement %lu: %d\n", measurement, *(pResults + |
tichise | 0:9d485cd4147c | 303 | // measurement)); |
tichise | 0:9d485cd4147c | 304 | |
tichise | 0:9d485cd4147c | 305 | total = total + *(pResults + measurement); |
tichise | 0:9d485cd4147c | 306 | } |
tichise | 0:9d485cd4147c | 307 | |
tichise | 0:9d485cd4147c | 308 | averageRange = total / tryCount; |
tichise | 0:9d485cd4147c | 309 | } |
tichise | 0:9d485cd4147c | 310 | |
tichise | 0:9d485cd4147c | 311 | free(pResults); |
tichise | 0:9d485cd4147c | 312 | } |
tichise | 0:9d485cd4147c | 313 | |
tichise | 0:9d485cd4147c | 314 | // printf("VL53L0X measurement average %d\n", averageRange); |
tichise | 0:9d485cd4147c | 315 | |
tichise | 0:9d485cd4147c | 316 | return averageRange; |
tichise | 0:9d485cd4147c | 317 | } |
tichise | 0:9d485cd4147c | 318 | |
tichise | 0:9d485cd4147c | 319 | void TI_VL53L0X::stopMeasurement() { |
tichise | 0:9d485cd4147c | 320 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 321 | // printf("VL53L0X stop measurement\n"); |
tichise | 0:9d485cd4147c | 322 | status = VL53L0X_StopMeasurement(pMyDevice); |
tichise | 0:9d485cd4147c | 323 | } |
tichise | 0:9d485cd4147c | 324 | |
tichise | 0:9d485cd4147c | 325 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 326 | printf("VL53L0X wait stop completed\n"); |
tichise | 0:9d485cd4147c | 327 | // 停止完了を待つ |
tichise | 0:9d485cd4147c | 328 | status = TI_VL53L0X::waitStopCompleted(pMyDevice); |
tichise | 0:9d485cd4147c | 329 | } |
tichise | 0:9d485cd4147c | 330 | |
tichise | 0:9d485cd4147c | 331 | if (status == VL53L0X_ERROR_NONE) { |
tichise | 0:9d485cd4147c | 332 | status = VL53L0X_ClearInterruptMask( |
tichise | 0:9d485cd4147c | 333 | pMyDevice, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY); |
tichise | 0:9d485cd4147c | 334 | } |
tichise | 0:9d485cd4147c | 335 | } |
tichise | 0:9d485cd4147c | 336 |