BNO055をRaspberry Pi Picoで使う方法！9軸IMUで姿勢・角度を簡単取得

import time
from machine import I2C
from bno055 import BNO055

i2c = I2C(0)
imu = BNO055(i2c)

while True:
    q = imu.quaternion()  # (w, x, y, z)
    if q:
        w, x, y, z = q
        print("{:.6f},{:.6f},{:.6f},{:.6f}".format(w, x, y, z))
    time.sleep_ms(20)
    

import re
import time
import threading
from dataclasses import dataclass, field

import numpy as np
import serial


from PyQt5 import QtWidgets, QtCore
import pyqtgraph as pg
import pyqtgraph.opengl as gl


# ===== 設定 =====
PORT = "COM11" 
BAUD = 115200

# 描画更新（fps）
UI_HZ = 60

# クォータニオン平滑化（0〜1、大きいほど追従が速い）
SMOOTH = 0.35

STATIC_THRESH_DEG = 0.3   # ← 0.2〜0.5 くらいで調整


# パース（数値を4つ拾う）
num_re = re.compile(r"[-+]?\d+(?:\.\d+)?")

def parse_four_floats(line: str):
    vals = num_re.findall(line)
    if len(vals) < 4:
        return None
    return float(vals[0]), float(vals[1]), float(vals[2]), float(vals[3])


def quat_normalize(q):
    q = np.array(q, dtype=float)
    n = np.linalg.norm(q)
    if n == 0:
        return np.array([1.0, 0.0, 0.0, 0.0], dtype=float)
    return q / n


def quat_fix_sign(prev_q, q):
    # q と -q は同じ姿勢。符号が反転すると「切り替わった」感じが出るので補正。
    if prev_q is None:
        return q
    if float(np.dot(prev_q, q)) < 0.0:
        return -q
    return q


def quat_nlerp(q_prev, q_new, alpha):
    # 軽い平滑化（SLERPより簡単で十分）
    q = (1.0 - alpha) * q_prev + alpha * q_new
    return quat_normalize(q)

def quat_angle_diff_deg(q1, q2):
    # q1, q2 は正規化済み
    dot = np.clip(np.dot(q1, q2), -1.0, 1.0)
    # 回転角 = 2 * acos(|dot|)
    return np.degrees(2.0 * np.arccos(abs(dot)))


def quat_to_rotmat(q):
    # q = [w, x, y, z]
    w, x, y, z = q
    return np.array([
        [1 - 2*(y*y + z*z),     2*(x*y - z*w),     2*(x*z + y*w)],
        [    2*(x*y + z*w), 1 - 2*(x*x + z*z),     2*(y*z - x*w)],
        [    2*(x*z - y*w),     2*(y*z + x*w), 1 - 2*(x*x + y*y)],
    ], dtype=float)


@dataclass
class Shared:
    q: np.ndarray = field(default_factory=lambda: np.array([1.0, 0.0, 0.0, 0.0], dtype=float))
    t: float = 0.0


shared = Shared()
lock = threading.Lock()
stop_flag = False


def serial_thread():
    global stop_flag
    try:
        ser = serial.Serial(PORT, BAUD, timeout=0.1)
    except Exception as e:
        print(f"[ERR] シリアルを開けません: {e}")
        stop_flag = True
        return

    time.sleep(0.5)
    ser.reset_input_buffer()  # 溜まってる古いデータを捨てる

    prev_q = None

    while not stop_flag:
        try:
            line = ser.readline()
            if not line:
                continue
            s = line.decode("utf-8", errors="ignore").strip()
            parsed = parse_four_floats(s)
            if not parsed:
                continue

            q = quat_normalize(parsed)          # w,x,y,z
            q = quat_fix_sign(prev_q, q)
            prev_q = q

            with lock:
                shared.q = q
                shared.t = time.time()

        except Exception:
            # ゴミ行/一時的エラーは無視して継続
            continue

    try:
        ser.close()
    except Exception:
        pass


class Viewer(QtWidgets.QWidget):
    def __init__(self):
        super().__init__()
        self.setWindowTitle("Pico BNO055 Quaternion Viewer")

        layout = QtWidgets.QVBoxLayout(self)

        self.view = gl.GLViewWidget()
        self.view.setCameraPosition(distance=4)
        layout.addWidget(self.view)

        # ワールド座標軸
        self.view.addItem(gl.GLAxisItem(size=pg.Vector(1.5, 1.5, 1.5)))

        # 立方体（線で描く）
        self.verts0, self.edges = self.make_cube(size=1.5)
        self.lines = []
        for a, b in self.edges:
            pts = np.vstack([self.verts0[a], self.verts0[b]])
            item = gl.GLLinePlotItem(pos=pts, width=2, antialias=True, mode='lines')
            self.view.addItem(item)
            self.lines.append(item)

        self.info = QtWidgets.QLabel("q = (w,x,y,z)")
        layout.addWidget(self.info)

        self.q_state = np.array([1.0, 0.0, 0.0, 0.0], dtype=float)

        self.timer = QtCore.QTimer(self)
        self.timer.timeout.connect(self.tick)
        self.timer.start(int(1000 / UI_HZ))

    def make_cube(self, size=1.0):
        s = size / 2.0
        V = np.array([
            [-s, -s, -s],
            [ s, -s, -s],
            [ s,  s, -s],
            [-s,  s, -s],
            [-s, -s,  s],
            [ s, -s,  s],
            [ s,  s,  s],
            [-s,  s,  s],
        ], dtype=float)
        edges = [
            (0, 1), (1, 2), (2, 3), (3, 0),
            (4, 5), (5, 6), (6, 7), (7, 4),
            (0, 4), (1, 5), (2, 6), (3, 7),
        ]
        return V, edges

    def tick(self):
        with lock:
            q_latest = shared.q.copy()
            t_latest = shared.t

        # 符号補正
        q_latest = quat_fix_sign(self.q_state, q_latest)

        # ★ 静止判定
        angle_diff = quat_angle_diff_deg(self.q_state, q_latest)

        if angle_diff < STATIC_THRESH_DEG:
            # 微振動 → 完全に止める
            q_use = self.q_state
        else:
            # 動いている → 平滑化して追従
            q_use = quat_nlerp(self.q_state, q_latest, SMOOTH)

        self.q_state = q_use

        R = quat_to_rotmat(self.q_state)
        V = (R @ self.verts0.T).T

        for i, (a, b) in enumerate(self.edges):
            pts = np.vstack([V[a], V[b]])
            self.lines[i].setData(pos=pts)

        age_ms = (time.time() - t_latest) * 1000.0 if t_latest else 9999.0
        w, x, y, z = self.q_state
        self.info.setText(
            f"q=(w={w:+.4f}, x={x:+.4f}, y={y:+.4f}, z={z:+.4f}) "
            f"Δθ={angle_diff:.3f}° age={age_ms:.0f}ms"
        )



def main():
    global stop_flag
    th = threading.Thread(target=serial_thread, daemon=True)
    th.start()

    app = QtWidgets.QApplication([])
    w = Viewer()
    w.resize(900, 700)
    w.show()

    def on_exit():
        global stop_flag
        stop_flag = True

    app.aboutToQuit.connect(on_exit)
    app.exec_()


if __name__ == "__main__":
    main()