はじめに
今回は Raspberry Pi で BNO055（9軸センサ）を使ったときに遭遇した
「Yaw（ヨー角）だけがドリフトしていく」問題と、
その原因である 動作モードの扱い・キャリブレーション処理の正しい方法
についてまとめます。
なぜ Yaw だけがドリフトするのか？
BNO055 を何も考えずにchatGPTで作ってもらったコードで起動したところ、
IMUPLUS（磁気なし） で動作してしまいました。
IMUPLUS は以下のような性質があります：
- 加速度＋ジャイロのみを使用
- 地磁気（コンパス）を使わない
- Yaw はジャイロの積分で求めるため、時間と共に必ずずれる
その結果、机に置いている間は静かですが、
少しゆすると Yaw だけがじわじわ回転していく という挙動になります。
Yaw を安定させるには NDOF モード（磁気・加速度・ジャイロ全部使用） が必須です。
Raspberry Pi でモードが反映されない？
setMode() が正しく機能していない罠
最初に作ったコードは、setMode() が CONFIG モードを経由しない実装になってしまっていました
BNO055 はデータシート上、
- OPR_MODE を書き換えるときは 必ず CONFIG モードで行う
- 書き換え後 最小 19～30ms の wait が必要
と定義されています。
正しいモード切り替え方法
そこで、BNO055 の仕様通り
- CONFIG モードへ移行
- 30ms 以上待機
- OPR_MODE を書き換え
- 30ms 以上待機
という処理を行う safeSetMode を作成しました。
def safeSetMode(self, mode):
self.setMode(BNO055.OPERATION_MODE_CONFIG)
time.sleep(0.03)
self.setMode(mode)
time.sleep(0.03)
この SafeMode 経由でモード変更を行うことで、
- MAGONLY
- IMUPLUS
- NDOF
いずれのモードも Raspberry Pi で安定して切り替えられるようになりました。
キャリブレーションについて
BNO055 を NDOF のようなフュージョンモードで使用する場合、
正確なキャリブレーションがとても重要です。
BNO055 は出荷時にもある程度のキャリブレーションは行われていますが、
地磁気環境・重力方向・温度・取り付け姿勢が変われば内部補正値は常に変化します。
そのため、初回の使用時やセンサーの取り付け環境が変わったときには必ずキャリブレーションを行う必要があります。
また BNO055 は特徴的で、キャリブレーションが完了（各ステータスが 3/3）しても、その後も裏で自動的に補正を続けます。
- 起動直後 → 補正値がゼロで不安定になりがち
- しばらく動かすと → 内部アルゴリズムが勝手に補正を進める
- sys（システム）のキャリブ状態が 3→0 に戻ることもある（異常ではなく再調整）
という動作が仕様です。
そのため今回は、
- calibrator.py で一度キャリブレーションを実行し、
- そのときのオフセット値を JSON に保存し、
- 起動時にその値を読み込んで「安定した初期状態」を作る
という方法を採用しています。
これはあくまで 起動直後の不安定な時間を短くするためであり、
その後の細かな補正は BNO055 が内部で自動的に進めてくれます。
キャリブレーション用スクリプト
BNO055 のキャリブレーションは、
- GYRO（静止）
- ACCEL（6方向）
- MAG（8の字軌道）
を行い、SYS/GYRO/ACC/MAG がすべて 3/3 になると完了です。
特にポイントなのは、
- キャリブレーションが完了した瞬間、オフセット値は自動的に内部レジスタへ書き込まれる
- 補正値を読み込むときは、CONFIGモードに入れる
という点です。
キャリブレーション結果は 0x55〜0x6A の 22 バイトに保存されます。
ただし、電源を切ると値は消えるので、
このオフセット値を JSON として保存し、
次回起動時に CONFIG モード中に書き戻す運用にしました。
下記にキャリブレーション用のスクリプトを示します
import BNO055
import time
import json
import os
# キャリブレーションデータを保存するファイル名
CALIBRATION_FILE = 'bno055_calibration.json'
def get_bno_calibration_status(bno):
"""BNO055からキャリブレーションステータスを取得し、表示する"""
# System, Gyro, Accel, Mag の順に 0〜3 の値を取得
sys, gyro, acc, mag = bno.getCalibration()
# 3が最高レベルのキャリブレーション完了を示す
status_str = (
f"SYS:{sys}/3 | GYRO:{gyro}/3 | ACCEL:{acc}/3 | MAG:{mag}/3"
)
return sys, gyro, acc, mag, status_str
def read_calibration_offsets(bno):
"""
現在のオフセットレジスタ(0x55〜0x6A)を22バイト読み込んでリストで返す
"""
# 今のモードを退避
mode_back = bno._mode
# CONFIG モードに一時的に切り替え
bno.setMode(bno.OPERATION_MODE_CONFIG)
time.sleep(0.03) # 20〜30ms くらい待つ
data = bno.readBytes(bno.ACCEL_OFFSET_X_LSB_ADDR, 22)
# 元のモードに戻す
bno.setMode(mode_back)
time.sleep(0.03)
return list(data)
def print_offset_diff(before, after):
"""
キャリブ前後のオフセットを比較して表示する
"""
print("\n=== キャリブレーション結果 (オフセット比較) ===")
print(f" バイト数: {len(before)}")
print("\n [0] 初期オフセット:")
print(f" {before}")
print("\n [1] キャリブ後オフセット:")
print(f" {after}")
print("\n [2] 変更があったバイトのみ差分表示:")
for i, (b, a) in enumerate(zip(before, after)):
if b != a:
print(f" index {i:2d}: {b:6d} -> {a:6d}")
print("=== 比較終了 ===\n")
def save_calibration_data(calibration_data):
"""キャリブレーションデータをファイルに保存する"""
print(f"-> キャリブレーションデータを {CALIBRATION_FILE} に保存中...")
# BNO055のオフセットレジスタからデータを読み出す
data = calibration_data
# Pythonのリストに変換してJSONファイルに保存
with open(CALIBRATION_FILE, 'w') as f:
json.dump(list(data), f)
print("-> 保存完了。")
def main():
# BNO055インスタンスの作成 (NDOFモードで開始)
bno = BNO055.BNO055()
if not bno.begin(mode=bno.OPERATION_MODE_NDOF, calib=True):
#if not bno.begin(mode=bno.OPERATION_MODE_MAGONLY):
raise RuntimeError('BNO055の初期化に失敗しました')
bno.start()
print("=== 起動直後のオフセットを読み込みます ===")
initial_offsets = read_calibration_offsets(bno)
print(f"初期オフセット(raw 22バイト):\n {initial_offsets}\n")
print("--- BNO055 キャリブレーション開始 ---")
print("目標: 全てのステータスを 3/3 にする (特に MAG)")
print("GYROは機体を完全に停止、ACCELは6つの面（前後、上下、左右）を下にして数秒停止、MAGは八の字を描くようにすべての向きを変えることによりステータスがあがります")
is_calibrated = False
# --- 地磁気監視用の変数 (新規) ---
mag_min = [float('inf'), float('inf'), float('inf')]
mag_max = [float('-inf'), float('-inf'), float('-inf')]
try:
while not is_calibrated:
# BNO055インスタンスの current_data から直接読み取る
data = bno.current_data
print(data)
if data is not None and 'mag' in data:
raw_mag = data['mag']
#print(raw_mag)
# 地磁気の最小値と最大値を更新
for i in range(3):
mag_min[i] = min(mag_min[i], raw_mag[i])
mag_max[i] = max(mag_max[i], raw_mag[i])
# 監視情報を表示
mag_range_str = (
f" | Range X:{mag_max[0] - mag_min[0]:.0f}"
f" Y:{mag_max[1] - mag_min[1]:.0f}"
f" Z:{mag_max[2] - mag_min[2]:.0f}"
)
else:
mag_range_str = ""
# センサーの状態を取得し表示
sys, gyro, acc, mag, status_str = get_bno_calibration_status(bno)
print(sys, gyro, acc, mag)
# コンソールに状態を表示
print(f"Magのステータス: {mag_range_str}")
print(f"現在のステータス: {status_str}")
if sys == 3 and mag == 3 and acc == 3: # 最重要: システムと地磁気が完了したら終了
is_calibrated = True
print("\n キャリブレーション完了！")
# 地磁気センサーの校正方法のガイダンス
if mag < 3:
print("MAG (地磁気) が未完了です。機体を空中で『8の字』にゆっくり動かしてください。")
time.sleep(0.5)
final_offsets = read_calibration_offsets(bno)
print_offset_diff(initial_offsets, final_offsets)
# 完了後、キャリブレーションデータを保存
save_calibration_data(final_offsets)
except KeyboardInterrupt:
print("\nキャリブレーションを中断します。")
finally:
bno.stop()
if __name__ == '__main__':
main()
これを起動させて、センサーを静止させておくと、GYRO:3/3になります
次に、前後、上下、左右の6面を上に向けて静止させたり、くるくるいろんな面を回します
そうすると、SYSとACCELとMAGが3/3になります
少し時間がかかったり、突然SYSが0に戻ったりしてしまいます
どうしてもうまく進まなければ、再起動したりして試してみます
キャリブレーション比較の結果
キャリブレーション前後でレジスタを読み出して比較したところ、
Magのステータス: | Range X:238 Y:223 Z:149
現在のステータス: SYS:3/3 | GYRO:3/3 | ACCEL:3/3 | MAG:3/3
キャリブレーション完了！
=== キャリブレーション結果 (オフセット比較) ===
バイト数: 22
[0] 初期オフセット:
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 232, 3, 0, 0]
[1] キャリブ後オフセット:
[247, 255, 47, 0, 48, 0, 127, 0, 197, 255, 31, 0, 255, 255, 254, 255, 0, 0, 232, 3, 207, 7]
[2] 変更があったバイトのみ差分表示:
index 0: 0 -> 247
index 1: 0 -> 255
index 2: 0 -> 47
index 4: 0 -> 48
index 6: 0 -> 127
index 8: 0 -> 197
index 9: 0 -> 255
index 10: 0 -> 31
index 12: 0 -> 255
index 13: 0 -> 255
index 14: 0 -> 254
index 15: 0 -> 255
index 20: 0 -> 207
index 21: 0 -> 7
=== 比較終了 ===
-> キャリブレーションデータを bno055_calibration.json に保存中...
-> 保存完了。
- ACCEL
- MAG
- GYRO
のオフセット値が大きく更新されており、
BNO055 内でキャリブレーション処理が正しく行われていることを確認できました。
特に MAG の値は ±数百 LSB 程度の変化があり、
地磁気センサの初期誤差が大きく補正されています。
- MAG の “範囲（max-min）” が大きいほど、現場の磁場環境を十分に走査できている
- 実測では Range X:238, Y:190, Z:149 → 妥当な磁場振れ幅
という結果になりました。
BNO055の動作確認
それではキャリブレーションも完了したので、動作確認をしてみます。
下記にスクリプト全文を示します。
import smbus
import time
import struct
import threading
import json
class BNO055:
BNO055_ADDRESS_A = 0x28
BNO055_ADDRESS_B = 0x29
BNO055_ID = 0xA0
# Power mode settings
POWER_MODE_NORMAL = 0X00
POWER_MODE_LOWPOWER = 0X01
POWER_MODE_SUSPEND = 0X02
# Operation mode settings
OPERATION_MODE_CONFIG = 0X00
OPERATION_MODE_ACCONLY = 0X01
OPERATION_MODE_MAGONLY = 0X02
OPERATION_MODE_GYRONLY = 0X03
OPERATION_MODE_ACCMAG = 0X04
OPERATION_MODE_ACCGYRO = 0X05
OPERATION_MODE_MAGGYRO = 0X06
OPERATION_MODE_AMG = 0X07
OPERATION_MODE_IMUPLUS = 0X08
OPERATION_MODE_COMPASS = 0X09
OPERATION_MODE_M4G = 0X0A
OPERATION_MODE_NDOF_FMC_OFF = 0X0B
OPERATION_MODE_NDOF = 0X0C
# Output vector type
VECTOR_ACCELEROMETER = 0x08
VECTOR_MAGNETOMETER = 0x0E
VECTOR_GYROSCOPE = 0x14
VECTOR_EULER = 0x1A
VECTOR_LINEARACCEL = 0x28
VECTOR_GRAVITY = 0x2E
# REGISTER DEFINITION START
BNO055_PAGE_ID_ADDR = 0X07
BNO055_CHIP_ID_ADDR = 0x00
BNO055_ACCEL_REV_ID_ADDR = 0x01
BNO055_MAG_REV_ID_ADDR = 0x02
BNO055_GYRO_REV_ID_ADDR = 0x03
BNO055_SW_REV_ID_LSB_ADDR = 0x04
BNO055_SW_REV_ID_MSB_ADDR = 0x05
BNO055_BL_REV_ID_ADDR = 0X06
# Accel data register
BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08
BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09
BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A
BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B
BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C
BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D
# Mag data register
BNO055_MAG_DATA_X_LSB_ADDR = 0X0E
BNO055_MAG_DATA_X_MSB_ADDR = 0X0F
BNO055_MAG_DATA_Y_LSB_ADDR = 0X10
BNO055_MAG_DATA_Y_MSB_ADDR = 0X11
BNO055_MAG_DATA_Z_LSB_ADDR = 0X12
BNO055_MAG_DATA_Z_MSB_ADDR = 0X13
# Gyro data registers
BNO055_GYRO_DATA_X_LSB_ADDR = 0X14
BNO055_GYRO_DATA_X_MSB_ADDR = 0X15
BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16
BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17
BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18
BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19
# Euler data registers
BNO055_EULER_H_LSB_ADDR = 0X1A
BNO055_EULER_H_MSB_ADDR = 0X1B
BNO055_EULER_R_LSB_ADDR = 0X1C
BNO055_EULER_R_MSB_ADDR = 0X1D
BNO055_EULER_P_LSB_ADDR = 0X1E
BNO055_EULER_P_MSB_ADDR = 0X1F
# Quaternion data registers
BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20
BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21
BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22
BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23
BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24
BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25
BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26
BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27
# Linear acceleration data registers
BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28
BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29
BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A
BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B
BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C
BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D
# Gravity data registers
BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E
BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F
BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30
BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31
BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32
BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33
# Temperature data register
BNO055_TEMP_ADDR = 0X34
# Status registers
BNO055_CALIB_STAT_ADDR = 0X35
BNO055_SELFTEST_RESULT_ADDR = 0X36
BNO055_INTR_STAT_ADDR = 0X37
BNO055_SYS_CLK_STAT_ADDR = 0X38
BNO055_SYS_STAT_ADDR = 0X39
BNO055_SYS_ERR_ADDR = 0X3A
# Unit selection register
BNO055_UNIT_SEL_ADDR = 0X3B
BNO055_DATA_SELECT_ADDR = 0X3C
# Mode registers
BNO055_OPR_MODE_ADDR = 0X3D
BNO055_PWR_MODE_ADDR = 0X3E
BNO055_SYS_TRIGGER_ADDR = 0X3F
BNO055_TEMP_SOURCE_ADDR = 0X40
# Axis remap registers
BNO055_AXIS_MAP_CONFIG_ADDR = 0X41
BNO055_AXIS_MAP_SIGN_ADDR = 0X42
# SIC registers
BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43
BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44
BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45
BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46
BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47
BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48
BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49
BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A
BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B
BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C
BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D
BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E
BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F
BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50
BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51
BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52
BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53
BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54
# Accelerometer Offset registers
ACCEL_OFFSET_X_LSB_ADDR = 0X55
ACCEL_OFFSET_X_MSB_ADDR = 0X56
ACCEL_OFFSET_Y_LSB_ADDR = 0X57
ACCEL_OFFSET_Y_MSB_ADDR = 0X58
ACCEL_OFFSET_Z_LSB_ADDR = 0X59
ACCEL_OFFSET_Z_MSB_ADDR = 0X5A
# Magnetometer Offset registers
MAG_OFFSET_X_LSB_ADDR = 0X5B
MAG_OFFSET_X_MSB_ADDR = 0X5C
MAG_OFFSET_Y_LSB_ADDR = 0X5D
MAG_OFFSET_Y_MSB_ADDR = 0X5E
MAG_OFFSET_Z_LSB_ADDR = 0X5F
MAG_OFFSET_Z_MSB_ADDR = 0X60
# Gyroscope Offset registers
GYRO_OFFSET_X_LSB_ADDR = 0X61
GYRO_OFFSET_X_MSB_ADDR = 0X62
GYRO_OFFSET_Y_LSB_ADDR = 0X63
GYRO_OFFSET_Y_MSB_ADDR = 0X64
GYRO_OFFSET_Z_LSB_ADDR = 0X65
GYRO_OFFSET_Z_MSB_ADDR = 0X66
# Radius registers
ACCEL_RADIUS_LSB_ADDR = 0X67
ACCEL_RADIUS_MSB_ADDR = 0X68
MAG_RADIUS_LSB_ADDR = 0X69
MAG_RADIUS_MSB_ADDR = 0X6A
# REGISTER DEFINITION END
_AXIS_MAP = {
'X': 0x00, 'Y': 0x01, 'Z': 0x02,
'-X': 0x04, '-Y': 0x08, '-Z': 0x10 # 符号付きのビットフラグを仮定義
}
# 軸マッピング値 (レジスタ値)
_AXIS_MAP_CONFIG = {
'X': 0x00, 'Y': 0x01, 'Z': 0x02
}
# 軸符号反転ビット (レジスタ値)
_AXIS_MAP_SIGN = {
'X': 0x00, '-X': 0x01,
'Y': 0x00, '-Y': 0x02,
'Z': 0x00, '-Z': 0x04
}
def __init__(self, address=0x28, mount_axis_x='X', mount_axis_y='Y'):
self._address = address
self._bus = smbus.SMBus(1)
self.running = False
self.current_data = None
self._mount_x = mount_axis_x
self._mount_y = mount_axis_y
def start(self):
if not hasattr(self, "_mode"):
raise RuntimeError("start() を呼ぶ前に begin() を呼んでください")
self.running = True
# threading.Thread(target=self.run).start()
self.thread = threading.Thread(target=self.run)
self.thread.daemon = True # この行を追加
self.thread.start()
def run(self):
while self.running:
self.current_data = self.get_all_data()
time.sleep(0.01)
def stop(self):
self.running = False
self.thread.join() # スレッドが終了するまで待機
def load_calibration_data(self, data):
"""保存されたキャリブレーションデータをBNO055のレジスタに書き込む"""
# 0x55 (ACCEL_OFFSET_X_LSB_ADDR) から 22バイトのデータを書き込む
self.writeBytes(BNO055.ACCEL_OFFSET_X_LSB_ADDR, data)
print("BNO055: キャリブレーションデータがロードされました。")
def get_all_data(self):
euler = self.getVector(BNO055.VECTOR_EULER)
accel = self.getVector(BNO055.VECTOR_ACCELEROMETER)
gyro = self.getVector(BNO055.VECTOR_GYROSCOPE)
mag = self.getVector(BNO055.VECTOR_MAGNETOMETER)
quat = self.getQuat()
return {'euler': euler, 'accel': accel, 'gyro': gyro, 'mag': mag, 'quat': quat}
def _get_remap_config(self):
"""インスタンス設定 (_mount_x, _mount_y) からレジスタ値を計算する"""
# マッピング設定 (X, Y, Z の順番を定義)
# BNO055のAXIS_MAP_CONFIGは、X-Y-Zの順に2ビットずつ、合計6ビットで定義されます。
# Z軸は残りの軸に自動的に割り当てられます。
# 符号を除いた軸名を取得
x_axis_name = self._mount_x.replace('-', '')
y_axis_name = self._mount_y.replace('-', '')
# Z軸を決定 (X, Yが使用されていない軸)
used_axes = {x_axis_name, y_axis_name}
all_axes = {'X', 'Y', 'Z'}
z_axis_name = list(all_axes - used_axes)[0]
# マッピング値を計算 (Z << 4 | Y << 2 | X)
map_config = (
self._AXIS_MAP_CONFIG[z_axis_name] << 4 |
self._AXIS_MAP_CONFIG[y_axis_name] << 2 |
self._AXIS_MAP_CONFIG[x_axis_name]
)
# 符号設定 (符号ビットをOR演算で組み合わせる)
map_sign = 0
if self._mount_x.startswith('-'): map_sign |= self._AXIS_MAP_SIGN['-X']
if self._mount_y.startswith('-'): map_sign |= self._AXIS_MAP_SIGN['-Y']
if self._mount_x.startswith('Z'): # Z軸の符号反転が必要な場合も考慮
if self._mount_x.startswith('-'): map_sign |= self._AXIS_MAP_SIGN['-Z']
# (Z軸の符号は、X, Yで使われなかった軸の符号として計算する必要があるため複雑ですが、
# ここではX, Y軸の符号反転のみを扱います。)
return (map_config, map_sign)
def begin(self, mode=None, cal_file=None, calib=False):
if mode is None: mode = BNO055.OPERATION_MODE_IMUPLUS
self._mode = mode
if cal_file is None: cal_file = 'bno055_calibration.json'
# Open I2C bus
self._bus = smbus.SMBus(1)
# Make sure we have the right device
if self.readBytes(BNO055.BNO055_CHIP_ID_ADDR)[0] != BNO055.BNO055_ID:
time.sleep(1) # Wait for the device to boot up
if self.readBytes(BNO055.BNO055_CHIP_ID_ADDR)[0] != BNO055.BNO055_ID:
return False
# Switch to config mode
self.setMode(BNO055.OPERATION_MODE_CONFIG)
time.sleep(0.03)
# Trigger a reset and wait for the device to boot up again
self.writeBytes(BNO055.BNO055_SYS_TRIGGER_ADDR, [0x20])
time.sleep(1)
while self.readBytes(BNO055.BNO055_CHIP_ID_ADDR)[0] != BNO055.BNO055_ID:
time.sleep(0.01)
time.sleep(0.05)
# Set to normal power mode
self.writeBytes(BNO055.BNO055_PWR_MODE_ADDR, [BNO055.POWER_MODE_NORMAL])
time.sleep(0.01)
self.writeBytes(BNO055.BNO055_PAGE_ID_ADDR, [0])
self.writeBytes(BNO055.BNO055_SYS_TRIGGER_ADDR, [0])
time.sleep(0.01)
# キャリブレーションモード以外での起動時に、補正値を読み込む
if not calib:
try:
# ファイルの存在を確認し、読み込み
with open(cal_file, 'r') as f:
cal_data = json.load(f)
# 外部に定義されたメソッドを使ってデータを書き込む
self.load_calibration_data(cal_data)
print(f"[BNO055] キャリブレーションデータ '{cal_file}' を適用しました。")
except FileNotFoundError:
print(
f"[BNO055] Warning: キャリブレーションファイル '{cal_file}' が見つかりません。補正なしで起動します。")
except Exception as e:
# ファイル形式の破損など
print(f"[BNO055] Error: キャリブレーションデータのロード中にエラーが発生しました: {e}")
# --- BNO055 Axis Remap 設定の適用 ---
# 1. Axis Remap の設定値を計算
map_config, map_sign = self._get_remap_config()
# 2. レジスタに書き込む
self.writeBytes(BNO055.BNO055_AXIS_MAP_CONFIG_ADDR, [map_config])
self.writeBytes(BNO055.BNO055_AXIS_MAP_SIGN_ADDR, [map_sign])
time.sleep(0.01)
# Set the requested mode
self.setMode(mode)
time.sleep(0.02)
return True
def setMode(self, mode):
self._mode = mode
self.writeBytes(BNO055.BNO055_OPR_MODE_ADDR, [self._mode])
time.sleep(0.03)
def getMode(self):
"""現在のOPR_MODEを取得（0x00〜0x0C）"""
data = self.readBytes(BNO055.BNO055_OPR_MODE_ADDR, 1)[0]
time.sleep(0.02)
return data
def safeSetMode(self, mode):
self.setMode(BNO055.OPERATION_MODE_CONFIG)
time.sleep(0.03)
self.setMode(mode)
time.sleep(0.03)
def setExternalCrystalUse(self, useExternalCrystal=True):
prevMode = self._mode
self.setMode(BNO055.OPERATION_MODE_CONFIG)
time.sleep(0.025)
self.writeBytes(BNO055.BNO055_PAGE_ID_ADDR, [0])
self.writeBytes(BNO055.BNO055_SYS_TRIGGER_ADDR, [0x80] if useExternalCrystal else [0])
time.sleep(0.01)
self.setMode(prevMode)
time.sleep(0.02)
def getSystemStatus(self):
self.writeBytes(BNO055.BNO055_PAGE_ID_ADDR, [0])
(sys_stat, sys_err) = self.readBytes(BNO055.BNO055_SYS_STAT_ADDR, 2)
self_test = self.readBytes(BNO055.BNO055_SELFTEST_RESULT_ADDR)[0]
return (sys_stat, self_test, sys_err)
def getRevInfo(self):
(accel_rev, mag_rev, gyro_rev) = self.readBytes(BNO055.BNO055_ACCEL_REV_ID_ADDR, 3)
sw_rev = self.readBytes(BNO055.BNO055_SW_REV_ID_LSB_ADDR, 2)
sw_rev = sw_rev[0] | sw_rev[1] << 8
bl_rev = self.readBytes(BNO055.BNO055_BL_REV_ID_ADDR)[0]
return (accel_rev, mag_rev, gyro_rev, sw_rev, bl_rev)
def getCalibration(self):
calData = self.readBytes(BNO055.BNO055_CALIB_STAT_ADDR)[0]
return (calData >> 6 & 0x03, calData >> 4 & 0x03, calData >> 2 & 0x03, calData & 0x03)
def getTemp(self):
return self.readBytes(BNO055.BNO055_TEMP_ADDR)[0]
def getVector(self, vectorType):
buf = self.readBytes(vectorType, 6)
xyz = struct.unpack('hhh', struct.pack('BBBBBB', buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]))
if vectorType == BNO055.VECTOR_MAGNETOMETER:
scalingFactor = 16.0
elif vectorType == BNO055.VECTOR_GYROSCOPE:
scalingFactor = 900.0
elif vectorType == BNO055.VECTOR_EULER:
scalingFactor = 16.0
elif vectorType == BNO055.VECTOR_GRAVITY:
scalingFactor = 100.0
else:
scalingFactor = 1.0
return tuple([i / scalingFactor for i in xyz])
def getQuat(self):
buf = self.readBytes(BNO055.BNO055_QUATERNION_DATA_W_LSB_ADDR, 8)
wxyz = struct.unpack('hhhh',
struct.pack('BBBBBBBB', buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]))
return tuple([i * (1.0 / (1 << 14)) for i in wxyz])
def readBytes(self, register, numBytes=1):
return self._bus.read_i2c_block_data(self._address, register, numBytes)
def writeBytes(self, register, byteVals):
return self._bus.write_i2c_block_data(self._address, register, byteVals)
# 使用例
def demo():
bno = BNO055()
# beginでモードを指定して起動してもいいし、あとからsafeSetModeで切り替えてもよい
if not bno.begin():
# if not bno.begin(mode=bno.OPERATION_MODE_NDOF):
# if not bno.begin(mode=bno.OPERATION_MODE_MAGONLY):
raise RuntimeError('BNO055の初期化に失敗しました')
bno.start()
bno.safeSetMode(bno.OPERATION_MODE_NDOF)
# bno.safeSetMode(bno.OPERATION_MODE_MAGONLY)
try:
while True:
data = bno.current_data
mode = bno.getMode()
if data is None:
print("No data yet...")
time.sleep(0.5)
continue
e = data['euler'] # (heading, roll, pitch)
a = data['accel'] # (ax, ay, az)
g = data['gyro'] # (gx, gy, gz)
m = data['mag'] # (mx, my, mz)
# 小数点2桁フォーマット
e_str = f"{e[0]:6.2f}, {e[1]:6.2f}, {e[2]:6.2f}"
a_str = f"{a[0]:6.2f}, {a[1]:6.2f}, {a[2]:6.2f}"
g_str = f"{g[0]:6.2f}, {g[1]:6.2f}, {g[2]:6.2f}"
m_str = f"{m[0]:6.2f}, {m[1]:6.2f}, {m[2]:6.2f}"
print(
f"Euler(Y,R,P)= {e_str} | "
f"Acc= {a_str} | "
f"Gyro= {g_str} | "
f"Mag= {m_str} | "
f"Mode=0x{mode:02X}"
)
time.sleep(0.5)
except KeyboardInterrupt:
print("Stop demo.")
finally:
bno.stop()
if __name__ == "__main__":
demo()
補足
本当はこのクラスでリマップという機能を使ってみたかったのです
センサーの向きを逆さまで取り付けたりしても、リマップの機能で補正できると思っていました。
しかし、この機能を使ったところ、よくわからないのですが、NDOFで動かしているはずなのに、IMUPLUSっぽい挙動になりました。
原因はよくわかっていませんが、リマップの機能は使わず、出てきた値に補正をかけてあげるほうがいいと思います。
最後に（まとめ）
今回 Raspberry Pi で BNO055 を扱ってわかったことを整理すると：
- Yaw がドリフトするのは IMUPLUS（地磁気なし）で動いていたため
- 正しい NDOF へ切り替えるには CONFIG モード経由が必須
- NDOFを使う場合、キャリブレーション推奨
- キャリブレーションは完了時に自動でオフセットが保存される
- キャリブレーションの補正データを読み込んだり書き込んだりするときはCONFIGモード経由が必須
- 電源断で消えるため JSON に保存して復元するのが実運用向け
- リマップの機能はあるみたいだが、使わない方がよい
これで Raspberry Pi でも BNO055 のフュージョン機能を正しく・安定して使える環境が整いました。
Discussion