generated from ztimson/template
119 lines
3.7 KiB
Python
119 lines
3.7 KiB
Python
import sys
|
|
import json
|
|
import time
|
|
import math
|
|
import board
|
|
import busio
|
|
from adafruit_bno08x.i2c import BNO08X_I2C
|
|
from adafruit_bno08x import BNO_REPORT_ROTATION_VECTOR
|
|
|
|
POLL_INTERVAL = 0.1 # seconds between output lines
|
|
REPORT_INTERVAL_US = 50000 # sensor report rate (20 Hz)
|
|
STUCK_TIMEOUT = 10.0 # re-init if quaternion is bit-identical this long
|
|
MAX_CONSECUTIVE_ERRORS = 10
|
|
|
|
|
|
def quaternion_to_euler(x, y, z, w):
|
|
"""Adafruit BNO08X quaternion (x, y, z, w) -> yaw, pitch, roll in degrees."""
|
|
sinr_cosp = 2.0 * (w * x + y * z)
|
|
cosr_cosp = 1.0 - 2.0 * (x * x + y * y)
|
|
roll = math.atan2(sinr_cosp, cosr_cosp)
|
|
|
|
sinp = max(-1.0, min(1.0, 2.0 * (w * y - z * x)))
|
|
pitch = math.asin(sinp)
|
|
|
|
siny_cosp = 2.0 * (w * z + x * y)
|
|
cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
|
|
yaw = math.atan2(siny_cosp, cosy_cosp)
|
|
|
|
return math.degrees(yaw), math.degrees(pitch), math.degrees(roll)
|
|
|
|
|
|
def valid_quaternion(quat):
|
|
"""Return a normalized (x, y, z, w) tuple, or None if the data is garbage."""
|
|
if not quat or len(quat) != 4:
|
|
return None
|
|
if any(v is None or math.isnan(v) or math.isinf(v) for v in quat):
|
|
return None
|
|
norm = math.sqrt(sum(v * v for v in quat))
|
|
# A healthy unit quaternion has norm ~1. Corrupt I2C reads usually don't.
|
|
if norm < 0.9 or norm > 1.1:
|
|
return None
|
|
return tuple(v / norm for v in quat)
|
|
|
|
|
|
def init_sensor(i2c):
|
|
bno = BNO08X_I2C(i2c, address=0x4B)
|
|
try:
|
|
bno.enable_feature(BNO_REPORT_ROTATION_VECTOR, REPORT_INTERVAL_US)
|
|
except TypeError:
|
|
# Older library versions don't accept a report interval
|
|
bno.enable_feature(BNO_REPORT_ROTATION_VECTOR)
|
|
time.sleep(0.5)
|
|
return bno
|
|
|
|
|
|
def main():
|
|
try:
|
|
i2c = busio.I2C(board.SCL, board.SDA)
|
|
bno = init_sensor(i2c)
|
|
|
|
last_quat = None
|
|
last_change = time.time()
|
|
errors = 0
|
|
|
|
while True:
|
|
try:
|
|
raw = bno.quaternion # (x, y, z, w) from rotation vector
|
|
quat = valid_quaternion(raw)
|
|
if quat is None:
|
|
raise ValueError("invalid quaternion")
|
|
|
|
errors = 0
|
|
|
|
# Stuck-sensor detection
|
|
if raw != last_quat:
|
|
last_quat = raw
|
|
last_change = time.time()
|
|
elif time.time() - last_change > STUCK_TIMEOUT:
|
|
raise RuntimeError("sensor output frozen")
|
|
|
|
yaw, pitch, roll = quaternion_to_euler(*quat)
|
|
if yaw < 0:
|
|
yaw += 360.0
|
|
|
|
print(json.dumps({
|
|
"yaw": round(yaw, 2),
|
|
"pitch": round(pitch, 2),
|
|
"roll": round(roll, 2),
|
|
}))
|
|
sys.stdout.flush()
|
|
|
|
except (ValueError, TypeError, IndexError, KeyError, OSError,
|
|
RuntimeError) as e:
|
|
errors += 1
|
|
frozen = isinstance(e, RuntimeError)
|
|
if frozen or errors >= MAX_CONSECUTIVE_ERRORS:
|
|
print(json.dumps({"warning": f"re-initializing sensor ({e})"}),
|
|
file=sys.stderr)
|
|
sys.stderr.flush()
|
|
try:
|
|
bno = init_sensor(i2c)
|
|
except Exception as init_err:
|
|
print(json.dumps({"warning": f"re-init failed: {init_err}"}),
|
|
file=sys.stderr)
|
|
time.sleep(1.0)
|
|
errors = 0
|
|
last_quat = None
|
|
last_change = time.time()
|
|
|
|
time.sleep(POLL_INTERVAL)
|
|
|
|
except Exception as e:
|
|
print(json.dumps({"error": str(e)}), file=sys.stderr)
|
|
sys.exit(1)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|