#
# hello.4754.RP2040.py
# Adafruit 4754, BNO085 IMU, XIAO RP2040 hello-world
#
# Neil Gershenfeld 10/21/23
#
# This work may be reproduced, modified, distributed,
# performed, and displayed for any purpose, but must
# acknowledge this project. Copyright is retained and
# must be preserved. The work is provided as is; no
# warranty is provided, and users accept all liability.
#
# IMU
# https://learn.adafruit.com/adafruit-9-dof-orientation-imu-fusion-breakout-bno085/
# jumper PS0 for UART RVC mode
# install MicroPython:
# https://wiki.seeedstudio.com/XIAO-RP2040-with-MicroPython/
# save as main.py to run at boot
#
import os,sys,time
#
# open IMU
#
import machine
from machine import UART,Pin
IMU = UART(0,baudrate=115200,timeout=1000)
#
# read IMU
#
def read_IMU():
while (True):
#
# look for framing
#
c1 = 0
while (True):
c0 = c1
c1 = IMU.read(1)
if ((c0 == b'\xaa') and (c1 == b'\xaa')):
break
#
# read data
#
index = int.from_bytes(IMU.read(1),"big")
yaw0 = int.from_bytes(IMU.read(1),"big")
yaw1 = int.from_bytes(IMU.read(1),"big")
pitch0 = int.from_bytes(IMU.read(1),"big")
pitch1 = int.from_bytes(IMU.read(1),"big")
roll0 = int.from_bytes(IMU.read(1),"big")
roll1 = int.from_bytes(IMU.read(1),"big")
x0 = int.from_bytes(IMU.read(1),"big")
x1 = int.from_bytes(IMU.read(1),"big")
y0 = int.from_bytes(IMU.read(1),"big")
y1 = int.from_bytes(IMU.read(1),"big")
z0 = int.from_bytes(IMU.read(1),"big")
z1 = int.from_bytes(IMU.read(1),"big")
#
# combine high and low bytes
#
yaw = yaw0+256*yaw1
pitch = pitch0+256*pitch1
roll = roll0+256*roll1
ax = x0+256*x1
ay = y0+256*y1
az = z0+256*z1
#
# two's complement sign
#
if (yaw > 32767): yaw = yaw-65536
if (pitch > 32767): pitch = pitch-65536
if (roll > 32767): roll = roll-65535
if (ax > 32767): ax = ax-65536
if (ay > 32767): ay = ay-65536
if (az > 32767): az = az-65536
#
# convert to degrees
#
yaw = yaw/100
pitch = pitch/100
roll = roll/100
#
# check for valid data (don't know why this is needed)
#
if ((yaw <= 180) and (yaw >= -180) or
(pitch <= 90) and (pitch >= -90) or
(roll <= 180) and (roll >= -180) or
(ax <= 5000) and (ax >= -5000) or
(ay <= 5000) and (ay >= -5000) or
(az <= 5000) and (az >= -5000)):
break
#
# return data
#
return f"{yaw},{pitch},{roll},{ax},{ay},{az}"
#
# main
#
while True:
print(read_IMU())