UMEHOSHI ITA TOP PAGE COMPUTER SHIEN LAB
import time
from machine import I2C, Pin
debug_flag=True # デバック用debug_print関数の出力を行うかの有無指定
def debug_print(*args, **kwargs):
if debug_flag : print(*args, **kwargs)
# ============ VL53L1Xの距離センサー関連の各種定義 =================================
class VL53L1X:
DEFAULT_ADDRESS = 0x29 # I2C ADDRESS
IDENTIFICATION__MODEL_ID = 0x010F # センサーモデル情報の取得レジスタ
IDENTIFICATION__MODULE_TYPE = 0x0110 # センサータイプ情報の取得レジスタ
IDENTIFICATION__REVISION_ID = 0x0111 # センサーリビジョン情報の取得レジスタ
IDENTIFICATION__MODULE_ID = 0x0112 # センサーモジュール情報の取得レジスタ
FIRMWARE__SYSTEM_STATUS = 0x00E5 # Boot 判定レジスタ(non-zero = booted)
VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND = 0x0008 # VHV(Vertical High Voltage)
# ---GPIO / interrupt---------------------------------------------------------
GPIO_HV_MUX__CTRL = 0x0030 # GPIO1ピン(データ準備完了を知らせる割り込み通知ピン)の出力設定用
GPIO__TIO_HV_STATUS = 0x0031 # データ準備完了などを調べるレジスタ
SYSTEM__INTERRUPT_CLEAR = 0x0086 # センサーに次の測定を始めさせるためのレジスタ
# ---- set_distance_modeメソッドの変更で、同時に設定するレジスタ番号--------------
PHASECAL_CONFIG__TIMEOUT_MACROP = 0x004B # 位相キャリブレーション・タイムアウト
RANGE_CONFIG__TIMEOUT_MACROP_A_HI = 0x005E # レーザー光が戻りを待つ最大時間(タイムアウト時間)の上位バイト
RANGE_CONFIG__TIMEOUT_MACROP_B_HI = 0x0061 # レーザー光が戻りを待つ最大時間(タイムアウト時間)の下位バイト
RANGE_CONFIG__VCSEL_PERIOD_A = 0x0060 #フェーズAの照射するレーザー光のパルスの周期設定用レジスタ
RANGE_CONFIG__VCSEL_PERIOD_B = 0x0063 #フェーズBの照射するレーザー光のパルスの周期設定用レジスタ
RANGE_CONFIG__VALID_PHASE_HIGH = 0x0069 # 反射して戻ってきたレーザー光の位相の受け入れ閾値
SD_CONFIG__WOI_SD0 = 0x0078 # 反射して戻ってきた光の待ち受け時間(ウインドウ)の幅を調整するためのレジスタ
SD_CONFIG__INITIAL_PHASE_SD0 = 0x007A # 反射して戻ってきた光の待ち受ける開始タイミング調整するためのレジスタ
ROI_CONFIG__USER_ROI_CENTRE_SPAD = 0x007F # 測定視野の中心となる受光ダイオードを指定するレジスタ
ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE = 0x0080 # 測定視野のためのダイオードの利用幅を指定
SYSTEM__SEQUENCE_CONFIG = 0x0081 # 測定の中で、「どの内部処理(ステップ)を実行するか」の組み合わせを決める設定
SYSTEM__MODE_START = 0x0087 # 測定スタートを制御
# ---Result--測定結果-------------------------------------------------------
RESULT__RANGE_STATUS = 0x0089 # 直前に行われた距離測定が正しく成功したか、またはどのようなエラーが発生したか
RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0 = 0x008C # SPAD(光子アバランシェフォトダイオード)の使用個数
RESULT__AMBIENT_COUNT_RATE_MCPS_SD = 0x0090 # センサーが測定した周囲の環境光(ノイズとなる光)の強さ
RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0 = 0x0096 # 『距離(ミリメートル単位)』」
RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0 = 0x0098 # 邪魔な反射を除いた最強の光の強さ
#-- Distance modes 短距離、中距離、長距離の3つの距離モードがあるが、ここでは2つを指定できる指定にした
DISTANCE_MODE_SHORT = 1 # 短距離向け 約1.3 m
DISTANCE_MODE_LONG = 2 # 長距離向け 最大約4 m
# ULD default configuration------------------------------------------
DEFAULT_CONFIGURATION = bytes([
0x00, # 0x2D: VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND (VHVキャリブレーション タイムアウト制御)
0x00, # 0x2E: VHV_CONFIG__COUNT_THRESH (VHVカウント閾値)
0x00, # 0x2F: ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS (クロストーク補正オフセット [MSB])
0x01, # 0x30: ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS (クロストーク補正オフセット [LSB])
0x02, # 0x31: ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS (クロストークX方向勾配 [MSB])
0x00, # 0x32: ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS (クロストークX方向勾配 [LSB])
0x02, # 0x33: ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS (クロストークY方向勾配 [MSB])
0x08, # 0x34: ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS (クロストークY方向勾配 [LSB])
0x00, # 0x35: ALGO__PAUSE_SOLID_ARRAY_CHECK (ソリッドアレイ検証の一時停止フラグ)
0x08, # 0x36: ALGO__PART_TO_PART_RANGE_OFFSET_MM (個体差距離補正オフセット(mm) [MSB])
0x10, # 0x37: ALGO__PART_TO_PART_RANGE_OFFSET_MM (個体差距離補正オフセット(mm) [LSB])
0x01, # 0x38: MM_CONFIG__INNER_OFFSET_MM (内部測定領域オフセット(mm) [MSB])
0x01, # 0x39: MM_CONFIG__INNER_OFFSET_MM (内部測定領域オフセット(mm) [LSB])
0x00, # 0x3A: MM_CONFIG__OUTER_OFFSET_MM (外部測定領域オフセット(mm) [MSB])
0x00, # 0x3B: MM_CONFIG__OUTER_OFFSET_MM (外部測定領域オフセット(mm) [LSB])
0x00, # 0x3C: DSS_CONFIG__TARGET_TOTAL_RATE_MCPS (DSS目標トータルリターンレート(Mcps) [MSB])
0x00, # 0x3D: DSS_CONFIG__TARGET_TOTAL_RATE_MCPS (DSS目標トータルリターンレート(Mcps) [LSB])
0xFF, # 0x3E: DEBUG__CTRL (内部デバッグ制御)
0x00, # 0x3F: TEST_MODE__CTRL (テストモード制御)
0x0F, # 0x40: CLK__CONFIG (内部メインクロック設定)
0x00, # 0x41: SYSTEM__START (システムトリガー/開始制御)
0x00, # 0x42: SYSTEM__HISTORY_CTRL (履歴バッファ制御)
0x00, # 0x43: SYSTEM__STREAM_COUNT_CTRL (ストリームカウント制御)
0x00, # 0x44: SYSTEM__SEQUENCE_CONFIG (測定シーケンス有効化設定)
0x00, # 0x45: SYSTEM__GROUPED_PARAMETER_HOLD_0 (パラメータ一括適用保持 0)
0x20, # 0x46: VHV_CONFIG__INIT (VHV初期化制御)
0x0B, # 0x47: VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND (VHVループ制限値)
0x00, # 0x48: MYCRO_TUNE (内部アナログチューニング 1)
0x00, # 0x49: MYCRO_TUNE (内部アナログチューニング 2)
0x02, # 0x4A: GLOBAL_CONFIG__SPAD_ENABLES_REF_0 (参照SPAD有効化マップ 0)
0x0A, # 0x4B: GLOBAL_CONFIG__SPAD_ENABLES_REF_1 (参照SPAD有効化マップ 1)
0x21, # 0x4C: GLOBAL_CONFIG__SPAD_ENABLES_REF_2 (参照SPAD有効化マップ 2)
0x00, # 0x4D: GLOBAL_CONFIG__SPAD_ENABLES_REF_3 (参照SPAD有効化マップ 3)
0x00, # 0x4E: GLOBAL_CONFIG__SPAD_ENABLES_REF_4 (参照SPAD有効化マップ 4)
0x05, # 0x4F: GLOBAL_CONFIG__SPAD_ENABLES_REF_5 (参照SPAD有効化マップ 5)
0x00, # 0x50: GLOBAL_CONFIG__REF_EN_START_SELECT (参照SPAD開始選択)
0x00, # 0x51: REF_SPAD_MAN__NUM_REQUESTED_REF_SPADS (要求参照SPAD数)
0x00, # 0x52: REF_SPAD_MAN__REF_LOCATION (参照SPAD配置場所)
0x00, # 0x53: ALGO__CONSISTENCY_CHECK__REFERENCE_SIGNAL_NON_KIN_INTRINSIC (参照信号整合性チェック)
0xC8, # 0x54: ALGO__CONSISTENCY_CHECK__EVENT_CONSISTENCY (イベント整合性閾値 0)
0x00, # 0x55: ALGO__CONSISTENCY_CHECK__EVENT_CONSISTENCY (イベント整合性閾値 1)
0x00, # 0x56: ALGO__CONSISTENCY_CHECK__EVENT_CONSISTENCY (イベント整合性閾値 2)
0x38, # 0x57: ALGO__CONSISTENCY_CHECK__EVENT_CONSISTENCY (イベント整合性閾値 3)
0xFF, # 0x58: ALGO__RANGE_IGNORE_THRESHOLD_MCPS (無視するノイズ距離閾値 [MSB])
0x01, # 0x59: ALGO__RANGE_IGNORE_THRESHOLD_MCPS (無視するノイズ距離閾値 [LSB])
0x00, # 0x5A: ALGO__RANGE_MIN_CLIP (最小クリッピング範囲)
0x08, # 0x5B: ALGO__CONSISTENCY_CHECK__TOTAL_RATE_PER_SPAD_MCPS (SPADあたりレート整合性 [MSB])
0x00, # 0x5C: ALGO__CONSISTENCY_CHECK__TOTAL_RATE_PER_SPAD_MCPS (SPADあたりレート整合性 [LSB])
0x00, # 0x5D: SD_CONFIG__WOI_CONFIG (受光タイミングウィンドウWOI設定 0)
0x01, # 0x5E: SD_CONFIG__WOI_CONFIG (受光タイミングウィンドウWOI設定 1)
0xCC, # 0x5F: SD_CONFIG__INITIAL_PHASE_CONFIG (初期位相設定 0)
0x0F, # 0x60: SD_CONFIG__INITIAL_PHASE_CONFIG (初期位相設定 1)
0x01, # 0x61: SYSTEM__GROUPED_PARAMETER_HOLD_0 (パラメータ更新の保留設定)
0xF1, # 0x62: SYSTEM__THRESH_HIGH (距離割り込み上限閾値 [MSB])
0x0D, # 0x63: SYSTEM__THRESH_HIGH (距離割り込み上限閾値 [LSB])
0x01, # 0x64: SYSTEM__THRESH_LOW (距離割り込み下限閾値 [MSB])
0x68, # 0x65: SYSTEM__THRESH_LOW (距離割り込み下限閾値 [LSB])
0x00, # 0x66: SYSTEM__ENABLE_CONFIG_0 (システム機能有効化フラグ 0)
0x80, # 0x67: SYSTEM__ENABLE_CONFIG_1 (システム機能有効化フラグ 1)
0x08, # 0x68: SYSTEM__SEQUENCE_CONFIG (シーケンサ動作制御)
0xB8, # 0x69: SYSTEM__HISTOGRAM_BIN (ヒストグラムBin初期構成)
0x00, # 0x6A: ROI_CONFIG__USER_ROI_CENTRE_SPAD (ユーザー設定ROIの中心SPAD番号)
0x00, # 0x6B: ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE (ユーザー設定ROIのXYサイズ)
0x00, # 0x6C: SYSTEM__CONFIG_VALID_HEADER (設定ヘッダー有効検証)
0x00, # 0x6D: SYSTEM__SEQUENCE_CONFIG (内部シーケンス追加制御)
0x0F, # 0x6E: SYSTEM__GROUPED_PARAMETER_HOLD_1 (一括パラメータ保持 1)
0x89, # 0x6F: SYSTEM__GROUPED_PARAMETER_HOLD (一括パラメータ保持トリガー)
0x00, # 0x70: SD_CONFIG__QUANTIFIER_CONFIG (シグナル検出クオンティファイア設定)
0x00, # 0x71: ROI_CONFIG__ROI_CONTROL (関心領域ROI動的制御)
0x00, # 0x72: FIRMWARE__SYSTEM_STATUS (ファームウェアステータス初期化)
0x00, # 0x73: INTERRUPT_CONFIG (割り込み信号出力設定)
0x00, # 0x74: INTERRUPT_CLEAR (割り込みクリアフラグ)
0x00, # 0x75: INTERRUPT_MODE (割り込み検出モード)
0x00, # 0x76: GPIO_HV_PAD__CTRL (GPIO高電圧パッド制御)
0x01, # 0x77: GPIO_HV_MUX__CTRL (GPIOマルチプレクサ機能選択)
0x0F, # 0x78: GPIO__TIO_HV_STATUS (GPIOピン入出力ステータス)
0x0D, # 0x79: SYSTEM__INTERRUPT_CLEAR (システム割り込み解除)
0x0E, # 0x7A: SYSTEM__MODE_START (測距モード開始/レンジング有効化)
0x0E, # 0x7B: PAD_I2C_HV__CONFIG (I2C通信パッド設定)
0x00, # 0x7C: ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS (トリミング用補正値 0x7C)
0x00, # 0x7D: ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS (トリミング用補正値 0x7D)
0x02, # 0x7E: ALGO__PAUSE_SOLID_ARRAY_CHECK (アレイ制御内部トリミング)
0xC7, # 0x7F: ALGO__PART_TO_PART_RANGE_OFFSET_MM (内部距離トリミングレジスタ)
0xFF, # 0x80: ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS (内部補正トリミングレジスタ)
0x9B, # 0x81: GLOBAL_CONFIG__SPAD_ENABLES_REF (内部SPAD調整トリミング)
0x00, # 0x82: SD_CONFIG (シグナル検出内部パラメータ 0x82)
0x00, # 0x83: SD_CONFIG (シグナル検出内部パラメータ 0x83)
0x00, # 0x84: SD_CONFIG (シグナル検出内部パラメータ 0x84)
0x01, # 0x85: SD_CONFIG (シグナル検出内部パラメータ 0x85)
0x00, # 0x86: SYSTEM__START (システム起動最終確定フラグ)
0x00, # 0x87: SYSTEM__GROUPED_PARAMETER_HOLD (一括設定バッファクリア)
])
# ------Timing budget tables 「各処理に割り当てらる制限時間(許容時間)」
# MM(Macro Period)タイムアウト:レーザーを本格的に照射する前に、周囲の環境光(ノイズ)の強さを測定するフェーズの制限時間
# Range(測距)タイムアウト:実際にレーザーを飛ばして、戻ってくるのを待つフェーズの制限時間
TIMING_SHORT = { # 1回の測定にかける時間:(MM:Macro Period, Range:測距)
15: (0x001D, 0x0027), # 1S のMMとRangeの制限時間
20: (0x0051, 0x006E),
33: (0x00D6, 0x006E),
50: (0x01AE, 0x01E8),
100: (0x02E1, 0x0388),
200: (0x03E1, 0x0496),
500: (0x0591, 0x05C1),
}
TIMING_LONG = {
20: (0x001E, 0x0022),
33: (0x0060, 0x006E),
50: (0x00AD, 0x00C6),
100: (0x01CC, 0x01EA),
200: (0x02D9, 0x02F8),
500: (0x048F, 0x04A4),
}
# ---------------------------------コンストラクタ---------
def __init__(self, i2c, address=DEFAULT_ADDRESS):
self.i2c = i2c
self.address = address
self.distance_mode = self.DISTANCE_MODE_LONG
self.timing_budget_ms = 50
# ------- I2Cでの8bit レジスタ書き込み ------
def _write8(self, reg, value):
buf = bytes([
(reg >> 8) & 0xFF,
reg & 0xFF,
value & 0xFF
])
self.i2c.writeto(self.address, buf)
#debug_print("W8 %04X <- %02X" % (reg, value))
# ------- I2Cでの16bit レジスタ書き込み ------
def _write16(self, reg, value):
buf = bytes([
(reg >> 8) & 0xFF,
reg & 0xFF,
(value >> 8) & 0xFF,
value & 0xFF
])
self.i2c.writeto(self.address, buf)
#debug_print("W16 %04X <- %04X" % (reg, value))
# ------- I2Cでの32bit レジスタ書き込み ------
def _write32(self, reg, value):
buf = bytes([
(reg >> 8) & 0xFF,
reg & 0xFF,
(value >> 24) & 0xFF,
(value >> 16) & 0xFF,
(value >> 8) & 0xFF,
value & 0xFF
])
self.i2c.writeto(self.address, buf)
#debug_print("W32 %04X <- %08X" % (reg, value))
# ------- I2Cでの8bit レジスタ読み込み ------
def _read8(self, reg):
self.i2c.writeto(
self.address,
bytes([
(reg >> 8) & 0xFF,
reg & 0xFF
])
)
value = self.i2c.readfrom(self.address, 1)[0]
#debug_print("R8 %04X -> %02X" % (reg, value))
return value
# ------- I2Cでの16bit レジスタ読み込み ------
def _read16(self, reg):
self.i2c.writeto(
self.address,
bytes([ (reg >> 8) & 0xFF, reg & 0xFF ])
)
data = self.i2c.readfrom(self.address, 2)
value = (data[0] << 8) | data[1]
#debug_print("R16 %04X -> %04X" % (reg, value))
return value
# ------- I2Cでの32bit レジスタ読み込み ------
def _read32(self, reg):
self.i2c.writeto(
self.address,
bytes([ (reg >> 8) & 0xFF, reg & 0xFF ])
)
data = self.i2c.readfrom(self.address, 4)
value = ( (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3] )
#debug_print("R32 %04X -> %08X" % (reg, value))
return value
# --- 各種 チップ情報取得メソッド ----------------------------------
def get_model_id(self):
return self._read8(self.IDENTIFICATION__MODEL_ID)
def get_module_type(self):
return self._read8(self.IDENTIFICATION__MODULE_TYPE)
def get_revision_id(self):
return self._read8(self.IDENTIFICATION__REVISION_ID)
def get_module_id(self):
return self._read16(self.IDENTIFICATION__MODULE_ID)
def check_device(self):
model = self.get_model_id()
if model != 0xEA:
raise RuntimeError(
"VL53L1X model ID error: 0x%02X" % model
)
return True
# Bootが終わっているかの判定-------------------------------------
def boot_state(self):
return self._read8( self.FIRMWARE__SYSTEM_STATUS ) != 0
# Bootが終わっるまで、待つ-------------------------------------
def wait_booted(self, timeout_ms=1000):
start = time.ticks_ms()
while not self.boot_state():
if time.ticks_diff(
time.ticks_ms(),
start
) > timeout_ms:
raise RuntimeError(
"VL53L1X boot timeout"
)
time.sleep_ms(5)
# これはUltra Lite Driverとしてセンサー初期化---------
def sensor_init(self):
self.check_device()
for offset in range(len(self.DEFAULT_CONFIGURATION)):
reg = 0x2D + offset
value = self.DEFAULT_CONFIGURATION[offset]
self._write8(reg, value)
self.start_measurement()
self.wait_data_ready( timeout_ms=1000 )
self.clear_interrupt()
self.stop_measurement()
self._write8( self.VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09 ) # 自動電圧調整(VHV)の設定
self._write8( 0x000B,0x00 ) # VHV(VHVキャリブレーション)最初に1回だけ自動調整を行わせる
# ----最大測定距離のモード指定 mode 1 = Short, 2 = Long ------------
def set_distance_mode(self, mode):
if mode not in ( self.DISTANCE_MODE_SHORT, self.DISTANCE_MODE_LONG ):
raise ValueError( "distance mode must be 1 or 2")
old_budget = self.timing_budget_ms # Save current timing budget.
if mode == self.DISTANCE_MODE_SHORT:
self._write8( self.PHASECAL_CONFIG__TIMEOUT_MACROP, 0x14 ) # 位相キャリブレーション・タイムアウト
self._write8( self.RANGE_CONFIG__VCSEL_PERIOD_A, 0x07 )# VCSEL period A
self._write8( self.RANGE_CONFIG__VCSEL_PERIOD_B, 0x05)# VCSEL period B
self._write8( self.RANGE_CONFIG__VALID_PHASE_HIGH, 0x38 )# Valid phase high
self._write16( self.SD_CONFIG__WOI_SD0, 0x0705 ) # 戻ってきた光を受け取る受け時間の幅
self._write16( self.SD_CONFIG__INITIAL_PHASE_SD0, 0x0606 ) # 戻ってきた光の受光のタイミング
else:
self._write8( self.PHASECAL_CONFIG__TIMEOUT_MACROP, 0x0A ) # 位相キャリブレーション・タイムアウト
self._write8( self.RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F )# VCSEL period A
self._write8( self.RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D )# VCSEL period B
self._write8( self.RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8 )# Valid phase high
self._write16( self.SD_CONFIG__WOI_SD0, 0x0F0D ) # 戻ってきた光を受け取る受け時間の幅
self._write16( self.SD_CONFIG__INITIAL_PHASE_SD0, 0x0E0E ) # 戻ってきた光の受光のタイミング
self.distance_mode = mode
if old_budget is not None: # 距離モードを変更した後、以前のタイミングバジェットを再適用
self.set_timing_budget(old_budget)
# ---Timing budget STSW-IMG009 supported timing budgets--測定時間の設定------------
def set_timing_budget(self, budget_ms):
if self.distance_mode == self.DISTANCE_MODE_SHORT:
table = self.TIMING_SHORT # Short: 15, 20, 33, 50, 100, 200, 500 ms
else:
table = self.TIMING_LONG # Long: 20, 33, 50, 100, 200, 500 ms
if budget_ms not in table:
raise ValueError(
"timing budget %d ms is not valid for "
"distance mode %d"
% ( budget_ms, self.distance_mode)
)
timeout_a, timeout_b = table[budget_ms]
self._write16( self.RANGE_CONFIG__TIMEOUT_MACROP_A_HI, timeout_a )
self._write16( self.RANGE_CONFIG__TIMEOUT_MACROP_B_HI, timeout_b )
self.timing_budget_ms = budget_ms
# -- 現在VL53L1Xに設定されている測定時間(Timing Budget)が何msなのかを、レジスタ値から読み出して返すメソッド
def get_timing_budget(self):
a = self._read16( self.RANGE_CONFIG__TIMEOUT_MACROP_A_HI )
if self.distance_mode == self.DISTANCE_MODE_SHORT:
values = {
0x001D: 15,
0x0051: 20,
0x00D6: 33,
0x01AE: 50,
0x02E1: 100,
0x03E1: 200,
0x0591: 500,
}
else:
values = {
0x001E: 20,
0x0060: 33,
0x00AD: 50,
0x01CC: 100,
0x02D9: 200,
0x048F: 500,
}
return values.get(a, 0)
# -------Measurement:測定のスタート・ストップ制御----------------
def start_measurement(self): # 測定のスタート
self._write8( self.SYSTEM__MODE_START, 0x40 )
def stop_measurement(self): # 測定のストップ
self._write8( self.SYSTEM__MODE_START, 0x00 )
# -Interrup「polarity:極性:LOWで知らせるか、HIGHで知らせるか」を取得と設定---
def get_interrupt_polarity(self):
value = self._read8( self.GPIO_HV_MUX__CTRL )
return 0 if (value & 0x10) else 1 #
def set_interrupt_polarity(self, polarity): # 判定論理の極性を設定
value = self._read8( self.GPIO_HV_MUX__CTRL )
value &= 0xEF
value |= ( 0x00 if (polarity & 1) else 0x10 )
self._write8( self.GPIO_HV_MUX__CTRL, value )
def data_ready(self): # 測定結果が得られる状態になったら Trueを返す
polarity = self.get_interrupt_polarity()
status = self._read8( self.GPIO__TIO_HV_STATUS )
return (status & 0x01) == polarity
# 測定結果が得られる状態になるまで待つ(タイムアウトの引数あり)
def wait_data_ready(self, timeout_ms=1000):
start = time.ticks_ms() # マイコンの電源をオン0として、そこから数えた現在のミリ秒
while not self.data_ready():
if time.ticks_diff( time.ticks_ms(), start ) > timeout_ms:
raise RuntimeError( "VL53L1X data-ready timeout")
time.sleep_ms(1)
# ---------- データを受け取ったから、割り込み信号を消して次の測定する合図(クリア処理)-------
def clear_interrupt(self):
self._write8( self.SYSTEM__INTERRUPT_CLEAR, 0x01 )
# ---- 測距測定データを取得して返す --------------------------------------------------------
def get_ranging_measurement_data(self):
status = self._read8(self.RESULT__RANGE_STATUS) # 測定の成功・失敗の状態
distance_mm = self._read16(
self.RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0 # 距離(mm)
)
ambient = self._read16( self.RESULT__AMBIENT_COUNT_RATE_MCPS_SD ) # 環境光
signal = self._read16(
self.RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0
)
spads = self._read16( self.RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0 )/256 # SPAD使用数
return {
"status": status, # 測定の成功・失敗の状態
"distance_mm": distance_mm,# 距離(mm)
"ambient": ambient,# 環境光
"signal": signal, # MCPS: Million Counts Per Second(1秒間に何百万回カウントされたか)
"spads": spads, # 強すぎる光に対する自動的に調整されたSingle-Photon Avalanche Dioの使用数
}
# ---- One-shot convenience function -----------------------------------
def read(self):
self.wait_data_ready()
result = self.get_ranging_measurement_data()
self.clear_interrupt()
return result
# -------Continuous measurement generator------------
def measurements(self):
while True:
self.wait_data_ready()
result = ( self.get_ranging_measurement_data())
self.clear_interrupt()
yield result
# -----Initialize everything ---全て初期化-------------------------------
def init( self, distance_mode=DISTANCE_MODE_LONG, timing_budget_ms=50 ):
self.wait_booted() # Wait until the embedded firmware is booted.
self.sensor_init() # STSW-IMG009 SensorInit().
self.set_distance_mode( distance_mode ) # Configure distance mode.
self.set_timing_budget( timing_budget_ms ) # Configure timing budget.
self.start_measurement() # Start continuous ranging.
i2c = I2C( 0, scl=Pin(22), sda=Pin(21), freq=400000)
debug_print("I2C devices:", [hex(x) for x in i2c.scan()])
tof = VL53L1X( i2c, address=0x29)
# ----- Initialize Long mode 20 ms timing budget ------------------------
tof.init( distance_mode=VL53L1X.DISTANCE_MODE_LONG, timing_budget_ms=20 )
debug_print( "VL53L1X initialized")
debug_print( "Distance mode:", tof.distance_mode )
debug_print( "Timing budget:", tof.get_timing_budget(), "ms" )
debug_print("Model ID :", hex(tof.get_model_id()))
debug_print("Module Type :", hex(tof.get_module_type()))
debug_print("Revision :", hex(tof.get_revision_id()))
debug_print("Module ID :", hex(tof.get_module_id()))
for _ in range(20):
start = time.ticks_ms()
result = tof.read() # 測定
period=time.ticks_diff( time.ticks_ms(), start )
print(
"distance=%4d mm "
"status=%d "
"ambient=%d "
"signal=%d "
"spads=%d "
"period=%d"
% (
result["distance_mm"], # 距離(ミリメートル単位)
result["status"], # 正しく想定できたか?:9であれば成功
result["ambient"], # 環境光(小さければ小さいほど良い値)
result["signal"], # 「光の粒(フォトン)が何個帰ってきたか」という「光の量(強さ)」
result["spads"], # 強すぎる光に対して自動的に調整された、受光素子数
period
)
)
R:\drone>mpremote connect COM3 run vl53l1x_uld.py I2C devices: ['0x28', '0x29'] VL53L1X initialized Distance mode: 2 Timing budget: 20 ms Model ID : 0xea Module Type : 0xcc Revision : 0x10 Module ID : 0xfffe distance= 840 mm status=9 ambient=30 signal=1410 spads=203 period=79 distance= 786 mm status=9 ambient=27 signal=1406 spads=203 period=36 distance= 787 mm status=9 ambient=24 signal=1439 spads=203 period=100 distance= 785 mm status=9 ambient=26 signal=1422 spads=203 period=102 distance= 777 mm status=9 ambient=29 signal=1442 spads=203 period=100 distance= 786 mm status=9 ambient=31 signal=1402 spads=203 period=101 distance= 789 mm status=9 ambient=33 signal=1442 spads=203 period=100 distance= 794 mm status=9 ambient=37 signal=1365 spads=203 period=100 distance= 781 mm status=9 ambient=26 signal=1491 spads=203 period=102 distance= 788 mm status=9 ambient=33 signal=1332 spads=203 period=100 distance= 782 mm status=9 ambient=27 signal=1471 spads=203 period=101 distance= 784 mm status=9 ambient=29 signal=1407 spads=203 period=100 distance= 794 mm status=9 ambient=32 signal=1457 spads=203 period=100 distance= 774 mm status=9 ambient=37 signal=1380 spads=203 period=102 distance= 788 mm status=9 ambient=37 signal=1369 spads=203 period=100 distance= 790 mm status=9 ambient=25 signal=1405 spads=203 period=100 distance= 790 mm status=9 ambient=23 signal=1446 spads=203 period=100 distance= 789 mm status=9 ambient=31 signal=1387 spads=203 period=100 distance= 799 mm status=9 ambient=28 signal=1399 spads=203 period=102 distance= 791 mm status=9 ambient=21 signal=1531 spads=203 period=100 R:\drone>Long mode 20 ms timing budget で動かした時の距離測定周期の実測値は約100mSでした。
BootState ↓ SensorInit ├─ 0x2D〜0x87 の ULD default configuration ├─ 初回測距 ├─ DataReady待ち ├─ Interrupt Clear ├─ Stop └─ VHV設定 ↓ SetDistanceMode ↓ SetTimingBudget ↓ StartMeasurement ↓ DataReady ↓ GetRangingMeasurementData ↓ ClearInterruptset_distance_mode(距離モード変更)を呼び出した瞬間に、下記レジスタ群が裏で同時に書き換えられています。
ショートモードにした時: パルスを細かく速くし、待ち時間を短くし、余計なデータが入らないよう受け入れを厳しくする。 → だからノイズに強く、高速に測れる。 ロングモードにした時: パルスをゆったりにし、遠くから光が戻るまでじっくり待ち、受け入れる判定を広くする。 → だから4m先まで届く。VL53L1Xの初期化時にレジスタアドレス 0x002D から 0x0087 へ連続書き込みされるデフォルト設定値(ST公式Ultra Lite Driver仕様)のコメントです。
DEFAULT_CONFIGURATION = bytes([
0x00, # 0x2D: VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND (VHVキャリブレーション タイムアウト制御)
0x00, # 0x2E: VHV_CONFIG__COUNT_THRESH (VHVカウント閾値)
0x00, # 0x2F: ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS (クロストーク補正オフセット [MSB])
0x01, # 0x30: ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS (クロストーク補正オフセット [LSB])
0x02, # 0x31: ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS (クロストークX方向勾配 [MSB])
0x00, # 0x32: ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS (クロストークX方向勾配 [LSB])
0x02, # 0x33: ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS (クロストークY方向勾配 [MSB])
0x08, # 0x34: ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS (クロストークY方向勾配 [LSB])
0x00, # 0x35: ALGO__PAUSE_SOLID_ARRAY_CHECK (ソリッドアレイ検証の一時停止フラグ)
0x08, # 0x36: ALGO__PART_TO_PART_RANGE_OFFSET_MM (個体差距離補正オフセット(mm) [MSB])
0x10, # 0x37: ALGO__PART_TO_PART_RANGE_OFFSET_MM (個体差距離補正オフセット(mm) [LSB])
0x01, # 0x38: MM_CONFIG__INNER_OFFSET_MM (内部測定領域オフセット(mm) [MSB])
0x01, # 0x39: MM_CONFIG__INNER_OFFSET_MM (内部測定領域オフセット(mm) [LSB])
0x00, # 0x3A: MM_CONFIG__OUTER_OFFSET_MM (外部測定領域オフセット(mm) [MSB])
0x00, # 0x3B: MM_CONFIG__OUTER_OFFSET_MM (外部測定領域オフセット(mm) [LSB])
0x00, # 0x3C: DSS_CONFIG__TARGET_TOTAL_RATE_MCPS (DSS目標トータルリターンレート(Mcps) [MSB])
0x00, # 0x3D: DSS_CONFIG__TARGET_TOTAL_RATE_MCPS (DSS目標トータルリターンレート(Mcps) [LSB])
0xFF, # 0x3E: DEBUG__CTRL (内部デバッグ制御)
0x00, # 0x3F: TEST_MODE__CTRL (テストモード制御)
0x0F, # 0x40: CLK__CONFIG (内部メインクロック設定)
0x00, # 0x41: SYSTEM__START (システムトリガー/開始制御)
0x00, # 0x42: SYSTEM__HISTORY_CTRL (履歴バッファ制御)
0x00, # 0x43: SYSTEM__STREAM_COUNT_CTRL (ストリームカウント制御)
0x00, # 0x44: SYSTEM__SEQUENCE_CONFIG (測定シーケンス有効化設定)
0x00, # 0x45: SYSTEM__GROUPED_PARAMETER_HOLD_0 (パラメータ一括適用保持 0)
0x20, # 0x46: VHV_CONFIG__INIT (VHV初期化制御)
0x0B, # 0x47: VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND (VHVループ制限値)
0x00, # 0x48: MYCRO_TUNE (内部アナログチューニング 1)
0x00, # 0x49: MYCRO_TUNE (内部アナログチューニング 2)
0x02, # 0x4A: GLOBAL_CONFIG__SPAD_ENABLES_REF_0 (参照SPAD有効化マップ 0)
0x0A, # 0x4B: GLOBAL_CONFIG__SPAD_ENABLES_REF_1 (参照SPAD有効化マップ 1)
0x21, # 0x4C: GLOBAL_CONFIG__SPAD_ENABLES_REF_2 (参照SPAD有効化マップ 2)
0x00, # 0x4D: GLOBAL_CONFIG__SPAD_ENABLES_REF_3 (参照SPAD有効化マップ 3)
0x00, # 0x4E: GLOBAL_CONFIG__SPAD_ENABLES_REF_4 (参照SPAD有効化マップ 4)
0x05, # 0x4F: GLOBAL_CONFIG__SPAD_ENABLES_REF_5 (参照SPAD有効化マップ 5)
0x00, # 0x50: GLOBAL_CONFIG__REF_EN_START_SELECT (参照SPAD開始選択)
0x00, # 0x51: REF_SPAD_MAN__NUM_REQUESTED_REF_SPADS (要求参照SPAD数)
0x00, # 0x52: REF_SPAD_MAN__REF_LOCATION (参照SPAD配置場所)
0x00, # 0x53: ALGO__CONSISTENCY_CHECK__REFERENCE_SIGNAL_NON_KIN_INTRINSIC (参照信号整合性チェック)
0xC8, # 0x54: ALGO__CONSISTENCY_CHECK__EVENT_CONSISTENCY (イベント整合性閾値 0)
0x00, # 0x55: ALGO__CONSISTENCY_CHECK__EVENT_CONSISTENCY (イベント整合性閾値 1)
0x00, # 0x56: ALGO__CONSISTENCY_CHECK__EVENT_CONSISTENCY (イベント整合性閾値 2)
0x38, # 0x57: ALGO__CONSISTENCY_CHECK__EVENT_CONSISTENCY (イベント整合性閾値 3)
0xFF, # 0x58: ALGO__RANGE_IGNORE_THRESHOLD_MCPS (無視するノイズ距離閾値 [MSB])
0x01, # 0x59: ALGO__RANGE_IGNORE_THRESHOLD_MCPS (無視するノイズ距離閾値 [LSB])
0x00, # 0x5A: ALGO__RANGE_MIN_CLIP (最小クリッピング範囲)
0x08, # 0x5B: ALGO__CONSISTENCY_CHECK__TOTAL_RATE_PER_SPAD_MCPS (SPADあたりレート整合性 [MSB])
0x00, # 0x5C: ALGO__CONSISTENCY_CHECK__TOTAL_RATE_PER_SPAD_MCPS (SPADあたりレート整合性 [LSB])
0x00, # 0x5D: SD_CONFIG__WOI_CONFIG (受光タイミングウィンドウWOI設定 0)
0x01, # 0x5E: SD_CONFIG__WOI_CONFIG (受光タイミングウィンドウWOI設定 1)
0xCC, # 0x5F: SD_CONFIG__INITIAL_PHASE_CONFIG (初期位相設定 0)
0x0F, # 0x60: SD_CONFIG__INITIAL_PHASE_CONFIG (初期位相設定 1)
0x01, # 0x61: SYSTEM__GROUPED_PARAMETER_HOLD_0 (パラメータ更新の保留設定)
0xF1, # 0x62: SYSTEM__THRESH_HIGH (距離割り込み上限閾値 [MSB])
0x0D, # 0x63: SYSTEM__THRESH_HIGH (距離割り込み上限閾値 [LSB])
0x01, # 0x64: SYSTEM__THRESH_LOW (距離割り込み下限閾値 [MSB])
0x68, # 0x65: SYSTEM__THRESH_LOW (距離割り込み下限閾値 [LSB])
0x00, # 0x66: SYSTEM__ENABLE_CONFIG_0 (システム機能有効化フラグ 0)
0x80, # 0x67: SYSTEM__ENABLE_CONFIG_1 (システム機能有効化フラグ 1)
0x08, # 0x68: SYSTEM__SEQUENCE_CONFIG (シーケンサ動作制御)
0xB8, # 0x69: SYSTEM__HISTOGRAM_BIN (ヒストグラムBin初期構成)
0x00, # 0x6A: ROI_CONFIG__USER_ROI_CENTRE_SPAD (ユーザー設定ROIの中心SPAD番号)
0x00, # 0x6B: ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE (ユーザー設定ROIのXYサイズ)
0x00, # 0x6C: SYSTEM__CONFIG_VALID_HEADER (設定ヘッダー有効検証)
0x00, # 0x6D: SYSTEM__SEQUENCE_CONFIG (内部シーケンス追加制御)
0x0F, # 0x6E: SYSTEM__GROUPED_PARAMETER_HOLD_1 (一括パラメータ保持 1)
0x89, # 0x6F: SYSTEM__GROUPED_PARAMETER_HOLD (一括パラメータ保持トリガー)
0x00, # 0x70: SD_CONFIG__QUANTIFIER_CONFIG (シグナル検出クオンティファイア設定)
0x00, # 0x71: ROI_CONFIG__ROI_CONTROL (関心領域ROI動的制御)
0x00, # 0x72: FIRMWARE__SYSTEM_STATUS (ファームウェアステータス初期化)
0x00, # 0x73: INTERRUPT_CONFIG (割り込み信号出力設定)
0x00, # 0x74: INTERRUPT_CLEAR (割り込みクリアフラグ)
0x00, # 0x75: INTERRUPT_MODE (割り込み検出モード)
0x00, # 0x76: GPIO_HV_PAD__CTRL (GPIO高電圧パッド制御)
0x01, # 0x77: GPIO_HV_MUX__CTRL (GPIOマルチプレクサ機能選択)
0x0F, # 0x78: GPIO__TIO_HV_STATUS (GPIOピン入出力ステータス)
0x0D, # 0x79: SYSTEM__INTERRUPT_CLEAR (システム割り込み解除)
0x0E, # 0x7A: SYSTEM__MODE_START (測距モード開始/レンジング有効化)
0x0E, # 0x7B: PAD_I2C_HV__CONFIG (I2C通信パッド設定)
0x00, # 0x7C: ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS (トリミング用補正値 0x7C)
0x00, # 0x7D: ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS (トリミング用補正値 0x7D)
0x02, # 0x7E: ALGO__PAUSE_SOLID_ARRAY_CHECK (アレイ制御内部トリミング)
0xC7, # 0x7F: ALGO__PART_TO_PART_RANGE_OFFSET_MM (内部距離トリミングレジスタ)
0xFF, # 0x80: ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS (内部補正トリミングレジスタ)
0x9B, # 0x81: GLOBAL_CONFIG__SPAD_ENABLES_REF (内部SPAD調整トリミング)
0x00, # 0x82: SD_CONFIG (シグナル検出内部パラメータ 0x82)
0x00, # 0x83: SD_CONFIG (シグナル検出内部パラメータ 0x83)
0x00, # 0x84: SD_CONFIG (シグナル検出内部パラメータ 0x84)
0x01, # 0x85: SD_CONFIG (シグナル検出内部パラメータ 0x85)
0x00, # 0x86: SYSTEM__START (システム起動最終確定フラグ)
0x00, # 0x87: SYSTEM__GROUPED_PARAMETER_HOLD (一括設定バッファクリア)
])
1.役割:センサーの動作モードと内部パラメータを一括定義する:ST公式ドライバ(Ultra Lite Driver)準拠。| ステータス番号 | 意味(公式定義) | 状態の説明と対策 |
|---|---|---|
| 0 | Ranging_Valid | 測定成功。 エラーはなく、測定された距離データは高精度で信頼できます。 |
| 1 | Sigma_Fail | バラつき大(警告)。
戻ってきた光のタイミングのズレ(標準偏差)が基準を超えています。 ターゲットが動いているか、周囲のノイズが強い時に発生します。 精度は落ちていますが、参考値として距離は使えます。 |
| 2 | Signal_Fail | 光量不足(エラー)。 反射光が弱すぎて正しく測定できていません。 センサーの測定距離の限界(虚空を向いている)、またはターゲットの反射率が極端に低い(黒い服など)状態です。 |
| 4 | Out_Of_Bounds_Fail | 測定範囲外(エラー)。 内部の位相(フェーズ)測定が計算の限界を超えています。 信号が異常な反射を起こしている可能性があります。 |
| 7 | Wrap_Target_Fail | 回り込みエラー(偽反射)。 センサーの最大測定距離(約4m)よりもさらに遠くにある物体から、遅れて反射光が戻ってきた場合に発生します。 実際より手前にあるように誤認している状態です。 |
| 8 | Wrap_Target_Fail | ハードウェアの異常。 センサー自体に問題が起きている可能性があります。 実際より手前にあるように誤認している状態です。 |
| 9 | 「エラー(失敗)」ではなく、「測定完了(正常)」を意味しています。 | |
| 255 | キャッチオール | これはデータ準備が未完了のタイミングで読み込んだ時に、返る場合がある。 |