UMEHOSHI ITA TOP PAGE COMPUTER SHIEN LAB
「UMEHOSHI ITA」にESP32-WROOM-32E-N16を、「UMEHOSHI ITA」へ取り付ける例を示しています。
（「UMEHOSHI ITA」は一部の端子だけをCN11とCN12を介して引き出し、そこに別途の自作基板を取り付けて、少しだけ利用することを想定しています。）
|
|
 |
 両面スルーホール2.54mm×2mmピッチ基板に CN11とCN12の2mmピッチソケットを付けています。 |
| CN12 | ESP32-WROOM-32D端子名 | PIC32MX端子No:名 | - | CN11 | ESP32-WROOM-32D端子名 | PIC32MX端子No:名 |
|---|---|---|---|---|---|---|
| CN12-1 | 2: 3V3 | 13: VDD | CN11-10 | 11: IO26 | ||
| CN12-2 | 35:TXD0 出力 | 12: RX (RA4) 入力 | CN11-9 | 10: IO25 | ||
| CN12-3 | 34:RXD0 入力 | 11: TX (RB4) 出力 | CN11-8 | 37: IO23 | ||
| CN12-4 | 6: IO34 | CN11-7 | 36: IO22(SCL)(RTS) | |||
| CN12-5 | ジャンパー配線でSW3へ | CN11-6 | JMP10-OPEN-U19-23: IO15 | 18: RTS (RB9) 出力 | ||
| CN12-6 | 7: IO35 | CN11-5 | 33:IO21 (SDA) | |||
| CN12-7 | 8: IO32 | CN11-4 | 5: SENSOR_VN | |||
| CN12-8 | 9: IO33 | CN11-3 | 4: SENSER_VP | |||
| CN12-9 | 12: IO27 | CN11-2 | JUM11-OPEN-16: IO13 | 17: CTS (RB8) 入力 | ||
| CN12-10 | GND | 19: Vss | CN11-1 | 31: IO19(CTS) |
from machine import Pin, I2C
i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=100000)
print("--- I2C Bus Scan Start ---")
# I2Cバス上のデバイスをスキャン
devices = i2c.scan()
if len(devices) == 0:
print("No I2C devices were found.")
else:
for device in devices:
print(f"device address: {hex(device)}")
print("That is all.")
上記をmpremoteを使って実行する例を下記に示します。R:\drone>mpremote connect COM3 run scanI2C.py --- I2C Bus Scan Start --- device address: 0x28 device address: 0x29 That is all. R:\drone>0x29番地のアドレスを持つデバイスが、VL53L1X使用の距離センサーで、
import time
from machine import I2C, Pin
debug_flag=False # デバック用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つの距離モードがあるが、ここでは２つを指定できる指定にした
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()])
# ========================= BNO005関連の各種定義 =====================================================
FILENAME = "bno_offsets.bin" # キャブリレーションの補正データを記憶するファイル
calibration_flag = False; # これがTrueであればキャブリレーションを行う。
ADDR_BNO005 = 0x28 # I2CにおけるBNO005デバイスのアドレス
# レジスタアドレス定義
REG_OPR_MODE = 0x3D #動作モードを設定するレジスタ
REG_EUL_DATA = 0x1A # オイラー角データ開始レジスタ (Heading, Roll, Pitch)
REG_CALIB_STAT = 0x35 # キャリブレーションステータスレジスタ
REG_OFFSET_START = 0x55 # オフセットデータ開始アドレス (22バイト)
# 動作モード定義
MODE_CONFIG = 0x00 # CONFIGMODE:
MODE_NDOF = 0x0C # 9軸フュージョンモード「設定・キャリブレーション・自己診断を行うためのシステム待機モード」
# BNO状態変更(キャブリレーションやフュージョンモードへの切り替え用)
def set_mode(mode):
i2c.writeto_mem(ADDR_BNO005, REG_OPR_MODE, bytes([mode]))
time.sleep_ms(200)
# キャブリレーション情報取得
def read_calibration():
cal_data = i2c.readfrom_mem(ADDR_BNO005, REG_CALIB_STAT, 1)[0]
return (
(cal_data >> 6) & 0x03, # sys:System（システム全体）
(cal_data >> 4) & 0x03, # gyro:Gyroscope（ジャイロ）
(cal_data >> 2) & 0x03, # accel:Accelerometer（加速度）
cal_data & 0x03, # mag:Magnetometer（磁気）
)
# キャブリレーションしてファイル保存
def save_offsets_by_calibration():
set_mode(MODE_NDOF) # キャブリレーションはNDOFで行う
for n in range(1000):
sys, gyro, accel, mag = read_calibration()
debug_print(f"Calib -> SYS:{sys} GYR:{gyro} ACC:{accel} MAG:{mag}")
time.sleep(0.1)
if sys == 3 and gyro == 3 and accel == 3 and mag == 3:
break# 初回全コンポーネント較正完了（すべて3）のタイミング
else: return False # 校正失敗（途中の場合を含む）
#オフセット（22バイト）を取得してファイルに保存
offsets = i2c.readfrom_mem(ADDR_BNO005, REG_OFFSET_START, 22)
with open(FILENAME, "wb") as f: f.write(offsets)
return False # 校正成功して、オフセットデータ（レジスタ 0x55?0x6A の22バイト） を保存
# ファイルからキャブリレーションの補正データを読み出してBNO055に書き込む
def load_offsets():
try:
with open(FILENAME, "rb") as f:
offsets = f.read()
if len(offsets) == 22:
set_mode(MODE_CONFIG)
i2c.writeto_mem(ADDR_BNO005, REG_OFFSET_START, offsets)
debug_print("The existing calibration offset has been loaded and applied.")
return True # 既存のキャリブレーションオフセットが読み込まれ、適用されました。
#
except OSError: # 保存されたオフセットファイルが見つかりません。新規作成が必要です。
debug_print("The saved offset file could not be found. A new one needs to be created.")
return False
# レジスタ定義（オイラー角）
EULER_H_LSB = 0x1A # Heading (Yaw)
EULER_R_LSB = 0x1C # Roll
EULER_P_LSB = 0x1E # Pitch
def read16(reg): # 各オイラー角の取得用関数
data = i2c.readfrom_mem(ADDR_BNO005, reg, 2)
value= data[0] | (data[1] << 8)# データはリトルエンディアン形式（下位→上位の順）
if value >= 0x8000: value -= 0x10000 # 符号ありとして使う
return value
# yaw roll pitch 取得
def read_euler():
# 16bit raw → degree に変換（1LSB = 1/16度）
yaw = read16(EULER_H_LSB) / 16.0 # 方位角（北基準のYAW） heading
roll = read16(EULER_R_LSB) / 16.0 # ロール角（左右の傾き）
pitch = read16(EULER_P_LSB) / 16.0 # ピッチ角（前後の傾き）
return yaw, roll, pitch
# --- 9軸センサー初期化 ---
set_mode(MODE_CONFIG)
calibration_succ = load_offsets()# 保存されたオフセットがあれば適用。
if calibration_flag or not calibration_succ:
calibration_succ = save_offsets_by_calibration()# キャブリレーションして保存
set_mode(MODE_NDOF)
debug_print(f"calibration_succ:{calibration_succ}")
# --- 距離センサー初期化 ---
tof = VL53L1X( i2c, address=0x29)
PCとUMEHOSHI ITAをUSBケーブルで接続し、
SW1(赤)リセットと、
SW2(白)を押して、
SW1(赤)だけ離した後に３秒待ってLEDが消えた後に、SW2(白)スイッチを離します。mpremote connect COM3 fs cp sensor.py :sensor.py上記で、: 直後のファイル名がマイコン側であることを意味します。
import sensor
import time
# ----- Initialize Long mode 20 ms timing budget ------------------------
sensor.tof.init( distance_mode=sensor.VL53L1X.DISTANCE_MODE_LONG, timing_budget_ms=20 )
start = time.ticks_ms()
ss=[]
n=0
while n < 500:
if sensor.tof.data_ready():
result = sensor.tof.get_ranging_measurement_data()
sensor.tof.clear_interrupt()
distance_mm=result["distance_mm"]
else: distance_mm = 0
period=time.ticks_diff( time.ticks_ms(), start )
if period < 20:
time.sleep_ms(1)
continue
start = time.ticks_ms()
n+=1
yaw, roll, pitch = sensor.read_euler()
s=f"distance_mm={distance_mm:8.2f}, Yaw={yaw:.2f}, Roll={roll:.2f}, Pitch={pitch:.2f}" f", period={period}"
ss.append( s )
print(s)
#for s in ss: print(s)
MicroPythonにて、VL53L1Xによる距離測定の周期は約100ms、
BNO055による9軸センサーの姿勢（Yaw, Roll, Pitch）測定の周期は約20msでした。R:\drone>mpremote connect COM3 run sensor_test.py distance_mm= 0.00, Yaw=19.00, Roll=0.06, Pitch=4.69, period=20 distance_mm= 0.00, Yaw=19.00, Roll=0.06, Pitch=4.69, period=20 distance_mm= 0.00, Yaw=19.00, Roll=0.06, Pitch=4.69, period=20 distance_mm= 0.00, Yaw=19.00, Roll=0.06, Pitch=4.69, period=20 distance_mm= 0.00, Yaw=19.00, Roll=0.06, Pitch=4.69, period=22 distance_mm= 779.00, Yaw=19.00, Roll=0.06, Pitch=4.69, period=21 distance_mm= 0.00, Yaw=19.00, Roll=0.06, Pitch=4.69, period=20 distance_mm= 0.00, Yaw=19.00, Roll=0.06, Pitch=4.69, period=20 distance_mm= 0.00, Yaw=19.00, Roll=0.06, Pitch=4.69, period=20 distance_mm= 0.00, Yaw=19.00, Roll=0.06, Pitch=4.69, period=20 distance_mm= 783.00, Yaw=19.00, Roll=0.06, Pitch=4.69, period=22 distance_mm= 0.00, Yaw=19.00, Roll=0.06, Pitch=4.69, period=20 ・・・・・・・・ R:\drone>