UMEHOSHI ITA TOP PAGE 　　　COMPUTER SHIEN LAB

BNO055使用の9軸センサージュールをMicroPythonで操作

• ESP32の SCL (IO22) → BNO055デバイスの SCL
• ESP32の SDA (IO21) → BNO055デバイスの SDA

data = i2c.readfrom_mem(0x28, 0x1A, 6)

import time
from machine import I2C, Pin

debug_flag=True # デバック用debug_print関数の出力を行うかの有無指定

def debug_print(*args, **kwargs):
    if debug_flag : print(*args, **kwargs)

FILENAME = "bno_offsets.bin" # キャブリレーションの補正データを記憶するファイル
calibration_flag = False; # これがTrueであればキャブリレーションを行う。

i2c = I2C( 0, scl=Pin(22), sda=Pin(21), freq=400000)
debug_print("I2C devices:", [hex(x) for x in i2c.scan()])
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

# --- 初期化手順 ---
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}")

# yaw, roll, pitch の測定と表示の繰り返し
for _ in range(20):
    yaw, roll, pitch = read_euler()
    debug_print("Yaw={:.2f}, Roll={:.2f}, Pitch={:.2f}".format(yaw, roll, pitch))
    time.sleep(0.5)

R:\drone>mpremote connect COM3 run bno055_uld.py
I2C devices: ['0x28', '0x29']
The existing calibration offset has been loaded and applied.
calibration_succ:True
Yaw= 351.69, Roll=   0.25, Pitch=  -9.50
Yaw=  13.31, Roll=   3.25, Pitch=  -7.38
Yaw=  36.69, Roll=   2.69, Pitch=  -7.25
Yaw=  32.56, Roll=   3.25, Pitch=  -7.69
Yaw=  15.81, Roll=   1.31, Pitch=  -8.63
Yaw= 353.50, Roll=  -2.44, Pitch= -11.75
Yaw= 343.75, Roll=  -6.19, Pitch=  -8.31
Yaw= 342.38, Roll=  10.94, Pitch=  -6.69
Yaw= 341.06, Roll=  34.06, Pitch=  -2.94
Yaw=  70.75, Roll=  49.38, Pitch=  -3.94
Yaw= 341.00, Roll=  31.88, Pitch=  -5.38
Yaw= 342.50, Roll=   8.25, Pitch=  -3.81
Yaw= 346.25, Roll= -10.25, Pitch=  -4.06
Yaw= 343.63, Roll=  -8.75, Pitch=   2.44
Yaw= 335.31, Roll=  -4.63, Pitch=  16.38
Yaw= 334.31, Roll=  -1.19, Pitch=  34.81
Yaw= 336.44, Roll=   1.56, Pitch=  45.63
Yaw= 335.50, Roll=   1.50, Pitch=  30.19
Yaw= 335.63, Roll=  -0.25, Pitch=   8.00
Yaw= 339.50, Roll=  -1.00, Pitch= -11.31

R:\drone>