COmmit 1 of unb roken repo

.gitignore

@@ -0,0 +1,286 @@
+# ---> macOS
+# General
+.DS_Store
+.AppleDouble
+.LSOverride
+
+# Icon must end with two \r
+Icon
+
+# Thumbnails
+._*
+
+# Files that might appear in the root of a volume
+.DocumentRevisions-V100
+.fseventsd
+.Spotlight-V100
+.TemporaryItems
+.Trashes
+.VolumeIcon.icns
+.com.apple.timemachine.donotpresent
+
+# Directories potentially created on remote AFP share
+.AppleDB
+.AppleDesktop
+Network Trash Folder
+Temporary Items
+.apdisk
+
+# ---> Linux
+*~
+
+# temporary files which can be created if a process still has a handle open of a deleted file
+.fuse_hidden*
+
+# KDE directory preferences
+.directory
+
+# Linux trash folder which might appear on any partition or disk
+.Trash-*
+
+# .nfs files are created when an open file is removed but is still being accessed
+.nfs*
+
+# ---> Python
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+pip-wheel-metadata/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+.python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# ---> Node
+# Logs
+logs
+*.log
+npm-debug.log*
+yarn-debug.log*
+yarn-error.log*
+lerna-debug.log*
+
+# Diagnostic reports (https://nodejs.org/api/report.html)
+report.[0-9]*.[0-9]*.[0-9]*.[0-9]*.json
+
+# Runtime data
+pids
+*.pid
+*.seed
+*.pid.lock
+
+# Directory for instrumented libs generated by jscoverage/JSCover
+lib-cov
+
+# Coverage directory used by tools like istanbul
+coverage
+*.lcov
+
+# nyc test coverage
+.nyc_output
+
+# Grunt intermediate storage (https://gruntjs.com/creating-plugins#storing-task-files)
+.grunt
+
+# Bower dependency directory (https://bower.io/)
+bower_components
+
+# node-waf configuration
+.lock-wscript
+
+# Compiled binary addons (https://nodejs.org/api/addons.html)
+build/Release
+
+# Dependency directories
+node_modules/
+jspm_packages/
+
+# TypeScript v1 declaration files
+typings/
+
+# TypeScript cache
+*.tsbuildinfo
+
+# Optional npm cache directory
+.npm
+
+# Optional eslint cache
+.eslintcache
+
+# Microbundle cache
+.rpt2_cache/
+.rts2_cache_cjs/
+.rts2_cache_es/
+.rts2_cache_umd/
+
+# Optional REPL history
+.node_repl_history
+
+# Output of 'npm pack'
+*.tgz
+
+# Yarn Integrity file
+.yarn-integrity
+
+# dotenv environment variables file
+.env
+.env.test
+
+# parcel-bundler cache (https://parceljs.org/)
+.cache
+
+# Next.js build output
+.next
+
+# Nuxt.js build / generate output
+.nuxt
+dist
+
+# Gatsby files
+.cache/
+# Comment in the public line in if your project uses Gatsby and not Next.js
+# https://nextjs.org/blog/next-9-1#public-directory-support
+# public
+
+# vuepress build output
+.vuepress/dist
+
+# Serverless directories
+.serverless/
+
+# FuseBox cache
+.fusebox/
+
+# DynamoDB Local files
+.dynamodb/
+
+# TernJS port file
+.tern-port
+
+# Stores VSCode versions used for testing VSCode extensions
+.vscode-test
+
+.vscode/*
+.vscode
+.vscode/settings.json

Control/Jetson/apriltag_detector.py

@@ -0,0 +1,172 @@
+from threading import Thread
+from dataclasses import dataclass
+import cv2
+import time
+import apriltag
+import logging
+import platform
+import math
+
+logging.basicConfig(level='INFO')
+
+@dataclass
+class CameraConstants:
+    fx: float
+    fy: float
+    cx: float
+    cy: float
+
+# [[fx, 00, cx],
+#  [00, fy, cy],
+#  [00, 00, 01]]
+
+class CameraConstants:
+    __SPEDAL_COEF = CameraConstants(fx=9.67695578e+02, fy=9.60280613e+02, cx=1.01330362e+03, cy=4.47474757e+02)
+    __LOGI_COEF = CameraConstants(fx=1.42241810e+03, fy=1.42140968e+03, cx=9.66357252e+02, cy=5.59109177e+02)
+    def __init__(self) -> None:
+        pass
+
+    def get_logi_cam_constants(self):
+        return [self.__LOGI_COEF.fx, self.__LOGI_COEF.fy, self.__LOGI_COEF.cx, self.__LOGI_COEF.cy]
+
+    def get_spedal_120_cam_constants(self):
+        return [self.__SPEDAL_COEF.fx, self.__SPEDAL_COEF.fy, self.__SPEDAL_COEF.cx, self.__SPEDAL_COEF.cy]
+
+
+class VideoStream:
+    def __init__(self, cam_id=1):
+        self.stream = self.open_cam_usb(1280, 720)
+        _, img = self.stream.read()
+        self.buffer = img
+        self.running = False
+
+    def start(self):
+        t = Thread(target=self.update, args=())
+        t.daemon = True
+        self.running = True
+        t.start()
+        self.start_time = time.time()
+        return self
+
+    def update(self):
+        while self.running:
+            success, frame = self.stream.read()
+            assert success
+            self.buffer = frame
+        self.stream.release()
+
+    def read(self):
+        return self.buffer
+
+    def stop(self):
+        self.running = False
+
+    def open_cam_usb(self, width, height):
+        gst_str = ('v4l2src device=/dev/cam_track ! video/x-raw, width=(int){}, height=(int){} ! videoconvert ! appsink').format(width, height)
+        return cv2.VideoCapture(gst_str, cv2.CAP_GSTREAMER)
+
+
+class CUDA:
+    def __init__(self) -> None:
+        self.log = logging.getLogger("CUDA_helper")
+        self.log.setLevel("DEBUG")
+        self.cuda_allowed = False
+        self.use_CUDA = True
+        if self.use_CUDA:
+            self.log.warning("CUDA has been disabled due to performance issues...")
+            self.use_CUDA = False
+        else:
+            cuda_devices = cv2.cuda.getCudaEnabledDeviceCount()
+            if cuda_devices == 0:
+                self.log.warning("No CUDA devices found, will use CPU")
+                self.use_CUDA = False
+            else:
+                self.log.info("Running with CUDA 😎")
+
+    def bgr2gray(self, frame):
+        if self.use_CUDA:
+            gpu_frame = cv2.cuda_GpuMat()
+            gpu_frame.upload(frame)
+            gray_cuda = cv2.cuda.cvtColor(gpu_frame, cv2.COLOR_BGR2GRAY)
+            return gray_cuda.download()
+        else:
+            return cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+
+
+class Detector:
+    def __init__(self) -> None:
+        self.log = logging.getLogger("apriltag_detector")
+        self.log.setLevel("DEBUG")
+        self.options = apriltag.DetectorOptions(families="tag36h11", nthreads=4)
+        self.detector = apriltag.Detector(self.options)
+        self.streams = []
+        self.CUDA = CUDA()
+        self.camera_params = CameraConstants().get_spedal_120_cam_constants()
+
+    def start_streams(self, dev_id=1):
+        self.streams.append(VideoStream(dev_id).start())
+
+    def get_tags(self, stream_id=0):
+        frame = self.streams[stream_id].read()
+        if frame.any():
+            gray_frame = self.CUDA.bgr2gray(frame)
+            result = self.detector.detect(gray_frame)
+            if result:
+                pose = self.detector.detection_pose(result[0], self.camera_params)
+                raw_transformation_matrix = pose[0]
+                euler_angles = self.euler_to_rotVec(raw_transformation_matrix)
+                return result, euler_angles
+            return None, None
+
+    def _get_tags_pc(self):
+        # for stream in self.streams:
+        frame = self.streams[0].read()
+        if frame.any():
+            gray_frame = self.CUDA.bgr2gray(frame)
+            result = self.detector.detect(gray_frame)
+            if result:
+                pose = self.detector.detection_pose(result[0], self.camera_params)
+                raw_transformation_matrix = pose[0]
+                euler_angles = self.euler_to_rotVec(raw_transformation_matrix)
+                return gray_frame, result, euler_angles
+            return gray_frame, None, None
+        return None, None
+
+    def euler_to_rotVec(self, Rmat):
+        theta = math.acos(((Rmat[0, 0] + Rmat[1, 1] + Rmat[2, 2]) - 1) / 2)
+        sin_theta = math.sin(theta)
+        if sin_theta == 0:
+            rx, ry, rz = 0.0, 0.0, 0.0
+        else:
+            multi = 1 / (2 * math.sin(theta))
+            rx = multi * (Rmat[2, 1] - Rmat[1, 2]) * theta
+            ry = multi * (Rmat[0, 2] - Rmat[2, 0]) * theta
+            rz = multi * (Rmat[1, 0] - Rmat[0, 1]) * theta
+        return rx, ry, rz
+
+    def stop_streams(self):
+        for stream in self.streams:
+            stream.stop()
+
+
+if __name__ =="__main__":
+    D = Detector()
+    D.start_streams(1)
+    while True:
+        frame, result, angles = D._get_tags_pc()
+        center = result[0].center
+        pos_x = 2 * (center[0] - (1280/2)) / 1280
+        pos_y = 2 * (center[1] - (720/2)) / 720
+        clean_pos_y = float("{0:.3f}".format(pos_y))
+        clean_pos_x = float("{0:.3f}".format(pos_x))
+        translation = (clean_pos_x, clean_pos_y)
+
+        print(f"Center: {center}")
+        print(f"pos_x: {pos_x}")
+        print(f"pos_y: {pos_y}")
+        print(f"clean_pos_y: {clean_pos_y}")
+        print(f"clean_pos_x: {clean_pos_x}")
+        print(f"Angles: {angles}")
+        print("-------------------------------------")
+        cv2.imshow("frame", frame)
+        cv2.waitKey(0)

Control/Jetson/dronekit_server_client.py

@@ -0,0 +1,178 @@
+import json
+import logging
+import time
+from math import asin, cos, radians, sin, sqrt
+
+import requests
+
+logging.basicConfig(level='INFO')
+
+def haversine(lon1, lat1, lon2, lat2):
+    """
+    Calculate the great circle distance between two points
+    on the earth (specified in decimal degrees)
+    """
+    # convert decimal degrees to radians
+    lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
+
+    # haversine formula
+    dlon = lon2 - lon1
+    dlat = lat2 - lat1
+    a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2
+    c = 2 * asin(sqrt(a))
+    r = 6371 # Radius of earth in kilometers. Use 3956 for miles
+    return c * r
+
+class DroneKitComms:
+    def __init__(self) -> None:
+        self.log = logging.getLogger("dronekit_comms")
+        self.log.setLevel("DEBUG")
+        self.url = 'http://192.168.5.2:5000'
+        self.heartbeat_endpoint = '/alive'
+        self.speed_endpoint = '/set_speed'
+        self.steering_endpoint = '/set_steering'
+        self.speed_steering_endpoint = '/set_speed_steering'
+        self.gps_navigation_endpoint = '/go_to_gps'
+        self.set_state_endpoint = '/set_state'
+        self.get_state_endpoint = '/get_state'
+        self.arm_endpoint = '/arm'
+        self.disarm_endpoint = '/disarm'
+        self.close_endpoint = '/close'
+        self.get_gps_endpoint = '/get_gps'
+        self.nudge_fork_vert_endpoint = '/nudge_fork_vert'
+        self.nudge_fork_horiz_endpoint = '/nudge_fork_horiz'
+        self.nudge_drive_endpoint = '/nudge_drive'
+        self.approach_pallet_endpoint = '/approach_pallet'
+        self.gps_target_radius_km = 0.0025
+        rsp = self.make_request(self.heartbeat_endpoint)
+        if rsp.get('Success', False):
+            self.log.info('Server alive')
+        else:
+            self.log.critical('Server dead! Check route table')
+
+    def make_request(self, endpoint, data = {}):
+        rsp = None
+        rsp = requests.post(self.url + endpoint, json=data, timeout=240)
+        if rsp:
+            if rsp.ok:
+                json_rsp = json.loads(rsp.text)
+                status = json_rsp.get('Success', None)
+                if status != True:
+                    error = json_rsp.get('Error', None)
+                    self.log.error(f'Got error: {error}')
+                return json_rsp
+            else:
+                self.log.error(f'Failed to process request, code: {rsp.status_code}, text: {rsp.text}')
+        else:
+            self.log.critical('Cannot connect to server')
+
+    def set_steering(self, steering_pwm):
+        data = {'steering': steering_pwm}
+        _ = self.make_request(self.steering_endpoint, data)
+        # print(rsp)
+
+    def set_speed(self, speed_pwm):
+        data = {'speed': speed_pwm}
+        _ = self.make_request(self.speed_endpoint, data)
+        # print(rsp)
+
+    def set_speed_steering(self, speed_pwm, steering_pwm):
+        data = {'steering': steering_pwm, 'speed': speed_pwm}
+        _ = self.make_request(self.speed_steering_endpoint, data)
+        # print(rsp)
+
+    def nudge_fork_vert(self, go_up: bool, amount: float):
+        data = {'go_up': go_up, 'amount': amount}
+        _ = self.make_request(self.nudge_fork_vert_endpoint, data)
+        # print(rsp)
+
+    def nudge_fork_horiz(self, go_left: bool, amount: float):
+        data = {'go_left': go_left, 'amount': amount}
+        _ = self.make_request(self.nudge_fork_horiz_endpoint, data)
+        # print(rsp)
+
+    def nudge_drive(self, go_forward: bool, amount: float, steering: int = 1500):
+        data = {'go_forward': go_forward, 'amount': amount, 'steering': steering}
+        _ = self.make_request(self.nudge_drive_endpoint, data)
+        # print(rsp)
+
+    def go_to_gps(self, lat, lon):
+        data = {'lat': lat, 'lon': lon}
+        _ = self.make_request(self.gps_navigation_endpoint, data)
+
+    def set_arm(self):
+        _ = self.make_request(self.arm_endpoint)
+        # print(rsp)
+
+    def set_disarm(self):
+        _ = self.make_request(self.disarm_endpoint)
+        # print(rsp)
+
+    def get_state(self):
+        rsp = self.make_request(self.get_state_endpoint)
+        machine_state = rsp.get("State", "UNKNOWN")
+        return machine_state
+
+    def set_state(self, state):
+        data = {'State': state}
+        _ = self.make_request(self.set_state_endpoint, data)
+
+    def get_gps(self):
+        rsp = self.make_request(self.get_gps_endpoint)
+        lat = rsp.get("lat", 0)
+        lon = rsp.get("lon", 0)
+        return (lat, lon)
+
+    def check_gps_in_radius(self, target_gps):
+        actual_lat, actual_lon = self.get_gps()
+        target_lat, target_lon = target_gps
+        if actual_lat == 0 or actual_lon == 0:
+            logging.critical("Can not get GPS fix!")
+            return False, 99999
+        radius = self.gps_target_radius_km
+        distance = haversine(target_lat, target_lon, actual_lat, actual_lon)
+        logging.info(f'Distance to target (m) : {distance*1000}')
+        if distance <= radius:
+            return True, distance*1000
+        return False, distance*1000
+
+    def zero_fork_vert_pickup(self):
+        data = {'go_up': False, 'amount': 1.0}
+        _ = self.make_request(self.nudge_fork_vert_endpoint, data)
+        _ = self.make_request(self.nudge_fork_vert_endpoint, data)
+        _ = self.make_request(self.nudge_fork_vert_endpoint, data)
+        _ = self.make_request(self.nudge_fork_vert_endpoint, data)
+        _ = self.make_request(self.nudge_fork_vert_endpoint, data)
+        time.sleep(2)
+        data = {'go_up': True, 'amount': 0.6}
+        _ = self.make_request(self.nudge_fork_vert_endpoint, data)
+
+    def zero_fork_vert_movement(self):
+        data = {'go_up': False, 'amount': 1.0}
+        _ = self.make_request(self.nudge_fork_vert_endpoint, data)
+        _ = self.make_request(self.nudge_fork_vert_endpoint, data)
+        _ = self.make_request(self.nudge_fork_vert_endpoint, data)
+        _ = self.make_request(self.nudge_fork_vert_endpoint, data)
+        _ = self.make_request(self.nudge_fork_vert_endpoint, data)
+        time.sleep(2)
+        data = {'go_up': True, 'amount': 0.9}
+        _ = self.make_request(self.nudge_fork_vert_endpoint, data)
+        _ = self.make_request(self.nudge_fork_vert_endpoint, data)
+
+    def zero_fork_horiz(self):
+        self.nudge_fork_horiz(go_left=True, amount=0.9)
+        self.nudge_fork_horiz(go_left=True, amount=0.9)
+        self.nudge_fork_horiz(go_left=True, amount=0.9)
+        time.sleep(2)
+        self.nudge_fork_horiz(go_left=False, amount=0.9)
+        self.nudge_fork_horiz(go_left=False, amount=0.4)  # TODO: tune this
+
+
+    def approach_pallet(self):
+        _ = self.make_request(self.gps_target_radius_km)
+        # print(rsp)
+
+
+    # def close_vehicle(self):
+    #     rsp = self.make_request(self.close_endpoint)
+    #     print(rsp)

Control/Jetson/main.py

@@ -0,0 +1,259 @@
+#!/usr/bin/python3
+import logging
+import signal
+import subprocess
+import sys
+import threading
+import time
+from queue import Queue
+
+import flask
+from flask import jsonify, request
+
+from dronekit_server_client import DroneKitComms
+from vision_cargo_approach import VisionCargoApproacher
+from pallet_hole_alignment_c import HoleAligner
+
+
+# def reinit_networking():
+#     logging.info('Resetting dhcpcd service, wait 5 seconds...')
+#     subprocess.run(["sudo", "service", "dhcpcd", "restart"])
+#     time.sleep(5)
+#     subprocess.run(["route", "-n"])
+
+
+JOB_QUEUE = Queue(maxsize=10)
+KILL_SIGNAL = False
+
+def exit_handler(*_):
+    global KILL_SIGNAL
+    print('\nROOT: CTRL-C pressed, killing everything')
+    KILL_SIGNAL = True
+    sys.exit(0)
+signal.signal(signal.SIGINT, exit_handler)
+
+logging.basicConfig()
+logging.root.setLevel(logging.NOTSET)
+logging.basicConfig(level=logging.NOTSET)
+
+executor_logger = logging.getLogger("LOGISTICS EXECUTOR")
+executor_logger.setLevel(logging.DEBUG)
+
+
+#--- <Pseudoprivate methods
+def __go_to_gps_waypoints(vehicle_ctrl, waypoints: list):
+    for ix, waypoint in enumerate(waypoints):
+        target_lat = waypoint['lat']
+        target_lon = waypoint['lon']
+        executor_logger.info(f"Going to waypoint {ix} at lat: {target_lat}, lon: {target_lon}")
+        success = __go_to_gps(vehicle_ctrl, lat=target_lat, lon=target_lon)
+        if not success:
+            executor_logger.error("Navigate to gps watpoint func returned error, chack logs, aborting")
+            return False
+    return True
+
+def __go_to_gps(vehicle_ctrl, lat: float, lon: float):
+    executor_logger.info(f"Starting GPS navigation to lat: {lat}, lon: {lon}")
+    timeout_time = 300 + time.time()  # 5 minutes to get to target
+    vehicle_in_target = False
+    vehicle_ctrl.go_to_gps(lat=lat, lon=lon)
+    while vehicle_in_target == False and time.time() < timeout_time:
+        time.sleep(0.2)  # Don't DDoS the Pi!
+        vehicle_in_target, distance = vehicle_ctrl.check_gps_in_radius((lat, lon))
+    if vehicle_in_target:
+        executor_logger.info(f'We are in radius ({distance})')
+        return True
+    else:
+        executor_logger.critical(f"5 minuted have elapsed and we are not in the target area ({distance}), aborting")
+        return False
+
+def __go_to_cargo_vision(vision_system):
+    executor_logger.info(f"Starting vision to cargo navigation")
+    success = vision_system.navigate_to_cargo_vision()
+    return success
+
+def __align_forks_with_holes(hole_alignment_system):
+    executor_logger.info(f"Starting pallet hole alignment")
+    success = hole_alignment_system.align_fork()
+    return success
+
+def reverse_turn(vehicle_ctrl):
+    executor_logger.info(f"Picking up cargo")
+    __reverse_turn(vehicle_ctrl)
+    return True
+
+def __pick_up_cargo(vehicle_ctrl):
+    vehicle_ctrl.approach_pallet()
+    time.sleep(1)
+    vehicle_ctrl.zero_fork_vert_movement()
+    return True
+
+def __put_down_cargo(vehicle_ctrl):
+    vehicle_ctrl.zero_fork_vert_pickup()
+    time.sleep(1)
+    vehicle_ctrl.nudge_drive(False, 9.9)
+    return True
+
+
+def __put_cargo_down_and_reverse_turn(vehicle_ctrl):
+    executor_logger.info(f"Dropping cargo")
+    __put_down_cargo(vehicle_ctrl)
+    __reverse_turn(vehicle_ctrl)
+    return True
+
+
+def __reverse_turn(vehicle_ctrl):
+    executor_logger.info(f"performing reverse turn")
+    vehicle_ctrl.nudge_drive(False, 5, 2000)
+    vehicle_ctrl.nudge_drive(False, 6, 2000)
+    vehicle_ctrl.nudge_drive(True, 5, 1000)
+    vehicle_ctrl.nudge_drive(True, 6, 1000)
+
+    return True
+
+#--- <Pseudoprivate methods
+
+
+def queue_executor_thread():
+    executor_logger.info("Starting task executor...")
+    executor_logger.info("Reiniting networking...")
+    # reinit_networking() # Reinits dhcpcd to enable Pi communications ---We fixed this by tweaking /etc/network/interfaces
+    executor_logger.info("Init vehicle control...")
+    FORKLIFT = DroneKitComms()
+    executor_logger.info("Importing Vision system...")
+    VISION_CARGO_APPROACH = VisionCargoApproacher(FORKLIFT)
+    executor_logger.info("Importing Pallet hole aligner...")
+    HOLE_ALIGNMENT_SYSTEM = HoleAligner(FORKLIFT)
+    executor_logger.info("ARMING VEHICLE...")
+    FORKLIFT.set_arm()
+    executor_logger.info("Zeroing fork...")
+    FORKLIFT.zero_fork_vert_movement()
+    FORKLIFT.zero_fork_horiz()
+    FORKLIFT.set_state("READY_FOR_NEXT_TASK")
+    executor_logger.info("Ready.")
+
+
+    while not KILL_SIGNAL:
+        if not JOB_QUEUE.empty():
+            current_job = JOB_QUEUE.get()
+            executor_logger.info(f"Executing new job: {current_job}")
+            #execute job.....
+
+            FORKLIFT.set_state("NAVIGATING_TO_POINT_GPS")
+            gps_navigation_succcess = __go_to_gps_waypoints(FORKLIFT, current_job['pickup_waypoints'])
+            if not gps_navigation_succcess:
+                executor_logger.critical("Go to GPS was not successful, check logs !")
+                FORKLIFT.set_state("OTHER_ERROR")
+                return False
+
+            FORKLIFT.set_state("NAVIGATING_TO_POINT_VISION")
+            vision_to_cargo_succcess = __go_to_cargo_vision(VISION_CARGO_APPROACH)
+            if not vision_to_cargo_succcess:
+                executor_logger.critical("Go to cargo with vision was not successful, check logs !")
+                FORKLIFT.set_state("OTHER_ERROR")
+                return False
+
+            FORKLIFT.zero_fork_vert_pickup()
+
+            FORKLIFT.set_state("MANIPULATING_CARGO")
+            align_forks_sucess = __align_forks_with_holes(HOLE_ALIGNMENT_SYSTEM)
+            if not align_forks_sucess:
+                executor_logger.critical("Fork hole alignment was not successful, check logs !")
+                FORKLIFT.set_state("OTHER_ERROR")
+                return False
+
+
+            FORKLIFT.set_state("MANIPULATING_CARGO")
+            cargo_pickupe = __pick_up_cargo(FORKLIFT)
+            if not cargo_pickupe:
+                executor_logger.critical("Problem with cargo pickup, check logs !")
+                FORKLIFT.set_state("OTHER_ERROR")
+                return False
+
+
+            reverse_turn = reverse_turn(FORKLIFT)
+            if not reverse_turn:
+                executor_logger.critical("Problem with reverse turn, check logs !")
+                FORKLIFT.set_state("OTHER_ERROR")
+                return False
+
+
+            FORKLIFT.set_state("NAVIGATING_TO_POINT_GPS")
+            gps_dropoff_succcess = __go_to_gps_waypoints(FORKLIFT, current_job['dropoff_waypoints'])
+            if not gps_dropoff_succcess:
+                executor_logger.critical("Go to GPS was not successful, check logs !")
+                FORKLIFT.set_state("OTHER_ERROR")
+                return False
+
+            FORKLIFT.set_state("MANIPULATING_CARGO")
+            cargo_down_success = __put_cargo_down_and_reverse_turn(FORKLIFT)
+            if not cargo_down_success:
+                executor_logger.critical("Problem with cargo put down, check logs !")
+                FORKLIFT.set_state("OTHER_ERROR")
+                return False
+
+            if current_job['on_finish']['goto'] == True:
+                FORKLIFT.set_state("NAVIGATING_TO_POINT_GPS")
+                gps_return_success = __go_to_gps_waypoints(FORKLIFT, current_job['on_finish']['waypoints'])
+                if not gps_return_success:
+                    executor_logger.critical("Go to GPS was not successful, check logs !")
+                    FORKLIFT.set_state("OTHER_ERROR")
+                    return False
+
+            FORKLIFT.set_state("READY_FOR_NEXT_TASK")
+            logging.info("Task complete... Sleeping for 5 and Waiting for next task")
+            time.sleep(5)
+
+    executor_logger.info("Kill signal activated, exiting")
+
+
+job_executor = threading.Thread(target=queue_executor_thread)
+job_executor.start()
+
+
+app = flask.Flask("task_upload_server")
+app.logger.setLevel("DEBUG")
+
+DEFULT_JOB_TEMPLATE = {
+    "job_id": 0,
+    "cargo_id": 0,
+    "pickup_waypoints": [{"lat": 0, "lon": 0}, {"lat": 1, "lon": 1}, {"lat": 2, "lon": 2}],
+    "dropoff_waypoints": [{"lat": 2, "lon": 2}, {"lat": 3, "lon": 3}, {"lat": 4, "lon": 4}],
+    "on_finish": {"goto": True, "waypoints": [{"lat": 5, "lon": 5}, {"lat": 1, "lon": 1}, {"lat": 0, "lon": 0}]},
+}
+
+DEFAULT_JOB_TEMPLATE_KEYS = DEFULT_JOB_TEMPLATE.keys()
+
+def verify_json_data(uploaded_data):
+    data_keys = uploaded_data.keys()
+    for item in DEFAULT_JOB_TEMPLATE_KEYS:
+        if item not in data_keys:
+            app.logger.error(f"Item  {item} not in uploaded job uploaded_data keys")
+            app.logger.error(f"uploaded job dump: {uploaded_data}")
+            return False
+    return True
+
+@app.route('/', methods=['GET'])
+def home():
+    return "<h1>Logistics executionner is running...</h1>"
+
+@app.route('/alive', methods=['POST'])
+def alive():
+    app.logger.info(f"Returning alive")
+    return jsonify({'Success': True})
+
+@app.route('/add_job', methods=['POST'])
+def add_job():
+    if request.is_json:
+        job_request = request.get_json()
+        if not verify_json_data(job_request):
+            app.logger.error("Error processing job request: check logs...")
+            return jsonify({'Error': 'Error processing job request: check logs...'})
+        app.logger.info(f"Got valid job request: {job_request}")
+        JOB_QUEUE.put(job_request)
+        return jsonify({'Success': True})
+    else:
+        return jsonify({'Error': 'Not JSON'})
+
+
+app.run(host='0.0.0.0', port=8080)

Control/Jetson/pallet_hole_alignment.py

@@ -0,0 +1,327 @@
+import pyrealsense2.pyrealsense2 as rs
+import numpy as np
+import cv2
+from threading import Thread
+import logging
+import time
+
+
+logging.basicConfig(level='INFO')
+
+
+class DepthStream:
+    def __init__(self, resolution) -> None:
+        self.resolution = resolution
+        self.depth_stream = self.init_depth_stream()
+        self.start_depth_stream()
+        self.buffer = self.get_new_depth_image()
+        self.updating = False
+        self.new_frame_ready = False
+
+    def init_depth_stream(self):
+        self.pipeline = rs.pipeline()
+        self.config = rs.config()
+        self.config.enable_stream(rs.stream.depth, self.resolution[0], self.resolution[1], rs.format.z16, 5)
+        # self.config.enable_stream(rs.stream.depth, self.resolution[0], self.resolution[1], rs.format.z16, 30)
+
+    def start_depth_stream(self):
+        self.pipeline_profile = self.pipeline.start(self.config)
+        device = self.pipeline_profile.get_device()
+        depth_stream = device.query_sensors()[0]
+        depth_stream.set_option(rs.option.laser_power, 360)  # Set laser power to max for (hopefully) better results
+
+    def get_new_depth_image(self):
+        frames = self.pipeline.wait_for_frames()
+        depth_frame = frames.get_depth_frame()
+        depth_image = np.asanyarray(depth_frame.get_data())
+        return depth_image
+
+    def update(self):
+        while self.updating:
+            new_frame = self.get_new_depth_image()
+            self.buffer = new_frame
+            self.new_frame_ready = True
+
+    def start(self):
+        t = Thread(target=self.update, args=())
+        t.daemon = True
+        self.updating = True
+        t.start()
+        self.start_time = time.time()
+        return self
+
+    def read(self):
+        while not self.new_frame_ready:
+            pass
+        self.new_frame_ready = False
+        return self.buffer
+
+    def stop(self):
+        self.updating = False
+        self.pipeline.stop()
+
+
+class HoleDetector:
+    def __init__(self) -> None:
+        self.thresh_value = 90
+        self.thresh_delta = 5
+        self.current_contours = []
+        self.running = False
+        self.alpha = 0.07
+        self.current_contours = []
+        self.centres = [(-1, -1), (-1, -1)]
+        self.resolution = (848, 480)
+        # self.resolution = (1280, 720)
+        if self.resolution == (1280, 720):
+            self.hole_area_lb = 1500
+            self.hole_area_ub = 6500
+            self.blur_amount = 11
+        else:
+            self.hole_area_lb = 1000
+            self.hole_area_ub = 3000
+            self.blur_amount = 5
+        self.log = logging.getLogger("HOLE DETECTOR")
+
+    def start(self):
+        self.running = True
+        self.stream = DepthStream(self.resolution).start()
+
+    def stop(self):
+        self.running = False
+        self.stream.stop()
+
+    def get_new_frame(self):
+        raw_frame = self.stream.read()
+        return raw_frame
+
+    def preprocess_frame(self, raw_frame):
+        gray_colormap = cv2.convertScaleAbs(raw_frame, alpha=self.alpha)
+        _, black_mask = cv2.threshold(gray_colormap, 10, 255, cv2.THRESH_BINARY_INV)
+        fill_blur = cv2.medianBlur(gray_colormap, 51)
+        black_filled = gray_colormap.copy()
+        black_filled[np.where(black_mask == 255)] = fill_blur[np.where(black_mask == 255)]
+        sharper = cv2.equalizeHist(black_filled)
+        self.output = cv2.cvtColor(sharper, cv2.COLOR_GRAY2BGR)
+        blur = cv2.medianBlur(sharper, self.blur_amount)
+
+        cv2.imshow("orig", gray_colormap)
+        # cv2.imshow("fill blur", fill_blur)
+        # cv2.imshow("black mask", black_mask)
+        cv2.imshow("black filled", black_filled)
+        # cv2.imshow("minmax", minmax)
+        cv2.imshow("sharper", sharper)
+        cv2.imshow("blur", blur)
+        return blur
+
+    def run_thresh(self, blur_frame, correcting=False):
+        _, thresh = cv2.threshold(blur_frame, self.thresh_value, 255, cv2.THRESH_BINARY_INV)  # if we are clipping, have it move the 75 up by 5 and down by 5, and save new threah value that gave 2 correct area contours
+        canny = cv2.Canny(thresh, 75, 200)
+        # cv2.imshow("thresh", thresh)
+        cv2.imshow("canny", canny)
+        if correcting:
+            self.log.info(f"Running correcting thresh with {self.thresh_value}")
+            # cv2.waitKey(0)
+        return canny
+
+    def find_holes(self, canny_frame, blur_frame):
+        # contours, _ = cv2.findContours(canny_frame, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)CHAIN_APPROX_SIMPLE
+        contours, _ = cv2.findContours(canny_frame, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+        self.current_contours = []
+        # print("*********AREAS: ")
+        for contour in contours:
+            area = cv2.contourArea(contour)
+            # print(area)
+            if self.hole_area_lb < area < self.hole_area_ub:
+                self.current_contours.append(contour)
+                self.log.warning(f"Valid contour? {area}")
+        self.current_contours = self.current_contours[::2]
+
+        if len(self.current_contours) != 2:
+            self.log.critical(f"*** FOUND {len(self.current_contours)} CONTORUS INSTEAD OF 2!!! ***")
+            self.log.critical(f"ATTEMPTING TO CORRECT...")
+            self.log.info(f"Current thresh: {self.thresh_value}")
+            self.___correct_for_offset(blur_frame)
+
+            self.log.error(F'Total contours: {len(contours)}')
+            for x in contours:
+                self.log.error(f"Size: {cv2.contourArea(x)}")
+            self.log.error("")
+            return False
+
+        else:
+            for ix in range(2):
+                M = cv2.moments(self.current_contours[ix])
+                cX = int(M["m10"] / M["m00"])
+                cY = int(M["m01"] / M["m00"])
+                self.centres[ix] = (cX, cY)
+                cv2.fillPoly(self.output, pts = [self.current_contours[ix]], color=(255, 0, 0))
+                cv2.line(self.output, (cX-50, cY), (cX+50, cY), color=(0, 0, 255), thickness=3)
+                cv2.line(self.output, (cX, cY-50), (cX, cY+50), color=(0, 0, 255), thickness=3)
+            cv2.imshow("output", self.output)
+            return True
+
+
+    def get_hole_positions(self):
+        if not self.running:
+            self.start()
+        new_frame = self.get_new_frame()
+        blur_frame = self.preprocess_frame(new_frame)
+        canny_frame = self.run_thresh(blur_frame)
+        found_holes = self.find_holes(canny_frame, blur_frame)
+        cv2.waitKey(0)
+        if found_holes:
+            return (self.centres[0], self.centres[1])
+        return None, None
+
+    def __count_holes(self, canny_frame):
+        contours, _ = cv2.findContours(canny_frame, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+        current_contours = []
+        # self.log.info(f"Detected correction hole areas:")
+        for contour in contours:
+            area = cv2.contourArea(contour)
+            # self.log.info(area)
+            if self.hole_area_lb < area < self.hole_area_ub:
+                self.log.info(f"{area} is valid ({self.hole_area_lb}, {self.hole_area_ub})")
+                current_contours.append(contour)
+        current_contours = current_contours[::2]
+        if len(current_contours) == 2:
+            return True
+        return False
+
+    def ___correct_for_offset(self, blur_frame):
+        orig_thresh = self.thresh_value
+        self.log.error(f"Failed to detect with {self.thresh_value}, trying {orig_thresh + 0.5*self.thresh_delta}")
+        self.thresh_value = orig_thresh + 0.5*self.thresh_delta
+        canny_frame = self.run_thresh(blur_frame, correcting=True)
+        success = self.__count_holes(canny_frame)
+        if success:
+            return
+        else:
+            self.log.error(f"Failed to correct with {self.thresh_value}, trying {orig_thresh - 0.5*self.thresh_delta}")
+            self.thresh_value = orig_thresh - 0.5*self.thresh_delta
+            canny_frame = self.run_thresh(blur_frame, correcting=True)
+            success = self.__count_holes(canny_frame)
+            if success:
+                return
+            else:
+                self.log.error(f"Failed to correct with {self.thresh_value}, trying {orig_thresh + 1*self.thresh_delta}")
+                self.thresh_value = orig_thresh + 1*self.thresh_delta
+                canny_frame = self.run_thresh(blur_frame, correcting=True)
+                success = self.__count_holes(canny_frame)
+                if success:
+                    return
+                else:
+                    self.log.error(f"Failed to correct with {self.thresh_value}, trying {orig_thresh - 1*self.thresh_delta}")
+                    self.thresh_value = orig_thresh - 1*self.thresh_delta
+                    canny_frame = self.run_thresh(blur_frame, correcting=True)
+                    success = self.__count_holes(canny_frame)
+                    if success:
+                        return
+                    else:
+                        self.log.error(f"Failed to correct with {self.thresh_value}, trying {orig_thresh + 2*self.thresh_delta}")
+                        self.thresh_value = orig_thresh + 2*self.thresh_delta
+                        canny_frame = self.run_thresh(blur_frame, correcting=True)
+                        success = self.__count_holes(canny_frame)
+                        if success:
+                            return
+                        else:
+                            self.log.error(f"Failed to correct with {self.thresh_value}, trying {orig_thresh - 2*self.thresh_delta}")
+                            self.thresh_value = orig_thresh - 2*self.thresh_delta
+                            canny_frame = self.run_thresh(blur_frame, correcting=True)
+                            success = self.__count_holes(canny_frame)
+                            if success:
+                                return
+                            else:
+                                self.log.error(f"Failed to correct with {self.thresh_value}, trying {orig_thresh + 3*self.thresh_delta}")
+                                self.thresh_value = orig_thresh + 3*self.thresh_delta
+                                canny_frame = self.run_thresh(blur_frame, correcting=True)
+                                success = self.__count_holes(canny_frame)
+                                if success:
+                                    return
+                                else:
+                                    self.log.error(f"Failed to correct with {self.thresh_value}, trying {orig_thresh - 3*self.thresh_delta}")
+                                    self.thresh_value = orig_thresh - 3*self.thresh_delta
+                                    canny_frame = self.run_thresh(blur_frame)
+                                    success = self.__count_holes(canny_frame)
+                                    if success:
+                                        return
+                                    else:
+                                        self.log.critical("FAILED to correct thresh :(")
+                                        self.thresh_value = orig_thresh  # give up
+
+
+class HoleAligner:
+    def __init__(self, fl_ctrl) -> None:
+        self.log = logging.getLogger("HOLE ALIGNER")
+        self.vehicle = fl_ctrl
+        self.detector = HoleDetector()
+        self.left_hole_x_offset = 0  # TODO: find offsets with camera mounted
+        self.left_hole_y_offset = 0  # TODO: find offsets with camera mounted
+
+    def run_iteration(self):
+        hole_1, hole_2 = self.detector.get_hole_positions()
+        if hole_1[0] < hole_2[0]:
+            hole_of_interest = hole_1
+        else:
+            hole_of_interest = hole_2
+        offset_x = hole_of_interest[0] - self.left_hole_x_offset
+        offset_y = hole_of_interest[1] - self.left_hole_y_offset
+
+        self.log.info(f"Relative hole offsets: X: {offset_x}, Y: {offset_y}")
+
+        x_aligned = False
+        y_aligned = False
+
+        go_left = False
+        if offset_x > 0:
+            go_left = True
+
+        go_up = False
+        if offset_y < 0:
+            go_up = True
+
+        if offset_x < 50:
+            x_aligned = True
+        elif abs(offset_x) > 50:  # TODO: calibrate this
+            self.vehicle.nudge_fork_horiz(go_left, 0.2)
+        elif abs(offset_x) > 100:  # TODO: calibrate this
+            self.vehicle.nudge_fork_horiz(go_left, 0.4)
+        elif abs(offset_x) > 200:  # TODO: calibrate this
+            self.vehicle.nudge_fork_horiz(go_left, 0.6)
+        elif abs(offset_x) > 500:  # TODO: calibrate this
+            self.vehicle.nudge_fork_horiz(go_left, 1.0)
+        else:
+            self.log.critical("THIS SHOULD NOT HAPPEN!!!!! ()")
+
+        if offset_y < 50:
+            y_aligned = True
+        elif abs(offset_y) > 50:  # TODO: calibrate this
+            self.vehicle.nudge_fork_vert(go_up, 0.2)
+        elif abs(offset_y) > 100:  # TODO: calibrate this
+            self.vehicle.nudge_fork_vert(go_up, 0.4)
+        elif abs(offset_y) > 200:  # TODO: calibrate this
+            self.vehicle.nudge_fork_vert(go_up, 0.6)
+        elif abs(offset_y) > 500:  # TODO: calibrate this
+            self.vehicle.nudge_fork_vert(go_up, 1.0)
+        else:
+            self.log.critical("THIS SHOULD NOT HAPPEN!!!!!")
+
+        if x_aligned and y_aligned:
+            return True
+        return False
+
+    def perform_alignment(self):
+        self.detector.start()
+        alignment_done = False
+        while not alignment_done:
+            alignment_done = self.run_iteration()
+        self.log.info("Alignment complete")
+        return True
+
+
+
+if __name__ == "__main__":
+    a = HoleDetector()
+    a.start()
+    while True:
+        print(a.get_hole_positions())

Control/Jetson/pallet_hole_alignment_c.py

@@ -0,0 +1,223 @@
+from threading import Thread
+import cv2
+import time
+import apriltag
+import logging
+import platform
+import math
+from dataclasses import dataclass
+
+logging.basicConfig(level='DEBUG')
+
+class CUDA:
+    def __init__(self) -> None:
+        self.log = logging.getLogger("CUDA_helper")
+        self.log.setLevel("DEBUG")
+        self.cuda_allowed = False
+        self.use_CUDA = True
+        if self.use_CUDA:
+            self.log.warning("CUDA has been disabled due to performance issues...")
+            self.use_CUDA = False
+        else:
+            cuda_devices = cv2.cuda.getCudaEnabledDeviceCount()
+            if cuda_devices == 0:
+                self.log.warning("No CUDA devices found, will use CPU")
+                self.use_CUDA = False
+            else:
+                self.log.info("Running with CUDA 😎")
+
+    def bgr2gray(self, frame):
+        if self.use_CUDA:
+            gpu_frame = cv2.cuda_GpuMat()
+            gpu_frame.upload(frame)
+            gray_cuda = cv2.cuda.cvtColor(gpu_frame, cv2.COLOR_BGR2GRAY)
+            return gray_cuda.download()
+        else:
+            return cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+
+
+class VideoStream:
+    def __init__(self, cam_id=0) -> None:  # NOTE: id should be 1
+        self.stream = self.open_cam_usb(1280, 720)
+        _, img = self.stream.read()
+        self.buffer = img
+        self.running = False
+
+    def start(self):
+        t = Thread(target=self.update, args=())
+        t.daemon = True
+        self.running = True
+        t.start()
+        self.start_time = time.time()
+        return self
+
+    def update(self):
+        while self.running:
+            success, frame = self.stream.read()
+            assert success
+            self.buffer = frame
+        self.stream.release()
+
+    def read(self):
+        return self.buffer
+
+    def stop(self):
+        self.running = False
+
+    def open_cam_usb(self, width, height):
+        gst_str = ('v4l2src device=/dev/cam_align ! video/x-raw, width=(int){}, height=(int){} ! videoconvert ! appsink').format(width, height)
+        return cv2.VideoCapture(gst_str, cv2.CAP_GSTREAMER)
+
+
+class HoleDetector:
+    def __init__(self) -> None:
+        self.log = logging.getLogger("_C_ HOLE DETECTOR")
+        self.log.setLevel("DEBUG")
+        self.CUDA = CUDA()
+        self.options = apriltag.DetectorOptions(families="tag36h11", nthreads=4)
+        self.detector = apriltag.Detector(self.options)
+        self.stream = None
+
+    def start_stream(self):
+        self.stream = VideoStream().start()
+
+    def stop_streams(self):
+        self.stream.stop()
+
+    def get_tags(self):
+        frame = self.stream.read()
+        if frame.any():
+            gray_frame = self.CUDA.bgr2gray(frame)
+            # cv2.imshow("GR", gray_frame)
+            # cv2.waitKey(1)
+            result = self.detector.detect(gray_frame)
+            if result:
+                return result
+        return None
+
+
+@dataclass
+class Resolution:
+    width: int
+    height: int
+
+
+class HoleAligner:
+    def __init__(self, vehicle_ctrl) -> None:
+        self.log = logging.getLogger("_C_ HOLE ALIGNER")
+        self.log.setLevel("INFO")
+        self.detector = HoleDetector()
+        self.detector.start_stream()
+        self.resolution = Resolution(width=1280, height=720)
+        self.vehicle = vehicle_ctrl
+        self.tag_offset_x = 0
+        self.tag_offset_y = 0.4
+        self.tag_tresh_x = 0.1
+        self.tag_tresh_y = 0.1
+        self.iterations = 0
+         #(-0.08, 0.08)
+        self.range = 'X: -0.93 -> 0.93 | Y: -0.87 -> 0.87  ||  -1 -> 1'
+
+    def find_apriltag(self):
+        results = self.detector.get_tags()
+        if results:
+            center = results[0].center
+            pos_x = 2 * (center[0] - (self.resolution.width/2)) / self.resolution.width
+            pos_y = 2 * (center[1] - (self.resolution.height/2)) / self.resolution.height
+            clean_pos_y = float("{0:.3f}".format(pos_y))
+            clean_pos_x = float("{0:.3f}".format(pos_x))
+
+            translation = (clean_pos_x, clean_pos_y)
+            self.log.debug(f" X: {'+' if clean_pos_x > 0 else ''}{clean_pos_x}| Y: {'+' if clean_pos_y >0 else ''}{clean_pos_y}")
+            return translation
+        return None
+
+    def calculate_error(self, tag_location):
+        tag_x, tag_y = tag_location
+        err_x = float("{0:.2f}".format(tag_x + self.tag_offset_x))
+        err_y = float("{0:.2f}".format(tag_y + self.tag_offset_y))
+        self.log.info(f"Alignment error: X {err_x} | Y {err_y}")
+        return (err_x, err_y)
+
+    def run_adjustment_iteration(self, errors):
+        self.iterations += 1
+        if self.iterations > 4:
+            self.log.error(f"Too many iterations, exiting.")
+            return True
+        err_x, err_y = errors
+
+        x_aligned = False
+        y_aligned = False
+
+        go_left = False
+        if err_x < 0:
+            go_left = True
+
+        go_up = False
+        if err_y < 0:
+            go_up = True
+
+        if abs(err_x) > 0.3:  # TODO: calibrate this
+            self.vehicle.nudge_fork_horiz(go_left, 1.0)
+        elif abs(err_x) > 0.15:  # TODO: calibrate this
+            self.vehicle.nudge_fork_horiz(go_left, 0.6)
+        elif abs(err_x) > 0.1:  # TODO: calibrate this
+            self.vehicle.nudge_fork_horiz(go_left, 0.4)
+        elif abs(err_x) > 0.08:  # TODO: calibrate this
+            self.vehicle.nudge_fork_horiz(go_left, 0.2)
+        elif abs(err_x) <= 0.08:
+            x_aligned = True
+        else:
+            self.log.critical("THIS SHOULD NOT HAPPEN!!!!!")
+
+        # if abs(err_y) > 0.3:  # TODO: calibrate this
+        #     self.vehicle.nudge_fork_vert(go_up, 1.0)
+        # elif abs(err_y) > 0.15:  # TODO: calibrate this
+        #     self.vehicle.nudge_fork_vert(go_up, 0.6)
+        # elif abs(err_y) > 0.1:  # TODO: calibrate this
+        #     self.vehicle.nudge_fork_vert(go_up, 0.4)
+        # elif abs(err_y) > 0.08:  # TODO: calibrate this
+        #     self.vehicle.nudge_fork_vert(go_up, 0.2)
+        # elif abs(err_y) <= 0.08:
+        #     y_aligned = True
+        # else:
+        #     self.log.critical("THIS SHOULD NOT HAPPEN!!!!!")
+
+        if x_aligned:
+            self.log.info("We aligned.")
+            return True
+        return False
+
+    def align_fork(self):
+        is_aligned = False
+        while not is_aligned:
+            tag_position = self.find_apriltag()
+            if not tag_position:
+                self.log.info("Could not detect c̶o̶d̶e̶ holes")
+                continue
+            err = self.calculate_error(tag_position)
+            is_aligned = self.run_adjustment_iteration(err)
+            if is_aligned:
+                return True
+
+
+
+class DemoFL:
+    def __init__(self) -> None:
+        self.a = True
+    def nudge_fork_vert(self, dir, amt):
+        print(f"Moving {'UP' if dir else 'DOWN'} for {amt} s.")
+    def nudge_fork_horiz(self, dir, amt):
+        print(f"Moving {'LEFT' if dir else 'RIGHT'} for {amt} s.")
+
+if __name__ == "__main__":
+    demo_fl = DemoFL()
+    x = HoleAligner(demo_fl)
+    while True:
+        tag_pos = x.find_apriltag()
+        if not tag_pos:
+            # notify about failed identification
+            continue
+        err = x.calculate_error(tag_pos)
+        print(err)
+        # aligned = x.run_adjustment_iteration(err)

Control/Jetson/slash/etc/udev/rules.d/99-usb-camera.rules

@@ -0,0 +1,3 @@
+SUBSYSTEM=="video4linux", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="0892", SYMLINK+="cam_testing"
+SUBSYSTEM=="video4linux", ATTRS{idVendor}=="0c45", ATTRS{idProduct}=="636b", SYMLINK+="cam_track"
+SUBSYSTEM=="video4linux", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="082d", SYMLINK+="cam_align"

Control/Jetson/tests/MACHINE_STATE

@@ -0,0 +1 @@
+7

Control/Jetson/tests/__vision_threaded_cuda.py

@@ -0,0 +1,97 @@
+from threading import Thread
+
+import cv2
+import time
+import apriltag
+import logging
+
+class VideoStream:
+    def __init__(self, cam_id=0):
+        # self.stream = cv2.VideoCapture(cam_id)
+        # self.stream.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
+        # self.stream.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) test.mp4
+        self.stream = self.open_cam_usb(cam_id, 1280, 720)
+        _, img = self.stream.read()
+        self.buffer = img
+        self.frames = 0
+        self.running = False
+
+    def start(self):
+        t = Thread(target=self.update, args=())
+        t.daemon = True
+        self.running = True
+        t.start()
+        self.start_time = time.time()
+        return self
+
+    def update(self):
+        while self.running:
+            _, frame = self.stream.read()
+            self.buffer = frame
+
+            self.frames += 1
+            if self.frames == 30:
+                time_taken = time.time() - self.start_time
+                self.start_time = time.time()
+                self.frames = 0
+                fps = 30 / time_taken
+                print(f"##### INGEST FPS: {fps} #####")
+            # print('#')
+        self.stream.release()
+
+    def read(self):
+        return self.buffer
+
+    def stop(self):
+        self.running = False
+
+    def open_cam_usb(self, dev, width, height):
+    # We want to set width and height here, otherwise we could just do:
+    #     return cv2.VideoCapture(dev)
+    # We might get better framerates with this implementation...
+        gst_str = ('v4l2src device=/dev/video{} ! '
+                'video/x-raw, width=(int){}, height=(int){} ! '
+                'videoconvert ! appsink').format(dev, width, height)
+        return cv2.VideoCapture(gst_str, cv2.CAP_GSTREAMER)
+
+
+def cuda_gray_conveter(frame):
+    gpu_frame = cv2.cuda_GpuMat()
+    gpu_frame.upload(frame)
+    gray_cuda = cv2.cuda.cvtColor(gpu_frame, cv2.COLOR_BGR2GRAY)
+    gray = gray_cuda.download()
+    return gray
+
+
+options = apriltag.DetectorOptions(families="tag36h11", nthreads=2)
+detector = apriltag.Detector(options)
+
+logging.basicConfig(level='INFO')
+
+stream_0 = VideoStream(0).start()
+# stream_2 = VideoStream(1).start()
+
+frames = 0
+start = time.time()
+running = True
+while running:
+    try:
+        frame = stream_0.read()
+        if frame.any():
+            gray_frame = cuda_gray_conveter(frame)
+            result = detector.detect(gray_frame)
+            if result:
+                logging.info(result[0].center)
+            frames += 1
+            if frames == 30:
+                time_taken = time.time() - start
+                start = time.time()
+                frames = 0
+                fps = 30 / time_taken
+                print(f"##### TOTAL PROCESSING FPS: {fps} #####")
+    except Exception as e:
+        print('Exiting cleanly...')
+        stream_0.stop()
+        running = False
+
+print('Exited')

Control/Jetson/tests/debug.py

@@ -0,0 +1,76 @@
+import cv2
+import cv2.aruco as aruco
+# import dronekit
+# from dronekit import connect
+# from dronekit import VehicleMode
+# from dronekit import mavutil
+import logging
+import time
+
+logging.basicConfig(level='INFO')
+
+class VisionSystem:
+    def __init__(self) -> None:
+        self.capture = cv2.VideoCapture(0)
+        self.capture.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
+        self.capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
+        _, frame = self.capture.read()
+        logging.info("Initialised camera")
+        logging.info('Resolution: ' + str(frame.shape[0]) + ' x ' + str(frame.shape[1]))
+
+    def find_marker(self, img, markerSize = 6, totalMarkers=250):
+        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+        key = getattr(aruco, f'DICT_{markerSize}X{markerSize}_{totalMarkers}')
+        aruco_dict = aruco.Dictionary_get(key)
+        aruco_param = aruco.DetectorParameters_create()
+        bboxs, ids, rejected = aruco.detectMarkers(gray, aruco_dict, parameters=aruco_param)
+        if ids:
+            tl = bboxs[0][0][0][0], bboxs[0][0][0][1]
+            tr = bboxs[0][0][1][0], bboxs[0][0][1][1]
+            br = bboxs[0][0][2][0], bboxs[0][0][2][1]
+            bl = bboxs[0][0][3][0], bboxs[0][0][3][1]
+            # logging.info(f"{tl} | {tr}")
+            # logging.info(f"{bl} | {br}")
+            # logging.info(f"-----------")
+
+            # Get centre point of marker
+            total_x = [p[0] for p in [tr, tl, bl, br]]
+            total_y = [p[1] for p in [tr, tl, bl, br]]
+            centroid = (sum(total_x) / 4, sum(total_y) / 4)
+
+            # Get offset from centre
+            offset_y = (centroid[0] - (1280/2)) / 1280
+            offset_x = (centroid[1] - (720/2)) / 720
+            clean_offset_y = float("{0:.4f}".format(offset_y))
+            clean_offset_x = float("{0:.4f}".format(offset_x))
+
+            return (clean_offset_x, clean_offset_y)
+        return None
+
+    def calculate_angular_offset(self, pixel_offset):
+        '''
+        -0.5 -> 0.5
+        '''
+        logging.info("Calculating offset PWM signal")
+        x = pixel_offset[0]
+        y = float("{0:.1f}".format((pixel_offset[1] + 1.5) * 1000))
+        logging.info(f"Calculated offset: {y}")
+        return (x, y)
+
+    def get_pallet_angle(self):
+        while True:
+            success, img = self.capture.read()
+            logging.warning(f"Capture success: {success}")
+            logging.info("Looking for marker...")
+            raw_cargo_offset = self.find_marker(img)
+            logging.info(f"Data from find_marker(img): {raw_cargo_offset}")
+            # cv2.imshow('img',img)
+            if raw_cargo_offset:
+                logging.info(f"Found marker in get_pallet_angle {raw_cargo_offset}")
+                # cargo_offset = self.calculate_angular_offset(raw_cargo_offset)
+                # logging.info(f"PWM signal: {cargo_offset[1]}")
+                # return cargo_offset
+            # return None
+
+v = VisionSystem()
+v.get_pallet_angle()

Control/Jetson/tests/hole_finder/cv2_depth_frames.py

@@ -0,0 +1,47 @@
+import pyrealsense2.pyrealsense2 as rs
+import numpy as np
+import cv2
+
+# Configure depth and color streams
+pipeline = rs.pipeline()
+config = rs.config()
+
+# Get device product line for setting a supporting resolution
+pipeline_wrapper = rs.pipeline_wrapper(pipeline)
+pipeline_profile = config.resolve(pipeline_wrapper)
+device = pipeline_profile.get_device()
+device_product_line = str(device.get_info(rs.camera_info.product_line))
+
+found_rgb = True
+
+config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
+
+# Start streaming
+pipeline.start(config)
+
+try:
+    while True:
+
+        # Wait for a coherent pair of frames: depth and color
+        frames = pipeline.wait_for_frames()
+        depth_frame = frames.get_depth_frame()
+        if not depth_frame:
+            continue
+
+        # Convert images to numpy arrays
+        depth_image = np.asanyarray(depth_frame.get_data())
+
+        # Apply colormap on depth image (image must be converted to 8-bit per pixel first)
+        depth_colormap_c = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.2), cv2.COLORMAP_BONE)
+        depth_colormap = cv2.cvtColor(depth_colormap_c, cv2.COLOR_BGR2GRAY)
+        # depth_colormap = depth_colormap_c
+        # print(type(depth_colormap))
+
+        # Show images
+        cv2.imshow('RealSense', depth_colormap)
+        cv2.waitKey(1)
+
+finally:
+
+    # Stop streaming
+    pipeline.stop()

Control/Jetson/tests/hole_finder/depth_test.py

@@ -0,0 +1,230 @@
+import cv2
+import numpy as np
+import time
+
+
+# stream = cv2.VideoCapture("/Users/max/Desktop/THEA_CODE/Control/Jetson/tests/depth_vid.mp4")
+stream = cv2.VideoCapture("/Users/max/Desktop/THEA_CODE/Control/Jetson/tests/pallet_depth.mov")
+# ^^ somehow we need to pipe depth data into a minmaxed greyscale frame...
+
+THRESH_VALUE = 75
+
+class HoleFinder:
+    def __init__(self) -> None:
+        self.thresh_value = 75
+        self.thresh_delta = 5
+        self.current_contours = []
+        self.stream = cv2.VideoCapture("/Users/max/Desktop/THEA_CODE/Control/Jetson/tests/pallet_depth.mov")
+
+    def get_next_frame(self):
+        _, big_frame = self.stream.read()
+        frame = cv2.resize(big_frame, (852, 480))  # resize needs to be appropriate for speed optimizations
+        return frame
+
+    def disp_img(self, name, frame):
+        cv2.imshow(name, frame)
+
+    def grey_blur(self, frame):
+        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)  # this normalizes
+        blur = cv2.medianBlur(gray, 21)  # This is to prevent contouring issues and make them smooth
+        return blur
+
+    def thresh_canny(self, frame):
+        _, thresh = cv2.threshold(frame, self.thresh_value, 255, cv2.THRESH_BINARY_INV)  # if we are clipping, have it move the 75 up by 5 and down by 5, and save new threah value that gave 2 correct area contours
+        canny = cv2.Canny(thresh, 75, 200)
+        return canny
+
+    def find_contours(self, canny_frame):
+        contours, hierarchy = cv2.findContours(canny_frame, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
+        self.current_contours = []
+        for contour in contours:
+            area = cv2.contourArea(contour)
+            if 5000 < area < 11000:
+                self.current_contours.append(contour)
+        self.current_contours = self.current_contours[::2]
+
+    def cosmetics(self, source_frame):
+        msg = "Total holes: {}".format(len(self.current_contours))
+        cv2.putText(source_frame, msg, (20, 40), cv2.FONT_HERSHEY_PLAIN, 2, (0, 0, 255), 2, cv2.LINE_AA)
+        cv2.drawContours(source_frame, self.current_contours, -1, (0, 255, 0), 2)
+        for ix, i in enumerate(self.current_contours):
+            cv2.fillPoly(source_frame, pts = [i], color=(0, 0,255))
+
+    def evaluate_centres(self, source_frame):
+        # loop over the contours
+        for contour in self.current_contours:
+            # compute the center of the contour
+            M = cv2.moments(contour)
+            cX = int(M["m10"] / M["m00"])
+            cY = int(M["m01"] / M["m00"])
+            cv2.circle(source_frame, (cX, cY), 7, (255, 255, 255), -1)
+            print(f"### {cX}, {cY}")
+            cv2.line(source_frame, (cX-50, cY), (cX+50, cY), color=(255, 128, 0), thickness=3)
+            cv2.line(source_frame, (cX, cY-50), (cX, cY+50), color=(255, 255, 0), thickness=3)
+
+
+    def evaluate_contours(self, blur_frame):
+        canny_frame = self.thresh_canny(blur_frame)
+        self.disp_img("canny_orig", canny_frame)
+        self.find_contours(canny_frame)
+        if len(self.current_contours) < 2:
+            self.correct_for_offset(blur_frame)
+
+    def correct_for_offset(self, blur_frame):
+        orig_thresh = self.thresh_value
+        self.thresh_value = orig_thresh + 0.5*self.thresh_delta
+        success = self.test_contours(blur_frame)
+        if success:
+            return self.evaluate_contours(blur_frame)
+        else:
+            self.thresh_value = orig_thresh - 0.5*self.thresh_delta
+            success = self.test_contours(blur_frame)
+            if success:
+                return self.evaluate_contours(blur_frame)
+            else:
+                self.thresh_value = orig_thresh + 1*self.thresh_delta
+                success = self.test_contours(blur_frame)
+                if success:
+                    return self.evaluate_contours(blur_frame)
+                else:
+                    self.thresh_value = orig_thresh - 1*self.thresh_delta
+                    success = self.test_contours(blur_frame)
+                    if success:
+                        return self.evaluate_contours(blur_frame)
+                    else:
+                        self.thresh_value = orig_thresh + 1.5*self.thresh_delta
+                        success = self.test_contours(blur_frame)
+                        if success:
+                            return self.evaluate_contours(blur_frame)
+                        else:
+                            self.thresh_value = orig_thresh - 1.5*self.thresh_delta
+                            success = self.test_contours(blur_frame)
+                            if success:
+                                return self.evaluate_contours(blur_frame)
+                            else:
+                                self.thresh_value = orig_thresh + 2*self.thresh_delta
+                                success = self.test_contours(blur_frame)
+                                if success:
+                                    return self.evaluate_contours(blur_frame)
+                                else:
+                                    self.thresh_value = orig_thresh - 2*self.thresh_delta
+                                    success = self.test_contours(blur_frame)
+                                    if success:
+                                        return self.evaluate_contours(blur_frame)
+                                    else:
+                                        self.thresh_value = orig_thresh + 2.5*self.thresh_delta
+                                        success = self.test_contours(blur_frame)
+                                        if success:
+                                            return self.evaluate_contours(blur_frame)
+                                        else:
+                                            self.thresh_value = orig_thresh - 2.5*self.thresh_delta
+                                            success = self.test_contours(blur_frame)
+                                            if success:
+                                                return self.evaluate_contours(blur_frame)
+                                            else:
+                                                self.thresh_value = orig_thresh + 3*self.thresh_delta
+                                                success = self.test_contours(blur_frame)
+                                                if success:
+                                                    return self.evaluate_contours(blur_frame)
+                                                else:
+                                                    self.thresh_value = orig_thresh - 3*self.thresh_delta
+                                                    success = self.test_contours(blur_frame)
+                                                    if success:
+                                                        return self.evaluate_contours(blur_frame)
+                                                    else:
+                                                        self.thresh_value = orig_thresh + 4*self.thresh_delta
+                                                        success = self.test_contours(blur_frame)
+                                                        if success:
+                                                            return self.evaluate_contours(blur_frame)
+                                                        else:
+                                                            self.thresh_value = orig_thresh - 4*self.thresh_delta
+                                                            success = self.test_contours(blur_frame)
+                                                            if success:
+                                                                return self.evaluate_contours(blur_frame)
+                                                            else:
+                                                                self.thresh_value = orig_thresh  # give up
+
+    def test_contours(self, blur_frame):
+        canny_frame = self.thresh_canny(blur_frame)
+        self.disp_img("canny_test", canny_frame)
+        self.find_contours(canny_frame)
+        if len(self.current_contours) == 2:
+            return True
+        return False
+
+    def main(self):
+        # time.sleep(5)
+        while True:
+            source_frame = self.get_next_frame()
+            blur_frame = self.grey_blur(source_frame)
+            self.evaluate_contours(blur_frame)
+            self.cosmetics(source_frame)
+            self.evaluate_centres(source_frame)
+            self.disp_img("main", source_frame)
+            cv2.waitKey(1)
+
+
+a = HoleFinder()
+a.main()
+
+
+
+# while True:
+#     _, big_frame = stream.read()
+
+#     # frame = cv2.resize(big_frame, (852, 480))  # resize needs to be appropriate for speed optimizations
+#     # # cv2.imshow("orig", frame)
+
+#     # gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)  # this normalizes
+
+#     # blur = cv2.medianBlur(gray, 21)#.astype('uint8')
+#     # cv2.imshow("blur", blur)
+
+
+#     # _, thresh = cv2.threshold(blur, 75, 255, cv2.THRESH_BINARY_INV)  # if we are clipping, have it move the 75 up by 5 and down by 5, and save new threah value that gave 2 correct area contours
+#     # cv2.imshow("thresh", thresh)
+
+#     # canny = cv2.Canny(thresh, 75, 200)
+#     # cv2.imshow('canny', canny)
+
+#     contours, hierarchy = cv2.findContours(canny, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
+#     contour_list = []
+#     print("NEW_FRAME-----")
+#     for contour in contours:
+
+#         # approx = cv2.approxPolyDP(contour, 0.04* cv2.arcLength(contour, True), True)
+#         area = cv2.contourArea(contour)
+#         if 5000 < area < 11000:
+#             contour_list.append(contour)
+#     contour_list = contour_list[::2]
+
+#     print(len(contour_list))
+
+#     msg = "Total holes: {}".format(len(contour_list))
+#     cv2.putText(frame, msg, (20, 40), cv2.FONT_HERSHEY_PLAIN, 2, (0, 0, 255), 2, cv2.LINE_AA)
+
+#     cv2.drawContours(frame, contour_list, -1, (0, 255, 0), 2)
+
+
+#     for ix, i in enumerate(contour_list):
+#         cv2.fillPoly(frame, pts = [i], color=(0, 0,255))
+
+#     # loop over the contours
+#     # for contour in contour_list:
+#     #     # compute the center of the contour
+#     #     M = cv2.moments(contour)
+#     #     cX = int(M["m10"] / M["m00"])
+#     #     cY = int(M["m01"] / M["m00"])
+#     #     cv2.circle(frame, (cX, cY), 7, (255, 255, 255), -1)
+#     #     print(f"### {cX}, {cY}")
+#     #     cv2.line(frame, (cX-50, cY), (cX+50, cY), color=(255, 128, 0), thickness=2)
+#     #     cv2.line(frame, (cX, cY-50), (cX, cY+50), color=(255, 128, 0), thickness=2)
+
+
+#     # print()
+#     # print(len(contour_list))
+
+
+#     cv2.imshow('Objects Detected', frame)
+
+#     cv2.waitKey(0)

Control/Jetson/tests/hole_finder/forklift_alignment_testing_rgb.py

@@ -0,0 +1,73 @@
+import cv2
+import numpy as np
+
+frame = cv2.imread("/Users/max/Desktop/THEA_CODE/Control/Jetson/tests/flash.jpg")
+# img = cv2.imread("/Users/max/Desktop/THEA_CODE/Control/Jetson/tests/otest.png")
+# cv2.imshow("original", img)
+
+stream = cv2.VideoCapture(1)
+stream.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
+stream.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
+
+while True:
+    _, frame = stream.read()
+
+    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+    cv2.imshow("gray", gray)
+
+    blur = cv2.medianBlur(gray, 31)#.astype('uint8')
+    # cv2.imshow("blur", blur)
+
+    ret, thresh = cv2.threshold(blur, 127, 255, cv2.THRESH_OTSU)
+    # thresh = cv2.adaptiveThreshold(blur,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY,11,2)
+    cv2.imshow("thresh", thresh)
+
+    canny = cv2.Canny(thresh, 75, 200)
+    cv2.imshow('canny', canny)
+    # cv2.waitKey(0)
+
+    contours, hierarchy = cv2.findContours(canny, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
+    # contours, hierarchy = cv2.findContours(canny, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
+    contour_list = []
+    for contour in contours:
+
+        approx = cv2.approxPolyDP(contour, 0.04* cv2.arcLength(contour, True), True)
+        if len(approx) < 6:
+            print(len(approx))
+            area = cv2.contourArea(contour)
+            if 10000 < area < 60000:
+                contour_list.append(contour)
+
+    msg = "Total holes: {}".format(len(contour_list)//2)
+    cv2.putText(frame, msg, (20, 40), cv2.FONT_HERSHEY_PLAIN, 2, (0, 0, 255), 5, cv2.LINE_AA)
+
+
+    # contour_list = contour_list[::2]
+
+
+    cv2.drawContours(frame, contour_list, -1, (0, 255, 0), 2)
+
+
+    for ix, i in enumerate(contour_list):
+        cv2.fillPoly(frame, pts = [i], color=(0, 0,255))
+
+    # loop over the contours
+    for contour in contour_list:
+        # compute the center of the contour
+        M = cv2.moments(contour)
+        cX = int(M["m10"] / M["m00"])
+        cY = int(M["m01"] / M["m00"])
+        cv2.circle(frame, (cX, cY), 7, (255, 255, 255), -1)
+        print(f"### {cX}, {cY}")
+        cv2.line(frame, (cX-50, cY), (cX+50, cY), color=(255, 128, 0), thickness=2)
+        cv2.line(frame, (cX, cY-50), (cX, cY+50), color=(255, 128, 0), thickness=2)
+
+
+    # print()
+    # print(len(contour_list))
+
+
+    cv2.imshow('Objects Detected', frame)
+
+
+    cv2.waitKey(1)

Control/Jetson/tests/hole_finder/hole_detector.py

@@ -0,0 +1,107 @@
+import pyrealsense2.pyrealsense2 as rs
+import numpy as np
+import cv2
+from threading import Thread
+import logging
+import time
+
+
+logging.basicConfig(level='INFO')
+
+
+class DepthStream:
+    def __init__(self) -> None:
+        self.depth_stream = self.init_depth_stream()
+        self.start_depth_stream()
+        self.buffer = self.get_new_depth_image()
+        self.updating = False
+        self.new_frame_ready = False
+
+    def init_depth_stream(self):
+        self.pipeline = rs.pipeline()
+        self.config = rs.config()
+        self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
+
+    def start_depth_stream(self):
+        self.pipeline.start(self.config)
+
+    def get_new_depth_image(self):
+        frames = self.pipeline.wait_for_frames()
+        depth_frame = frames.get_depth_frame()
+        depth_image = np.asanyarray(depth_frame.get_data())
+        return depth_image
+
+    def update(self):
+        while self.updating:
+            new_frame = self.get_new_depth_image()
+            self.buffer = new_frame
+            self.new_frame_ready = True
+
+    def start(self):
+        t = Thread(target=self.update, args=())
+        t.daemon = True
+        self.updating = True
+        t.start()
+        self.start_time = time.time()
+        return self
+
+    def read(self):
+        while not self.new_frame_ready:
+            pass
+        self.new_frame_ready = False
+        return self.buffer
+
+    def stop(self):
+        self.updating = False
+        self.pipeline.stop()
+
+
+class HoleDetector:
+    def __init__(self) -> None:
+        self.thresh_value = 75
+        self.thresh_delta = 5
+        self.current_contours = []
+        self.running = False
+        self.alpha = 0.1
+
+    def start(self):
+        self.running = True
+        self.stream = DepthStream().start()
+
+    def stop(self):
+        self.running = False
+        self.stream.stop()
+
+    def get_new_frame(self):
+        raw_frame = self.stream.read()
+        return raw_frame
+
+    def preprocess_frame(self, raw_frame):
+        gray_colormap = cv2.convertScaleAbs(raw_frame, alpha=self.alpha)
+        # NOTE: night still need to do a bgr2gray to minmax the frame
+        blur = cv2.medianBlur(gray_colormap, 21)
+        _, thresh = cv2.threshold(blur, self.thresh_value, 255, cv2.THRESH_BINARY_INV)  # if we are clipping, have it move the 75 up by 5 and down by 5, and save new threah value that gave 2 correct area contours
+        canny = cv2.Canny(thresh, 75, 200)
+        cv2.imshow("gray_colormap", gray_colormap)
+        cv2.imshow("blur", blur)
+        cv2.imshow("thresh", thresh)
+        cv2.imshow("canny", canny)
+        return canny
+
+
+    def find_holes(self):
+        pass
+
+    def run(self):
+        self.start()
+        try:
+            while self.running:
+                new_frame = self.get_new_frame()
+                preprocessed_frame = self.preprocess_frame(new_frame)
+                cv2.waitKey(1)
+            self.stop()
+        finally:
+            self.stop()
+
+if __name__ == "__main__":
+    a = HoleDetector().run()

Control/Jetson/tests/state_machine_test.py

@@ -0,0 +1,55 @@
+import os
+import logging
+logging.basicConfig(level='INFO')
+
+class StateMachineHandler:
+    def __init__(self) -> None:
+        self.state_machine_file = "/Users/max/Desktop/THEA_CODE/Control/Jetson/tests/MACHINE_STATE"
+        # self.state_machine_file = "/home/pi/MACHINE_STATE"
+        self.__MACHINE_STATES = {
+                "IDLE": 0,
+                "NAVIGATING_TO_POINT_GPS": 1,
+                "NAVIGATING_TO_POINT_VISION": 2,
+                "MANIPULATING_CARGO": 3,
+                "READY_FOR_NEXT_STAGE": 4,
+                "RC_CONTROL": 5,
+                "PROXIMITY_STOP": 6,
+                "ESTOP": 7,
+                "OTHER_ERROR": 8
+            }
+        self.init_file()
+
+    def get_state(self):
+        if os.path.isfile(self.state_machine_file):
+            with open(self.state_machine_file, 'r') as smfile:
+                state = int(smfile.read())
+                return state, list(self.__MACHINE_STATES.keys())[list(self.__MACHINE_STATES.values()).index(int(state))]
+
+    def init_file(self, force=False):
+        if not os.path.isfile(self.state_machine_file) or force:
+            logging.info(f"Creating new state file, force flag is {force}")
+            with open(self.state_machine_file, 'w+') as smfile:
+                smfile.write(str(self.__MACHINE_STATES["IDLE"]))
+        else:
+            logging.info("State file already exists and force flag is absent, leaving file as is")
+
+    def set_state(self, state):
+        if self.check_state_validity():
+            with open(self.state_machine_file, 'w') as smfile:
+                smfile.write(str(self.__MACHINE_STATES.get(state, 8)))
+
+    def check_state_validity(self):
+        _, state = self.get_state()
+        if state == "ESTOP" or state == "OTHER_ERROR":
+            logging.critical(f"Machine state is {state}! Cannot perform operation")
+            return False
+        return True
+
+sm = StateMachineHandler()
+print(sm.get_state())
+sm.set_state("RC_CONTROL")
+print(sm.get_state())
+sm.set_state("ESTOP")
+print(sm.get_state())
+sm.set_state("NAVIGATING_TO_POINT_VISION")
+print(sm.get_state())

Control/Jetson/vision_cargo_approach.py

@@ -0,0 +1,285 @@
+import logging
+import math
+import platform
+
+import time
+from dataclasses import dataclass
+
+import cv2
+
+import apriltag_detector
+
+
+logging.basicConfig(level='INFO')
+
+@dataclass
+class Resolution:
+    width: int
+    height: int
+
+def remap(in_value, in_min, in_max, out_min, out_max):
+    remapped = (((in_value - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min
+    return remapped
+
+
+class VisionSystem:
+    def __init__(self) -> None:
+        self.log = logging.getLogger("Vision_system")
+        self.log.setLevel("DEBUG")
+        self.detector = apriltag_detector.Detector()
+        self.detector.start_streams(1)
+        self.resolution = Resolution(width=1280, height=720)
+
+    def find_apriltag(self):
+        results, pose = self.detector.get_tags()
+        if results:
+            center = results[0].center
+
+            corners = results[0].corners
+            top_left = (int(corners[0][0]), int(corners[0][1]))
+            top_right = (int(corners[1][0]), int(corners[1][1]))
+            botom_right = (int(corners[2][0]), int(corners[2][1]))
+            tag_area = self.get_apriltag_size(top_left, top_right, botom_right)
+
+            pos_x = 3 * (center[0] - (self.resolution.width/2)) / self.resolution.width
+            pos_y = 3 * (center[1] - (self.resolution.height/2)) / self.resolution.height
+            clean_pos_y = float("{0:.3f}".format(pos_y))
+            clean_pos_x = float("{0:.3f}".format(pos_x))
+            translation = (clean_pos_x, clean_pos_y)
+            x, y, z = pose
+            yaw = y
+            if yaw < -0.5:
+                yaw += 0.78  # Small hack to fix the pose estimator angle jump
+            rotation = (x, yaw, z)
+            return translation, rotation, tag_area
+        return None, None, None
+
+    def get_pallet_pose(self):
+        translation, rotation, area = self.find_apriltag()
+        if translation and rotation:
+            return (translation, rotation, area)
+        return None
+
+    def get_apriltag_size(self, p1, p2, p3):
+        top_left_x = p1[0]
+        top_left_y = p1[1]
+        top_right_x = p2[0]
+        top_right_y = p2[1]
+        bottom_right_x = p3[0]
+        bottom_right_y = p3[1]
+
+        dist_top_left_right = ((((top_right_x - top_left_x )**2) + ((top_right_y-top_left_y)**2) )**0.5)
+        dist_right_top_bottom = ((((top_right_x - bottom_right_x )**2) + ((top_right_y-bottom_right_y)**2) )**0.5)
+        area = dist_top_left_right * dist_right_top_bottom
+        return int(area)
+
+    def check_cargo_alignment(self, thresh_x, thresh_theta, position):
+        cargo_pos_x, cargo_pos_t = position
+        if cargo_pos_x > thresh_x[0] and cargo_pos_x < thresh_x[1]:
+            if cargo_pos_t > thresh_theta[0] and cargo_pos_t < thresh_theta[1]:
+                return True
+        return False
+
+    def find_apriltag_pc(self):
+        frame, results, pose = self.detector.get_tags_pc()
+        if results:
+            center = results[0].center
+            corners = results[0].corners
+            top_left = (int(corners[0][0]), int(corners[0][1]))
+            top_right = (int(corners[1][0]), int(corners[1][1]))
+            botom_right = (int(corners[2][0]), int(corners[2][1]))
+            bottom_left = (int(corners[3][0]), int(corners[3][1]))
+
+            font = cv2.FONT_HERSHEY_SIMPLEX
+            cv2.putText(frame, str(top_left), (int(corners[0][0]), int(corners[0][1])), font, 0.7, (255, 0, 255), 2, cv2.LINE_AA)
+            cv2.putText(frame, str(top_right), (int(corners[1][0]), int(corners[1][1])), font, 0.7, (255, 0, 255), 2, cv2.LINE_AA)
+            cv2.putText(frame, str(botom_right), (int(corners[2][0]), int(corners[2][1])), font, 0.7, (255, 0, 255), 2, cv2.LINE_AA)
+            cv2.putText(frame, str(bottom_left), (int(corners[3][0]), int(corners[3][1])), font, 0.7, (255, 0, 255), 2, cv2.LINE_AA)
+            tag_area = self.get_apriltag_size(top_left, top_right, botom_right)
+            print(tag_area)
+            print('---')
+            pos_x = 2 * (center[0] - (self.resolution.width/2)) / self.resolution.width
+            pos_y = 2 * (center[1] - (self.resolution.height/2)) / self.resolution.height
+            clean_pos_y = float("{0:.3f}".format(pos_y))
+            clean_pos_x = float("{0:.3f}".format(pos_x))
+            translation = (clean_pos_x, clean_pos_y)
+            x, y, z = pose
+            yaw = y
+            if yaw < -0.5:
+                yaw += 0.78
+            rotation = (x, yaw, z)
+            return frame, translation, rotation, tag_area
+        return frame, None, None, None
+
+    def get_pallet_pose_pc(self):
+        frame, translation, rotation, area = self.find_apriltag_pc()
+        if frame.any():
+            cv2.imshow('frame', frame)
+            if translation and rotation and area:
+                # print(f"Translation_x: {translation[0]}, Rotation_y: {rotation[1]}")
+                return (translation, rotation, area)
+
+
+class CargoApproachingSystem:
+    def __init__(self) -> None:
+        self.ky = 2.2
+        self.kt = -1
+
+    def calculate(self, ey, et):
+        delta = self.ky * math.atan(ey) + self.kt * et
+        return delta
+
+
+class Tester:
+    def __init__(self) -> None:
+        self.log = logging.getLogger("TESTER")
+        self.log.setLevel("DEBUG")
+        self.camera = VisionSystem()
+        self.steering_law = CargoApproachingSystem()
+        self.no_detects = 0
+        self.max_approach_area_thresh = 40000
+        self.centre_distance_thresh = (-0.04, 0.08)
+        self.centre_angle_thresh = (-0.2, 0.2)
+
+    def run_pc(self):
+        while True:
+            pallet_pose = self.camera.get_pallet_pose_pc()
+            if pallet_pose:
+                position, rotation, area = pallet_pose
+                error_y = position[0]
+                error_theta = rotation[1]
+                # print(error_y, error_theta)
+                delta = self.steering_law.calculate(error_y, error_theta)
+                # delta_PWM = remap(delta, -0.8, 0.8, 1000, 2000)
+                delta_PWM = remap(delta, -0.7, 0.7, 500, 2500)
+                print(f"PWM: {delta_PWM}, ey: {error_y}, et: {error_theta}")
+            cv2.waitKey(1)
+
+    def run_jetson(self):
+        self.log.info("Starting vision system")
+        self.is_running = True
+        while self.is_running:
+            time.sleep(0.05)
+            pallet_pose = self.camera.get_pallet_pose()  # getting the position, rotation and size of the tracker
+            if pallet_pose:  # If we found a tracker
+                self.no_detects = 0
+                position, rotation, area = pallet_pose
+                error_y = position[0]  # centre offset distance
+                error_theta = rotation[1]  # centre offset angle
+                # print(error_y, error_theta)
+                delta = self.steering_law.calculate(error_y, error_theta)  # getting the steering angle from the control law
+                delta_PWM = remap(delta, -0.7, 0.7, 500, 2500)  # Converting steering law output into PWM values
+                self.log.info(f"PWM: {delta_PWM}, ey: {error_y}, et: {error_theta}")
+                if area < self.max_approach_area_thresh:  # If we are far enough from the cargo, move towards it
+                    self.log.info(f"Setting speed: 1950, steering: {delta_PWM}")
+                else:
+                    self.log.info(f"We are close enough to cargo!, {area}")
+                    is_aligned = self.camera.check_cargo_alignment(self.centre_distance_thresh, self.centre_angle_thresh, (error_y, error_theta))
+                    if is_aligned:
+                        self.log.info(f'Cargo aligned: {error_y} in {self.centre_distance_thresh} and {error_theta} in {self.centre_angle_thresh}')
+                    else:
+                        self.log.info(f'Cargo NOT aligned: {error_y} not in {self.centre_distance_thresh} or {error_theta} inot n {self.centre_angle_thresh}')
+
+            else:
+                self.no_detects +=1
+                if self.no_detects > 3:  # if we do not detect anything 3 times in a row, stop the vehicle
+                    self.log.info("No cargo detected, setting speed to 1500")
+
+
+class VisionCargoApproacher:
+    def __init__(self, forklift_comms) -> None:
+        self.log = logging.getLogger("VISION CARGO APPROACHER")
+        self.log.setLevel("DEBUG")
+        self.camera = VisionSystem()
+        self.forklift = forklift_comms
+        self.steering_law = CargoApproachingSystem()
+        self.is_running = False
+        self.no_detects = 0  # for how many frames did we not detect an apriltag (stops forklift)
+        self.no_detect_limit = 3  # how many no detect frames are allowed to pass before we stop the forklift
+        # self.max_approach_area_thresh = 3000  # How much area should the Apriltag take up (pixels) before we send the stop comand (how close to the cargo do we stop)
+        self.max_approach_area_thresh = 4500  # How much area should the Apriltag take up (pixels) before we send the stop comand (how close to the cargo do we stop)
+        self.centre_distance_thresh = (-0.2, 0.2)  # horizontal tolerance for pallet alignment pre fork alignment
+        self.centre_angle_thresh = (-0.2, 0.2)  # angular tolerance for pallet alignment pre fork alignment
+        self.horizontal_camera_offset = 0  # In case the camera isn't perfectly straight, we can compansate for that in code
+
+    def navigate_to_cargo_vision(self):
+        self.log.info("Starting vision system")
+        self.is_running = True
+        self.forklift.set_state('NAVIGATING_TO_POINT_VISION')
+        while self.is_running:
+            time.sleep(0.05)
+            pallet_pose = self.camera.get_pallet_pose()  # getting the position, rotation and size of the tracker
+            if pallet_pose:  # If we found a tracker
+                self.no_detects = 0
+                position, rotation, area = pallet_pose
+                error_x = position[0]  # centre offset distance
+                error_theta = rotation[1]  # centre offset angle
+                error_x += self.horizontal_camera_offset  # adding offset
+                # print(error_x, error_theta)
+                delta = self.steering_law.calculate(error_x, error_theta)  # getting the steering angle from the control law
+                self.log.info("***")
+                self.log.info(f"Error y {delta} | error theta {error_x} | delta {error_theta}")
+                self.log.info("***")
+                # delta_PWM = remap(delta, -0.8, 0.8, 1000, 2000)
+                delta_PWM = remap(delta, -0.7, 0.7, 3000, 1)  # Converting steering law output into PWM values
+                self.log.info(f"PWM: {delta_PWM}, ey: {error_x}, et: {error_theta}")
+                self.log.warning(f"TAG AREA: {area}")
+                if area < self.max_approach_area_thresh:  # If we are far enough from the cargo, move towards it
+                    speed_pwm = remap(area, 40000, 1000, 1500, 2000)
+                    self.forklift.set_speed_steering(speed_pwm, delta_PWM)
+                else:
+                    self.forklift.set_speed_steering(1500, 1500)  # If we close enough to the cargo, stop and move to next stage
+                    self.log.info(f"We are close enough to cargo!, {area}")
+                    # TODO: check that we properly lighed up with cargo, if that is not the case, drive back for a bit with correct angle and rerun the approacher
+                    is_aligned = self.camera.check_cargo_alignment(self.centre_distance_thresh, self.centre_angle_thresh, (error_x, error_theta))
+                    if is_aligned:
+                        self.log.info(f"WE ALIGNED, MOVE BACK! (centre_distance_thresh: {error_x}, centre_angle_thresh: {error_theta}")
+                        time.sleep(1)
+                        success = True
+                        self.is_running = False
+                        # self.camera.detector.stop_streams()
+                        return success
+                    else:
+                        self.log.info(f"NOT ALIGNED, MOVE BACK! (centre_distance_thresh: {error_x}, centre_angle_thresh: {error_theta}")
+                        time.sleep(0.2)
+                        return True
+                        delta = self.steering_law.calculate(error_x, error_theta)  # getting the steering angle from the control law
+                        delta_PWM = remap(delta, 0.7, -0.7, 2000, 1000)
+                        self.forklift.set_speed_steering(1000, delta_PWM)
+                        time.sleep(3)
+                        self.forklift.set_speed_steering(1500, 1500)
+                        # Move back for n secnods with x steering (depending on what side the error is)
+            else:
+                self.no_detects +=1
+                if self.no_detects > self.no_detect_limit:  # if we do not detect anything 3 times in a row, stop the vehicle
+                    self.log.warning("DID NOT DETECT CODE!")
+                    self.forklift.set_speed(1500)
+
+    # def stop(self):
+    #     self.is_running = False
+    #     self.forklift.set_disarm()
+
+    # def end(self):
+    #     self.stop()
+    #     self.forklift.vehicle.close()
+
+# TESTING = False
+
+# if platform.release() == '4.9.201-tegra':
+#     if TESTING:
+#         a = Tester()
+#         a.run_jetson()
+#     else:
+#         run = Super()
+#         run.start()
+
+
+# elif platform.release() in ['5.11.0-18-generic', '20.5.0', '5.10.17-v7l+']:
+#     a = Tester()
+#     a.run_pc()
+
+# else:
+#     logging.critical(f"Platform {platform.release()} not tested!")
+
+if __name__ == "__main__":
+    print("ERROR: This script is no longer used in standalone mode")

Control/Pi/ICM20948.py

@@ -0,0 +1,210 @@
+import math
+import time
+from dataclasses import dataclass
+from threading import Thread
+import logging
+
+# import matplotlib.pyplot as plt
+import numpy as np
+import serial
+
+logging.basicConfig()
+logging.root.setLevel(logging.NOTSET)
+logging.basicConfig(level=logging.NOTSET)
+
+@dataclass
+class CompassOffsets:
+    min_x: float
+    max_x: float
+    min_y: float
+    max_y: float
+    hard_offset_x: float
+    hard_offset_y: float
+
+
+pre_calibrated_values = CompassOffsets(0, 0, 0, 0, 0, 0) #-17.25, 16.95, -43.8, -13.95, 0.15000000000000036, 28.875 fl outside
+
+
+class ICM20948DataStream:
+    def __init__(self, com_port="/dev/cu.usbmodem1462201", baudrate=115200) -> None:
+        """
+        Other com port: /dev/cu.SLAB_USBtoUART
+        """
+        self.serial = serial.Serial(com_port, baudrate=baudrate)
+        init_message = str(self.serial.readline())
+        assert 'I2C begin Failed' not in init_message, "No I2C sensor detected!"
+        self.mag = {"x": 0, "y": 0, "z": 0}
+        self.running = False
+        self.log = logging.getLogger("COMPASS DATA STREAM")
+        self.log.setLevel(logging.DEBUG)
+        time.sleep(1)
+
+    def start(self):
+        t = Thread(target=self.update, args=())
+        t.daemon = True
+        self.running = True
+        t.start()
+        return self
+
+    def update(self):
+        while self.running:
+            try:
+                ser_bytes = str(self.serial.readline())
+                useful = ser_bytes.split('\\')[0].split('\'')[1]
+                mag_data_raw = useful.split(';')
+                mag_raw_x = float(mag_data_raw[0])
+                mag_raw_y = float(mag_data_raw[1])
+                mag_raw_z = float(mag_data_raw[2])
+                self.mag = {"x": mag_raw_x, "y": mag_raw_y, "z": mag_raw_z}
+            except Exception as e:
+                self.log.error(f"Error: {e}")
+                self.log.error(f"Got bytes: {ser_bytes}")
+
+    def read(self):
+        return self.mag
+
+    def stop(self):
+        self.running = False
+
+
+class ICM20948:
+    def __init__(self, sensor, pre_calibreted=None):
+        self.sensor = sensor
+        self.offsets = CompassOffsets(0, 0, 0, 0, 0, 0)
+        self.calibrated = False
+        if pre_calibreted:
+            self.calibrated = True
+            self.offsets = pre_calibreted
+        self.__permanent_offset = 0
+        self.history = np.zeros(15)
+        self.current_heading = 0
+        self.previous_heading = 0
+        self.running = False
+        self.log = logging.getLogger("MAIN COMPASS CLASS")
+        self.log.setLevel(logging.DEBUG)
+
+    def record(self, rec_time=30):
+        end_time = time.time() + rec_time
+        readings_x = []
+        readings_y = []
+        self.log.info("Start recording")
+        while time.time() < end_time:
+            time.sleep(0.01)
+            new_reading = self.sensor.read()
+            if new_reading['x'] == 0 or new_reading['y'] == 0:
+                self.log.warning("Sensor warning!")
+            else:
+                readings_x.append(new_reading['x'])
+                readings_y.append(new_reading['y'])
+        self.log.info("Done")
+        return readings_x, readings_y
+
+    def set_offset(self, offset):
+        self.__permanent_offset = offset
+
+    def calibrate(self, calib_time=30):
+        raw_x, raw_y = self.record(calib_time)
+        min_x, max_x, min_y, max_y = self.clean_calib_data(raw_x, raw_y)
+        hard_offset_x = -(min_x + max_x) / 2
+        hard_offset_y = -(min_y + max_y) / 2
+        self.offsets.min_x = min_x
+        self.offsets.max_x = max_x
+        self.offsets.min_y = min_y
+        self.offsets.max_y = max_y
+        self.offsets.hard_offset_x = hard_offset_x
+        self.offsets.hard_offset_y = hard_offset_y
+        self.log.info(f"Calibration complete, Values: min_x: {min_x}, max_x: {max_x}, min_y: {min_y}, max_y: {max_y}, hard_offset_x: {hard_offset_x}, hard_offset_y: {hard_offset_y}")
+        self.log.info(f"Copyable calib params: {min_x}, {max_x}, {min_y}, {max_y}, {hard_offset_x}, {hard_offset_y}")
+        self.calibrated = True
+        self.previous_heading = 0
+
+    def clean_calib_data(self, x, y, max_deviations=2):
+        x_np = np.asarray(x)
+        y_np = np.asarray(y)
+        # Clean x axis outlier pairs
+        x_std = np.std(x_np)
+        x_mean = np.mean(x_np)
+        x_distance_from_mean = abs(x_np - x_mean)
+        x_not_outlier = x_distance_from_mean < max_deviations * x_std
+        x_outlier = x_distance_from_mean > max_deviations * x_std
+        x_np_clean = x_np[x_not_outlier]
+        y_np_clean = y_np[x_not_outlier]
+        # Clean y axis outlier pairs
+        y_std = np.std(y_np_clean)
+        y_mean = np.mean(y_np_clean)
+        y_distance_from_mean = abs(y_np_clean - y_mean)
+        y_not_outlier = y_distance_from_mean < max_deviations * y_std
+        y_outlier = y_distance_from_mean > max_deviations * y_std
+        y_np_clean = y_np_clean[y_not_outlier]
+        x_np_clean = x_np_clean[y_not_outlier]
+
+        assert len(x_np_clean) == len(y_np_clean)
+        self.log.info(f"Total outliers: {len(x_np) - len(x_np_clean)}")
+        self.log.info(f"Original Min x: {min(x)}, Max x: {max(x)}, Min y: {min(y)}, Max y:{max(y)}")
+        self.log.info(f"Clean Min x: {min(x_np_clean)}, Max x: {max(x_np_clean)}, Min y: {min(y_np_clean)}, Max y: {max(y_np_clean)}")
+        return min(x_np_clean), max(x_np_clean), min(y_np_clean), max(y_np_clean)
+
+    def raw_2_correct(self, x, y):
+        assert self.calibrated
+        # hard_corrected_x = x + self.offsets.hard_offset_x
+        # hard_corrected_y = y + self.offsets.hard_offset_y
+
+        fully_corrected_x = (x - self.offsets.min_x) / (self.offsets.max_x - self.offsets.min_x) * 2 - 1
+        fully_corrected_y = (y - self.offsets.min_y) / (self.offsets.max_y - self.offsets.min_y) * 2 - 1
+        return fully_corrected_x, fully_corrected_y
+#         return hard_corrected_x, hard_corrected_y
+
+    def calculate_immediate_heading(self):
+        heading, _ = self.get_heading_and_data()
+        return heading
+
+    def get_heading_and_data(self):
+        new_reading = self.sensor.read()
+        if new_reading['x'] == 0 or new_reading['y'] == 0:
+            self.log.warning("Sensor warning!")
+            return self.previous_heading, (0, 0)
+        else:
+            corrected_x, corrected_y = self.raw_2_correct(new_reading['x'], new_reading['y'])
+            heading = -(180 * math.atan2(corrected_y, corrected_x) / math.pi)
+            heading += self.__permanent_offset
+            if heading < 0:
+                heading += 360
+            if heading > 360:
+                heading -= 360
+            self.previous_heading = heading
+            return heading, (corrected_x, corrected_y)
+
+    def get_smooth_heading(self):
+        new_heading = self.calculate_immediate_heading()
+        self.history = np.roll(self.history, -1)
+        self.history[-1] = new_heading
+        x = y = 0
+        for angle in self.history:
+            x += math.cos(math.radians(angle))
+            y += math.sin(math.radians(angle))
+        average_angle_rad = math.atan2(y, x)
+        average_angle = 180 * average_angle_rad / math.pi
+        if average_angle < 0:
+            average_angle += 360
+        if average_angle > 360:
+            average_angle -= 360
+        # print(f"Avg: {average_angle}, Hist: {self.history}, x: {x}, y: {y}")
+        return average_angle
+
+    def update(self):
+        while self.running:
+            self.current_heading = self.get_smooth_heading()
+            time.sleep(0.05)
+
+    def read(self):
+        return self.current_heading
+
+    def stop(self):
+        self.running = False
+
+    def start(self):
+        t2 = Thread(target=self.update, args=())
+        t2.daemon = True
+        self.running = True
+        t2.start()
+        return self

Control/Pi/PCU_status_server/package-lock.json

@@ -0,0 +1,751 @@
+{
+  "name": "system_status_ui_server",
+  "version": "1.0.0",
+  "lockfileVersion": 1,
+  "requires": true,
+  "dependencies": {
+    "accepts": {
+      "version": "1.3.7",
+      "resolved": "https://registry.npmjs.org/accepts/-/accepts-1.3.7.tgz",
+      "integrity": "sha512-Il80Qs2WjYlJIBNzNkK6KYqlVMTbZLXgHx2oT0pU/fjRHyEp+PEfEPY0R3WCwAGVOtauxh1hOxNgIf5bv7dQpA==",
+      "requires": {
+        "mime-types": "~2.1.24",
+        "negotiator": "0.6.2"
+      }
+    },
+    "ajv": {
+      "version": "6.12.6",
+      "resolved": "https://registry.npmjs.org/ajv/-/ajv-6.12.6.tgz",
+      "integrity": "sha512-j3fVLgvTo527anyYyJOGTYJbG+vnnQYvE0m5mmkc1TK+nxAppkCLMIL0aZ4dblVCNoGShhm+kzE4ZUykBoMg4g==",
+      "requires": {
+        "fast-deep-equal": "^3.1.1",
+        "fast-json-stable-stringify": "^2.0.0",
+        "json-schema-traverse": "^0.4.1",
+        "uri-js": "^4.2.2"
+      }
+    },
+    "array-flatten": {
+      "version": "1.1.1",
+      "resolved": "https://registry.npmjs.org/array-flatten/-/array-flatten-1.1.1.tgz",
+      "integrity": "sha1-ml9pkFGx5wczKPKgCJaLZOopVdI="
+    },
+    "asn1": {
+      "version": "0.2.4",
+      "resolved": "https://registry.npmjs.org/asn1/-/asn1-0.2.4.tgz",
+      "integrity": "sha512-jxwzQpLQjSmWXgwaCZE9Nz+glAG01yF1QnWgbhGwHI5A6FRIEY6IVqtHhIepHqI7/kyEyQEagBC5mBEFlIYvdg==",
+      "requires": {
+        "safer-buffer": "~2.1.0"
+      }
+    },
+    "assert-plus": {
+      "version": "1.0.0",
+      "resolved": "https://registry.npmjs.org/assert-plus/-/assert-plus-1.0.0.tgz",
+      "integrity": "sha1-8S4PPF13sLHN2RRpQuTpbB5N1SU="
+    },
+    "asynckit": {
+      "version": "0.4.0",
+      "resolved": "https://registry.npmjs.org/asynckit/-/asynckit-0.4.0.tgz",
+      "integrity": "sha1-x57Zf380y48robyXkLzDZkdLS3k="
+    },
+    "aws-sign2": {
+      "version": "0.7.0",
+      "resolved": "https://registry.npmjs.org/aws-sign2/-/aws-sign2-0.7.0.tgz",
+      "integrity": "sha1-tG6JCTSpWR8tL2+G1+ap8bP+dqg="
+    },
+    "aws4": {
+      "version": "1.11.0",
+      "resolved": "https://registry.npmjs.org/aws4/-/aws4-1.11.0.tgz",
+      "integrity": "sha512-xh1Rl34h6Fi1DC2WWKfxUTVqRsNnr6LsKz2+hfwDxQJWmrx8+c7ylaqBMcHfl1U1r2dsifOvKX3LQuLNZ+XSvA=="
+    },
+    "axios": {
+      "version": "0.21.1",
+      "resolved": "https://registry.npmjs.org/axios/-/axios-0.21.1.tgz",
+      "integrity": "sha512-dKQiRHxGD9PPRIUNIWvZhPTPpl1rf/OxTYKsqKUDjBwYylTvV7SjSHJb9ratfyzM6wCdLCOYLzs73qpg5c4iGA==",
+      "requires": {
+        "follow-redirects": "^1.10.0"
+      }
+    },
+    "basic-auth": {
+      "version": "2.0.1",
+      "resolved": "https://registry.npmjs.org/basic-auth/-/basic-auth-2.0.1.tgz",
+      "integrity": "sha512-NF+epuEdnUYVlGuhaxbbq+dvJttwLnGY+YixlXlME5KpQ5W3CnXA5cVTneY3SPbPDRkcjMbifrwmFYcClgOZeg==",
+      "requires": {
+        "safe-buffer": "5.1.2"
+      }
+    },
+    "bcrypt-pbkdf": {
+      "version": "1.0.2",
+      "resolved": "https://registry.npmjs.org/bcrypt-pbkdf/-/bcrypt-pbkdf-1.0.2.tgz",
+      "integrity": "sha1-pDAdOJtqQ/m2f/PKEaP2Y342Dp4=",
+      "requires": {
+        "tweetnacl": "^0.14.3"
+      }
+    },
+    "body-parser": {
+      "version": "1.19.0",
+      "resolved": "https://registry.npmjs.org/body-parser/-/body-parser-1.19.0.tgz",
+      "integrity": "sha512-dhEPs72UPbDnAQJ9ZKMNTP6ptJaionhP5cBb541nXPlW60Jepo9RV/a4fX4XWW9CuFNK22krhrj1+rgzifNCsw==",
+      "requires": {
+        "bytes": "3.1.0",
+        "content-type": "~1.0.4",
+        "debug": "2.6.9",
+        "depd": "~1.1.2",
+        "http-errors": "1.7.2",
+        "iconv-lite": "0.4.24",
+        "on-finished": "~2.3.0",
+        "qs": "6.7.0",
+        "raw-body": "2.4.0",
+        "type-is": "~1.6.17"
+      }
+    },
+    "bytes": {
+      "version": "3.1.0",
+      "resolved": "https://registry.npmjs.org/bytes/-/bytes-3.1.0.tgz",
+      "integrity": "sha512-zauLjrfCG+xvoyaqLoV8bLVXXNGC4JqlxFCutSDWA6fJrTo2ZuvLYTqZ7aHBLZSMOopbzwv8f+wZcVzfVTI2Dg=="
+    },
+    "caseless": {
+      "version": "0.12.0",
+      "resolved": "https://registry.npmjs.org/caseless/-/caseless-0.12.0.tgz",
+      "integrity": "sha1-G2gcIf+EAzyCZUMJBolCDRhxUdw="
+    },
+    "combined-stream": {
+      "version": "1.0.8",
+      "resolved": "https://registry.npmjs.org/combined-stream/-/combined-stream-1.0.8.tgz",
+      "integrity": "sha512-FQN4MRfuJeHf7cBbBMJFXhKSDq+2kAArBlmRBvcvFE5BB1HZKXtSFASDhdlz9zOYwxh8lDdnvmMOe/+5cdoEdg==",
+      "requires": {
+        "delayed-stream": "~1.0.0"
+      }
+    },
+    "content-disposition": {
+      "version": "0.5.3",
+      "resolved": "https://registry.npmjs.org/content-disposition/-/content-disposition-0.5.3.tgz",
+      "integrity": "sha512-ExO0774ikEObIAEV9kDo50o+79VCUdEB6n6lzKgGwupcVeRlhrj3qGAfwq8G6uBJjkqLrhT0qEYFcWng8z1z0g==",
+      "requires": {
+        "safe-buffer": "5.1.2"
+      }
+    },
+    "content-type": {
+      "version": "1.0.4",
+      "resolved": "https://registry.npmjs.org/content-type/-/content-type-1.0.4.tgz",
+      "integrity": "sha512-hIP3EEPs8tB9AT1L+NUqtwOAps4mk2Zob89MWXMHjHWg9milF/j4osnnQLXBCBFBk/tvIG/tUc9mOUJiPBhPXA=="
+    },
+    "cookie": {
+      "version": "0.4.0",
+      "resolved": "https://registry.npmjs.org/cookie/-/cookie-0.4.0.tgz",
+      "integrity": "sha512-+Hp8fLp57wnUSt0tY0tHEXh4voZRDnoIrZPqlo3DPiI4y9lwg/jqx+1Om94/W6ZaPDOUbnjOt/99w66zk+l1Xg=="
+    },
+    "cookie-signature": {
+      "version": "1.0.6",
+      "resolved": "https://registry.npmjs.org/cookie-signature/-/cookie-signature-1.0.6.tgz",
+      "integrity": "sha1-4wOogrNCzD7oylE6eZmXNNqzriw="
+    },
+    "core-util-is": {
+      "version": "1.0.2",
+      "resolved": "https://registry.npmjs.org/core-util-is/-/core-util-is-1.0.2.tgz",
+      "integrity": "sha1-tf1UIgqivFq1eqtxQMlAdUUDwac="
+    },
+    "dashdash": {
+      "version": "1.14.1",
+      "resolved": "https://registry.npmjs.org/dashdash/-/dashdash-1.14.1.tgz",
+      "integrity": "sha1-hTz6D3y+L+1d4gMmuN1YEDX24vA=",
+      "requires": {
+        "assert-plus": "^1.0.0"
+      }
+    },
+    "debug": {
+      "version": "2.6.9",
+      "resolved": "https://registry.npmjs.org/debug/-/debug-2.6.9.tgz",
+      "integrity": "sha512-bC7ElrdJaJnPbAP+1EotYvqZsb3ecl5wi6Bfi6BJTUcNowp6cvspg0jXznRTKDjm/E7AdgFBVeAPVMNcKGsHMA==",
+      "requires": {
+        "ms": "2.0.0"
+      }
+    },
+    "delayed-stream": {
+      "version": "1.0.0",
+      "resolved": "https://registry.npmjs.org/delayed-stream/-/delayed-stream-1.0.0.tgz",
+      "integrity": "sha1-3zrhmayt+31ECqrgsp4icrJOxhk="
+    },
+    "depd": {
+      "version": "1.1.2",
+      "resolved": "https://registry.npmjs.org/depd/-/depd-1.1.2.tgz",
+      "integrity": "sha1-m81S4UwJd2PnSbJ0xDRu0uVgtak="
+    },
+    "destroy": {
+      "version": "1.0.4",
+      "resolved": "https://registry.npmjs.org/destroy/-/destroy-1.0.4.tgz",
+      "integrity": "sha1-l4hXRCxEdJ5CBmE+N5RiBYJqvYA="
+    },
+    "ecc-jsbn": {
+      "version": "0.1.2",
+      "resolved": "https://registry.npmjs.org/ecc-jsbn/-/ecc-jsbn-0.1.2.tgz",
+      "integrity": "sha1-OoOpBOVDUyh4dMVkt1SThoSamMk=",
+      "requires": {
+        "jsbn": "~0.1.0",
+        "safer-buffer": "^2.1.0"
+      }
+    },
+    "ee-first": {
+      "version": "1.1.1",
+      "resolved": "https://registry.npmjs.org/ee-first/-/ee-first-1.1.1.tgz",
+      "integrity": "sha1-WQxhFWsK4vTwJVcyoViyZrxWsh0="
+    },
+    "encodeurl": {
+      "version": "1.0.2",
+      "resolved": "https://registry.npmjs.org/encodeurl/-/encodeurl-1.0.2.tgz",
+      "integrity": "sha1-rT/0yG7C0CkyL1oCw6mmBslbP1k="
+    },
+    "escape-html": {
+      "version": "1.0.3",
+      "resolved": "https://registry.npmjs.org/escape-html/-/escape-html-1.0.3.tgz",
+      "integrity": "sha1-Aljq5NPQwJdN4cFpGI7wBR0dGYg="
+    },
+    "etag": {
+      "version": "1.8.1",
+      "resolved": "https://registry.npmjs.org/etag/-/etag-1.8.1.tgz",
+      "integrity": "sha1-Qa4u62XvpiJorr/qg6x9eSmbCIc="
+    },
+    "express": {
+      "version": "4.17.1",
+      "resolved": "https://registry.npmjs.org/express/-/express-4.17.1.tgz",
+      "integrity": "sha512-mHJ9O79RqluphRrcw2X/GTh3k9tVv8YcoyY4Kkh4WDMUYKRZUq0h1o0w2rrrxBqM7VoeUVqgb27xlEMXTnYt4g==",
+      "requires": {
+        "accepts": "~1.3.7",
+        "array-flatten": "1.1.1",
+        "body-parser": "1.19.0",
+        "content-disposition": "0.5.3",
+        "content-type": "~1.0.4",
+        "cookie": "0.4.0",
+        "cookie-signature": "1.0.6",
+        "debug": "2.6.9",
+        "depd": "~1.1.2",
+        "encodeurl": "~1.0.2",
+        "escape-html": "~1.0.3",
+        "etag": "~1.8.1",
+        "finalhandler": "~1.1.2",
+        "fresh": "0.5.2",
+        "merge-descriptors": "1.0.1",
+        "methods": "~1.1.2",
+        "on-finished": "~2.3.0",
+        "parseurl": "~1.3.3",
+        "path-to-regexp": "0.1.7",
+        "proxy-addr": "~2.0.5",
+        "qs": "6.7.0",
+        "range-parser": "~1.2.1",
+        "safe-buffer": "5.1.2",
+        "send": "0.17.1",
+        "serve-static": "1.14.1",
+        "setprototypeof": "1.1.1",
+        "statuses": "~1.5.0",
+        "type-is": "~1.6.18",
+        "utils-merge": "1.0.1",
+        "vary": "~1.1.2"
+      }
+    },
+    "extend": {
+      "version": "3.0.2",
+      "resolved": "https://registry.npmjs.org/extend/-/extend-3.0.2.tgz",
+      "integrity": "sha512-fjquC59cD7CyW6urNXK0FBufkZcoiGG80wTuPujX590cB5Ttln20E2UB4S/WARVqhXffZl2LNgS+gQdPIIim/g=="
+    },
+    "extsprintf": {
+      "version": "1.3.0",
+      "resolved": "https://registry.npmjs.org/extsprintf/-/extsprintf-1.3.0.tgz",
+      "integrity": "sha1-lpGEQOMEGnpBT4xS48V06zw+HgU="
+    },
+    "fast-deep-equal": {
+      "version": "3.1.3",
+      "resolved": "https://registry.npmjs.org/fast-deep-equal/-/fast-deep-equal-3.1.3.tgz",
+      "integrity": "sha512-f3qQ9oQy9j2AhBe/H9VC91wLmKBCCU/gDOnKNAYG5hswO7BLKj09Hc5HYNz9cGI++xlpDCIgDaitVs03ATR84Q=="
+    },
+    "fast-json-stable-stringify": {
+      "version": "2.1.0",
+      "resolved": "https://registry.npmjs.org/fast-json-stable-stringify/-/fast-json-stable-stringify-2.1.0.tgz",
+      "integrity": "sha512-lhd/wF+Lk98HZoTCtlVraHtfh5XYijIjalXck7saUtuanSDyLMxnHhSXEDJqHxD7msR8D0uCmqlkwjCV8xvwHw=="
+    },
+    "finalhandler": {
+      "version": "1.1.2",
+      "resolved": "https://registry.npmjs.org/finalhandler/-/finalhandler-1.1.2.tgz",
+      "integrity": "sha512-aAWcW57uxVNrQZqFXjITpW3sIUQmHGG3qSb9mUah9MgMC4NeWhNOlNjXEYq3HjRAvL6arUviZGGJsBg6z0zsWA==",
+      "requires": {
+        "debug": "2.6.9",
+        "encodeurl": "~1.0.2",
+        "escape-html": "~1.0.3",
+        "on-finished": "~2.3.0",
+        "parseurl": "~1.3.3",
+        "statuses": "~1.5.0",
+        "unpipe": "~1.0.0"
+      }
+    },
+    "follow-redirects": {
+      "version": "1.14.2",
+      "resolved": "https://registry.npmjs.org/follow-redirects/-/follow-redirects-1.14.2.tgz",
+      "integrity": "sha512-yLR6WaE2lbF0x4K2qE2p9PEXKLDjUjnR/xmjS3wHAYxtlsI9MLLBJUZirAHKzUZDGLxje7w/cXR49WOUo4rbsA=="
+    },
+    "forever-agent": {
+      "version": "0.6.1",
+      "resolved": "https://registry.npmjs.org/forever-agent/-/forever-agent-0.6.1.tgz",
+      "integrity": "sha1-+8cfDEGt6zf5bFd60e1C2P2sypE="
+    },
+    "form-data": {
+      "version": "2.3.3",
+      "resolved": "https://registry.npmjs.org/form-data/-/form-data-2.3.3.tgz",
+      "integrity": "sha512-1lLKB2Mu3aGP1Q/2eCOx0fNbRMe7XdwktwOruhfqqd0rIJWwN4Dh+E3hrPSlDCXnSR7UtZ1N38rVXm+6+MEhJQ==",
+      "requires": {
+        "asynckit": "^0.4.0",
+        "combined-stream": "^1.0.6",
+        "mime-types": "^2.1.12"
+      }
+    },
+    "forwarded": {
+      "version": "0.2.0",
+      "resolved": "https://registry.npmjs.org/forwarded/-/forwarded-0.2.0.tgz",
+      "integrity": "sha512-buRG0fpBtRHSTCOASe6hD258tEubFoRLb4ZNA6NxMVHNw2gOcwHo9wyablzMzOA5z9xA9L1KNjk/Nt6MT9aYow=="
+    },
+    "fresh": {
+      "version": "0.5.2",
+      "resolved": "https://registry.npmjs.org/fresh/-/fresh-0.5.2.tgz",
+      "integrity": "sha1-PYyt2Q2XZWn6g1qx+OSyOhBWBac="
+    },
+    "getpass": {
+      "version": "0.1.7",
+      "resolved": "https://registry.npmjs.org/getpass/-/getpass-0.1.7.tgz",
+      "integrity": "sha1-Xv+OPmhNVprkyysSgmBOi6YhSfo=",
+      "requires": {
+        "assert-plus": "^1.0.0"
+      }
+    },
+    "har-schema": {
+      "version": "2.0.0",
+      "resolved": "https://registry.npmjs.org/har-schema/-/har-schema-2.0.0.tgz",
+      "integrity": "sha1-qUwiJOvKwEeCoNkDVSHyRzW37JI="
+    },
+    "har-validator": {
+      "version": "5.1.5",
+      "resolved": "https://registry.npmjs.org/har-validator/-/har-validator-5.1.5.tgz",
+      "integrity": "sha512-nmT2T0lljbxdQZfspsno9hgrG3Uir6Ks5afism62poxqBM6sDnMEuPmzTq8XN0OEwqKLLdh1jQI3qyE66Nzb3w==",
+      "requires": {
+        "ajv": "^6.12.3",
+        "har-schema": "^2.0.0"
+      }
+    },
+    "http-errors": {
+      "version": "1.7.2",
+      "resolved": "https://registry.npmjs.org/http-errors/-/http-errors-1.7.2.tgz",
+      "integrity": "sha512-uUQBt3H/cSIVfch6i1EuPNy/YsRSOUBXTVfZ+yR7Zjez3qjBz6i9+i4zjNaoqcoFVI4lQJ5plg63TvGfRSDCRg==",
+      "requires": {
+        "depd": "~1.1.2",
+        "inherits": "2.0.3",
+        "setprototypeof": "1.1.1",
+        "statuses": ">= 1.5.0 < 2",
+        "toidentifier": "1.0.0"
+      }
+    },
+    "http-signature": {
+      "version": "1.2.0",
+      "resolved": "https://registry.npmjs.org/http-signature/-/http-signature-1.2.0.tgz",
+      "integrity": "sha1-muzZJRFHcvPZW2WmCruPfBj7rOE=",
+      "requires": {
+        "assert-plus": "^1.0.0",
+        "jsprim": "^1.2.2",
+        "sshpk": "^1.7.0"
+      }
+    },
+    "iconv-lite": {
+      "version": "0.4.24",
+      "resolved": "https://registry.npmjs.org/iconv-lite/-/iconv-lite-0.4.24.tgz",
+      "integrity": "sha512-v3MXnZAcvnywkTUEZomIActle7RXXeedOR31wwl7VlyoXO4Qi9arvSenNQWne1TcRwhCL1HwLI21bEqdpj8/rA==",
+      "requires": {
+        "safer-buffer": ">= 2.1.2 < 3"
+      }
+    },
+    "inherits": {
+      "version": "2.0.3",
+      "resolved": "https://registry.npmjs.org/inherits/-/inherits-2.0.3.tgz",
+      "integrity": "sha1-Yzwsg+PaQqUC9SRmAiSA9CCCYd4="
+    },
+    "ipaddr.js": {
+      "version": "1.9.1",
+      "resolved": "https://registry.npmjs.org/ipaddr.js/-/ipaddr.js-1.9.1.tgz",
+      "integrity": "sha512-0KI/607xoxSToH7GjN1FfSbLoU0+btTicjsQSWQlh/hZykN8KpmMf7uYwPW3R+akZ6R/w18ZlXSHBYXiYUPO3g=="
+    },
+    "is-typedarray": {
+      "version": "1.0.0",
+      "resolved": "https://registry.npmjs.org/is-typedarray/-/is-typedarray-1.0.0.tgz",
+      "integrity": "sha1-5HnICFjfDBsR3dppQPlgEfzaSpo="
+    },
+    "isstream": {
+      "version": "0.1.2",
+      "resolved": "https://registry.npmjs.org/isstream/-/isstream-0.1.2.tgz",
+      "integrity": "sha1-R+Y/evVa+m+S4VAOaQ64uFKcCZo="
+    },
+    "jsbn": {
+      "version": "0.1.1",
+      "resolved": "https://registry.npmjs.org/jsbn/-/jsbn-0.1.1.tgz",
+      "integrity": "sha1-peZUwuWi3rXyAdls77yoDA7y9RM="
+    },
+    "json-schema": {
+      "version": "0.2.3",
+      "resolved": "https://registry.npmjs.org/json-schema/-/json-schema-0.2.3.tgz",
+      "integrity": "sha1-tIDIkuWaLwWVTOcnvT8qTogvnhM="
+    },
+    "json-schema-traverse": {
+      "version": "0.4.1",
+      "resolved": "https://registry.npmjs.org/json-schema-traverse/-/json-schema-traverse-0.4.1.tgz",
+      "integrity": "sha512-xbbCH5dCYU5T8LcEhhuh7HJ88HXuW3qsI3Y0zOZFKfZEHcpWiHU/Jxzk629Brsab/mMiHQti9wMP+845RPe3Vg=="
+    },
+    "json-stringify-safe": {
+      "version": "5.0.1",
+      "resolved": "https://registry.npmjs.org/json-stringify-safe/-/json-stringify-safe-5.0.1.tgz",
+      "integrity": "sha1-Epai1Y/UXxmg9s4B1lcB4sc1tus="
+    },
+    "jsprim": {
+      "version": "1.4.1",
+      "resolved": "https://registry.npmjs.org/jsprim/-/jsprim-1.4.1.tgz",
+      "integrity": "sha1-MT5mvB5cwG5Di8G3SZwuXFastqI=",
+      "requires": {
+        "assert-plus": "1.0.0",
+        "extsprintf": "1.3.0",
+        "json-schema": "0.2.3",
+        "verror": "1.10.0"
+      }
+    },
+    "media-typer": {
+      "version": "0.3.0",
+      "resolved": "https://registry.npmjs.org/media-typer/-/media-typer-0.3.0.tgz",
+      "integrity": "sha1-hxDXrwqmJvj/+hzgAWhUUmMlV0g="
+    },
+    "merge-descriptors": {
+      "version": "1.0.1",
+      "resolved": "https://registry.npmjs.org/merge-descriptors/-/merge-descriptors-1.0.1.tgz",
+      "integrity": "sha1-sAqqVW3YtEVoFQ7J0blT8/kMu2E="
+    },
+    "methods": {
+      "version": "1.1.2",
+      "resolved": "https://registry.npmjs.org/methods/-/methods-1.1.2.tgz",
+      "integrity": "sha1-VSmk1nZUE07cxSZmVoNbD4Ua/O4="
+    },
+    "mime": {
+      "version": "1.6.0",
+      "resolved": "https://registry.npmjs.org/mime/-/mime-1.6.0.tgz",
+      "integrity": "sha512-x0Vn8spI+wuJ1O6S7gnbaQg8Pxh4NNHb7KSINmEWKiPE4RKOplvijn+NkmYmmRgP68mc70j2EbeTFRsrswaQeg=="
+    },
+    "mime-db": {
+      "version": "1.49.0",
+      "resolved": "https://registry.npmjs.org/mime-db/-/mime-db-1.49.0.tgz",
+      "integrity": "sha512-CIc8j9URtOVApSFCQIF+VBkX1RwXp/oMMOrqdyXSBXq5RWNEsRfyj1kiRnQgmNXmHxPoFIxOroKA3zcU9P+nAA=="
+    },
+    "mime-types": {
+      "version": "2.1.32",
+      "resolved": "https://registry.npmjs.org/mime-types/-/mime-types-2.1.32.tgz",
+      "integrity": "sha512-hJGaVS4G4c9TSMYh2n6SQAGrC4RnfU+daP8G7cSCmaqNjiOoUY0VHCMS42pxnQmVF1GWwFhbHWn3RIxCqTmZ9A==",
+      "requires": {
+        "mime-db": "1.49.0"
+      }
+    },
+    "morgan": {
+      "version": "1.10.0",
+      "resolved": "https://registry.npmjs.org/morgan/-/morgan-1.10.0.tgz",
+      "integrity": "sha512-AbegBVI4sh6El+1gNwvD5YIck7nSA36weD7xvIxG4in80j/UoK8AEGaWnnz8v1GxonMCltmlNs5ZKbGvl9b1XQ==",
+      "requires": {
+        "basic-auth": "~2.0.1",
+        "debug": "2.6.9",
+        "depd": "~2.0.0",
+        "on-finished": "~2.3.0",
+        "on-headers": "~1.0.2"
+      },
+      "dependencies": {
+        "depd": {
+          "version": "2.0.0",
+          "resolved": "https://registry.npmjs.org/depd/-/depd-2.0.0.tgz",
+          "integrity": "sha512-g7nH6P6dyDioJogAAGprGpCtVImJhpPk/roCzdb3fIh61/s/nPsfR6onyMwkCAR/OlC3yBC0lESvUoQEAssIrw=="
+        }
+      }
+    },
+    "ms": {
+      "version": "2.0.0",
+      "resolved": "https://registry.npmjs.org/ms/-/ms-2.0.0.tgz",
+      "integrity": "sha1-VgiurfwAvmwpAd9fmGF4jeDVl8g="
+    },
+    "negotiator": {
+      "version": "0.6.2",
+      "resolved": "https://registry.npmjs.org/negotiator/-/negotiator-0.6.2.tgz",
+      "integrity": "sha512-hZXc7K2e+PgeI1eDBe/10Ard4ekbfrrqG8Ep+8Jmf4JID2bNg7NvCPOZN+kfF574pFQI7mum2AUqDidoKqcTOw=="
+    },
+    "oauth-sign": {
+      "version": "0.9.0",
+      "resolved": "https://registry.npmjs.org/oauth-sign/-/oauth-sign-0.9.0.tgz",
+      "integrity": "sha512-fexhUFFPTGV8ybAtSIGbV6gOkSv8UtRbDBnAyLQw4QPKkgNlsH2ByPGtMUqdWkos6YCRmAqViwgZrJc/mRDzZQ=="
+    },
+    "on-finished": {
+      "version": "2.3.0",
+      "resolved": "https://registry.npmjs.org/on-finished/-/on-finished-2.3.0.tgz",
+      "integrity": "sha1-IPEzZIGwg811M3mSoWlxqi2QaUc=",
+      "requires": {
+        "ee-first": "1.1.1"
+      }
+    },
+    "on-headers": {
+      "version": "1.0.2",
+      "resolved": "https://registry.npmjs.org/on-headers/-/on-headers-1.0.2.tgz",
+      "integrity": "sha512-pZAE+FJLoyITytdqK0U5s+FIpjN0JP3OzFi/u8Rx+EV5/W+JTWGXG8xFzevE7AjBfDqHv/8vL8qQsIhHnqRkrA=="
+    },
+    "parseurl": {
+      "version": "1.3.3",
+      "resolved": "https://registry.npmjs.org/parseurl/-/parseurl-1.3.3.tgz",
+      "integrity": "sha512-CiyeOxFT/JZyN5m0z9PfXw4SCBJ6Sygz1Dpl0wqjlhDEGGBP1GnsUVEL0p63hoG1fcj3fHynXi9NYO4nWOL+qQ=="
+    },
+    "path": {
+      "version": "0.12.7",
+      "resolved": "https://registry.npmjs.org/path/-/path-0.12.7.tgz",
+      "integrity": "sha1-1NwqUGxM4hl+tIHr/NWzbAFAsQ8=",
+      "requires": {
+        "process": "^0.11.1",
+        "util": "^0.10.3"
+      }
+    },
+    "path-to-regexp": {
+      "version": "0.1.7",
+      "resolved": "https://registry.npmjs.org/path-to-regexp/-/path-to-regexp-0.1.7.tgz",
+      "integrity": "sha1-32BBeABfUi8V60SQ5yR6G/qmf4w="
+    },
+    "performance-now": {
+      "version": "2.1.0",
+      "resolved": "https://registry.npmjs.org/performance-now/-/performance-now-2.1.0.tgz",
+      "integrity": "sha1-Ywn04OX6kT7BxpMHrjZLSzd8nns="
+    },
+    "process": {
+      "version": "0.11.10",
+      "resolved": "https://registry.npmjs.org/process/-/process-0.11.10.tgz",
+      "integrity": "sha1-czIwDoQBYb2j5podHZGn1LwW8YI="
+    },
+    "proxy-addr": {
+      "version": "2.0.7",
+      "resolved": "https://registry.npmjs.org/proxy-addr/-/proxy-addr-2.0.7.tgz",
+      "integrity": "sha512-llQsMLSUDUPT44jdrU/O37qlnifitDP+ZwrmmZcoSKyLKvtZxpyV0n2/bD/N4tBAAZ/gJEdZU7KMraoK1+XYAg==",
+      "requires": {
+        "forwarded": "0.2.0",
+        "ipaddr.js": "1.9.1"
+      }
+    },
+    "psl": {
+      "version": "1.8.0",
+      "resolved": "https://registry.npmjs.org/psl/-/psl-1.8.0.tgz",
+      "integrity": "sha512-RIdOzyoavK+hA18OGGWDqUTsCLhtA7IcZ/6NCs4fFJaHBDab+pDDmDIByWFRQJq2Cd7r1OoQxBGKOaztq+hjIQ=="
+    },
+    "punycode": {
+      "version": "2.1.1",
+      "resolved": "https://registry.npmjs.org/punycode/-/punycode-2.1.1.tgz",
+      "integrity": "sha512-XRsRjdf+j5ml+y/6GKHPZbrF/8p2Yga0JPtdqTIY2Xe5ohJPD9saDJJLPvp9+NSBprVvevdXZybnj2cv8OEd0A=="
+    },
+    "qs": {
+      "version": "6.7.0",
+      "resolved": "https://registry.npmjs.org/qs/-/qs-6.7.0.tgz",
+      "integrity": "sha512-VCdBRNFTX1fyE7Nb6FYoURo/SPe62QCaAyzJvUjwRaIsc+NePBEniHlvxFmmX56+HZphIGtV0XeCirBtpDrTyQ=="
+    },
+    "range-parser": {
+      "version": "1.2.1",
+      "resolved": "https://registry.npmjs.org/range-parser/-/range-parser-1.2.1.tgz",
+      "integrity": "sha512-Hrgsx+orqoygnmhFbKaHE6c296J+HTAQXoxEF6gNupROmmGJRoyzfG3ccAveqCBrwr/2yxQ5BVd/GTl5agOwSg=="
+    },
+    "raw-body": {
+      "version": "2.4.0",
+      "resolved": "https://registry.npmjs.org/raw-body/-/raw-body-2.4.0.tgz",
+      "integrity": "sha512-4Oz8DUIwdvoa5qMJelxipzi/iJIi40O5cGV1wNYp5hvZP8ZN0T+jiNkL0QepXs+EsQ9XJ8ipEDoiH70ySUJP3Q==",
+      "requires": {
+        "bytes": "3.1.0",
+        "http-errors": "1.7.2",
+        "iconv-lite": "0.4.24",
+        "unpipe": "1.0.0"
+      }
+    },
+    "request": {
+      "version": "2.88.2",
+      "resolved": "https://registry.npmjs.org/request/-/request-2.88.2.tgz",
+      "integrity": "sha512-MsvtOrfG9ZcrOwAW+Qi+F6HbD0CWXEh9ou77uOb7FM2WPhwT7smM833PzanhJLsgXjN89Ir6V2PczXNnMpwKhw==",
+      "requires": {
+        "aws-sign2": "~0.7.0",
+        "aws4": "^1.8.0",
+        "caseless": "~0.12.0",
+        "combined-stream": "~1.0.6",
+        "extend": "~3.0.2",
+        "forever-agent": "~0.6.1",
+        "form-data": "~2.3.2",
+        "har-validator": "~5.1.3",
+        "http-signature": "~1.2.0",
+        "is-typedarray": "~1.0.0",
+        "isstream": "~0.1.2",
+        "json-stringify-safe": "~5.0.1",
+        "mime-types": "~2.1.19",
+        "oauth-sign": "~0.9.0",
+        "performance-now": "^2.1.0",
+        "qs": "~6.5.2",
+        "safe-buffer": "^5.1.2",
+        "tough-cookie": "~2.5.0",
+        "tunnel-agent": "^0.6.0",
+        "uuid": "^3.3.2"
+      },
+      "dependencies": {
+        "qs": {
+          "version": "6.5.2",
+          "resolved": "https://registry.npmjs.org/qs/-/qs-6.5.2.tgz",
+          "integrity": "sha512-N5ZAX4/LxJmF+7wN74pUD6qAh9/wnvdQcjq9TZjevvXzSUo7bfmw91saqMjzGS2xq91/odN2dW/WOl7qQHNDGA=="
+        }
+      }
+    },
+    "safe-buffer": {
+      "version": "5.1.2",
+      "resolved": "https://registry.npmjs.org/safe-buffer/-/safe-buffer-5.1.2.tgz",
+      "integrity": "sha512-Gd2UZBJDkXlY7GbJxfsE8/nvKkUEU1G38c1siN6QP6a9PT9MmHB8GnpscSmMJSoF8LOIrt8ud/wPtojys4G6+g=="
+    },
+    "safer-buffer": {
+      "version": "2.1.2",
+      "resolved": "https://registry.npmjs.org/safer-buffer/-/safer-buffer-2.1.2.tgz",
+      "integrity": "sha512-YZo3K82SD7Riyi0E1EQPojLz7kpepnSQI9IyPbHHg1XXXevb5dJI7tpyN2ADxGcQbHG7vcyRHk0cbwqcQriUtg=="
+    },
+    "send": {
+      "version": "0.17.1",
+      "resolved": "https://registry.npmjs.org/send/-/send-0.17.1.tgz",
+      "integrity": "sha512-BsVKsiGcQMFwT8UxypobUKyv7irCNRHk1T0G680vk88yf6LBByGcZJOTJCrTP2xVN6yI+XjPJcNuE3V4fT9sAg==",
+      "requires": {
+        "debug": "2.6.9",
+        "depd": "~1.1.2",
+        "destroy": "~1.0.4",
+        "encodeurl": "~1.0.2",
+        "escape-html": "~1.0.3",
+        "etag": "~1.8.1",
+        "fresh": "0.5.2",
+        "http-errors": "~1.7.2",
+        "mime": "1.6.0",
+        "ms": "2.1.1",
+        "on-finished": "~2.3.0",
+        "range-parser": "~1.2.1",
+        "statuses": "~1.5.0"
+      },
+      "dependencies": {
+        "ms": {
+          "version": "2.1.1",
+          "resolved": "https://registry.npmjs.org/ms/-/ms-2.1.1.tgz",
+          "integrity": "sha512-tgp+dl5cGk28utYktBsrFqA7HKgrhgPsg6Z/EfhWI4gl1Hwq8B/GmY/0oXZ6nF8hDVesS/FpnYaD/kOWhYQvyg=="
+        }
+      }
+    },
+    "serve-static": {
+      "version": "1.14.1",
+      "resolved": "https://registry.npmjs.org/serve-static/-/serve-static-1.14.1.tgz",
+      "integrity": "sha512-JMrvUwE54emCYWlTI+hGrGv5I8dEwmco/00EvkzIIsR7MqrHonbD9pO2MOfFnpFntl7ecpZs+3mW+XbQZu9QCg==",
+      "requires": {
+        "encodeurl": "~1.0.2",
+        "escape-html": "~1.0.3",
+        "parseurl": "~1.3.3",
+        "send": "0.17.1"
+      }
+    },
+    "setprototypeof": {
+      "version": "1.1.1",
+      "resolved": "https://registry.npmjs.org/setprototypeof/-/setprototypeof-1.1.1.tgz",
+      "integrity": "sha512-JvdAWfbXeIGaZ9cILp38HntZSFSo3mWg6xGcJJsd+d4aRMOqauag1C63dJfDw7OaMYwEbHMOxEZ1lqVRYP2OAw=="
+    },
+    "sshpk": {
+      "version": "1.16.1",
+      "resolved": "https://registry.npmjs.org/sshpk/-/sshpk-1.16.1.tgz",
+      "integrity": "sha512-HXXqVUq7+pcKeLqqZj6mHFUMvXtOJt1uoUx09pFW6011inTMxqI8BA8PM95myrIyyKwdnzjdFjLiE6KBPVtJIg==",
+      "requires": {
+        "asn1": "~0.2.3",
+        "assert-plus": "^1.0.0",
+        "bcrypt-pbkdf": "^1.0.0",
+        "dashdash": "^1.12.0",
+        "ecc-jsbn": "~0.1.1",
+        "getpass": "^0.1.1",
+        "jsbn": "~0.1.0",
+        "safer-buffer": "^2.0.2",
+        "tweetnacl": "~0.14.0"
+      }
+    },
+    "statuses": {
+      "version": "1.5.0",
+      "resolved": "https://registry.npmjs.org/statuses/-/statuses-1.5.0.tgz",
+      "integrity": "sha1-Fhx9rBd2Wf2YEfQ3cfqZOBR4Yow="
+    },
+    "toidentifier": {
+      "version": "1.0.0",
+      "resolved": "https://registry.npmjs.org/toidentifier/-/toidentifier-1.0.0.tgz",
+      "integrity": "sha512-yaOH/Pk/VEhBWWTlhI+qXxDFXlejDGcQipMlyxda9nthulaxLZUNcUqFxokp0vcYnvteJln5FNQDRrxj3YcbVw=="
+    },
+    "tough-cookie": {
+      "version": "2.5.0",
+      "resolved": "https://registry.npmjs.org/tough-cookie/-/tough-cookie-2.5.0.tgz",
+      "integrity": "sha512-nlLsUzgm1kfLXSXfRZMc1KLAugd4hqJHDTvc2hDIwS3mZAfMEuMbc03SujMF+GEcpaX/qboeycw6iO8JwVv2+g==",
+      "requires": {
+        "psl": "^1.1.28",
+        "punycode": "^2.1.1"
+      }
+    },
+    "tunnel-agent": {
+      "version": "0.6.0",
+      "resolved": "https://registry.npmjs.org/tunnel-agent/-/tunnel-agent-0.6.0.tgz",
+      "integrity": "sha1-J6XeoGs2sEoKmWZ3SykIaPD8QP0=",
+      "requires": {
+        "safe-buffer": "^5.0.1"
+      }
+    },
+    "tweetnacl": {
+      "version": "0.14.5",
+      "resolved": "https://registry.npmjs.org/tweetnacl/-/tweetnacl-0.14.5.tgz",
+      "integrity": "sha1-WuaBd/GS1EViadEIr6k/+HQ/T2Q="
+    },
+    "type-is": {
+      "version": "1.6.18",
+      "resolved": "https://registry.npmjs.org/type-is/-/type-is-1.6.18.tgz",
+      "integrity": "sha512-TkRKr9sUTxEH8MdfuCSP7VizJyzRNMjj2J2do2Jr3Kym598JVdEksuzPQCnlFPW4ky9Q+iA+ma9BGm06XQBy8g==",
+      "requires": {
+        "media-typer": "0.3.0",
+        "mime-types": "~2.1.24"
+      }
+    },
+    "unpipe": {
+      "version": "1.0.0",
+      "resolved": "https://registry.npmjs.org/unpipe/-/unpipe-1.0.0.tgz",
+      "integrity": "sha1-sr9O6FFKrmFltIF4KdIbLvSZBOw="
+    },
+    "uri-js": {
+      "version": "4.4.1",
+      "resolved": "https://registry.npmjs.org/uri-js/-/uri-js-4.4.1.tgz",
+      "integrity": "sha512-7rKUyy33Q1yc98pQ1DAmLtwX109F7TIfWlW1Ydo8Wl1ii1SeHieeh0HHfPeL2fMXK6z0s8ecKs9frCuLJvndBg==",
+      "requires": {
+        "punycode": "^2.1.0"
+      }
+    },
+    "util": {
+      "version": "0.10.4",
+      "resolved": "https://registry.npmjs.org/util/-/util-0.10.4.tgz",
+      "integrity": "sha512-0Pm9hTQ3se5ll1XihRic3FDIku70C+iHUdT/W926rSgHV5QgXsYbKZN8MSC3tJtSkhuROzvsQjAaFENRXr+19A==",
+      "requires": {
+        "inherits": "2.0.3"
+      }
+    },
+    "utils-merge": {
+      "version": "1.0.1",
+      "resolved": "https://registry.npmjs.org/utils-merge/-/utils-merge-1.0.1.tgz",
+      "integrity": "sha1-n5VxD1CiZ5R7LMwSR0HBAoQn5xM="
+    },
+    "uuid": {
+      "version": "3.4.0",
+      "resolved": "https://registry.npmjs.org/uuid/-/uuid-3.4.0.tgz",
+      "integrity": "sha512-HjSDRw6gZE5JMggctHBcjVak08+KEVhSIiDzFnT9S9aegmp85S/bReBVTb4QTFaRNptJ9kuYaNhnbNEOkbKb/A=="
+    },
+    "vary": {
+      "version": "1.1.2",
+      "resolved": "https://registry.npmjs.org/vary/-/vary-1.1.2.tgz",
+      "integrity": "sha1-IpnwLG3tMNSllhsLn3RSShj2NPw="
+    },
+    "verror": {
+      "version": "1.10.0",
+      "resolved": "https://registry.npmjs.org/verror/-/verror-1.10.0.tgz",
+      "integrity": "sha1-OhBcoXBTr1XW4nDB+CiGguGNpAA=",
+      "requires": {
+        "assert-plus": "^1.0.0",
+        "core-util-is": "1.0.2",
+        "extsprintf": "^1.2.0"
+      }
+    }
+  }
+}

Control/Pi/PCU_status_server/package.json

@@ -0,0 +1,18 @@
+{
+  "name": "system_status_ui_server",
+  "version": "1.0.0",
+  "description": "",
+  "main": "index.js",
+  "scripts": {
+    "test": "echo \"Error: no test specified\" && exit 1"
+  },
+  "author": "",
+  "license": "ISC",
+  "dependencies": {
+    "axios": "^0.21.1",
+    "express": "^4.17.1",
+    "morgan": "^1.10.0",
+    "path": "^0.12.7",
+    "request": "^2.88.2"
+  }
+}

Control/Pi/PCU_status_server/public/index.html

@@ -0,0 +1,241 @@
+<html>
+
+<head>
+    <title> PCU system status
+    </title>
+    <meta name="viewport" content="initial-scale=1.0, user-scalable=no">
+    <meta charset="utf-8">
+    <script src="https://maps.googleapis.com/maps/api/js?key=AIzaSyAK0ovprzLWo9wrcZFsFy2Gm3FRUjBCvHw"></script>
+    <style>
+        #map {
+            height: 100%;
+        }
+        
+        html,
+        body {
+            height: 100%;
+            margin: 0;
+            padding: 0;
+        }
+        
+        .gmap {
+            position: relative;
+            width: 50%;
+            height: 50% !important;
+            /* left: 25% */
+        }
+    </style>
+</head>
+
+<body>
+    <center>
+        <h1> System status</h1>
+        <div id="system_status"> System status</div>
+        <div id="system_heading"> System heading</div>
+        <div id="system_gps"> System gps</div>
+        <div id="system_speed"> System speed</div>
+        <div id="system_steering"> System steering</div>
+        <div id="estop"> Estop: </div>
+        <div id="armed"> Armed: </div>
+        <button onclick="estop()">ESTOP</button>
+        <div id="map" class="gmap">
+    </center>
+</body>
+
+<script>
+    var initCoords = {
+            lat: 52.216531,
+            lon: -2.22221
+                //GPS: LAT: 52.2165315 LON: -2.2222162 = Worcester
+
+        } //change this to relevant starting place
+    var map = new google.maps.Map(document.getElementById('map'), {
+        zoom: 18,
+        tilt: 0,
+        center: new google.maps.LatLng(initCoords.lat, initCoords.lon),
+        mapTypeId: google.maps.MapTypeId.SATELLITE
+    });
+    var polygons = []
+    var circles = []
+
+    var current_heading = 0
+    var current_coords = {
+        lat: 0,
+        lon: 0
+    }
+
+    function drawCircle(lat, lon) {
+        var newCircle = new google.maps.Circle({
+            strokeColor: "#FF0000",
+            strokeOpacity: 0.8,
+            strokeWeight: 2,
+            fillColor: "#FF0000",
+            fillOpacity: 0.35,
+            map: map,
+            center: {
+                lat: lat,
+                lng: lon
+            },
+            radius: 3
+        });
+        circles.push(newCircle)
+            // map.fitBounds(newCircle.getBounds());
+    }
+
+    function drawLine(lat1, lon1, bearing) {
+        line_end = calcLineCoords(lat1, lon1, bearing)
+        var polygonCoords = [{
+            lat: lat1,
+            lng: lon1
+        }, {
+            lat: line_end.lat,
+            lng: line_end.lon
+        }];
+        var myPolygon = new google.maps.Polygon({
+            paths: polygonCoords,
+            strokeColor: '#FF0000',
+            strokeOpacity: 0.8,
+            strokeWeight: 2,
+            fillColor: '#FF0000',
+            fillOpacity: 0.35
+        });
+        polygons.push(myPolygon)
+        myPolygon.setMap(map);
+    }
+
+    function degrees_to_radians(degrees) {
+        var pi = Math.PI;
+        return degrees * (pi / 180);
+    }
+
+    function radians_to_degrees(radians) {
+        var pi = Math.PI;
+        return radians * (180 / pi);
+    }
+
+    function calcLineCoords(lat, lon, bearing_deg) {
+        var dist = 20 / 1000 //km?
+
+        R = 6378.1 // Radius of the Earth
+        bearing_rad = degrees_to_radians(bearing_deg)
+        brng = bearing_rad // Bearing is 90 degrees converted to radians.
+        d = dist // Distance in km
+
+        lat1 = degrees_to_radians(lat) // Current lat point converted to radians
+        lon1 = degrees_to_radians(lon) // Current long point converted to radians
+
+        lat2 = Math.asin(Math.sin(lat1) * Math.cos(d / R) +
+            Math.cos(lat1) * Math.sin(d / R) * Math.cos(brng))
+
+        lon2 = lon1 + Math.atan2(Math.sin(brng) * Math.sin(d / R) * Math.cos(lat1),
+            Math.cos(d / R) - Math.sin(lat1) * Math.sin(lat2))
+
+        lat2 = radians_to_degrees(lat2)
+        lon2 = radians_to_degrees(lon2)
+        return {
+            lat: lat2,
+            lon: lon2
+        }
+    }
+
+    function clearMap() {
+        circles.forEach(function(item, index) {
+            console.log(item, index);
+            item.setMap(null)
+        });
+        polygons.forEach(function(item, index) {
+            console.log(item, index);
+            item.setMap(null)
+        });
+    }
+
+    function updateVehiclePos(lat, lon, heading) {
+        clearMap()
+        drawCircle(lat, lon)
+        drawLine(lat, lon, heading)
+    }
+
+
+    function estop() {
+        fetch("/estop", {
+                method: "POST",
+                // body: JSON.stringify(data)
+            })
+            .then(response => response.json())
+            .then(data => {
+                document.getElementById("estop").innerHTML = "Estop: " + data.success
+            })
+    }
+
+    function update_data() {
+        fetch("/heading", {
+                method: "GET",
+                // body: JSON.stringify(data)
+            })
+            .then(response => response.json())
+            .then(data => {
+                document.getElementById("system_heading").innerHTML = "Heading: " + data.heading
+                current_heading = data.heading
+            })
+        fetch("/state", {
+                method: "GET",
+                // body: JSON.stringify(data)
+            })
+            .then(response => response.json())
+            .then(data => {
+                document.getElementById("system_status").innerHTML = "System state: " + data.state
+            })
+        fetch("/gps", {
+                method: "GET",
+                // body: JSON.stringify(data)
+            })
+            .then(response => response.json())
+            .then(data => {
+                document.getElementById("system_gps").innerHTML = "GPS: LAT: " + data.lat + " LON: " + data.lon
+                current_coords.lat = data.lat
+                current_coords.lon = data.lon
+            })
+        fetch("/speed", {
+                method: "GET",
+                // body: JSON.stringify(data)
+            })
+            .then(response => response.json())
+            .then(data => {
+                document.getElementById("system_speed").innerHTML = "Speed: " + data.speed + " %"
+            })
+        fetch("/steering", {
+                method: "GET",
+                // body: JSON.stringify(data)
+            })
+            .then(response => response.json())
+            .then(data => {
+                document.getElementById("system_steering").innerHTML = "Steering: " + data.steering + " %"
+            })
+
+        fetch("/get_estop", {
+                method: "GET",
+                // body: JSON.stringify(data)
+            })
+            .then(response => response.json())
+            .then(data => {
+                document.getElementById("estop").innerHTML = "Estop: " + data.estop
+            })
+        fetch("/get_arm", {
+                method: "GET",
+                // body: JSON.stringify(data)
+            })
+            .then(response => response.json())
+            .then(data => {
+                document.getElementById("armed").innerHTML = "Armed: " + data.armed
+            })
+    }
+
+    update_data()
+
+    var intervalId = window.setInterval(function() {
+        update_data();
+        updateVehiclePos(current_coords.lat, current_coords.lon, current_heading)
+    }, 500);
+</script>
+
+</html>

Control/Pi/PCU_status_server/server.js

@@ -0,0 +1,123 @@
+const express = require('express')
+var logger = require('morgan');
+const axios = require('axios')
+var path = require('path');
+const app = express()
+const port = 8080
+
+app.use(logger('dev'));
+app.use(express.static(path.join(__dirname, 'public')));
+
+
+// app.get('/', (req, res) => {
+//     res.send('Hello World!')
+// })
+
+app.get('/heading', (req, res) => {
+    axios.post('http://0.0.0.0:3333/get_heading', {})
+        .then(navio_rsp => {
+            console.log(`heading state statusCode: ${navio_rsp.status}`)
+            console.log(navio_rsp.data)
+            res.json({ heading: navio_rsp.data.heading })
+        })
+        .catch(error => {
+            // console.error(error)
+            res.json({ error: error })
+        })
+})
+
+app.get('/state', (req, res) => {
+    axios.post('http://0.0.0.0:5000/get_state', {})
+        .then(navio_rsp => {
+            console.log(`system state statusCode: ${navio_rsp.status}`)
+            console.log(navio_rsp.data)
+            res.json({ state: navio_rsp.data.State })
+        })
+        .catch(error => {
+            // console.error(error)
+            res.json({ error: error })
+        })
+})
+
+app.get('/gps', (req, res) => {
+    axios.post('http://0.0.0.0:5000/get_gps', {})
+        .then(navio_rsp => {
+            console.log(`gps state statusCode: ${navio_rsp.status}`)
+            console.log(navio_rsp.data)
+            res.json({ lat: navio_rsp.data.lat, lon: navio_rsp.data.lon })
+        })
+        .catch(error => {
+            // console.error(error)
+            res.json({ error: error })
+        })
+})
+
+app.get('/speed', (req, res) => {
+    axios.post('http://0.0.0.0:5000/get_speed', {})
+        .then(navio_rsp => {
+            console.log(`speed state statusCode: ${navio_rsp.status}`)
+            console.log(navio_rsp.data)
+            res.json({ speed: navio_rsp.data.speed })
+        })
+        .catch(error => {
+            // console.error(error)
+            res.json({ error: error })
+        })
+})
+
+app.get('/steering', (req, res) => {
+    axios.post('http://0.0.0.0:5000/get_steering', {})
+        .then(navio_rsp => {
+            console.log(`steering state statusCode: ${navio_rsp.status}`)
+            console.log(navio_rsp.data)
+            res.json({ steering: navio_rsp.data.steering })
+        })
+        .catch(error => {
+            // console.error(error)
+            res.json({ error: error })
+        })
+})
+
+app.post('/estop', (req, res) => {
+    axios.post('http://0.0.0.0:5000/estop', {})
+        .then(navio_rsp => {
+            console.log(`estop trigger statusCode: ${navio_rsp.status}`)
+            console.log(navio_rsp.data)
+            res.json({ success: navio_rsp.data.estop })
+        })
+        .catch(error => {
+            // console.error(error)
+            res.json({ error: error })
+        })
+})
+
+app.get('/get_estop', (req, res) => {
+    axios.post('http://0.0.0.0:5000/get_estop', {})
+        .then(navio_rsp => {
+            console.log(`estop state statusCode: ${navio_rsp.status}`)
+            console.log(navio_rsp.data)
+            res.json({ estop: navio_rsp.data.estop })
+        })
+        .catch(error => {
+            // console.error(error)
+            res.json({ error: error })
+        })
+})
+
+app.get('/get_arm', (req, res) => {
+    axios.post('http://0.0.0.0:5000/get_arm', {})
+        .then(navio_rsp => {
+            console.log(`arm state statusCode: ${navio_rsp.status}`)
+            console.log(navio_rsp.data)
+            res.json({ armed: navio_rsp.data.armed })
+        })
+        .catch(error => {
+            // console.error(error)
+            res.json({ error: error })
+        })
+})
+
+
+app.listen(port, () => {
+    console.log(`Example app listening at http://localhost:${port}`)
+})

Control/Pi/compass_service.py

@@ -0,0 +1,99 @@
+#!/usr/bin/python3
+import logging
+import time
+
+import flask
+from flask import jsonify, request
+
+import os
+
+from ICM20948 import ICM20948DataStream, ICM20948, CompassOffsets
+
+logging.basicConfig()
+logging.root.setLevel(logging.NOTSET)
+logging.basicConfig(level=logging.NOTSET)
+
+logger = logging.getLogger("COMPASS SERVICE")
+logger.setLevel("DEBUG")
+
+def load_from_file():
+    if os.path.isfile('previous_mag_cal'):
+        with open('previous_mag_cal', 'r') as offsets:
+            raw = offsets.readline()
+            clean = raw.split('\n')[0].split(';')
+            offsets = CompassOffsets(clean[0], clean[1], clean[2], clean[3], clean[4], clean[5])
+            return offsets
+    return False
+
+START_ON_LAUNCH = False
+saved_offsets = load_from_file()
+if saved_offsets:
+    OFFSETS = saved_offsets
+    START_ON_LAUNCH = True
+
+# OFFSETS = None
+COMPASS_COM = ICM20948DataStream("/dev/compass", baudrate=115200).start()
+# COMPASS_COM = ICM20948DataStream("/dev/cu.SLAB_USBtoUART", baudrate=115200).start()
+if START_ON_LAUNCH:
+    COMPASS = ICM20948(COMPASS_COM, pre_calibreted=OFFSETS).start()
+else:
+    COMPASS = ICM20948(COMPASS_COM)
+
+app = flask.Flask(__name__)
+
+@app.route('/', methods=['GET'])
+def home():
+    return "<h1>Compass server is running...</h1>"
+
+@app.route('/alive', methods=['POST'])
+def alive():
+    logger.info(f"Returning alive")
+    return jsonify({'Success': True, 'is_running': COMPASS.running})
+
+@app.route('/get_heading', methods=['POST', 'GET'])
+def get_heading():
+    heading = COMPASS.read()
+    return jsonify({'Success': True, 'heading': heading})
+
+@app.route('/start_compass', methods=['POST'])
+def start_compass():
+    COMPASS.start()
+    return jsonify({'Success': True})
+
+@app.route('/stop_compass', methods=['POST'])
+def stop_compass():
+    COMPASS.stop()
+    return jsonify({'Success': True})
+
+@app.route('/calibrate', methods=['POST'])
+def calibrate():
+    if request.is_json:
+        content = request.get_json()
+        time_s = int(content.get('time', 60))
+        logger.info(f"Calibrating for {time_s} seconds")
+        logger.warning(f"This request will hang for {time_s} seconds, expect a timeout")
+        if COMPASS.running:
+            COMPASS.stop()
+        COMPASS.calibrate(calib_time=time_s)
+        COMPASS.start()
+        with open('previous_mag_cal', 'w+') as save_file:
+            nco = COMPASS.offsets
+            new_compass_offsets_str = f"{nco.min_x};{nco.max_x};{nco.min_y};{nco.max_y};{nco.hard_offset_x};{nco.hard_offset_y}"
+            save_file.writelines([new_compass_offsets_str])
+        return jsonify({'Success': True})
+    else:
+        return jsonify({'Error': 'Not JSON'})
+
+@app.route('/set_offset', methods=['POST'])
+def set_offset():
+    if request.is_json:
+        content = request.get_json()
+        offset = content.get('offset', 'nil')
+        assert offset != 'nil'
+        logger.info(f"Setting offset: {offset}")
+        COMPASS.set_offset(offset)
+        return jsonify({'Success': True})
+    else:
+        return jsonify({'Error': 'Not JSON'})
+
+app.run(host='0.0.0.0', port=3333)

Control/Pi/dronekit_class.py

@@ -0,0 +1,416 @@
+import dronekit
+from dronekit import connect
+from dronekit import VehicleMode
+from dronekit import LocationGlobal
+from pallet_proximity_sensor import PalletProximity
+from math import asin, cos, radians, sin, sqrt
+import numpy as np
+import requests
+import json
+
+import time
+
+import logging
+from state_machine import StateMachineHandler
+
+def haversine_m(lon1, lat1, lon2, lat2):
+    """
+    Calculate the great circle distance between two points
+    on the earth (specified in decimal degrees)
+    """
+    # convert decimal degrees to radians
+    lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
+
+    # haversine formula
+    dlon = lon2 - lon1
+    dlat = lat2 - lat1
+    a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2
+    c = 2 * asin(sqrt(a))
+    r = 6371 # Radius of earth in kilometers. Use 3956 for miles
+    return c * r * 1000
+
+
+def get_bearing(lat1, long1, lat2, long2):
+    dLon = (long2 - long1)
+    x = cos(radians(lat2)) * sin(radians(dLon))
+    y = cos(radians(lat1)) * sin(radians(lat2)) - sin(radians(lat1)) * cos(radians(lat2)) * cos(radians(dLon))
+    brng = np.arctan2(x,y)
+    brng = np.degrees(brng)
+    return brng
+
+def remap(in_value, in_min, in_max, out_min, out_max):
+    if in_value > in_max:
+        in_value = in_max
+    if in_value < in_min:
+        in_value = in_min
+    remapped = (((in_value - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min
+    return remapped
+
+
+class DroneInterface:
+    __ARDUROVER_IP = '127.0.0.1:14550'
+    __TARGET_REACHED_RADIUS = 2.5 #metres
+    def __init__(self) -> None:
+        self.log = logging.getLogger("ardurover_interface")
+        self.log.setLevel("DEBUG")
+        self.vehicle = connect(self.__ARDUROVER_IP, wait_ready=True)
+        time.sleep(2)
+        self.heading_endpoint = "http://localhost:3333/get_heading"
+        self.copmpass_alive_endpoint = "http://localhost:3333/alive"
+        self.STATE_MACHINE = StateMachineHandler()
+        self.estop_engaged = False
+        self.log.info("Connected to ardurover")
+        self.pallet_proximity_sensor = PalletProximity()
+        self.pallet_min_distance = 20  # TODO: calibrate this
+        self.log.info("Connected to pallet proximity sensor")
+        self.print_status()
+
+    def arm_vehicle(self):
+        # self.vehicle.mode = VehicleMode("GUIDED")
+        self.vehicle.arm()
+        self.log.info("Vehicle Armed")
+        # vehicle.simple_takeoff(aTargetAltitude)
+
+    def disarm_vehicle(self):
+        self.vehicle.channels.overrides = {}
+        self.vehicle.disarm()
+        self.log.info("Vehicle Disarmed")
+
+    def print_status(self):
+        self.log.info(f"Autopilot Firmware version: {self.vehicle.version}")
+        self.log.info(f"Autopilot capabilities (supports ftp): {self.vehicle.capabilities.ftp}")
+        self.log.info(f"Global Location: {self.vehicle.location.global_frame}")
+        self.log.info(f"Global Location (relative altitude): {self.vehicle.location.global_relative_frame}")
+        self.log.info(f"Local Location: {self.vehicle.location.local_frame}")
+        self.log.info(f"Attitude: {self.vehicle.attitude}")
+        self.log.info(f"Velocity: {self.vehicle.velocity}")
+        self.log.info(f"GPS: {self.vehicle.gps_0}")
+        self.log.info(f"Groundspeed: {self.vehicle.groundspeed}")
+        self.log.info(f"Airspeed: {self.vehicle.airspeed}")
+        self.log.info(f"Gimbal status: {self.vehicle.gimbal}")
+        self.log.info(f"Battery: {self.vehicle.battery}")
+        self.log.info(f"EKF OK?: {self.vehicle.ekf_ok}")
+        self.log.info(f"Last Heartbeat: {self.vehicle.last_heartbeat}")
+        # self.log.info(f"Heading: {self.vehicle.heading}")
+        self.log.info(f"Is Armable?: {self.vehicle.is_armable}")
+        self.log.info(f"System status: {self.vehicle.system_status.state}")
+        self.log.info(f"Mode: {self.vehicle.mode.name}")
+        self.log.info(f"Armed: {self.vehicle.armed}")
+
+    def get_heading(self, data = {}):
+        rsp = None
+        rsp = requests.post(self.heading_endpoint, json=data, timeout=1)
+        if rsp:
+            if rsp.ok:
+                json_rsp = json.loads(rsp.text)
+                status = json_rsp.get('Success', None)
+                if status != True:
+                    error = json_rsp.get('Error', None)
+                    self.log.error(f'Got error: {error}')
+                heading = json_rsp['heading']
+                return heading
+            else:
+                self.log.error(f'Failed to process request, code: {rsp.status_code}, text: {rsp.text}')
+        else:
+            self.log.critical('Cannot connect to server')
+
+    def verify_compass(self, data={}):
+        rsp = None
+        try:
+            rsp = requests.post(self.copmpass_alive_endpoint, json=data, timeout=1)
+            if rsp:
+                if rsp.ok:
+                    json_rsp = json.loads(rsp.text)
+                    status = json_rsp.get('Success', None)
+                    if status != True:
+                        error = json_rsp.get('Error', None)
+                        self.log.error(f'Got error: {error}')
+                        return False
+                    is_running = json_rsp['is_running']
+                    return is_running
+                else:
+                    self.log.error(f'Failed to process request, code: {rsp.status_code}, text: {rsp.text}')
+                    return False
+            else:
+                self.log.critical('Cannot connect to server')
+                return False
+        except Exception as e:
+            self.log.error(f"Error in verify_compass: {e}")
+            return False
+
+    def throttle_limiter(self, throttle, upper=1940, lower=1060):
+        if throttle > upper:
+            throttle = upper
+        if throttle < lower:
+            throttle = lower
+        return throttle
+
+    def steering_limiter(self, steering, upper=2000, lower=1000):
+        if steering > upper:
+            steering = upper
+        if steering < lower:
+            steering = lower
+        return steering
+
+    def set_vehicle_steering_and_speed(self, amount_steer, amount_speed):
+        '''
+        We are currently using rc overrides because
+        ardurover and mavlink have not implemented
+        functionality to change the ground steering... for some fucking reason.
+        channel 4 is the steering channel
+        channel 3 is the drive channel
+        '''
+        if self.verify_state_for(throttle_PWM=amount_speed):
+            self.vehicle.channels.overrides = {}
+            amount_steer = self.steering_limiter(amount_steer)
+            amount_speed = self.throttle_limiter(amount_speed)
+            # assert 999 < amount_speed < 2001, f"speed PWM out of bounds ({amount_speed} != 1000->2000)"
+            self.log.info(f"Sending PWM {amount_steer} to channel 4")
+            self.log.info(f"Sending PWM {amount_speed} to channel 3")
+            self.vehicle.channels.overrides['4'] = int(amount_steer)
+            self.vehicle.channels.overrides['3'] = int(amount_speed)
+
+    def set_vehicle_speed(self, amount_speed: int):
+        amount_speed = self.throttle_limiter(amount_speed)
+        # assert 999 < amount_speed < 2001, f"speed PWM out of bounds ({amount_speed} != 1000->2000)"
+        if self.verify_state_for(throttle_PWM=amount_speed):
+            self.vehicle.channels.overrides = {}
+            self.log.info(f"Sending PWM {amount_speed} to channel 3")
+            self.vehicle.channels.overrides['3'] = int(amount_speed)
+
+    def set_vehicle_steering(self, amount_steer: int):
+        amount_steer = self.steering_limiter(amount_steer)
+        if self.verify_state_for():
+            self.vehicle.channels.overrides = {}
+            self.log.info(f"Sending PWM {amount_steer} to channel 4")
+            self.vehicle.channels.overrides['4'] = int(amount_steer)
+
+    def clear_override_channels(self):
+        self.vehicle.channels.overrides['1'] = 1500
+        self.vehicle.channels.overrides['2'] = 1500
+        self.vehicle.channels.overrides['3'] = 1500
+        self.vehicle.channels.overrides['4'] = 1500
+        self.vehicle.channels.overrides = {}
+
+    def override_stop(self):
+        self.vehicle.channels.overrides = {}
+        self.vehicle.channels.overrides['1'] = 1500
+        self.vehicle.channels.overrides['2'] = 1500
+        self.vehicle.channels.overrides['3'] = 1500
+        self.vehicle.channels.overrides['4'] = 1500
+
+    def override_stop_drivetrain_fwd(self):
+        if self.vehicle.channels['3'] > 1550:
+            self.vehicle.channels.overrides = {}
+            self.vehicle.channels.overrides['3'] = 1500
+
+    def override_stop_drivetrain_back(self):
+        if self.vehicle.channels['3'] < 1450:
+            self.vehicle.channels.overrides = {}
+            self.vehicle.channels.overrides['3'] = 1500
+
+    def nudge_fork_vert(self, nudge_up: bool, nudge_amount: float):  # c1: lr, c2: ud
+        assert nudge_amount < 1.1, "Moving for over 1 second is not safe!"
+        if self.verify_state_for(is_fork=True):
+            self.vehicle.channels.overrides = {}
+            nudge_amt = 1950
+            if nudge_up:
+                nudge_amt = 1100
+            self.log.info(f"Nudging fork {'UP' if nudge_up else 'DOWN'} for {nudge_amount} seconds (chan 2)")
+            self.vehicle.channels.overrides['2'] = nudge_amt
+            time.sleep(nudge_amount)
+            self.vehicle.channels.overrides['2'] = 1500
+            self.vehicle.channels.overrides = {}
+
+    def nudge_fork_horiz(self, nudge_left: bool, nudge_amount: float):  # Dir: True is left, False is right
+        assert nudge_amount < 1.1, "Moving for over 1 second is not safe!"
+        if self.verify_state_for(is_fork=True):
+            self.vehicle.channels.overrides = {}
+            nudge_amt = 1100
+            if nudge_left:
+                nudge_amt = 2000
+            self.log.info(f"Nudging fork {'LEFT' if nudge_left else 'RIGHT'} for {nudge_amount} seconds (chan 1)")
+            self.vehicle.channels.overrides['1'] = nudge_amt
+            time.sleep(nudge_amount)
+            self.vehicle.channels.overrides['1'] = 1500
+            self.vehicle.channels.overrides = {}
+
+    def nudge_drive(self, nudge_forward: bool, nudge_amount: float, nudge_steering: int = 1500):  # Dir: True is left, False is right
+        assert nudge_amount < 10, "Moving for over 10 seconds is not safe!"
+        nudge = 1000
+        if nudge_forward:
+            nudge = 2000
+        if self.verify_state_for(throttle_PWM=nudge):
+            self.vehicle.channels.overrides = {}
+            if nudge_forward:
+                self.log.info(f"Nudging fork FORWARD for {nudge_amount} seconds at steering: {nudge_steering}")
+                nudge_increments = int(nudge_amount / 0.5)
+                for _ in range(nudge_increments):
+                    self.set_vehicle_steering_and_speed(nudge_steering, 1900)
+                    time.sleep(0.5)
+                self.clear_override_channels()
+            else:
+                self.log.info(f"Nudging fork BACKWARD for {nudge_amount} seconds at steering: {nudge_steering}")
+                nudge_increments = int(nudge_amount / 0.5)
+                for _ in range(nudge_increments):
+                    self.set_vehicle_steering_and_speed(nudge_steering, 1000)
+                    time.sleep(0.5)
+                self.clear_override_channels()
+            self.vehicle.channels.overrides = {}
+
+    def check_estop_signal(self):
+        estop = self.vehicle.channels['6']
+        if estop > 1700:
+            self.log.critical(f"Got {estop} on Aux ESTOP, executing Estop")
+            self.emergency_stop()
+            return True
+
+    def check_rc_signal(self):
+        rc_override = self.vehicle.channels['5']
+        if rc_override > 1700:
+            self.log.critical(f"Got {rc_override} on Aux RC_OVERRIDE, returning True")
+            return True
+        return False
+
+    def emergency_stop(self):
+        self.disarm_vehicle()
+        self.estop_engaged = True
+        self.STATE_MACHINE.set_state('ESTOP')
+        # maybe: self.press_brake_pedal()
+        self.log.critical(f"Estop triggered, disarming vehicle")
+
+    def verify_state_for(self, target_state=None, throttle_PWM=None, is_fork=False):
+        _, state = self.STATE_MACHINE.get_state()
+        if state in ["ESTOP", "RC_CONTROL", "OTHER_ERROR"]:
+            self.log.warning(f"Warning: machine state is {state}, this does not allow motion")
+            return False
+        elif state in ["PROXIMITY_STOP_FRONT", "PROXIMITY_STOP_REAR"]:
+            if is_fork:
+                return True
+            if throttle_PWM:
+                if (throttle_PWM > 1500 and state == "PROXIMITY_STOP_REAR") or (throttle_PWM < 1500 and state == "PROXIMITY_STOP_FRONT"):
+                    return True
+                self.log.warning(f"We are in proximity stop mode ({state}) while trying to send PWM {throttle_PWM}, this is not allowed.")
+            return False
+        if target_state:
+            # TODO: implement table to permit state switching from other state
+            pass
+        return True
+
+    def go_to_gps_bad(self, lat, lon, alt=200):
+        if self.verify_state_for("NAVIGATING_TO_POINT_GPS"):
+            self.vehicle.mode = VehicleMode("GUIDED")
+            a_location = LocationGlobal(lat, lon, alt)
+            print("-----")
+            print(a_location.lat)
+            print(a_location.lon)
+            print(a_location.alt)
+            print("-----")
+            self.log.info(f"Requesting vehicle to go to {lat}, {lon}")
+            self.vehicle.simple_goto(a_location)
+
+    def get_speed(self):
+        speed = self.vehicle.channels.overrides.get('3', 1500)
+        return speed
+
+    def get_steering(self):
+        steering = self.vehicle.channels.overrides.get('4', 1500)
+        return steering
+
+    def get_estop(self):
+        return self.estop_engaged
+
+    def get_arm(self):
+        return self.vehicle.armed
+
+    def go_to_gps(self, lat, lon):
+        if self.verify_state_for("NAVIGATING_TO_POINT_GPS"):
+            target_position = (lat, lon)
+            approaching = True
+            while approaching:
+                current_position = (self.vehicle.location.global_frame.lat, self.vehicle.location.global_frame.lon)
+                distance_to_target = haversine_m(target_position[0], target_position[1], current_position[0], current_position[1])
+                self.log.error(f"Distance: {distance_to_target} metres")
+                if distance_to_target < self.__TARGET_REACHED_RADIUS:
+                    self.log.info(f"Waypoint {lat}, {lon} reached")
+                    approaching = False
+                    self.override_stop()
+                    return True
+                bearing_to_target = get_bearing(current_position[0], current_position[1], target_position[0], target_position[1])
+                if bearing_to_target < 0:
+                    bearing_to_target += 360
+                current_heading = self.get_heading()
+                error = bearing_to_target - current_heading
+                if error > 180:
+                    error = -(360 - error)
+                elif error < -180:
+                    error = -(360 + error)
+                # TODO: PID
+                proportional = -2.5
+                # proportional = 1
+                integral = 1
+                derivative = 1
+                error = error * proportional * integral * derivative
+                steering = remap(error, -90, 90, 1000, 2000)
+                self.log.warning(f"Bearing: {bearing_to_target}, Heading: {current_heading}, Error: {error}, Steering: {steering}")
+                self.set_vehicle_steering_and_speed(steering, 1910)
+                time.sleep(0.3)
+        else:
+            self.log.error("Incorrect vehicle State!")
+
+    def go_to_heading(self, target_heading):
+        if self.verify_state_for("NAVIGATING_TO_POINT_GPS"):
+            approaching = True
+            while approaching:
+                if target_heading - 7 <= current_heading <= target_heading + 7:
+                    approaching = False
+                    self.log.info(f"Heading {target_heading} reached")
+                    self.override_stop()
+                    return True
+                current_heading = self.get_heading()
+                error = target_heading - current_heading
+                self.log.error(f"Angle: {error} deg.")
+                if error > 180:
+                    error = -(360 - error)
+                elif error < -180:
+                    error = -(360 + error)
+                proportional = -2.5
+                integral = 1
+                derivative = 1
+                error = error * proportional * integral * derivative
+                steering = remap(error, -90, 90, 1000, 2000)
+                self.log.warning(f"target hearing: {target_heading}, Heading: {current_heading}, Error: {error}, Steering: {steering}")
+                self.set_vehicle_steering_and_speed(steering, 1910)
+                time.sleep(0.3)
+
+    def check_gps_in_radius(self, target_gps):
+        actual_lat, actual_lon = self.get_gps()
+        target_lat, target_lon = target_gps
+        if actual_lat == 0 or actual_lon == 0:
+            logging.critical("Can not get GPS fix!")
+            return False, 99999
+        radius = self.gps_target_radius_km
+        distance = haversine_m(target_lat, target_lon, actual_lat, actual_lon)
+        logging.info(f'Distance to target (m) : {distance*1000}')
+        if distance <= radius:
+            return True, distance*1000
+        return False, distance*1000
+
+    def get_gps(self):
+        lat = self.vehicle.location.global_frame.lat
+        lon = self.vehicle.location.global_frame.lon
+        return (lat, lon)
+
+    def approach_pallet(self):
+        pallet_approached = False
+        while not pallet_approached:
+            time.sleep(0.2)
+            distance_to_pallet = self.pallet_proximity_sensor.get_prox_reading()
+            if distance_to_pallet < self.pallet_min_distance:
+                self.log.info(f"We are close enough to pallet ({distance_to_pallet} m)")
+                pallet_approached = True
+                self.clear_override_channels()
+            else:
+                self.log.info(f"Distance to pallet ({distance_to_pallet} m), moving forward...")
+                target_speed_pwm = 1930
+                self.set_vehicle_speed(target_speed_pwm)

Control/Pi/main_server.py

@@ -0,0 +1,311 @@
+#!/usr/bin/python3
+import logging
+import threading
+import time
+
+import os
+import psutil
+
+import flask
+from flask import jsonify, request
+
+logger = logging.getLogger("dronekit_server")
+logger.setLevel("DEBUG")
+
+from dronekit_class import DroneInterface
+from proximity_detector import Proximity
+from state_machine import StateMachineHandler
+
+# __MACHINE_STATES = {
+#     "IDLE": 0,
+#     "NAVIGATING_TO_POINT_GPS": 1,
+#     "NAVIGATING_TO_POINT_VISION": 2,
+#     "MANIPULATING_CARGO": 3,
+#     "READY_FOR_NEXT_STAGE": 4,
+#     "RC_CONTROL": 5,
+#     "PROXIMITY_STOP": 6,
+#     "ESTOP": 7
+# }
+
+STATE_MACHINE = StateMachineHandler(force=True)
+STATE_MACHINE.set_state("IDLE")
+
+PROXIMITY_STOP_ENABLED = False
+KILL_ON_DEAD_SERVICES = False
+
+FORKLIFT = DroneInterface()
+logger.info("Connected to ArduRover")
+if PROXIMITY_STOP_ENABLED:
+    PROXIMITY_ARRAY = Proximity(STATE_MACHINE, FORKLIFT)
+    logger.info("Connected to Proximity array")
+else:
+    logger.critical("*** PROXIMITY DISABLED!!! RUNNING SANS COLLISION AVOIDANCE ***")
+
+
+def remap(value, maxInput, minInput, maxOutput, minOutput):
+	value = maxInput if value > maxInput else value
+	value = minInput if value < minInput else value
+	inputSpan = maxInput - minInput
+	outputSpan = maxOutput - minOutput
+	scaledThrust = float(value - minInput) / float(inputSpan)
+	return minOutput + (scaledThrust * outputSpan)
+
+
+def safety_system():
+    logger.info('Started saferty system')
+    _, prev_state = STATE_MACHINE.get_state()
+    while True:
+        time.sleep(0.05)
+        _, curr_state = STATE_MACHINE.get_state()
+        is_estop = FORKLIFT.check_estop_signal()
+        is_rc_override = FORKLIFT.check_rc_signal()
+        if is_estop:
+            STATE_MACHINE.set_state("ESTOP")
+        if is_rc_override:
+            if curr_state != "RC_CONTROL":
+                logger.warning(f"***** Setting state to 'RC_CONTROL', prev state is saved as {curr_state}")
+                prev_state = curr_state
+                logger.info("Clearing override channels")
+                FORKLIFT.clear_override_channels()
+                STATE_MACHINE.set_state("RC_CONTROL")
+        if not is_rc_override and curr_state == "RC_CONTROL":
+            logger.info(f"Reverting state to {prev_state}")
+            STATE_MACHINE.set_state(prev_state)
+
+        compass_alive = FORKLIFT.verify_compass()
+        if not compass_alive and KILL_ON_DEAD_SERVICES:
+            logger.critical(f"Compass dead, killing...")
+            FORKLIFT.emergency_stop()
+            current_system_pid = os.getpid()
+            ThisSystem = psutil.Process(current_system_pid)
+            ThisSystem.terminate()
+
+        if PROXIMITY_STOP_ENABLED:
+            PROXIMITY_ARRAY.check_proximity()
+
+
+def safety_watchdog():
+    logger.info("Started safety watchdog")
+    if not KILL_ON_DEAD_SERVICES:
+        logger.warning("##### KILL_ON_DEAD_SERVICES disabled, be VERY VERY careful when running the vehicle! #####")
+    while True:
+        time.sleep(0.5)
+
+        compass_alive = FORKLIFT.verify_compass()
+        if not compass_alive and KILL_ON_DEAD_SERVICES:
+            logger.critical(f"Compass dead, killing...")
+            FORKLIFT.emergency_stop()
+            current_system_pid = os.getpid()
+            ThisSystem = psutil.Process(current_system_pid)
+            ThisSystem.terminate()
+
+        if PROXIMITY_STOP_ENABLED:
+            proximity_alive = PROXIMITY_ARRAY.alive
+            if not proximity_alive and KILL_ON_DEAD_SERVICES:
+                logger.critical(f"Proximity array dead, killing...")
+                FORKLIFT.emergency_stop()
+                current_system_pid = os.getpid()
+                ThisSystem = psutil.Process(current_system_pid)
+                ThisSystem.terminate()
+
+
+safety_system_thread = threading.Thread(target=safety_system)
+safety_system_thread.start()
+
+watchdog_thread = threading.Thread(target=safety_watchdog)
+watchdog_thread.start()
+
+
+app = flask.Flask(__name__)
+
+@app.route('/', methods=['GET'])
+def home():
+    return "<h1>DroneKit Wrapper server is running...</h1>"
+
+@app.route('/alive', methods=['POST', 'GET'])
+def alive():
+    logger.info(f"Returning alive")
+    return jsonify({'Success': True})
+
+@app.route('/arm', methods=['POST'])
+def arm():
+    logger.info(f"Arming forklift")
+    FORKLIFT.arm_vehicle()
+    return jsonify({'Success': True})
+
+@app.route('/disarm', methods=['POST'])
+def disarm():
+    logger.info(f"Disarming forklift")
+    FORKLIFT.disarm_vehicle()
+    return jsonify({'Success': True})
+
+@app.route('/set_speed', methods=['POST'])
+def set_speed():
+    if request.is_json:
+        content = request.get_json()
+        speed = content.get('speed', 0)
+        logger.info(f"Setting speed: {speed}")
+        FORKLIFT.set_vehicle_speed(speed)
+        return jsonify({'Success': True})
+    else:
+        return jsonify({'Error': 'Not JSON'})
+
+@app.route('/set_steering', methods=['POST'])
+def set_steering():
+    if request.is_json:
+        content = request.get_json()
+        steering = content.get('steering', 0)
+        logger.info(f"Setting steering: {steering}")
+        FORKLIFT.set_vehicle_steering(steering)
+        return jsonify({'Success': True})
+    else:
+        return jsonify({'Error': 'Not JSON'})
+
+@app.route('/nudge_fork_vert', methods=['POST'])
+def nudge_fork_vert():
+    if request.is_json:
+        content = request.get_json()
+        go_up = content.get('go_up', "nil")
+        amount = content.get('amount', "nil")
+        assert go_up != "nil" and amount != "nil", f"Params missing: go_up: {go_up}, amount: {amount}" # NOTE: I am sure there is a more elegant way to do this but I am running low on time so here this will live for now...
+        logger.info(f"Nudging fork {'UP' if go_up == True else 'DOWN'} for {amount} seconds")
+        FORKLIFT.nudge_fork_vert(nudge_up=go_up, nudge_amount=amount)
+        return jsonify({'Success': True})
+    else:
+        return jsonify({'Error': 'Not JSON'})
+
+@app.route('/nudge_fork_horiz', methods=['POST'])
+def nudge_fork_horiz():
+    if request.is_json:
+        content = request.get_json()
+        go_left = content.get('go_left', "nil")
+        amount = content.get('amount', "nil")
+        assert go_left != "nil" and amount != "nil", f"Params missing: go_left: {go_left}, amount: {amount}" # NOTE: I am sure there is a more elegant way to do this but I am running low on time so here this will live for now...
+        logger.info(f"Nudging fork {'UP' if go_left == True else 'DOWN'} for {amount} seconds")
+        FORKLIFT.nudge_fork_horiz(nudge_left=go_left, nudge_amount=amount)
+        return jsonify({'Success': True})
+    else:
+        return jsonify({'Error': 'Not JSON'})
+
+@app.route('/set_speed_steering', methods=['POST'])
+def set_speed_and_steering():
+    if request.is_json:
+        content = request.get_json()
+        speed = content.get('speed', 0)
+        steering = content.get('steering', 0)
+        logger.info(f"Setting speed: {speed} and steering {steering}")
+        FORKLIFT.set_vehicle_steering_and_speed(steering, speed)
+        return jsonify({'Success': True})
+    else:
+        return jsonify({'Error': 'Not JSON'})
+
+@app.route('/nudge_drive', methods=['POST'])
+def nudge_drive():
+    if request.is_json:
+        content = request.get_json()
+        go_forward = content.get('go_forward', "nil")
+        amount = content.get('amount', "nil")
+        steering = int(content.get('steering', 1500))
+        assert go_forward != "nil" and amount != "nil", f"Params missing: go_forward: {go_forward}, amount: {amount}" # NOTE: I am sure there is a more elegant way to do this but I am running low on time so here this will live for now...
+        logger.info(f"Nudging fork {'UP' if go_forward == True else 'DOWN'} for {amount} seconds")
+        FORKLIFT.nudge_drive(nudge_forward=go_forward, nudge_amount=amount, nudge_steering=steering)
+        return jsonify({'Success': True})
+    else:
+        return jsonify({'Error': 'Not JSON'})
+
+@app.route('/go_to_gps_old', methods=['POST'])
+def go_to_gps_old():
+    if request.is_json:
+        content = request.get_json()
+        lat = content.get('lat', 0)
+        lon = content.get('lon', 0)
+        assert lat != 0 and lon != 0, "Coordinates missing!"
+        logger.info(f"Navigating to lat: {lat}, lon: {lon}")
+        FORKLIFT.go_to_gps(lat=lat, lon=lon)
+        return jsonify({'Success': True})
+    else:
+        return jsonify({'Error': 'Not JSON'})
+
+@app.route('/go_to_gps', methods=['POST'])
+def go_to_gps():
+    if request.is_json:
+        content = request.get_json()
+        lat = content.get('lat', 0)
+        lon = content.get('lon', 0)
+        assert lat != 0 and lon != 0, "Coordinates missing!"
+        logger.info(f"Navigating to lat: {lat}, lon: {lon}")
+        FORKLIFT.go_to_gps(lat=lat, lon=lon)
+        return jsonify({'Success': True})
+    else:
+        return jsonify({'Error': 'Not JSON'})
+
+@app.route('/get_state', methods=['POST', 'GET'])
+def get_state():
+    _, state = STATE_MACHINE.get_state()
+    return jsonify({'Success': True, "State": state})
+
+@app.route('/set_state', methods=['POST'])
+def set_state():
+    if request.is_json:
+        content = request.get_json()
+        target_state = content.get('State', 0)
+        if target_state == 0:
+            return jsonify({'Error': f"State {target_state} not valid!"})
+        else:
+            logger.info(f"Setting machine state to {target_state}")
+            success = STATE_MACHINE.set_state(target_state)
+            if success:
+                return jsonify({'Success': True})
+            else:
+                return jsonify({'Error': 'State machine error, check logs...'})
+    else:
+        return jsonify({'Error': 'Not JSON'})
+
+@app.route('/get_gps', methods=['POST', 'GET'])
+def get_gps():
+    if request.is_json:
+        lat, lon = FORKLIFT.get_gps()
+        logger.info(f"GPS coords are: lat: {lat} lon: {lon}")
+        return jsonify({'Success': True, 'lat': lat, 'lon': lon})
+    else:
+        return jsonify({'Error': 'Not JSON'})
+
+@app.route('/estop', methods=['POST'])
+def estop():
+    FORKLIFT.emergency_stop()
+    return jsonify({'Success': True, 'estop': 'engaged'})
+
+@app.route('/get_estop', methods=['POST', 'GET'])
+def get_estop():
+    estop = FORKLIFT.estop_engaged
+    return jsonify({'Success': True, 'estop': estop})
+
+@app.route('/get_arm', methods=['POST', 'GET'])
+def get_arm():
+    armed = FORKLIFT.get_arm()
+    return jsonify({'Success': True, 'armed': armed})
+
+@app.route('/get_speed', methods=['POST', 'GET'])
+def get_speed():
+    speed = FORKLIFT.get_speed()
+    remapped_speed = remap(speed, 2000, 1000, 100, -100)
+    return jsonify({'Success': True, 'speed': remapped_speed})
+
+@app.route('/get_steering', methods=['POST', 'GET'])
+def get_steering():
+    steering = FORKLIFT.get_steering()
+    remapped_steering = remap(steering, 2000, 1000, 100, -100)
+    return jsonify({'Success': True, 'steering': remapped_steering})
+
+@app.route('/approach_pallet', methods=['POST'])
+def approach_pallet():
+    FORKLIFT.approach_pallet()
+    return jsonify({'Success': True, 'estop': 'engaged'})
+
+# @app.route('/close', methods=['POST'])
+# def close_forklift():
+#     logger.info(f"Closing forklift")
+#     FORKLIFT.vehicle.close()
+#     return jsonify({'Success': True})
+
+app.run(host='0.0.0.0', port=5000)

Control/Pi/pallet_proximity_sensor.py

@@ -0,0 +1,44 @@
+import serial
+import logging
+import time
+
+class PalletProximity:
+    def __init__(self) -> None:
+        self.log = logging.getLogger("PALLET PROXIMITY SENSOR")
+        self.log.setLevel("DEBUG")
+        # self.serial_string = '/dev/cu.usbmodem1442401'
+        self.serial_string = '/dev/micro'
+        self.alive = False
+        self.distance_to_pallet = 999
+        try:
+            self.ser = serial.Serial(self.serial_string, 9600, timeout=1)
+            self.alive = True
+            self.log.info('Proximity serial open')
+        except Exception as e:
+            self.log.critical(f"Cannot open serial device, {e}")
+            self.log.critical('Exiting')
+            exit(0)
+
+    def get_prox_reading(self):
+        try:
+            self.ser.flushInput()
+            time.sleep(0.05)
+            readings = 0
+            for _ in range(3):
+                raw = str(self.ser.readline())
+                raw2 = int(raw.split("|")[1])
+                readings += raw2
+            self.distance_to_pallet = int(readings / 3)
+            return self.distance_to_pallet
+        except Exception as e:
+            self.log.critical(f"ERROR: {e}, serial input: {raw}")
+            self.distance_to_pallet
+            # return False
+
+
+if __name__ == "__main__":
+    print("Testing...")
+    a = PalletProximity()
+    while True:
+        # time.sleep(0.1)
+        print(a.get_prox_reading())

Control/Pi/previous_mag_cal

@@ -0,0 +1 @@
+-17.25;18.95;-43.8;-13.95;-0.75;28.875

Control/Pi/proximity_detector.py

@@ -0,0 +1,121 @@
+import serial
+import logging
+import time
+
+
+class Proximity:
+    def __init__(self, state_machine, vehicle_ctrl) -> None:
+        self.log = logging.getLogger("PROXIMITY ARRAY")
+        self.log.setLevel("DEBUG")
+        # self.serial_string = '/dev/cu.usbmodem14601'
+        self.serial_string = '/dev/proximity'
+        self.alive = False
+        self.proximity_stopped = False
+        self.limit_cm = 30
+        self.failed_reads = 0
+        self.failed_reads_thresh = 4
+        # self.limit_cm = 5
+        self.state_machine = state_machine
+        self.vehicle_ctrl = vehicle_ctrl
+        self.rear_sensors = [3, 5, 2]  # TODO: change with more sensors
+        self.proximity_detects = 0
+        self.proximity_detect_thresh = 1
+        try:
+            self.ser = serial.Serial(self.serial_string, 9600, timeout=1)
+            self.alive = True
+            self.log.info('Proximity serial open')
+        except Exception as e:
+            self.log.critical(f"Cannot open serial device, {e}")
+            self.log.critical('Exiting')
+            exit(0)
+        self.proximities = [99, 99, 99, 99, 99, 99, 99, 99]
+
+    def get_prox_readings(self):
+        try:
+            self.ser.flushInput()
+            time.sleep(0.05)
+            p1 = p2 = p3 = p4 = p5 = p6 = p7 = p8 = p9 = p10 = p11 = p12 = 0
+            for _ in range(2):
+                raw = str(self.ser.readline())
+                raw2 = raw.split('\\r')[0].split("'")[1]
+                raw3 = raw2.split('|')
+                p1 += int(raw3[0])
+                p2 += int(raw3[1])
+                p3 += int(raw3[2])
+                p4 += int(raw3[3])
+                p5 += int(raw3[4])
+                p6 += int(raw3[5])
+                p7 += int(raw3[6])
+                p8 += int(raw3[7])
+                p9 += int(raw3[8])
+                p10 += int(raw3[9])
+                p11 += int(raw3[10])
+                p12 += int(raw3[11])
+                self.proximities = [int(p1/2), int(p2/2), int(p3/2), int(p4/2), int(p5/2), int(p6/2), int(p7/32), int(p8/2)]
+                self.failed_reads = 0
+                return self.proximities
+        except Exception as e:
+            self.log.critical(f"ERROR: {e}, serial input: {raw}")
+            self.failed_reads += 1
+            if self.failed_reads > self.failed_reads_thresh:
+                self.process_system_failure()
+
+    def process_system_failure(self):
+        self.log.critical(f"Got {self.failed_reads} sequential fails, killing process...")
+        self.alive = False
+
+    def check_proximity(self):
+        sensor, dist = self.perform_thresh()
+        # self.log.info(f"Proximities: FFR: {self.proximities[7]} | FFL: {self.proximities[6]} | FR: {self.proximities[1]} | FC: {self.proximities[4]} | FL: {self.proximities[0]} | RL: {self.proximities[3]} | RC: {self.proximities[5]} | RR: {self.proximities[2]}")
+        self.log.info(f"Proximities: {self.proximities}")
+        if sensor != None:
+            self.proximity_detects +=1
+            if self.proximity_detects > self.proximity_detect_thresh:
+                front_detect = True
+                if sensor in self.rear_sensors:
+                    front_detect = False
+                self.log.warning(f"Detected distance {dist} on sensor {sensor+1} ({'front' if front_detect else 'rear'}), it is below {self.limit_cm}")
+                self.proximity_stopped = True
+                _, prev_state = self.state_machine.get_state()
+                if prev_state == "NAVIGATING_TO_POINT_VISION" and front_detect:
+                    self.log.warning("Because we are in vision navigation, we are not stopping as this is likely caused by the pallet")
+                else:
+                    self.log.warning(f"Stopping!")
+                    if front_detect:
+                        self.state_machine.set_state("PROXIMITY_STOP_FRONT")
+                    else:
+                        self.state_machine.set_state("PROXIMITY_STOP_REAR")
+                    # self.vehicle_ctrl.clear_override_channels()
+                    while self.proximity_stopped:
+                        if front_detect:
+                            self.vehicle_ctrl.override_stop_drivetrain_fwd()
+                        else:
+                            self.vehicle_ctrl.override_stop_drivetrain_back()
+                        time.sleep(0.3)
+                        obstacle_is_clear = self.check_for_no_obstacle()
+                        if obstacle_is_clear:
+                            self.log.info("Obstacle is clear")
+                            self.state_machine.set_state(prev_state)
+                            self.proximity_stopped = False
+                            self.vehicle_ctrl.clear_override_channels()
+        else:
+            self.proximity_detects = 0
+
+
+    def check_for_no_obstacle(self):
+        sensor, _ = self.perform_thresh()
+        if sensor == None:
+            return True
+        return False
+
+    def perform_thresh(self):
+        self.get_prox_readings()
+        for ix, dist in enumerate(self.proximities):
+            if dist < self.limit_cm:
+                return ix, dist
+        return None, None
+
+if __name__ == "__main__":
+    test = Proximity()
+    while True:
+        test.check_proximity()

Control/Pi/proxmap

@@ -0,0 +1,11 @@
+1: FL
+2: FR
+3: RR
+4: RL
+5: FC
+6: RC
+7: FFL
+8: FFR
+(9):
+(10):
+

Control/Pi/slash/etc/udev/rules.d/99-usb.rules

@@ -0,0 +1,3 @@
+SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", ATTRS{serial}=="0001", SYMLINK+="compass"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="2341", ATTRS{idProduct}=="0042", ATTRS{serial}=="85037313735351108192", SYMLINK+="proximity"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="2341", ATTRS{idProduct}=="8036", SYMLINK+="micro"

Control/Pi/state_machine.py

@@ -0,0 +1,50 @@
+import os
+import logging
+
+class StateMachineHandler:
+    def __init__(self, force=False) -> None:
+        self.log = logging.getLogger("STATE MACHINE")
+        self.log.setLevel("DEBUG")
+        # self.state_machine_file = "/Users/max/Desktop/THEA_CODE/Control/Jetson/tests/MACHINE_STATE"
+        self.state_machine_file = "/home/pi/MACHINE_STATE"
+        self.__MACHINE_STATES = {
+                "IDLE": 0,
+                "NAVIGATING_TO_POINT_GPS": 1,
+                "NAVIGATING_TO_POINT_VISION": 2,
+                "MANIPULATING_CARGO": 3,
+                "READY_FOR_NEXT_TASK": 4,
+                "RC_CONTROL": 5,
+                "PROXIMITY_STOP_FRONT": 6,
+                "PROXIMITY_STOP_REAR": 7,
+                "ESTOP": 8,
+                "OTHER_ERROR": 9
+            }
+        self.init_file(force=force)
+
+    def get_state(self):
+        if os.path.isfile(self.state_machine_file):
+            with open(self.state_machine_file, 'r') as smfile:
+                state = int(smfile.read())
+                return state, list(self.__MACHINE_STATES.keys())[list(self.__MACHINE_STATES.values()).index(int(state))]
+
+    def init_file(self, force=False):
+        if not os.path.isfile(self.state_machine_file) or force:
+            self.log.info(f"Creating new state file, force flag is {force}")
+            with open(self.state_machine_file, 'w+') as smfile:
+                smfile.write(str(self.__MACHINE_STATES["IDLE"]))
+        else:
+            self.log.info("State file already exists and force flag is absent, leaving file as is")
+
+    def set_state(self, state):
+        if self.check_set_state_validity():
+            with open(self.state_machine_file, 'w') as smfile:
+                smfile.write(str(self.__MACHINE_STATES.get(state, 8)))
+            return True
+        return False
+
+    def check_set_state_validity(self):
+        _, state = self.get_state()
+        if state in ["ESTOP", "OTHER_ERROR"]:
+            self.log.critical(f"Machine state is {state}! Cannot perform operation")
+            return False
+        return True

Control/Pi/testing/CMPS14.py

@@ -0,0 +1,52 @@
+import time
+from threading import Thread
+
+import serial
+
+
+class CMPS14:
+    def __init__(self, com_port="/dev/cu.SLAB_USBtoUART", baudrate=115200) -> None:
+        """
+        Other com port: /dev/cu.SLAB_USBtoUART
+        """
+        self.serial = serial.Serial(com_port, baudrate=baudrate)
+        init_message = str(self.serial.readline())
+        assert 'I2C begin Failed' not in init_message, "No I2C sensor detected!"
+        self.heading = 0
+        self.heading_offset = 0
+        self.running = False
+        time.sleep(1)
+
+    def start(self):
+        t = Thread(target=self.update, args=())
+        t.daemon = True
+        self.running = True
+        t.start()
+        return self
+
+    def update(self):
+        while self.running:
+            try:
+                ser_bytes = str(self.serial.readline())
+                useful = ser_bytes.split('\\')[0].split('\'')[1]
+                heading = float(useful.split(':')[1])
+                self.heading = heading
+            except Exception as e:
+                print(f"Error: {e}")
+
+    def read(self):
+        corrected_heading = self.heading + self.heading_offset
+        if corrected_heading < 0:
+            corrected_heading += 360
+        if corrected_heading > 360:
+            corrected_heading -= 360
+        return corrected_heading
+
+    def set_heading_offset(self, offset):
+        self.heading_offset = offset
+
+    def update_heading_offset(self, offset):
+        self.heading_offset += offset
+
+    def stop(self):
+        self.running = False

Control/Pi/testing/mag_testing.py

@@ -0,0 +1,59 @@
+import time
+import board
+import adafruit_bno055
+import csv
+from math import atan2, pi
+
+
+i2c = board.I2C()
+sensor = adafruit_bno055.BNO055_I2C(i2c)
+
+
+def start_recording():
+    with open('results.csv', 'w', newline='') as csvfile:
+            csvwriter = csv.writer(csvfile, delimiter=',', quotechar='|', quoting=csv.QUOTE_MINIMAL)
+            csvwriter.writerow(['time', 'x', 'y', 'z'])
+    print("Starting recording for 60s.")
+    end_time = time.time() + 60
+    while time.time() < end_time:
+        taking_measurement_end = time.time() + 0.05
+        try:
+            mag_data = sensor.magnetic
+            do_write = True
+            for i in [0, 1]:
+                if abs(mag_data[i]) > 15:
+                    print(f"Got {mag_data[i]} on {'X' if i == 0 else 'Y' if i==1 else 'Z'}, skipping")
+                    do_write = False
+            if do_write:
+                with open('results.csv', 'a', newline='') as csvfile:
+                    csvwriter = csv.writer(csvfile, delimiter=',', quotechar='|', quoting=csv.QUOTE_MINIMAL)
+                    csvwriter.writerow([time.time(), mag_data[0], mag_data[1], mag_data[2]])
+            while time.time() < taking_measurement_end:
+                pass
+        except Exception as e:
+            print(f"Encountered exception: {e}")
+            continue
+
+    print("Done")
+
+def get_heading():
+    try:
+        mag_data = sensor.magnetic
+        for i in [0, 1]:
+            if abs(mag_data[i]) > 15:
+                print(f"Got {mag_data[i]} on {'X' if i == 0 else 'Y' if i==1 else 'Z'}, skipping")
+                return False
+        heading = 180 * atan2(mag_data[1],mag_data[0])/pi
+        if heading < 0:
+            heading += 360
+        print(f"Heading: {heading}")
+    except Exception as e:
+        print(f"Encountered exception: {e}")
+        return False
+
+
+start_recording()
+
+#while True:
+#    get_heading()
+#    time.sleep(0.05)

Control/Pi/testing/visualizer.py

@@ -0,0 +1,80 @@
+import cv2
+import time
+import board
+import adafruit_bno055
+import csv
+import numpy as np
+from math import atan2, pi, cos, sin, pi
+
+
+i2c = board.I2C()
+sensor = adafruit_bno055.BNO055_I2C(i2c)
+
+points = []
+
+def remap(value, maxInput, minInput, maxOutput, minOutput):
+	value = maxInput if value > maxInput else value
+	value = minInput if value < minInput else value
+	inputSpan = maxInput - minInput
+	outputSpan = maxOutput - minOutput
+	scaledThrust = float(value - minInput) / float(inputSpan)
+	return minOutput + (scaledThrust * outputSpan)
+
+def draw_frame(heading, mag_data):
+    mag_x, mag_y = mag_data
+    global points
+    frame = np.full(shape=[400, 400, 3], fill_value=255, dtype=np.uint8)
+    length = 150
+    P1 = (200, 200)
+    P2_0 = int(P1[0] + length * cos(heading * pi / 180.0))
+    P2_1 = int(P1[1] + length * sin(heading * pi / 180.0))
+    P2 = (P2_0, P2_1)
+
+    frame = cv2.line(frame,P1,P2,(255,0,0),2)
+    frame = cv2.circle(frame, (mag_x, mag_y), radius=1, color=(0, 255, 0), thickness=3)
+
+    for p in points:
+        frame = cv2.circle(frame, (p[0], p[1]), radius=1, color=(0, 0, 255), thickness=3)
+    points.append((mag_x, mag_y))
+    frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE)
+
+    font = cv2.FONT_HERSHEY_SIMPLEX
+    org_n = (200, 30)
+    org_s = (200, 370)
+    org_w = (30, 200)
+    org_e = (370, 200)
+    fontScale = 1
+    color = (255, 0, 0)
+    thickness = 2
+    frame = cv2.putText(frame, 'N', org_n, font, fontScale, color, thickness, cv2.LINE_AA)
+    frame = cv2.putText(frame, 'S', org_s, font, fontScale, color, thickness, cv2.LINE_AA)
+    frame = cv2.putText(frame, 'W', org_w, font, fontScale, color, thickness, cv2.LINE_AA)
+    frame = cv2.putText(frame, 'E', org_e, font, fontScale, color, thickness, cv2.LINE_AA)
+
+    cv2.imshow('direction', frame)
+
+def get_heading():
+    try:
+        mag_data = sensor.magnetic
+        for i in [0, 1]:
+            if abs(mag_data[i]) > 20:
+                print(f"Got {mag_data[i]} on {'X' if i == 0 else 'Y' if i==1 else 'Z'}, skipping")
+                return False
+        heading = 180 * atan2(mag_data[1],mag_data[0])/pi
+        if heading < 0:
+            heading += 360
+        mag_x = int(remap(mag_data[0], 20, -20, 0, 400))
+        mag_y = int(remap(mag_data[1], 20, -20, 0, 400))
+        print(f'Heading: {heading}')
+        return (heading+180, (mag_x, mag_y))
+    except Exception as e:
+        print(f"Encountered exception: {e}")
+        return False
+
+while True:
+    heading = get_heading()
+    if heading:
+        draw_frame(heading[0], heading[1])
+        cv2.waitKey(1)
+
+    time.sleep(0.2)

Embedded/ICM_mag_reader/ICM_mag_reader.ino

@@ -0,0 +1,52 @@
+#include <Adafruit_ICM20X.h>
+#include <Adafruit_ICM20948.h>
+#include <Adafruit_Sensor.h>
+#include <Wire.h>
+
+Adafruit_ICM20948 icm;
+uint16_t measurement_delay_us = 65535; // Delay between measurements for testing
+// // For SPI mode, we need a CS pin
+// #define ICM_CS 10
+// // For software-SPI mode we need SCK/MOSI/MISO pins
+// #define ICM_SCK 13
+// #define ICM_MISO 12
+// #define ICM_MOSI 11
+
+void setup(void) {
+  Serial.begin(115200);
+  while (!Serial)
+    delay(10); // will pause Zero, Leonardo, etc until serial console opens
+  // Try to initialize!
+  if (!icm.begin_I2C()) {
+    // if (!icm.begin_SPI(ICM_CS)) {
+    // if (!icm.begin_SPI(ICM_CS, ICM_SCK, ICM_MISO, ICM_MOSI)) {
+
+    Serial.println("Failed to find ICM20948 chip");
+    while (1) {
+      delay(10);
+    }
+  }
+}
+
+void loop() {
+
+  //  /* Get a new normalized sensor event */
+  sensors_event_t accel;
+  sensors_event_t gyro;
+  sensors_event_t mag;
+  sensors_event_t temp;
+  icm.getEvent(&accel, &gyro, &temp, &mag);
+
+  double mag_x = mag.magnetic.x;
+  double mag_y = mag.magnetic.y;
+  double mag_z = mag.magnetic.z;
+  
+  if(mag_z > -2000 && mag_z < 2000) {
+    Serial.print(mag.magnetic.x);
+    Serial.print(";");
+    Serial.print(mag.magnetic.y);
+    Serial.print(";");
+    Serial.println(mag.magnetic.z);
+  }
+  delay(11);
+}

Embedded/debugging/debugging.ino

@@ -0,0 +1,108 @@
+#include "Dynamixel_Servo.h"
+
+#define HALF_DUPLEX_DIRECTION_PIN 6
+#define SERVO_TIMEOUT 200 //milliseconds
+
+#define FORWARD_DIRECTION_PIN 7
+#define REVERSE_DIRECTION_PIN 8
+
+#define STEERING_INPUT 14
+#define THROTTLE_INPUT 15
+
+#define THROTTLE_A 3
+#define THROTTLE_B 5
+
+// #define LOWEST_PULSE_THROTTLE 17850
+// #define HIGHEST_PULSE_THROTTLE 18700
+
+#define LOWEST_PULSE_THROTTLE 17950
+#define HIGHEST_PULSE_THROTTLE 18900
+
+#define LOWEST_PULSE_STEERING 17780
+#define HIGHEST_PULSE_STEERING 18790
+
+
+static float CHAN_A_MIN_VOLTAGE = 1.37 - 0.11;
+static float CHAN_A_MAX_VOLTAGE = (3.82 - 0.28)*0.50;  // NOTE: Go lower bc we don't wanna go supersonic during the demo
+static float CHAN_B_MIN_VOLTAGE = 0.68 - 0.15;
+static float CHAN_B_MAX_VOLTAGE = (3.18 - 0.37)*0.50;  // NOTE: Go lower bc we don't wanna go supersonic during the demo
+
+int32_t throttle_in;
+int32_t steering_in;
+int32_t throttle_in_mapped;
+int32_t steering_in_mapped;
+
+int throttle_speed;
+int steering_amount;
+
+int mapped_throttle_chan_a;
+int mapped_throttle_chan_b;
+
+struct channel_outputs {
+    double chan_a;
+    double chan_b;
+};
+
+float mapping(float in, float in_min, float in_max, float out_min, float out_max){
+  float mapped = (((in - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min;
+  return mapped;
+}
+
+
+void setup() {
+  pinMode(STEERING_INPUT, INPUT_PULLUP);
+  pinMode(THROTTLE_INPUT, INPUT_PULLUP);
+  pinMode(THROTTLE_A, OUTPUT);
+  pinMode(THROTTLE_B, OUTPUT);
+  pinMode(FORWARD_DIRECTION_PIN, OUTPUT);
+  pinMode(REVERSE_DIRECTION_PIN, OUTPUT);
+  digitalWrite(FORWARD_DIRECTION_PIN, HIGH);
+  digitalWrite(REVERSE_DIRECTION_PIN, HIGH);
+
+  servo_init(&Serial1, HALF_DUPLEX_DIRECTION_PIN, SERVO_DEFAULT_BAUD);
+
+  Serial.begin(9600);
+  delay(2000);
+  Serial.println("Starting...");
+}
+
+
+void steering_zero() {
+  servo_error_t error;
+  int steering_angle_deg = 0;
+  error = servo_set(SERVO_DEFAULT_ID, SERVO_REGISTER_GOAL_ANGLE, steering_angle_deg, SERVO_TIMEOUT);
+  Serial.print(" STEERING ZERO ");
+}
+
+void set_steering(float angle_deg){
+  servo_error_t error;
+  float angle_rad = angle_deg * 1000. / 57296.;
+  error = servo_set(SERVO_DEFAULT_ID, SERVO_REGISTER_GOAL_ANGLE, angle_rad, SERVO_TIMEOUT);
+  if (! (error & SERVO_NO_ERROR)){
+    Serial.print("SERVO ERROR: ");
+    Serial.println(error);
+  }
+}
+
+
+void steering(int32_t steering_pwm) {
+  if (steering_pwm < 17000) {
+    steering_zero();
+  } else {
+    steering_in_mapped = map(steering_pwm, LOWEST_PULSE_STEERING, HIGHEST_PULSE_STEERING, 0, 1023);
+    set_steering(steering_in_mapped);
+  }
+}
+
+
+void loop() {
+  steering_in = pulseIn(STEERING_INPUT, LOW);
+  Serial.print(" SPIN: ");
+  Serial.print(steering_in);
+  // steering(steering_in);
+  set_steering(512);
+  delay(500);
+  set_steering(0);
+  delay(500);
+  Serial.println();
+}

Embedded/drivetrain_ctrl/drivetrain_ctrl.ino

@@ -0,0 +1,286 @@
+#include <Dynamixel2Arduino.h>
+#include <Adafruit_MCP4728.h>
+#include <Wire.h>
+
+#define DXL_SERIAL   Serial1
+#define DEBUG_SERIAL Serial
+const uint8_t DXL_DIR_PIN = 4; // DYNAMIXEL Shield DIR PIN
+
+#define MAX_V 5.2
+#define SPEED_MULTIPLIER 0.8
+
+const uint8_t DXL_ID = 1;
+const float DXL_PROTOCOL_VERSION = 1.0;
+
+Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
+using namespace ControlTableItem;
+
+Adafruit_MCP4728 mcp;
+
+long zero_offset = 0.0;
+
+//TOP THROT: 18792 | BOT THROT: 17991 | TOP STEER: 19092 | BOT STEER: 17691
+
+
+#define FORWARD_DIRECTION_PIN 7
+#define REVERSE_DIRECTION_PIN 8
+
+#define STEERING_INPUT 14
+#define THROTTLE_INPUT 15
+
+#define TRIM_RIGHT 16
+#define TRIM_LEFT 10 //TODO: change pins
+#define TRIM_DELTA 100 //TODO: tune this
+
+
+#define LOWEST_PULSE_THROTTLE 17970
+#define HIGHEST_PULSE_THROTTLE 18800
+
+// #define LOWEST_PULSE_STEERING 17650
+// #define HIGHEST_PULSE_STEERING 19150
+
+#define LOWEST_PULSE_STEERING 17600
+#define HIGHEST_PULSE_STEERING 19200
+
+
+static float CHAN_A_MIN_VOLTAGE = 1.37 + 0.0;
+static float CHAN_A_MAX_VOLTAGE = (3.82 + 0.0) * SPEED_MULTIPLIER;  // NOTE: Go lower bc we don't wanna go supersonic during the demo
+static float CHAN_B_MIN_VOLTAGE = 0.68 + 0.0;
+static float CHAN_B_MAX_VOLTAGE = (3.18 + 0.0) * SPEED_MULTIPLIER;  // NOTE: Go lower bc we don't wanna go supersonic during the demo
+
+int32_t throttle_in;
+int32_t steering_in;
+int32_t throttle_in_mapped;
+long steering_in_mapped;
+
+int throttle_speed;
+int steering_amount;
+
+int mapped_throttle_chan_a;
+int mapped_throttle_chan_b;
+
+struct channel_outputs {
+    double chan_a;
+    double chan_b;
+};
+
+float mapping(float in, float in_min, float in_max, float out_min, float out_max){
+  float mapped = (((in - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min;
+  return mapped;
+}
+
+void init_servo(){
+  dxl.begin(57600);
+  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
+  dxl.ping(DXL_ID);
+  dxl.torqueOff(DXL_ID);
+  dxl.setOperatingMode(DXL_ID, OP_EXTENDED_POSITION);
+  dxl.writeControlTableItem(RESOLUTION_DIVIDER, DXL_ID, 1);
+  dxl.torqueOn(DXL_ID);
+  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 0);
+  delay(500);
+}
+
+int convert_to_digital(float v){
+  int v_out;
+  v_out = (v/5.0) * 4095;
+  return v_out;
+}
+int minmax(int v){
+  if (v > 4095) {v = 4095;}
+  if (v < 0) {v = 0;}
+  return v;
+}
+
+float low_voltage_compensator(float v){
+  float v_offset = 5.0 - MAX_V;
+  float v_proportional = v/5.0;
+  float adjusted_v = v + (v_proportional * v_offset);
+//  return v;
+  return adjusted_v;
+}
+
+int process_voltage(float v){
+  float v_adj = low_voltage_compensator(v);
+  int v_int = convert_to_digital(v_adj);
+  v_int = minmax(v_int);
+  return v_int;
+}
+
+void set_voltage_a(float v){
+  int v_int = process_voltage(v);
+  mcp.setChannelValue(MCP4728_CHANNEL_A, v_int);
+}
+void set_voltage_b(float v){
+  int v_int = process_voltage(v);
+  mcp.setChannelValue(MCP4728_CHANNEL_B, v_int);
+}
+
+void increment_trim(int amount){
+  zero_offset = zero_offset + amount;
+}
+
+void decrement_trim(int amount){
+  zero_offset = zero_offset - amount;
+}
+
+void stop_throttle() {
+  set_voltage_a(CHAN_A_MIN_VOLTAGE);
+  set_voltage_b(CHAN_B_MIN_VOLTAGE);
+  Serial.print(" THROTTLE STOPPED ");
+}
+
+void steering_zero() {
+  dxl.setGoalPosition(DXL_ID, 0+zero_offset);
+}
+
+void set_throttle(int speed){
+  channel_outputs pwm_out = pwm_to_outputs(speed);
+  set_voltage_a(pwm_out.chan_a);
+  set_voltage_b(pwm_out.chan_b);
+}
+
+void go_forward(int speed){
+    set_forward();
+    set_throttle(speed);
+}
+
+void go_back(int speed){
+    set_reverse();
+    set_throttle(speed);
+}
+
+void stop(){
+    stop_throttle();
+    set_neutral();
+}
+
+void set_forward(){
+  digitalWrite(REVERSE_DIRECTION_PIN, HIGH);
+  digitalWrite(FORWARD_DIRECTION_PIN, LOW);
+}
+
+void set_reverse(){
+  digitalWrite(FORWARD_DIRECTION_PIN, HIGH);
+  digitalWrite(REVERSE_DIRECTION_PIN, LOW);
+}
+
+void set_neutral(){
+  digitalWrite(FORWARD_DIRECTION_PIN, HIGH);
+  digitalWrite(REVERSE_DIRECTION_PIN, HIGH);
+}
+
+void set_steering(long steering_raw) {
+  dxl.setGoalPosition(DXL_ID, steering_raw+zero_offset);
+}
+
+channel_outputs pwm_to_outputs(int pwm){
+  channel_outputs pwm_out;
+  float out_voltage_a = 0.;
+  float out_voltage_b = 0.;
+  int out_pwm_a;
+  int out_pwm_b;
+  out_voltage_a = mapping(pwm, 0., 255., CHAN_A_MIN_VOLTAGE, CHAN_A_MAX_VOLTAGE);
+  out_voltage_b = mapping(pwm, 0, 255, CHAN_B_MIN_VOLTAGE, CHAN_B_MAX_VOLTAGE);
+  Serial.print(" PWM: ");
+  Serial.print(pwm);
+  Serial.print(" T_A: ");
+  Serial.print(out_voltage_a);
+  Serial.print(" T_B: ");
+  Serial.print(out_voltage_b);
+  pwm_out.chan_a = out_voltage_a;
+  pwm_out.chan_b = out_voltage_b;
+  return (pwm_out);
+}
+
+void throttle(int32_t throttle_pwm) {
+  if (throttle_pwm < 1600) {
+    stop();
+  } else {
+    throttle_in_mapped = map(throttle_pwm, LOWEST_PULSE_THROTTLE, HIGHEST_PULSE_THROTTLE, 0, 1000);
+    if (throttle_in_mapped >= 400 && throttle_in_mapped <= 600) {stop();}
+    else if (throttle_in_mapped > 600) {
+      throttle_speed = map(throttle_in_mapped, 600, 1000, 0, 255);
+      go_forward(throttle_speed);
+    } else if (throttle_in_mapped < 400) {
+      throttle_speed = map(throttle_in_mapped, 0, 400, 0, 255);
+      throttle_speed = 255 - throttle_speed;
+      go_back(throttle_speed);
+    }
+  }
+}
+
+void steering(int32_t steering_pwm) {
+  if (steering_pwm < 17000) {
+    steering_zero();
+  } else {
+    steering_in_mapped = map(steering_pwm, LOWEST_PULSE_STEERING, HIGHEST_PULSE_STEERING, -7796, 7796);
+    Serial.print(" GT: ");
+    Serial.print(steering_in_mapped);
+    if (steering_in_mapped >= -200 && steering_in_mapped <= 200) {steering_zero();}
+    else {
+      set_steering(steering_in_mapped);
+    }
+  }
+}
+
+void check_trim_pins(){
+  if (digitalRead(TRIM_RIGHT) == LOW) {increment_trim(TRIM_DELTA); delay(100);}
+  if (digitalRead(TRIM_LEFT) == LOW) {decrement_trim(TRIM_DELTA); delay(100);}
+}
+
+
+void setup() {
+  delay(1000);                                                            // Give time for Dynamixel to start on power-up
+  pinMode(STEERING_INPUT, INPUT_PULLUP);
+  pinMode(THROTTLE_INPUT, INPUT_PULLUP);
+  pinMode(FORWARD_DIRECTION_PIN, OUTPUT);
+  pinMode(REVERSE_DIRECTION_PIN, OUTPUT);
+  pinMode(TRIM_RIGHT, INPUT_PULLUP);
+  pinMode(TRIM_LEFT, INPUT_PULLUP);
+  digitalWrite(FORWARD_DIRECTION_PIN, HIGH);
+  digitalWrite(REVERSE_DIRECTION_PIN, HIGH);
+
+  init_servo();
+
+  DEBUG_SERIAL.begin(9600);
+  delay(500);
+  Serial.println("Starting...");
+
+  long current_position = dxl.getPresentPosition(DXL_ID);
+  zero_offset = current_position;
+  Serial.print("Zero_offset: ");
+  Serial.println(zero_offset);
+  delay(100);
+
+  Serial.print("Going to Zero (we should not move...): ");
+  Serial.print(zero_offset);
+  Serial.println(" Please wait for 2000ms...");
+  dxl.setGoalPosition(DXL_ID, zero_offset); delay(2000);
+
+  current_position = dxl.getPresentPosition(DXL_ID);
+  Serial.print("now at: ");
+  Serial.println(current_position);
+
+  if (!mcp.begin()) {
+    Serial.println("Failed to find MCP4728 chip");
+    while (1) {
+      delay(10);
+    }
+  }
+  Serial.println("Started.");
+}
+
+void loop() {
+  throttle_in = pulseIn(THROTTLE_INPUT, LOW);
+  Serial.print(" TPIN: ");
+  Serial.print(throttle_in);
+  throttle(throttle_in);
+  Serial.print(" || ");
+  steering_in = pulseIn(STEERING_INPUT, LOW);
+  Serial.print(" SPIN: ");
+  Serial.print(steering_in);
+  steering(steering_in);
+  Serial.println();
+  check_trim_pins();
+}

Embedded/fork_ctrl/fork_ctrl.ino

@@ -0,0 +1,281 @@
+#include <Adafruit_MCP4728.h>
+#include <Wire.h>
+
+Adafruit_MCP4728 mcp;
+
+#define MAX_V 5.2
+
+#define FORK_VERT_INPUT 14
+#define FORK_HORIZ_INPUT 15
+
+#define LOWEST_PULSE 17770
+#define HIGHEST_PULSE 18790
+// Recorded: L: 17791 H: 18786
+// #define PULSE_DELTA = 1100; // this doesn't change
+// Measured voltages: V: LA:1.04 MA:2.40 HA:3.76 LB:0.80 MB:2.16 HB:3.52
+// Measured voltages: H: LA:1.04 MA:2.40 HA:3.76 LB:0.96 MB:2.32 HB:3.60
+/*
+    CHAN_A IS VERT A
+    CHAN_B IS VERT B
+    CHAN_C IS HORIZ A
+    CHAN_D IS HORIZ B
+*/
+
+static float CHAN_A_V_MIN_VOLTAGE = 1.04 + 0.0;
+static float CHAN_A_V_MAX_VOLTAGE = 3.76 + 0.0;
+static float CHAN_B_V_MIN_VOLTAGE = 0.80 - 0.0;
+static float CHAN_B_V_MAX_VOLTAGE = 3.52 - 0.0;
+
+static float CHAN_A_H_MIN_VOLTAGE = 1.04 + 0.0;
+static float CHAN_A_H_MAX_VOLTAGE = 3.76 + 0.0;
+static float CHAN_B_H_MIN_VOLTAGE = 0.96 + 0.0;
+static float CHAN_B_H_MAX_VOLTAGE = 3.60 + 0.0;
+
+static float STOP_VOLTAGE_V_A = 2.40 + 0.08;
+static float STOP_VOLTAGE_V_B = 2.16 + 0.00;
+
+static float STOP_VOLTAGE_H_A = 2.40 + 0.0;
+static float STOP_VOLTAGE_H_B = 2.32 + 0.0;
+
+int32_t fork_vert_in;
+int32_t fork_horiz_in;
+int32_t fork_vert_in_mapped;
+int32_t fork_horiz_in_mapped;
+
+int fork_vert_speed;
+int fork_horiz_speed;
+
+int mapped_fork_vert_chan_a;
+int mapped_fork_vert_chan_b;
+int mapped_fork_horiz_chan_a;
+int mapped_fork_horiz_chan_b;
+
+struct channel_outputs {
+    double chan_a;
+    double chan_b;
+};
+
+//helper functions 
+float mapping(float in, float in_min, float in_max, float out_min, float out_max){
+  float mapped = (((in - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min;
+  return mapped;
+}
+
+float low_voltage_compensator(float v){
+  float v_offset = 5.0 - MAX_V;
+  float v_proportional = v/5.0;
+  float adjusted_v = v + (v_proportional * v_offset);
+  return adjusted_v;
+}
+
+int convert_to_digital(float v){
+  int v_out;
+  v_out = (v/5.0) * 4095;
+  return v_out;
+}
+int minmax(int v){
+  if (v > 4095) {v = 4095;}
+  if (v < 0) {v = 0;}
+  return v;
+}
+
+int process_voltage(float v){
+  float v_adj = low_voltage_compensator(v);
+  int v_int = convert_to_digital(v_adj);
+  v_int = minmax(v_int);
+  return v_int;
+}
+//helper functions 
+
+void set_voltage_a(float v){
+  int v_int = process_voltage(v);
+  mcp.setChannelValue(MCP4728_CHANNEL_A, v_int);
+}
+void set_voltage_b(float v){
+  int v_int = process_voltage(v);
+  mcp.setChannelValue(MCP4728_CHANNEL_B, v_int);
+}
+void set_voltage_c(float v){
+  int v_int = process_voltage(v);
+  mcp.setChannelValue(MCP4728_CHANNEL_C, v_int);
+}
+void set_voltage_d(float v){
+  int v_int = process_voltage(v);
+  mcp.setChannelValue(MCP4728_CHANNEL_D, v_int);
+}
+
+void stop_fork_vert() {
+  set_voltage_a(low_voltage_compensator(STOP_VOLTAGE_V_A));
+  set_voltage_b(low_voltage_compensator(STOP_VOLTAGE_V_B));
+  Serial.print(" VERT STOPPED ");
+}
+
+void stop_fork_horiz() {
+  set_voltage_c(low_voltage_compensator(STOP_VOLTAGE_H_A));
+  set_voltage_d(low_voltage_compensator(STOP_VOLTAGE_H_B));
+  Serial.print(" HORIZ STOPPED ");
+}
+
+void fork_up(int speed){
+  channel_outputs pwm_out = pwm_to_outputs_v_low(speed);
+  set_voltage_a(pwm_out.chan_a);
+  set_voltage_b(pwm_out.chan_b);
+}
+
+void fork_down(int speed){
+  channel_outputs pwm_out = pwm_to_outputs_v_high(speed);
+  set_voltage_a(pwm_out.chan_a);
+  set_voltage_b(pwm_out.chan_b);
+}
+
+void fork_left(int speed){
+  channel_outputs pwm_out = pwm_to_outputs_h_high(speed);
+  set_voltage_c(pwm_out.chan_a);
+  set_voltage_d(pwm_out.chan_b);
+}
+
+void fork_right(int speed){
+channel_outputs pwm_out = pwm_to_outputs_h_low(speed);
+  set_voltage_c(pwm_out.chan_a);
+  set_voltage_d(pwm_out.chan_b);
+}
+
+channel_outputs pwm_to_outputs_v_high(int pwm){
+  channel_outputs pwm_out;
+  float out_voltage_a = 0.;
+  float out_voltage_b = 0.;
+  int out_pwm_a;
+  int out_pwm_b;
+  out_voltage_a = mapping(pwm, 0., 255., STOP_VOLTAGE_V_A, CHAN_A_V_MAX_VOLTAGE);
+  out_voltage_b = mapping(255 - pwm, 0, 255, CHAN_B_V_MIN_VOLTAGE, STOP_VOLTAGE_V_B);
+  Serial.print(" PWM: ");
+  Serial.print(pwm);
+  Serial.print(" HV_A: ");
+  Serial.print(out_voltage_a);
+  Serial.print(" HV_B: ");
+  Serial.print(out_voltage_b);
+  pwm_out.chan_a = out_voltage_a;
+  pwm_out.chan_b = out_voltage_b;
+  return (pwm_out);
+}
+
+channel_outputs pwm_to_outputs_v_low(int pwm){
+  channel_outputs pwm_out;
+  float out_voltage_a;
+  float out_voltage_b;
+  int out_pwm_a;
+  int out_pwm_b;
+  out_voltage_a = mapping(255 - pwm, 0., 255., CHAN_A_V_MIN_VOLTAGE, STOP_VOLTAGE_V_A);
+  out_voltage_b = mapping(pwm, 0., 255., STOP_VOLTAGE_V_B, CHAN_B_V_MAX_VOLTAGE);
+  Serial.print(" PWM: ");
+  Serial.print(pwm);
+  Serial.print(" LV_A: ");
+  Serial.print(out_voltage_a);
+  Serial.print(" LV_B: ");
+  Serial.print(out_voltage_b);
+  pwm_out.chan_a = out_voltage_a;
+  pwm_out.chan_b = out_voltage_b;
+  return (pwm_out);
+}
+
+channel_outputs pwm_to_outputs_h_low(int pwm){
+  channel_outputs pwm_out;
+  float out_voltage_a = 0.;
+  float out_voltage_b = 0.;
+  int out_pwm_a;
+  int out_pwm_b;
+  out_voltage_a = mapping(pwm, 0., 255., STOP_VOLTAGE_H_A, CHAN_A_H_MAX_VOLTAGE);
+  out_voltage_b = mapping(255 - pwm, 0, 255, CHAN_B_H_MIN_VOLTAGE, STOP_VOLTAGE_H_B);
+  Serial.print(" PWM: ");
+  Serial.print(pwm);
+  Serial.print(" HV_A: ");
+  Serial.print(out_voltage_a);
+  Serial.print(" HV_B: ");
+  Serial.print(out_voltage_b);
+  pwm_out.chan_a = out_voltage_a;
+  pwm_out.chan_b = out_voltage_b;
+  return (pwm_out);
+}
+
+channel_outputs pwm_to_outputs_h_high(int pwm){
+  channel_outputs pwm_out;
+  float out_voltage_a;
+  float out_voltage_b;
+  int out_pwm_a;
+  int out_pwm_b;
+  out_voltage_a = mapping(255 - pwm, 0., 255., CHAN_A_H_MIN_VOLTAGE, STOP_VOLTAGE_H_A);
+  out_voltage_b = mapping(pwm, 0., 255., STOP_VOLTAGE_H_B, CHAN_B_H_MAX_VOLTAGE);
+  Serial.print(" PWM: ");
+  Serial.print(pwm);
+  Serial.print(" LV_A: ");
+  Serial.print(out_voltage_a);
+  Serial.print(" LV_B: ");
+  Serial.print(out_voltage_b);
+  pwm_out.chan_a = out_voltage_a;
+  pwm_out.chan_b = out_voltage_b;
+  return (pwm_out);
+}
+
+void fork_vert(int32_t vert_pwm) {
+  if (vert_pwm < 17000) {
+    stop_fork_vert();
+  } else {
+    fork_vert_in_mapped = map(vert_pwm, LOWEST_PULSE, HIGHEST_PULSE, 0, 1000);
+    if (fork_vert_in_mapped >= 400 && fork_vert_in_mapped <= 600) {stop_fork_vert();}
+    else if (fork_vert_in_mapped > 600) {
+      fork_vert_speed = map(fork_vert_in_mapped, 600, 1000, 0, 255);
+      fork_up(fork_vert_speed);
+    } else if (fork_vert_in_mapped < 400) {
+      fork_vert_speed = map(fork_vert_in_mapped, 0, 400, 0, 255);
+      fork_vert_speed = 255 - fork_vert_speed;
+      fork_down(fork_vert_speed);
+    }
+  }
+}
+
+void fork_horiz(int32_t horiz_pwm) {
+  if (horiz_pwm < 17000) {
+    stop_fork_horiz();
+  } else {
+    fork_horiz_in_mapped = map(horiz_pwm, LOWEST_PULSE, HIGHEST_PULSE, 0, 1000);
+    if (fork_horiz_in_mapped >= 400 && fork_horiz_in_mapped <= 600) {stop_fork_horiz();}
+    else if (fork_horiz_in_mapped > 600) {
+      fork_horiz_speed = map(fork_horiz_in_mapped, 600, 1000, 0, 255);
+      fork_right(fork_horiz_speed);
+    } else if (fork_horiz_in_mapped < 400) {
+      fork_horiz_speed = map(fork_horiz_in_mapped, 0, 400, 0, 255);
+      fork_horiz_speed = 255 - fork_horiz_speed;
+      fork_left(fork_horiz_speed);
+    }
+  }
+}
+
+
+void setup() {
+  pinMode(FORK_VERT_INPUT, INPUT_PULLUP);
+  pinMode(FORK_HORIZ_INPUT, INPUT_PULLUP);
+  Serial.begin(9600);
+  delay(2000);
+  Serial.println("Starting...");
+  if (!mcp.begin()) {
+    Serial.println("Failed to find MCP4728 chip");
+    while (1) {
+      delay(10);
+    }
+  }
+  Serial.println("Started.");
+}
+
+void loop() {
+  fork_vert_in = pulseIn(FORK_VERT_INPUT, LOW);
+  Serial.print(" VPIN: ");
+  Serial.print(fork_vert_in);
+  fork_vert(fork_vert_in);
+  Serial.print(" || ");
+  fork_horiz_in = pulseIn(FORK_HORIZ_INPUT, LOW);
+  Serial.print(" HPIN: ");
+  Serial.print(fork_horiz_in);
+  fork_horiz(fork_horiz_in);
+  Serial.println();
+  delay(1);
+}

Embedded/libraries/Adafruit_BNO055/Adafruit_BNO055.cpp

@@ -0,0 +1,842 @@
+/*!
+ * @file Adafruit_BNO055.cpp
+ *
+ *  @mainpage Adafruit BNO055 Orientation Sensor
+ *
+ *  @section intro_sec Introduction
+ *
+ *  This is a library for the BNO055 orientation sensor
+ *
+ *  Designed specifically to work with the Adafruit BNO055 9-DOF Breakout.
+ *
+ *  Pick one up today in the adafruit shop!
+ *  ------> https://www.adafruit.com/product/2472
+ *
+ *  These sensors use I2C to communicate, 2 pins are required to interface.
+ *
+ *  Adafruit invests time and resources providing this open source code,
+ *  please support Adafruit andopen-source hardware by purchasing products
+ *  from Adafruit!
+ *
+ *  @section author Author
+ *
+ *  K.Townsend (Adafruit Industries)
+ *
+ *  @section license License
+ *
+ *  MIT license, all text above must be included in any redistribution
+ */
+
+#include "Arduino.h"
+
+#include <limits.h>
+#include <math.h>
+
+#include "Adafruit_BNO055.h"
+
+/*!
+ *  @brief  Instantiates a new Adafruit_BNO055 class
+ *  @param  sensorID
+ *          sensor ID
+ *  @param  address
+ *          i2c address
+ *  @param  theWire
+ *          Wire object
+ */
+Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address,
+                                 TwoWire *theWire) {
+// BNO055 clock stretches for 500us or more!
+#ifdef ESP8266
+  theWire->setClockStretchLimit(1000); // Allow for 1000us of clock stretching
+#endif
+
+  _sensorID = sensorID;
+  i2c_dev = new Adafruit_I2CDevice(address, theWire);
+}
+
+/*!
+ *  @brief  Sets up the HW
+ *  @param  mode
+ *          mode values
+ *           [OPERATION_MODE_CONFIG,
+ *            OPERATION_MODE_ACCONLY,
+ *            OPERATION_MODE_MAGONLY,
+ *            OPERATION_MODE_GYRONLY,
+ *            OPERATION_MODE_ACCMAG,
+ *            OPERATION_MODE_ACCGYRO,
+ *            OPERATION_MODE_MAGGYRO,
+ *            OPERATION_MODE_AMG,
+ *            OPERATION_MODE_IMUPLUS,
+ *            OPERATION_MODE_COMPASS,
+ *            OPERATION_MODE_M4G,
+ *            OPERATION_MODE_NDOF_FMC_OFF,
+ *            OPERATION_MODE_NDOF]
+ *  @return true if process is successful
+ */
+bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode) {
+
+  if (!i2c_dev->begin()) {
+    return false;
+  }
+
+  /* Make sure we have the right device */
+  uint8_t id = read8(BNO055_CHIP_ID_ADDR);
+  if (id != BNO055_ID) {
+    delay(1000); // hold on for boot
+    id = read8(BNO055_CHIP_ID_ADDR);
+    if (id != BNO055_ID) {
+      return false; // still not? ok bail
+    }
+  }
+
+  /* Switch to config mode (just in case since this is the default) */
+  setMode(OPERATION_MODE_CONFIG);
+
+  /* Reset */
+  write8(BNO055_SYS_TRIGGER_ADDR, 0x20);
+  /* Delay incrased to 30ms due to power issues https://tinyurl.com/y375z699 */
+  delay(30);
+  while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID) {
+    delay(10);
+  }
+  delay(50);
+
+  /* Set to normal power mode */
+  write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);
+  delay(10);
+
+  write8(BNO055_PAGE_ID_ADDR, 0);
+
+  /* Set the output units */
+  /*
+  uint8_t unitsel = (0 << 7) | // Orientation = Android
+                    (0 << 4) | // Temperature = Celsius
+                    (0 << 2) | // Euler = Degrees
+                    (1 << 1) | // Gyro = Rads
+                    (0 << 0);  // Accelerometer = m/s^2
+  write8(BNO055_UNIT_SEL_ADDR, unitsel);
+  */
+
+  /* Configure axis mapping (see section 3.4) */
+  /*
+  write8(BNO055_AXIS_MAP_CONFIG_ADDR, REMAP_CONFIG_P2); // P0-P7, Default is P1
+  delay(10);
+  write8(BNO055_AXIS_MAP_SIGN_ADDR, REMAP_SIGN_P2); // P0-P7, Default is P1
+  delay(10);
+  */
+
+  write8(BNO055_SYS_TRIGGER_ADDR, 0x0);
+  delay(10);
+  /* Set the requested operating mode (see section 3.3) */
+  setMode(mode);
+  delay(20);
+
+  return true;
+}
+
+/*!
+ *  @brief  Puts the chip in the specified operating mode
+ *  @param  mode
+ *          mode values
+ *           [OPERATION_MODE_CONFIG,
+ *            OPERATION_MODE_ACCONLY,
+ *            OPERATION_MODE_MAGONLY,
+ *            OPERATION_MODE_GYRONLY,
+ *            OPERATION_MODE_ACCMAG,
+ *            OPERATION_MODE_ACCGYRO,
+ *            OPERATION_MODE_MAGGYRO,
+ *            OPERATION_MODE_AMG,
+ *            OPERATION_MODE_IMUPLUS,
+ *            OPERATION_MODE_COMPASS,
+ *            OPERATION_MODE_M4G,
+ *            OPERATION_MODE_NDOF_FMC_OFF,
+ *            OPERATION_MODE_NDOF]
+ */
+void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode) {
+  _mode = mode;
+  write8(BNO055_OPR_MODE_ADDR, _mode);
+  delay(30);
+}
+
+/*!
+ *  @brief  Changes the chip's axis remap
+ *  @param  remapcode
+ *          remap code possible values
+ *          [REMAP_CONFIG_P0
+ *           REMAP_CONFIG_P1 (default)
+ *           REMAP_CONFIG_P2
+ *           REMAP_CONFIG_P3
+ *           REMAP_CONFIG_P4
+ *           REMAP_CONFIG_P5
+ *           REMAP_CONFIG_P6
+ *           REMAP_CONFIG_P7]
+ */
+void Adafruit_BNO055::setAxisRemap(
+    adafruit_bno055_axis_remap_config_t remapcode) {
+  adafruit_bno055_opmode_t modeback = _mode;
+
+  setMode(OPERATION_MODE_CONFIG);
+  delay(25);
+  write8(BNO055_AXIS_MAP_CONFIG_ADDR, remapcode);
+  delay(10);
+  /* Set the requested operating mode (see section 3.3) */
+  setMode(modeback);
+  delay(20);
+}
+
+/*!
+ *  @brief  Changes the chip's axis signs
+ *  @param  remapsign
+ *          remap sign possible values
+ *          [REMAP_SIGN_P0
+ *           REMAP_SIGN_P1 (default)
+ *           REMAP_SIGN_P2
+ *           REMAP_SIGN_P3
+ *           REMAP_SIGN_P4
+ *           REMAP_SIGN_P5
+ *           REMAP_SIGN_P6
+ *           REMAP_SIGN_P7]
+ */
+void Adafruit_BNO055::setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign) {
+  adafruit_bno055_opmode_t modeback = _mode;
+
+  setMode(OPERATION_MODE_CONFIG);
+  delay(25);
+  write8(BNO055_AXIS_MAP_SIGN_ADDR, remapsign);
+  delay(10);
+  /* Set the requested operating mode (see section 3.3) */
+  setMode(modeback);
+  delay(20);
+}
+
+/*!
+ *  @brief  Use the external 32.768KHz crystal
+ *  @param  usextal
+ *          use external crystal boolean
+ */
+void Adafruit_BNO055::setExtCrystalUse(boolean usextal) {
+  adafruit_bno055_opmode_t modeback = _mode;
+
+  /* Switch to config mode (just in case since this is the default) */
+  setMode(OPERATION_MODE_CONFIG);
+  delay(25);
+  write8(BNO055_PAGE_ID_ADDR, 0);
+  if (usextal) {
+    write8(BNO055_SYS_TRIGGER_ADDR, 0x80);
+  } else {
+    write8(BNO055_SYS_TRIGGER_ADDR, 0x00);
+  }
+  delay(10);
+  /* Set the requested operating mode (see section 3.3) */
+  setMode(modeback);
+  delay(20);
+}
+
+/*!
+ *   @brief  Gets the latest system status info
+ *   @param  system_status
+ *           system status info
+ *   @param  self_test_result
+ *           self test result
+ *   @param  system_error
+ *           system error info
+ */
+void Adafruit_BNO055::getSystemStatus(uint8_t *system_status,
+                                      uint8_t *self_test_result,
+                                      uint8_t *system_error) {
+  write8(BNO055_PAGE_ID_ADDR, 0);
+
+  /* System Status (see section 4.3.58)
+     0 = Idle
+     1 = System Error
+     2 = Initializing Peripherals
+     3 = System Iniitalization
+     4 = Executing Self-Test
+     5 = Sensor fusio algorithm running
+     6 = System running without fusion algorithms
+   */
+
+  if (system_status != 0)
+    *system_status = read8(BNO055_SYS_STAT_ADDR);
+
+  /* Self Test Results
+     1 = test passed, 0 = test failed
+
+     Bit 0 = Accelerometer self test
+     Bit 1 = Magnetometer self test
+     Bit 2 = Gyroscope self test
+     Bit 3 = MCU self test
+
+     0x0F = all good!
+   */
+
+  if (self_test_result != 0)
+    *self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR);
+
+  /* System Error (see section 4.3.59)
+     0 = No error
+     1 = Peripheral initialization error
+     2 = System initialization error
+     3 = Self test result failed
+     4 = Register map value out of range
+     5 = Register map address out of range
+     6 = Register map write error
+     7 = BNO low power mode not available for selected operat ion mode
+     8 = Accelerometer power mode not available
+     9 = Fusion algorithm configuration error
+     A = Sensor configuration error
+   */
+
+  if (system_error != 0)
+    *system_error = read8(BNO055_SYS_ERR_ADDR);
+
+  delay(200);
+}
+
+/*!
+ *  @brief  Gets the chip revision numbers
+ *  @param  info
+ *          revision info
+ */
+void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t *info) {
+  uint8_t a, b;
+
+  memset(info, 0, sizeof(adafruit_bno055_rev_info_t));
+
+  /* Check the accelerometer revision */
+  info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR);
+
+  /* Check the magnetometer revision */
+  info->mag_rev = read8(BNO055_MAG_REV_ID_ADDR);
+
+  /* Check the gyroscope revision */
+  info->gyro_rev = read8(BNO055_GYRO_REV_ID_ADDR);
+
+  /* Check the SW revision */
+  info->bl_rev = read8(BNO055_BL_REV_ID_ADDR);
+
+  a = read8(BNO055_SW_REV_ID_LSB_ADDR);
+  b = read8(BNO055_SW_REV_ID_MSB_ADDR);
+  info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a);
+}
+
+/*!
+ *  @brief  Gets current calibration state.  Each value should be a uint8_t
+ *          pointer and it will be set to 0 if not calibrated and 3 if
+ *          fully calibrated.
+ *          See section 34.3.54
+ *  @param  sys
+ *          Current system calibration status, depends on status of all sensors,
+ * read-only
+ *  @param  gyro
+ *          Current calibration status of Gyroscope, read-only
+ *  @param  accel
+ *          Current calibration status of Accelerometer, read-only
+ *  @param  mag
+ *          Current calibration status of Magnetometer, read-only
+ */
+void Adafruit_BNO055::getCalibration(uint8_t *sys, uint8_t *gyro,
+                                     uint8_t *accel, uint8_t *mag) {
+  uint8_t calData = read8(BNO055_CALIB_STAT_ADDR);
+  if (sys != NULL) {
+    *sys = (calData >> 6) & 0x03;
+  }
+  if (gyro != NULL) {
+    *gyro = (calData >> 4) & 0x03;
+  }
+  if (accel != NULL) {
+    *accel = (calData >> 2) & 0x03;
+  }
+  if (mag != NULL) {
+    *mag = calData & 0x03;
+  }
+}
+
+/*!
+ *  @brief  Gets the temperature in degrees celsius
+ *  @return temperature in degrees celsius
+ */
+int8_t Adafruit_BNO055::getTemp() {
+  int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR));
+  return temp;
+}
+
+/*!
+ *  @brief   Gets a vector reading from the specified source
+ *  @param   vector_type
+ *           possible vector type values
+ *           [VECTOR_ACCELEROMETER
+ *            VECTOR_MAGNETOMETER
+ *            VECTOR_GYROSCOPE
+ *            VECTOR_EULER
+ *            VECTOR_LINEARACCEL
+ *            VECTOR_GRAVITY]
+ *  @return  vector from specified source
+ */
+imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type) {
+  imu::Vector<3> xyz;
+  uint8_t buffer[6];
+  memset(buffer, 0, 6);
+
+  int16_t x, y, z;
+  x = y = z = 0;
+
+  /* Read vector data (6 bytes) */
+  readLen((adafruit_bno055_reg_t)vector_type, buffer, 6);
+
+  x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8);
+  y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
+  z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);
+
+  /*!
+   * Convert the value to an appropriate range (section 3.6.4)
+   * and assign the value to the Vector type
+   */
+  switch (vector_type) {
+  case VECTOR_MAGNETOMETER:
+    /* 1uT = 16 LSB */
+    xyz[0] = ((double)x) / 16.0;
+    xyz[1] = ((double)y) / 16.0;
+    xyz[2] = ((double)z) / 16.0;
+    break;
+  case VECTOR_GYROSCOPE:
+    /* 1dps = 16 LSB */
+    xyz[0] = ((double)x) / 16.0;
+    xyz[1] = ((double)y) / 16.0;
+    xyz[2] = ((double)z) / 16.0;
+    break;
+  case VECTOR_EULER:
+    /* 1 degree = 16 LSB */
+    xyz[0] = ((double)x) / 16.0;
+    xyz[1] = ((double)y) / 16.0;
+    xyz[2] = ((double)z) / 16.0;
+    break;
+  case VECTOR_ACCELEROMETER:
+    /* 1m/s^2 = 100 LSB */
+    xyz[0] = ((double)x) / 100.0;
+    xyz[1] = ((double)y) / 100.0;
+    xyz[2] = ((double)z) / 100.0;
+    break;
+  case VECTOR_LINEARACCEL:
+    /* 1m/s^2 = 100 LSB */
+    xyz[0] = ((double)x) / 100.0;
+    xyz[1] = ((double)y) / 100.0;
+    xyz[2] = ((double)z) / 100.0;
+    break;
+  case VECTOR_GRAVITY:
+    /* 1m/s^2 = 100 LSB */
+    xyz[0] = ((double)x) / 100.0;
+    xyz[1] = ((double)y) / 100.0;
+    xyz[2] = ((double)z) / 100.0;
+    break;
+  }
+
+  return xyz;
+}
+
+/*!
+ *  @brief  Gets a quaternion reading from the specified source
+ *  @return quaternion reading
+ */
+imu::Quaternion Adafruit_BNO055::getQuat() {
+  uint8_t buffer[8];
+  memset(buffer, 0, 8);
+
+  int16_t x, y, z, w;
+  x = y = z = w = 0;
+
+  /* Read quat data (8 bytes) */
+  readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, buffer, 8);
+  w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
+  x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
+  y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
+  z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
+
+  /*!
+   * Assign to Quaternion
+   * See
+   * https://cdn-shop.adafruit.com/datasheets/BST_BNO055_DS000_12.pdf
+   * 3.6.5.5 Orientation (Quaternion)
+   */
+  const double scale = (1.0 / (1 << 14));
+  imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
+  return quat;
+}
+
+/*!
+ *  @brief  Provides the sensor_t data for this sensor
+ *  @param  sensor
+ *          Sensor description
+ */
+void Adafruit_BNO055::getSensor(sensor_t *sensor) {
+  /* Clear the sensor_t object */
+  memset(sensor, 0, sizeof(sensor_t));
+
+  /* Insert the sensor name in the fixed length char array */
+  strncpy(sensor->name, "BNO055", sizeof(sensor->name) - 1);
+  sensor->name[sizeof(sensor->name) - 1] = 0;
+  sensor->version = 1;
+  sensor->sensor_id = _sensorID;
+  sensor->type = SENSOR_TYPE_ORIENTATION;
+  sensor->min_delay = 0;
+  sensor->max_value = 0.0F;
+  sensor->min_value = 0.0F;
+  sensor->resolution = 0.01F;
+}
+
+/*!
+ *  @brief  Reads the sensor and returns the data as a sensors_event_t
+ *  @param  event
+ *          Event description
+ *  @return always returns true
+ */
+bool Adafruit_BNO055::getEvent(sensors_event_t *event) {
+  /* Clear the event */
+  memset(event, 0, sizeof(sensors_event_t));
+
+  event->version = sizeof(sensors_event_t);
+  event->sensor_id = _sensorID;
+  event->type = SENSOR_TYPE_ORIENTATION;
+  event->timestamp = millis();
+
+  /* Get a Euler angle sample for orientation */
+  imu::Vector<3> euler = getVector(Adafruit_BNO055::VECTOR_EULER);
+  event->orientation.x = euler.x();
+  event->orientation.y = euler.y();
+  event->orientation.z = euler.z();
+
+  return true;
+}
+
+/*!
+ *  @brief  Reads the sensor and returns the data as a sensors_event_t
+ *  @param  event
+ *          Event description
+ *  @param  vec_type
+ *          specify the type of reading
+ *  @return always returns true
+ */
+bool Adafruit_BNO055::getEvent(sensors_event_t *event,
+                               adafruit_vector_type_t vec_type) {
+  /* Clear the event */
+  memset(event, 0, sizeof(sensors_event_t));
+
+  event->version = sizeof(sensors_event_t);
+  event->sensor_id = _sensorID;
+  event->timestamp = millis();
+
+  // read the data according to vec_type
+  imu::Vector<3> vec;
+  if (vec_type == Adafruit_BNO055::VECTOR_LINEARACCEL) {
+    event->type = SENSOR_TYPE_LINEAR_ACCELERATION;
+    vec = getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
+
+    event->acceleration.x = vec.x();
+    event->acceleration.y = vec.y();
+    event->acceleration.z = vec.z();
+  } else if (vec_type == Adafruit_BNO055::VECTOR_ACCELEROMETER) {
+    event->type = SENSOR_TYPE_ACCELEROMETER;
+    vec = getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);
+
+    event->acceleration.x = vec.x();
+    event->acceleration.y = vec.y();
+    event->acceleration.z = vec.z();
+  } else if (vec_type == Adafruit_BNO055::VECTOR_GRAVITY) {
+    event->type = SENSOR_TYPE_GRAVITY;
+    vec = getVector(Adafruit_BNO055::VECTOR_GRAVITY);
+
+    event->acceleration.x = vec.x();
+    event->acceleration.y = vec.y();
+    event->acceleration.z = vec.z();
+  } else if (vec_type == Adafruit_BNO055::VECTOR_EULER) {
+    event->type = SENSOR_TYPE_ORIENTATION;
+    vec = getVector(Adafruit_BNO055::VECTOR_EULER);
+
+    event->orientation.x = vec.x();
+    event->orientation.y = vec.y();
+    event->orientation.z = vec.z();
+  } else if (vec_type == Adafruit_BNO055::VECTOR_GYROSCOPE) {
+    event->type = SENSOR_TYPE_GYROSCOPE;
+    vec = getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
+
+    event->gyro.x = vec.x() * SENSORS_DPS_TO_RADS;
+    event->gyro.y = vec.y() * SENSORS_DPS_TO_RADS;
+    event->gyro.z = vec.z() * SENSORS_DPS_TO_RADS;
+  } else if (vec_type == Adafruit_BNO055::VECTOR_MAGNETOMETER) {
+    event->type = SENSOR_TYPE_MAGNETIC_FIELD;
+    vec = getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
+
+    event->magnetic.x = vec.x();
+    event->magnetic.y = vec.y();
+    event->magnetic.z = vec.z();
+  }
+
+  return true;
+}
+
+/*!
+ *  @brief  Reads the sensor's offset registers into a byte array
+ *  @param  calibData
+ *          Calibration offset (buffer size should be 22)
+ *  @return true if read is successful
+ */
+bool Adafruit_BNO055::getSensorOffsets(uint8_t *calibData) {
+  if (isFullyCalibrated()) {
+    adafruit_bno055_opmode_t lastMode = _mode;
+    setMode(OPERATION_MODE_CONFIG);
+
+    readLen(ACCEL_OFFSET_X_LSB_ADDR, calibData, NUM_BNO055_OFFSET_REGISTERS);
+
+    setMode(lastMode);
+    return true;
+  }
+  return false;
+}
+
+/*!
+ *  @brief  Reads the sensor's offset registers into an offset struct
+ *  @param  offsets_type
+ *          type of offsets
+ *  @return true if read is successful
+ */
+bool Adafruit_BNO055::getSensorOffsets(
+    adafruit_bno055_offsets_t &offsets_type) {
+  if (isFullyCalibrated()) {
+    adafruit_bno055_opmode_t lastMode = _mode;
+    setMode(OPERATION_MODE_CONFIG);
+    delay(25);
+
+    /* Accel offset range depends on the G-range:
+       +/-2g  = +/- 2000 mg
+       +/-4g  = +/- 4000 mg
+       +/-8g  = +/- 8000 mg
+       +/-1§g = +/- 16000 mg */
+    offsets_type.accel_offset_x = (read8(ACCEL_OFFSET_X_MSB_ADDR) << 8) |
+                                  (read8(ACCEL_OFFSET_X_LSB_ADDR));
+    offsets_type.accel_offset_y = (read8(ACCEL_OFFSET_Y_MSB_ADDR) << 8) |
+                                  (read8(ACCEL_OFFSET_Y_LSB_ADDR));
+    offsets_type.accel_offset_z = (read8(ACCEL_OFFSET_Z_MSB_ADDR) << 8) |
+                                  (read8(ACCEL_OFFSET_Z_LSB_ADDR));
+
+    /* Magnetometer offset range = +/- 6400 LSB where 1uT = 16 LSB */
+    offsets_type.mag_offset_x =
+        (read8(MAG_OFFSET_X_MSB_ADDR) << 8) | (read8(MAG_OFFSET_X_LSB_ADDR));
+    offsets_type.mag_offset_y =
+        (read8(MAG_OFFSET_Y_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Y_LSB_ADDR));
+    offsets_type.mag_offset_z =
+        (read8(MAG_OFFSET_Z_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Z_LSB_ADDR));
+
+    /* Gyro offset range depends on the DPS range:
+      2000 dps = +/- 32000 LSB
+      1000 dps = +/- 16000 LSB
+       500 dps = +/- 8000 LSB
+       250 dps = +/- 4000 LSB
+       125 dps = +/- 2000 LSB
+       ... where 1 DPS = 16 LSB */
+    offsets_type.gyro_offset_x =
+        (read8(GYRO_OFFSET_X_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_X_LSB_ADDR));
+    offsets_type.gyro_offset_y =
+        (read8(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Y_LSB_ADDR));
+    offsets_type.gyro_offset_z =
+        (read8(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Z_LSB_ADDR));
+
+    /* Accelerometer radius = +/- 1000 LSB */
+    offsets_type.accel_radius =
+        (read8(ACCEL_RADIUS_MSB_ADDR) << 8) | (read8(ACCEL_RADIUS_LSB_ADDR));
+
+    /* Magnetometer radius = +/- 960 LSB */
+    offsets_type.mag_radius =
+        (read8(MAG_RADIUS_MSB_ADDR) << 8) | (read8(MAG_RADIUS_LSB_ADDR));
+
+    setMode(lastMode);
+    return true;
+  }
+  return false;
+}
+
+/*!
+ *  @brief  Writes an array of calibration values to the sensor's offset
+ *  @param  calibData
+ *          calibration data
+ */
+void Adafruit_BNO055::setSensorOffsets(const uint8_t *calibData) {
+  adafruit_bno055_opmode_t lastMode = _mode;
+  setMode(OPERATION_MODE_CONFIG);
+  delay(25);
+
+  /* Note: Configuration will take place only when user writes to the last
+     byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.).
+     Therefore the last byte must be written whenever the user wants to
+     changes the configuration. */
+
+  /* A writeLen() would make this much cleaner */
+  write8(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]);
+  write8(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]);
+  write8(ACCEL_OFFSET_Y_LSB_ADDR, calibData[2]);
+  write8(ACCEL_OFFSET_Y_MSB_ADDR, calibData[3]);
+  write8(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]);
+  write8(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]);
+
+  write8(MAG_OFFSET_X_LSB_ADDR, calibData[6]);
+  write8(MAG_OFFSET_X_MSB_ADDR, calibData[7]);
+  write8(MAG_OFFSET_Y_LSB_ADDR, calibData[8]);
+  write8(MAG_OFFSET_Y_MSB_ADDR, calibData[9]);
+  write8(MAG_OFFSET_Z_LSB_ADDR, calibData[10]);
+  write8(MAG_OFFSET_Z_MSB_ADDR, calibData[11]);
+
+  write8(GYRO_OFFSET_X_LSB_ADDR, calibData[12]);
+  write8(GYRO_OFFSET_X_MSB_ADDR, calibData[13]);
+  write8(GYRO_OFFSET_Y_LSB_ADDR, calibData[14]);
+  write8(GYRO_OFFSET_Y_MSB_ADDR, calibData[15]);
+  write8(GYRO_OFFSET_Z_LSB_ADDR, calibData[16]);
+  write8(GYRO_OFFSET_Z_MSB_ADDR, calibData[17]);
+
+  write8(ACCEL_RADIUS_LSB_ADDR, calibData[18]);
+  write8(ACCEL_RADIUS_MSB_ADDR, calibData[19]);
+
+  write8(MAG_RADIUS_LSB_ADDR, calibData[20]);
+  write8(MAG_RADIUS_MSB_ADDR, calibData[21]);
+
+  setMode(lastMode);
+}
+
+/*!
+ *  @brief  Writes to the sensor's offset registers from an offset struct
+ *  @param  offsets_type
+ *          accel_offset_x = acceleration offset x
+ *          accel_offset_y = acceleration offset y
+ *          accel_offset_z = acceleration offset z
+ *
+ *          mag_offset_x   = magnetometer offset x
+ *          mag_offset_y   = magnetometer offset y
+ *          mag_offset_z   = magnetometer offset z
+ *
+ *          gyro_offset_x  = gyroscrope offset x
+ *          gyro_offset_y  = gyroscrope offset y
+ *          gyro_offset_z  = gyroscrope offset z
+ */
+void Adafruit_BNO055::setSensorOffsets(
+    const adafruit_bno055_offsets_t &offsets_type) {
+  adafruit_bno055_opmode_t lastMode = _mode;
+  setMode(OPERATION_MODE_CONFIG);
+  delay(25);
+
+  /* Note: Configuration will take place only when user writes to the last
+     byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.).
+     Therefore the last byte must be written whenever the user wants to
+     changes the configuration. */
+
+  write8(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF);
+  write8(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF);
+  write8(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF);
+  write8(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF);
+  write8(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF);
+  write8(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF);
+
+  write8(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF);
+  write8(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF);
+  write8(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF);
+  write8(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF);
+  write8(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF);
+  write8(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF);
+
+  write8(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF);
+  write8(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF);
+  write8(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF);
+  write8(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF);
+  write8(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF);
+  write8(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF);
+
+  write8(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF);
+  write8(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF);
+
+  write8(MAG_RADIUS_LSB_ADDR, (offsets_type.mag_radius) & 0x0FF);
+  write8(MAG_RADIUS_MSB_ADDR, (offsets_type.mag_radius >> 8) & 0x0FF);
+
+  setMode(lastMode);
+}
+
+/*!
+ *  @brief  Checks of all cal status values are set to 3 (fully calibrated)
+ *  @return status of calibration
+ */
+bool Adafruit_BNO055::isFullyCalibrated() {
+  uint8_t system, gyro, accel, mag;
+  getCalibration(&system, &gyro, &accel, &mag);
+
+  switch (_mode) {
+  case OPERATION_MODE_ACCONLY:
+    return (accel == 3);
+  case OPERATION_MODE_MAGONLY:
+    return (mag == 3);
+  case OPERATION_MODE_GYRONLY:
+  case OPERATION_MODE_M4G: /* No magnetometer calibration required. */
+    return (gyro == 3);
+  case OPERATION_MODE_ACCMAG:
+  case OPERATION_MODE_COMPASS:
+    return (accel == 3 && mag == 3);
+  case OPERATION_MODE_ACCGYRO:
+  case OPERATION_MODE_IMUPLUS:
+    return (accel == 3 && gyro == 3);
+  case OPERATION_MODE_MAGGYRO:
+    return (mag == 3 && gyro == 3);
+  default:
+    return (system == 3 && gyro == 3 && accel == 3 && mag == 3);
+  }
+}
+
+/*!
+ *  @brief  Enter Suspend mode (i.e., sleep)
+ */
+void Adafruit_BNO055::enterSuspendMode() {
+  adafruit_bno055_opmode_t modeback = _mode;
+
+  /* Switch to config mode (just in case since this is the default) */
+  setMode(OPERATION_MODE_CONFIG);
+  delay(25);
+  write8(BNO055_PWR_MODE_ADDR, 0x02);
+  /* Set the requested operating mode (see section 3.3) */
+  setMode(modeback);
+  delay(20);
+}
+
+/*!
+ *  @brief  Enter Normal mode (i.e., wake)
+ */
+void Adafruit_BNO055::enterNormalMode() {
+  adafruit_bno055_opmode_t modeback = _mode;
+
+  /* Switch to config mode (just in case since this is the default) */
+  setMode(OPERATION_MODE_CONFIG);
+  delay(25);
+  write8(BNO055_PWR_MODE_ADDR, 0x00);
+  /* Set the requested operating mode (see section 3.3) */
+  setMode(modeback);
+  delay(20);
+}
+
+/*!
+ *  @brief  Writes an 8 bit value over I2C
+ */
+bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value) {
+  uint8_t buffer[2] = {(uint8_t)reg, (uint8_t)value};
+  return i2c_dev->write(buffer, 2);
+}
+
+/*!
+ *  @brief  Reads an 8 bit value over I2C
+ */
+byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg) {
+  uint8_t buffer[1] = {reg};
+  i2c_dev->write_then_read(buffer, 1, buffer, 1);
+  return (byte)buffer[0];
+}
+
+/*!
+ *  @brief  Reads the specified number of bytes over I2C
+ */
+bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, byte *buffer,
+                              uint8_t len) {
+  uint8_t reg_buf[1] = {(uint8_t)reg};
+  return i2c_dev->write_then_read(reg_buf, 1, buffer, len);
+}

Embedded/libraries/Adafruit_BNO055/Adafruit_BNO055.h

@@ -0,0 +1,327 @@
+/*!
+ *  @file Adafruit_BNO055.h
+ *
+ *  This is a library for the BNO055 orientation sensor
+ *
+ *  Designed specifically to work with the Adafruit BNO055 Breakout.
+ *
+ *  Pick one up today in the adafruit shop!
+ *  ------> https://www.adafruit.com/product/2472
+ *
+ *  These sensors use I2C to communicate, 2 pins are required to interface.
+ *
+ *  Adafruit invests time and resources providing this open source code,
+ *  please support Adafruit andopen-source hardware by purchasing products
+ *  from Adafruit!
+ *
+ *  K.Townsend (Adafruit Industries)
+ *
+ *  MIT license, all text above must be included in any redistribution
+ */
+
+#ifndef __ADAFRUIT_BNO055_H__
+#define __ADAFRUIT_BNO055_H__
+
+#include "Arduino.h"
+
+#include "utility/imumaths.h"
+#include <Adafruit_I2CDevice.h>
+#include <Adafruit_Sensor.h>
+
+/** BNO055 Address A **/
+#define BNO055_ADDRESS_A (0x28)
+/** BNO055 Address B **/
+#define BNO055_ADDRESS_B (0x29)
+/** BNO055 ID **/
+#define BNO055_ID (0xA0)
+
+/** Offsets registers **/
+#define NUM_BNO055_OFFSET_REGISTERS (22)
+
+/** A structure to represent offsets **/
+typedef struct {
+  int16_t accel_offset_x; /**< x acceleration offset */
+  int16_t accel_offset_y; /**< y acceleration offset */
+  int16_t accel_offset_z; /**< z acceleration offset */
+
+  int16_t mag_offset_x; /**< x magnetometer offset */
+  int16_t mag_offset_y; /**< y magnetometer offset */
+  int16_t mag_offset_z; /**< z magnetometer offset */
+
+  int16_t gyro_offset_x; /**< x gyroscrope offset */
+  int16_t gyro_offset_y; /**< y gyroscrope offset */
+  int16_t gyro_offset_z; /**< z gyroscrope offset */
+
+  int16_t accel_radius; /**< acceleration radius */
+
+  int16_t mag_radius; /**< magnetometer radius */
+} adafruit_bno055_offsets_t;
+
+/*!
+ *  @brief  Class that stores state and functions for interacting with
+ *          BNO055 Sensor
+ */
+class Adafruit_BNO055 : public Adafruit_Sensor {
+public:
+  /** BNO055 Registers **/
+  typedef enum {
+    /* Page id register definition */
+    BNO055_PAGE_ID_ADDR = 0X07,
+
+    /* PAGE0 REGISTER DEFINITION START*/
+    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,
+
+    /* 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 register s*/
+    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
+  } adafruit_bno055_reg_t;
+
+  /** BNO055 power settings */
+  typedef enum {
+    POWER_MODE_NORMAL = 0X00,
+    POWER_MODE_LOWPOWER = 0X01,
+    POWER_MODE_SUSPEND = 0X02
+  } adafruit_bno055_powermode_t;
+
+  /** Operation mode settings **/
+  typedef enum {
+    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
+  } adafruit_bno055_opmode_t;
+
+  /** Remap settings **/
+  typedef enum {
+    REMAP_CONFIG_P0 = 0x21,
+    REMAP_CONFIG_P1 = 0x24, // default
+    REMAP_CONFIG_P2 = 0x24,
+    REMAP_CONFIG_P3 = 0x21,
+    REMAP_CONFIG_P4 = 0x24,
+    REMAP_CONFIG_P5 = 0x21,
+    REMAP_CONFIG_P6 = 0x21,
+    REMAP_CONFIG_P7 = 0x24
+  } adafruit_bno055_axis_remap_config_t;
+
+  /** Remap Signs **/
+  typedef enum {
+    REMAP_SIGN_P0 = 0x04,
+    REMAP_SIGN_P1 = 0x00, // default
+    REMAP_SIGN_P2 = 0x06,
+    REMAP_SIGN_P3 = 0x02,
+    REMAP_SIGN_P4 = 0x03,
+    REMAP_SIGN_P5 = 0x01,
+    REMAP_SIGN_P6 = 0x07,
+    REMAP_SIGN_P7 = 0x05
+  } adafruit_bno055_axis_remap_sign_t;
+
+  /** A structure to represent revisions **/
+  typedef struct {
+    uint8_t accel_rev; /**< acceleration rev */
+    uint8_t mag_rev;   /**< magnetometer rev */
+    uint8_t gyro_rev;  /**< gyroscrope rev */
+    uint16_t sw_rev;   /**< SW rev */
+    uint8_t bl_rev;    /**< bootloader rev */
+  } adafruit_bno055_rev_info_t;
+
+  /** Vector Mappings **/
+  typedef enum {
+    VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR,
+    VECTOR_MAGNETOMETER = BNO055_MAG_DATA_X_LSB_ADDR,
+    VECTOR_GYROSCOPE = BNO055_GYRO_DATA_X_LSB_ADDR,
+    VECTOR_EULER = BNO055_EULER_H_LSB_ADDR,
+    VECTOR_LINEARACCEL = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR,
+    VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR
+  } adafruit_vector_type_t;
+
+  Adafruit_BNO055(int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_A,
+                  TwoWire *theWire = &Wire);
+
+  bool begin(adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF);
+  void setMode(adafruit_bno055_opmode_t mode);
+  void setAxisRemap(adafruit_bno055_axis_remap_config_t remapcode);
+  void setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign);
+  void getRevInfo(adafruit_bno055_rev_info_t *);
+  void setExtCrystalUse(boolean usextal);
+  void getSystemStatus(uint8_t *system_status, uint8_t *self_test_result,
+                       uint8_t *system_error);
+  void getCalibration(uint8_t *system, uint8_t *gyro, uint8_t *accel,
+                      uint8_t *mag);
+
+  imu::Vector<3> getVector(adafruit_vector_type_t vector_type);
+  imu::Quaternion getQuat();
+  int8_t getTemp();
+
+  /* Adafruit_Sensor implementation */
+  bool getEvent(sensors_event_t *);
+  bool getEvent(sensors_event_t *, adafruit_vector_type_t);
+  void getSensor(sensor_t *);
+
+  /* Functions to deal with raw calibration data */
+  bool getSensorOffsets(uint8_t *calibData);
+  bool getSensorOffsets(adafruit_bno055_offsets_t &offsets_type);
+  void setSensorOffsets(const uint8_t *calibData);
+  void setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type);
+  bool isFullyCalibrated();
+
+  /* Power managments functions */
+  void enterSuspendMode();
+  void enterNormalMode();
+
+private:
+  byte read8(adafruit_bno055_reg_t);
+  bool readLen(adafruit_bno055_reg_t, byte *buffer, uint8_t len);
+  bool write8(adafruit_bno055_reg_t, byte value);
+
+  Adafruit_I2CDevice *i2c_dev = NULL; ///< Pointer to I2C bus interface
+
+  int32_t _sensorID;
+  adafruit_bno055_opmode_t _mode;
+};
+
+#endif

Embedded/libraries/Adafruit_BNO055/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2018 Adafruit Industries
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

Embedded/libraries/Adafruit_BNO055/OBJLoader/OBJLoader.txt

@@ -0,0 +1,47 @@
+# UTF-8 supported.
+
+# The name of your library as you want it formatted
+name = OBJLoader
+
+# List of authors. Links can be provided using the syntax [author name](url)
+authorList = [Tatsuya Saito](http://saito-tatsuya.net/) and [Matt Ditton](http://thequietvoid.com/)
+
+# A web page for your library, NOT a direct link to where to download it
+url = http://code.google.com/p/saitoobjloader/
+
+# The category of your library, must be one (or many) of the following:
+#   "3D"            "Animation"     "Compilations"      "Data"          
+#   "Fabrication"   "Geometry"      "GUI"               "Hardware"      
+#   "I/O"           "Language"      "Math"              "Simulation"    
+#   "Sound"         "Utilities"     "Typography"        "Video & Vision"  
+# 
+# If a value other than those listed is used, your library will listed as "Other."
+category = 3D
+
+# A short sentence (or fragment) to summarize the library's function. This will be
+# shown from inside the PDE when the library is being installed. Avoid repeating
+# the name of your library here. Also, avoid saying anything redundant like
+# mentioning that its a library. This should start with a capitalized letter, and
+# end with a period.
+sentence = .OBJ 3D model file loader 
+
+# Additional information suitable for the Processing website. The value of
+# 'sentence' always will be prepended, so you should start by writing the
+# second sentence here. If your library only works on certain operating systems,
+# mention it here.
+paragraph =  
+
+# Links in the 'sentence' and 'paragraph' attributes can be inserted using the
+# same syntax as for authors. That is, [here is a link to Processing](http://processing.org/)
+
+
+# A version number that increments once with each release. This
+# is used to compare different versions of the same library, and
+# check if an update is available. You should think of it as a
+# counter, counting the total number of releases you've had.
+version = 23  # This must be parsable as an int
+
+# The version as the user will see it. If blank, the version attribute will be used here
+prettyVersion = 0.99  # This is treated as a String
+
+

Embedded/libraries/Adafruit_BNO055/README.md

@@ -0,0 +1,32 @@
+Adafruit Unified BNO055 Driver (AHRS/Orientation)  [![Build Status](https://github.com/adafruit/Adafruit_BNO055/workflows/Arduino%20Library%20CI/badge.svg)](https://github.com/adafruit/Adafruit_BNO055/actions)[![Documentation](https://github.com/adafruit/ci-arduino/blob/master/assets/doxygen_badge.svg)](http://adafruit.github.io/Adafruit_BNO055/html/index.html)
+================
+
+<a href="https://www.adafruit.com/product/2472"><img src="assets/board.jpg?raw=true" width="500px"></a>
+
+This driver is for the Adafruit BNO055 Breakout, and is based on Adafruit's Unified Sensor Library (Adafruit_Sensor).
+
+Tested and works great with the Adafruit Si4713 Breakout Board.
+To work with the Arduino Zero, the BNO055's ADR pin must be high.
+* http://www.adafruit.com/products/2472
+
+## What is the Adafruit Unified Sensor Library? ##
+
+The Adafruit Unified Sensor Library ([Adafruit_Sensor](https://github.com/adafruit/Adafruit_Sensor)) provides a common interface and data type for any supported sensor.  It defines some basic information about the sensor (sensor limits, etc.), and returns standard SI units of a specific type and scale for each supported sensor type.
+
+It provides a simple abstraction layer between your application and the actual sensor HW, allowing you to drop in any comparable sensor with only one or two lines of code to change in your project (essentially the constructor since the functions to read sensor data and get information about the sensor are defined in the base Adafruit_Sensor class).
+
+This is imporant useful for two reasons:
+
+1.) You can use the data right away because it's already converted to SI units that you understand and can compare, rather than meaningless values like 0..1023.
+
+2.) Because SI units are standardised in the sensor library, you can also do quick sanity checks when working with new sensors, or drop in any comparable sensor if you need better sensitivity or if a lower cost unit becomes available, etc. 
+
+Light sensors will always report units in lux, gyroscopes will always report units in rad/s, etc. ... freeing you up to focus on the data, rather than digging through the datasheet to understand what the sensor's raw numbers really mean.
+
+Adafruit invests time and resources providing this open source code.  Please support Adafruit and open-source hardware by purchasing products from Adafruit!
+
+Kevin (KTOWN) Townsend Adafruit Industries.
+MIT license, check license.txt for more information
+All text above must be included in any redistribution
+
+To install, use the Arduino Library Manager and search for "Adafruit BNO055" and install the library.

Embedded/libraries/Adafruit_BNO055/code-of-conduct.md

@@ -0,0 +1,127 @@
+# Adafruit Community Code of Conduct
+
+## Our Pledge
+
+In the interest of fostering an open and welcoming environment, we as
+contributors and leaders pledge to making participation in our project and
+our community a harassment-free experience for everyone, regardless of age, body
+size, disability, ethnicity, gender identity and expression, level or type of
+experience, education, socio-economic status, nationality, personal appearance,
+race, religion, or sexual identity and orientation.
+
+## Our Standards
+
+We are committed to providing a friendly, safe and welcoming environment for
+all.
+
+Examples of behavior that contributes to creating a positive environment
+include:
+
+* Be kind and courteous to others
+* Using welcoming and inclusive language
+* Being respectful of differing viewpoints and experiences
+* Collaborating with other community members
+* Gracefully accepting constructive criticism
+* Focusing on what is best for the community
+* Showing empathy towards other community members
+
+Examples of unacceptable behavior by participants include:
+
+* The use of sexualized language or imagery and sexual attention or advances
+* The use of inappropriate images, including in a community member's avatar
+* The use of inappropriate language, including in a community member's nickname
+* Any spamming, flaming, baiting or other attention-stealing behavior
+* Excessive or unwelcome helping; answering outside the scope of the question
+  asked
+* Trolling, insulting/derogatory comments, and personal or political attacks
+* Public or private harassment
+* Publishing others' private information, such as a physical or electronic
+  address, without explicit permission
+* Other conduct which could reasonably be considered inappropriate
+
+The goal of the standards and moderation guidelines outlined here is to build
+and maintain a respectful community. We ask that you don’t just aim to be
+"technically unimpeachable", but rather try to be your best self. 
+
+We value many things beyond technical expertise, including collaboration and
+supporting others within our community. Providing a positive experience for
+other community members can have a much more significant impact than simply
+providing the correct answer.
+
+## Our Responsibilities
+
+Project leaders are responsible for clarifying the standards of acceptable
+behavior and are expected to take appropriate and fair corrective action in
+response to any instances of unacceptable behavior.
+
+Project leaders have the right and responsibility to remove, edit, or
+reject messages, comments, commits, code, issues, and other contributions
+that are not aligned to this Code of Conduct, or to ban temporarily or
+permanently any community member for other behaviors that they deem
+inappropriate, threatening, offensive, or harmful.
+
+## Moderation
+
+Instances of behaviors that violate the Adafruit Community Code of Conduct
+may be reported by any member of the community. Community members are
+encouraged to report these situations, including situations they witness
+involving other community members.
+
+You may report in the following ways:
+
+In any situation, you may send an email to <support@adafruit.com>.
+
+On the Adafruit Discord, you may send an open message from any channel
+to all Community Helpers by tagging @community helpers. You may also send an
+open message from any channel, or a direct message to @kattni#1507,
+@tannewt#4653, @Dan Halbert#1614, @cater#2442, @sommersoft#0222, or
+@Andon#8175.
+
+Email and direct message reports will be kept confidential.
+
+In situations on Discord where the issue is particularly egregious, possibly
+illegal, requires immediate action, or violates the Discord terms of service,
+you should also report the message directly to Discord.
+
+These are the steps for upholding our community’s standards of conduct.
+
+1. Any member of the community may report any situation that violates the
+Adafruit Community Code of Conduct. All reports will be reviewed and
+investigated.
+2. If the behavior is an egregious violation, the community member who
+committed the violation may be banned immediately, without warning.
+3. Otherwise, moderators will first respond to such behavior with a warning.
+4. Moderators follow a soft "three strikes" policy - the community member may
+be given another chance, if they are receptive to the warning and change their
+behavior.
+5. If the community member is unreceptive or unreasonable when warned by a
+moderator, or the warning goes unheeded, they may be banned for a first or
+second offense. Repeated offenses will result in the community member being
+banned.
+
+## Scope
+
+This Code of Conduct and the enforcement policies listed above apply to all
+Adafruit Community venues. This includes but is not limited to any community
+spaces (both public and private), the entire Adafruit Discord server, and
+Adafruit GitHub repositories. Examples of Adafruit Community spaces include
+but are not limited to meet-ups, audio chats on the Adafruit Discord, or
+interaction at a conference.
+
+This Code of Conduct applies both within project spaces and in public spaces
+when an individual is representing the project or its community. As a community
+member, you are representing our community, and are expected to behave
+accordingly.
+
+## Attribution
+
+This Code of Conduct is adapted from the [Contributor Covenant][homepage],
+version 1.4, available at
+<https://www.contributor-covenant.org/version/1/4/code-of-conduct.html>,
+and the [Rust Code of Conduct](https://www.rust-lang.org/en-US/conduct.html).
+
+For other projects adopting the Adafruit Community Code of
+Conduct, please contact the maintainers of those projects for enforcement.
+If you wish to use this code of conduct for your own project, consider
+explicitly mentioning your moderation policy or making a copy with your
+own moderation policy so as to avoid confusion.

Embedded/libraries/Adafruit_BNO055/examples/bunny/bunny.ino

@@ -0,0 +1,131 @@
+#include <Wire.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_BNO055.h>
+#include <utility/imumaths.h>
+
+/* This driver uses the Adafruit unified sensor library (Adafruit_Sensor),
+   which provides a common 'type' for sensor data and some helper functions.
+
+   To use this driver you will also need to download the Adafruit_Sensor
+   library and include it in your libraries folder.
+
+   You should also assign a unique ID to this sensor for use with
+   the Adafruit Sensor API so that you can identify this particular
+   sensor in any data logs, etc.  To assign a unique ID, simply
+   provide an appropriate value in the constructor below (12345
+   is used by default in this example).
+
+   Connections
+   ===========
+   Connect SCL to analog 5
+   Connect SDA to analog 4
+   Connect VDD to 3.3-5V DC
+   Connect GROUND to common ground
+
+   History
+   =======
+   2015/MAR/03  - First release (KTOWN)
+*/
+
+/* Set the delay between fresh samples */
+#define BNO055_SAMPLERATE_DELAY_MS (100)
+
+// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
+//                                   id, address
+Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);
+
+/**************************************************************************/
+/*
+    Displays some basic information on this sensor from the unified
+    sensor API sensor_t type (see Adafruit_Sensor for more information)
+*/
+/**************************************************************************/
+void displaySensorDetails(void)
+{
+  sensor_t sensor;
+  bno.getSensor(&sensor);
+  Serial.println("------------------------------------");
+  Serial.print  ("Sensor:       "); Serial.println(sensor.name);
+  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
+  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
+  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" xxx");
+  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" xxx");
+  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" xxx");
+  Serial.println("------------------------------------");
+  Serial.println("");
+  delay(500);
+}
+
+/**************************************************************************/
+/*
+    Arduino setup function (automatically called at startup)
+*/
+/**************************************************************************/
+void setup(void)
+{
+  Serial.begin(115200);
+  Serial.println("Orientation Sensor Test"); Serial.println("");
+
+  /* Initialise the sensor */
+  if(!bno.begin())
+  {
+    /* There was a problem detecting the BNO055 ... check your connections */
+    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
+    while(1);
+  }
+   
+  delay(1000);
+
+  /* Use external crystal for better accuracy */
+  bno.setExtCrystalUse(true);
+   
+  /* Display some basic information on this sensor */
+  displaySensorDetails();
+}
+
+/**************************************************************************/
+/*
+    Arduino loop function, called once 'setup' is complete (your own code
+    should go here)
+*/
+/**************************************************************************/
+void loop(void)
+{
+  /* Get a new sensor event */
+  sensors_event_t event;
+  bno.getEvent(&event);
+
+  /* Board layout:
+         +----------+
+         |         *| RST   PITCH  ROLL  HEADING
+     ADR |*        *| SCL
+     INT |*        *| SDA     ^            /->
+     PS1 |*        *| GND     |            |
+     PS0 |*        *| 3VO     Y    Z-->    \-X
+         |         *| VIN
+         +----------+
+  */
+
+  /* The processing sketch expects data as roll, pitch, heading */
+  Serial.print(F("Orientation: "));
+  Serial.print((float)event.orientation.x);
+  Serial.print(F(" "));
+  Serial.print((float)event.orientation.y);
+  Serial.print(F(" "));
+  Serial.print((float)event.orientation.z);
+  Serial.println(F(""));
+
+  /* Also send calibration data for each sensor. */
+  uint8_t sys, gyro, accel, mag = 0;
+  bno.getCalibration(&sys, &gyro, &accel, &mag);
+  Serial.print(F("Calibration: "));
+  Serial.print(sys, DEC);
+  Serial.print(F(" "));
+  Serial.print(gyro, DEC);
+  Serial.print(F(" "));
+  Serial.print(accel, DEC);
+  Serial.print(F(" "));
+  Serial.println(mag, DEC);
+
+  delay(BNO055_SAMPLERATE_DELAY_MS);
+}

Embedded/libraries/Adafruit_BNO055/examples/position/position.ino

@@ -0,0 +1,106 @@
+#include <Wire.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_BNO055.h>
+
+double xPos = 0, yPos = 0, headingVel = 0;
+uint16_t BNO055_SAMPLERATE_DELAY_MS = 10; //how often to read data from the board
+uint16_t PRINT_DELAY_MS = 500; // how often to print the data
+uint16_t printCount = 0; //counter to avoid printing every 10MS sample
+
+//velocity = accel*dt (dt in seconds)
+//position = 0.5*accel*dt^2
+double ACCEL_VEL_TRANSITION =  (double)(BNO055_SAMPLERATE_DELAY_MS) / 1000.0;
+double ACCEL_POS_TRANSITION = 0.5 * ACCEL_VEL_TRANSITION * ACCEL_VEL_TRANSITION;
+double DEG_2_RAD = 0.01745329251; //trig functions require radians, BNO055 outputs degrees
+
+// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
+//                                   id, address
+Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);
+
+void setup(void)
+{
+  Serial.begin(115200);
+  if (!bno.begin())
+  {
+    Serial.print("No BNO055 detected");
+    while (1);
+  }
+
+
+  delay(1000);
+}
+
+void loop(void)
+{
+  //
+  unsigned long tStart = micros();
+  sensors_event_t orientationData , linearAccelData;
+  bno.getEvent(&orientationData, Adafruit_BNO055::VECTOR_EULER);
+  //  bno.getEvent(&angVelData, Adafruit_BNO055::VECTOR_GYROSCOPE);
+  bno.getEvent(&linearAccelData, Adafruit_BNO055::VECTOR_LINEARACCEL);
+
+  xPos = xPos + ACCEL_POS_TRANSITION * linearAccelData.acceleration.x;
+  yPos = yPos + ACCEL_POS_TRANSITION * linearAccelData.acceleration.y;
+
+  // velocity of sensor in the direction it's facing
+  headingVel = ACCEL_VEL_TRANSITION * linearAccelData.acceleration.x / cos(DEG_2_RAD * orientationData.orientation.x);
+
+  if (printCount * BNO055_SAMPLERATE_DELAY_MS >= PRINT_DELAY_MS) {
+    //enough iterations have passed that we can print the latest data
+    Serial.print("Heading: ");
+    Serial.println(orientationData.orientation.x);
+    Serial.print("Position: ");
+    Serial.print(xPos);
+    Serial.print(" , ");
+    Serial.println(yPos);
+    Serial.print("Speed: ");
+    Serial.println(headingVel);
+    Serial.println("-------");
+
+    printCount = 0;
+  }
+  else {
+    printCount = printCount + 1;
+  }
+
+
+
+  while ((micros() - tStart) < (BNO055_SAMPLERATE_DELAY_MS * 1000))
+  {
+    //poll until the next sample is ready
+  }
+}
+
+void printEvent(sensors_event_t* event) {
+  Serial.println();
+  Serial.print(event->type);
+  double x = -1000000, y = -1000000 , z = -1000000; //dumb values, easy to spot problem
+  if (event->type == SENSOR_TYPE_ACCELEROMETER) {
+    x = event->acceleration.x;
+    y = event->acceleration.y;
+    z = event->acceleration.z;
+  }
+  else if (event->type == SENSOR_TYPE_ORIENTATION) {
+    x = event->orientation.x;
+    y = event->orientation.y;
+    z = event->orientation.z;
+  }
+  else if (event->type == SENSOR_TYPE_MAGNETIC_FIELD) {
+    x = event->magnetic.x;
+    y = event->magnetic.y;
+    z = event->magnetic.z;
+  }
+  else if ((event->type == SENSOR_TYPE_GYROSCOPE) || (event->type == SENSOR_TYPE_ROTATION_VECTOR)) {
+    x = event->gyro.x;
+    y = event->gyro.y;
+    z = event->gyro.z;
+  }
+
+  Serial.print(": x= ");
+  Serial.print(x);
+  Serial.print(" | y= ");
+  Serial.print(y);
+  Serial.print(" | z= ");
+  Serial.println(z);
+}
+

Embedded/libraries/Adafruit_BNO055/examples/rawdata/rawdata.ino

@@ -0,0 +1,112 @@
+#include <Wire.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_BNO055.h>
+#include <utility/imumaths.h>
+
+/* This driver reads raw data from the BNO055
+
+   Connections
+   ===========
+   Connect SCL to analog 5
+   Connect SDA to analog 4
+   Connect VDD to 3.3V DC
+   Connect GROUND to common ground
+
+   History
+   =======
+   2015/MAR/03  - First release (KTOWN)
+*/
+
+/* Set the delay between fresh samples */
+#define BNO055_SAMPLERATE_DELAY_MS (100)
+
+// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
+//                                   id, address
+Adafruit_BNO055 bno = Adafruit_BNO055(-1, 0x28);
+
+/**************************************************************************/
+/*
+    Arduino setup function (automatically called at startup)
+*/
+/**************************************************************************/
+void setup(void)
+{
+  Serial.begin(115200);
+  Serial.println("Orientation Sensor Raw Data Test"); Serial.println("");
+
+  /* Initialise the sensor */
+  if(!bno.begin())
+  {
+    /* There was a problem detecting the BNO055 ... check your connections */
+    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
+    while(1);
+  }
+
+  delay(1000);
+
+  /* Display the current temperature */
+  int8_t temp = bno.getTemp();
+  Serial.print("Current Temperature: ");
+  Serial.print(temp);
+  Serial.println(" C");
+  Serial.println("");
+
+  bno.setExtCrystalUse(true);
+
+  Serial.println("Calibration status values: 0=uncalibrated, 3=fully calibrated");
+}
+
+/**************************************************************************/
+/*
+    Arduino loop function, called once 'setup' is complete (your own code
+    should go here)
+*/
+/**************************************************************************/
+void loop(void)
+{
+  // Possible vector values can be:
+  // - VECTOR_ACCELEROMETER - m/s^2
+  // - VECTOR_MAGNETOMETER  - uT
+  // - VECTOR_GYROSCOPE     - rad/s
+  // - VECTOR_EULER         - degrees
+  // - VECTOR_LINEARACCEL   - m/s^2
+  // - VECTOR_GRAVITY       - m/s^2
+  imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
+
+  /* Display the floating point data */
+  Serial.print("X: ");
+  Serial.print(euler.x());
+  Serial.print(" Y: ");
+  Serial.print(euler.y());
+  Serial.print(" Z: ");
+  Serial.print(euler.z());
+  Serial.print("\t\t");
+
+  /*
+  // Quaternion data
+  imu::Quaternion quat = bno.getQuat();
+  Serial.print("qW: ");
+  Serial.print(quat.w(), 4);
+  Serial.print(" qX: ");
+  Serial.print(quat.x(), 4);
+  Serial.print(" qY: ");
+  Serial.print(quat.y(), 4);
+  Serial.print(" qZ: ");
+  Serial.print(quat.z(), 4);
+  Serial.print("\t\t");
+  */
+
+  /* Display calibration status for each sensor. */
+  uint8_t system, gyro, accel, mag = 0;
+  bno.getCalibration(&system, &gyro, &accel, &mag);
+  Serial.print("CALIBRATION: Sys=");
+  Serial.print(system, DEC);
+  Serial.print(" Gyro=");
+  Serial.print(gyro, DEC);
+  Serial.print(" Accel=");
+  Serial.print(accel, DEC);
+  Serial.print(" Mag=");
+  Serial.println(mag, DEC);
+
+  delay(BNO055_SAMPLERATE_DELAY_MS);
+}

Embedded/libraries/Adafruit_BNO055/examples/read_all_data/read_all_data.ino

@@ -0,0 +1,148 @@
+#include <Wire.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_BNO055.h>
+#include <utility/imumaths.h>
+
+/* This driver uses the Adafruit unified sensor library (Adafruit_Sensor),
+   which provides a common 'type' for sensor data and some helper functions.
+
+   To use this driver you will also need to download the Adafruit_Sensor
+   library and include it in your libraries folder.
+
+   You should also assign a unique ID to this sensor for use with
+   the Adafruit Sensor API so that you can identify this particular
+   sensor in any data logs, etc.  To assign a unique ID, simply
+   provide an appropriate value in the constructor below (12345
+   is used by default in this example).
+
+   Connections
+   ===========
+   Connect SCL to analog 5
+   Connect SDA to analog 4
+   Connect VDD to 3.3-5V DC
+   Connect GROUND to common ground
+
+   History
+   =======
+   2015/MAR/03  - First release (KTOWN)
+*/
+
+/* Set the delay between fresh samples */
+uint16_t BNO055_SAMPLERATE_DELAY_MS = 100;
+
+// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
+//                                   id, address
+Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);
+
+void setup(void)
+{
+  Serial.begin(115200);
+  Serial.println("Orientation Sensor Test"); Serial.println("");
+
+  /* Initialise the sensor */
+  if (!bno.begin())
+  {
+    /* There was a problem detecting the BNO055 ... check your connections */
+    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
+    while (1);
+  }
+
+  delay(1000);
+}
+
+void loop(void)
+{
+  //could add VECTOR_ACCELEROMETER, VECTOR_MAGNETOMETER,VECTOR_GRAVITY...
+  sensors_event_t orientationData , angVelocityData , linearAccelData, magnetometerData, accelerometerData, gravityData;
+  bno.getEvent(&orientationData, Adafruit_BNO055::VECTOR_EULER);
+  bno.getEvent(&angVelocityData, Adafruit_BNO055::VECTOR_GYROSCOPE);
+  bno.getEvent(&linearAccelData, Adafruit_BNO055::VECTOR_LINEARACCEL);
+  bno.getEvent(&magnetometerData, Adafruit_BNO055::VECTOR_MAGNETOMETER);
+  bno.getEvent(&accelerometerData, Adafruit_BNO055::VECTOR_ACCELEROMETER);
+  bno.getEvent(&gravityData, Adafruit_BNO055::VECTOR_GRAVITY);
+
+  printEvent(&orientationData);
+  printEvent(&angVelocityData);
+  printEvent(&linearAccelData);
+  printEvent(&magnetometerData);
+  printEvent(&accelerometerData);
+  printEvent(&gravityData);
+
+  int8_t boardTemp = bno.getTemp();
+  Serial.println();
+  Serial.print(F("temperature: "));
+  Serial.println(boardTemp);
+
+  uint8_t system, gyro, accel, mag = 0;
+  bno.getCalibration(&system, &gyro, &accel, &mag);
+  Serial.println();
+  Serial.print("Calibration: Sys=");
+  Serial.print(system);
+  Serial.print(" Gyro=");
+  Serial.print(gyro);
+  Serial.print(" Accel=");
+  Serial.print(accel);
+  Serial.print(" Mag=");
+  Serial.println(mag);
+
+  Serial.println("--");
+  delay(BNO055_SAMPLERATE_DELAY_MS);
+}
+
+void printEvent(sensors_event_t* event) {
+  double x = -1000000, y = -1000000 , z = -1000000; //dumb values, easy to spot problem
+  if (event->type == SENSOR_TYPE_ACCELEROMETER) {
+    Serial.print("Accl:");
+    x = event->acceleration.x;
+    y = event->acceleration.y;
+    z = event->acceleration.z;
+  }
+  else if (event->type == SENSOR_TYPE_ORIENTATION) {
+    Serial.print("Orient:");
+    x = event->orientation.x;
+    y = event->orientation.y;
+    z = event->orientation.z;
+  }
+  else if (event->type == SENSOR_TYPE_MAGNETIC_FIELD) {
+    Serial.print("Mag:");
+    x = event->magnetic.x;
+    y = event->magnetic.y;
+    z = event->magnetic.z;
+  }
+  else if (event->type == SENSOR_TYPE_GYROSCOPE) {
+    Serial.print("Gyro:");
+    x = event->gyro.x;
+    y = event->gyro.y;
+    z = event->gyro.z;
+  }
+  else if (event->type == SENSOR_TYPE_ROTATION_VECTOR) {
+    Serial.print("Rot:");
+    x = event->gyro.x;
+    y = event->gyro.y;
+    z = event->gyro.z;
+  }
+  else if (event->type == SENSOR_TYPE_LINEAR_ACCELERATION) {
+    Serial.print("Linear:");
+    x = event->acceleration.x;
+    y = event->acceleration.y;
+    z = event->acceleration.z;
+  }
+  else if (event->type == SENSOR_TYPE_GRAVITY) {
+    Serial.print("Gravity:");
+    x = event->acceleration.x;
+    y = event->acceleration.y;
+    z = event->acceleration.z;
+  }
+  else {
+    Serial.print("Unk:");
+  }
+
+  Serial.print("\tx= ");
+  Serial.print(x);
+  Serial.print(" |\ty= ");
+  Serial.print(y);
+  Serial.print(" |\tz= ");
+  Serial.println(z);
+}
+
+

Embedded/libraries/Adafruit_BNO055/examples/restore_offsets/restore_offsets.ino

@@ -0,0 +1,295 @@
+#include <Wire.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_BNO055.h>
+#include <utility/imumaths.h>
+#include <EEPROM.h>
+
+/* This driver uses the Adafruit unified sensor library (Adafruit_Sensor),
+   which provides a common 'type' for sensor data and some helper functions.
+
+   To use this driver you will also need to download the Adafruit_Sensor
+   library and include it in your libraries folder.
+
+   You should also assign a unique ID to this sensor for use with
+   the Adafruit Sensor API so that you can identify this particular
+   sensor in any data logs, etc.  To assign a unique ID, simply
+   provide an appropriate value in the constructor below (12345
+   is used by default in this example).
+
+   Connections
+   ===========
+   Connect SCL to analog 5
+   Connect SDA to analog 4
+   Connect VDD to 3-5V DC
+   Connect GROUND to common ground
+
+   History
+   =======
+   2015/MAR/03  - First release (KTOWN)
+   2015/AUG/27  - Added calibration and system status helpers
+   2015/NOV/13  - Added calibration save and restore
+   */
+
+/* Set the delay between fresh samples */
+#define BNO055_SAMPLERATE_DELAY_MS (100)
+
+// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
+//                                   id, address
+Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);
+
+/**************************************************************************/
+/*
+    Displays some basic information on this sensor from the unified
+    sensor API sensor_t type (see Adafruit_Sensor for more information)
+    */
+/**************************************************************************/
+void displaySensorDetails(void)
+{
+    sensor_t sensor;
+    bno.getSensor(&sensor);
+    Serial.println("------------------------------------");
+    Serial.print("Sensor:       "); Serial.println(sensor.name);
+    Serial.print("Driver Ver:   "); Serial.println(sensor.version);
+    Serial.print("Unique ID:    "); Serial.println(sensor.sensor_id);
+    Serial.print("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" xxx");
+    Serial.print("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" xxx");
+    Serial.print("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" xxx");
+    Serial.println("------------------------------------");
+    Serial.println("");
+    delay(500);
+}
+
+/**************************************************************************/
+/*
+    Display some basic info about the sensor status
+    */
+/**************************************************************************/
+void displaySensorStatus(void)
+{
+    /* Get the system status values (mostly for debugging purposes) */
+    uint8_t system_status, self_test_results, system_error;
+    system_status = self_test_results = system_error = 0;
+    bno.getSystemStatus(&system_status, &self_test_results, &system_error);
+
+    /* Display the results in the Serial Monitor */
+    Serial.println("");
+    Serial.print("System Status: 0x");
+    Serial.println(system_status, HEX);
+    Serial.print("Self Test:     0x");
+    Serial.println(self_test_results, HEX);
+    Serial.print("System Error:  0x");
+    Serial.println(system_error, HEX);
+    Serial.println("");
+    delay(500);
+}
+
+/**************************************************************************/
+/*
+    Display sensor calibration status
+    */
+/**************************************************************************/
+void displayCalStatus(void)
+{
+    /* Get the four calibration values (0..3) */
+    /* Any sensor data reporting 0 should be ignored, */
+    /* 3 means 'fully calibrated" */
+    uint8_t system, gyro, accel, mag;
+    system = gyro = accel = mag = 0;
+    bno.getCalibration(&system, &gyro, &accel, &mag);
+
+    /* The data should be ignored until the system calibration is > 0 */
+    Serial.print("\t");
+    if (!system)
+    {
+        Serial.print("! ");
+    }
+
+    /* Display the individual values */
+    Serial.print("Sys:");
+    Serial.print(system, DEC);
+    Serial.print(" G:");
+    Serial.print(gyro, DEC);
+    Serial.print(" A:");
+    Serial.print(accel, DEC);
+    Serial.print(" M:");
+    Serial.print(mag, DEC);
+}
+
+/**************************************************************************/
+/*
+    Display the raw calibration offset and radius data
+    */
+/**************************************************************************/
+void displaySensorOffsets(const adafruit_bno055_offsets_t &calibData)
+{
+    Serial.print("Accelerometer: ");
+    Serial.print(calibData.accel_offset_x); Serial.print(" ");
+    Serial.print(calibData.accel_offset_y); Serial.print(" ");
+    Serial.print(calibData.accel_offset_z); Serial.print(" ");
+
+    Serial.print("\nGyro: ");
+    Serial.print(calibData.gyro_offset_x); Serial.print(" ");
+    Serial.print(calibData.gyro_offset_y); Serial.print(" ");
+    Serial.print(calibData.gyro_offset_z); Serial.print(" ");
+
+    Serial.print("\nMag: ");
+    Serial.print(calibData.mag_offset_x); Serial.print(" ");
+    Serial.print(calibData.mag_offset_y); Serial.print(" ");
+    Serial.print(calibData.mag_offset_z); Serial.print(" ");
+
+    Serial.print("\nAccel Radius: ");
+    Serial.print(calibData.accel_radius);
+
+    Serial.print("\nMag Radius: ");
+    Serial.print(calibData.mag_radius);
+}
+
+
+/**************************************************************************/
+/*
+    Arduino setup function (automatically called at startup)
+    */
+/**************************************************************************/
+void setup(void)
+{
+    Serial.begin(115200);
+    delay(1000);
+    Serial.println("Orientation Sensor Test"); Serial.println("");
+
+    /* Initialise the sensor */
+    if (!bno.begin())
+    {
+        /* There was a problem detecting the BNO055 ... check your connections */
+        Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
+        while (1);
+    }
+
+    int eeAddress = 0;
+    long bnoID;
+    bool foundCalib = false;
+
+    EEPROM.get(eeAddress, bnoID);
+
+    adafruit_bno055_offsets_t calibrationData;
+    sensor_t sensor;
+
+    /*
+    *  Look for the sensor's unique ID at the beginning oF EEPROM.
+    *  This isn't foolproof, but it's better than nothing.
+    */
+    bno.getSensor(&sensor);
+    if (bnoID != sensor.sensor_id)
+    {
+        Serial.println("\nNo Calibration Data for this sensor exists in EEPROM");
+        delay(500);
+    }
+    else
+    {
+        Serial.println("\nFound Calibration for this sensor in EEPROM.");
+        eeAddress += sizeof(long);
+        EEPROM.get(eeAddress, calibrationData);
+
+        displaySensorOffsets(calibrationData);
+
+        Serial.println("\n\nRestoring Calibration data to the BNO055...");
+        bno.setSensorOffsets(calibrationData);
+
+        Serial.println("\n\nCalibration data loaded into BNO055");
+        foundCalib = true;
+    }
+
+    delay(1000);
+
+    /* Display some basic information on this sensor */
+    displaySensorDetails();
+
+    /* Optional: Display current status */
+    displaySensorStatus();
+
+   /* Crystal must be configured AFTER loading calibration data into BNO055. */
+    bno.setExtCrystalUse(true);
+
+    sensors_event_t event;
+    bno.getEvent(&event);
+    /* always recal the mag as It goes out of calibration very often */
+    if (foundCalib){
+        Serial.println("Move sensor slightly to calibrate magnetometers");
+        while (!bno.isFullyCalibrated())
+        {
+            bno.getEvent(&event);
+            delay(BNO055_SAMPLERATE_DELAY_MS);
+        }
+    }
+    else
+    {
+        Serial.println("Please Calibrate Sensor: ");
+        while (!bno.isFullyCalibrated())
+        {
+            bno.getEvent(&event);
+
+            Serial.print("X: ");
+            Serial.print(event.orientation.x, 4);
+            Serial.print("\tY: ");
+            Serial.print(event.orientation.y, 4);
+            Serial.print("\tZ: ");
+            Serial.print(event.orientation.z, 4);
+
+            /* Optional: Display calibration status */
+            displayCalStatus();
+
+            /* New line for the next sample */
+            Serial.println("");
+
+            /* Wait the specified delay before requesting new data */
+            delay(BNO055_SAMPLERATE_DELAY_MS);
+        }
+    }
+
+    Serial.println("\nFully calibrated!");
+    Serial.println("--------------------------------");
+    Serial.println("Calibration Results: ");
+    adafruit_bno055_offsets_t newCalib;
+    bno.getSensorOffsets(newCalib);
+    displaySensorOffsets(newCalib);
+
+    Serial.println("\n\nStoring calibration data to EEPROM...");
+
+    eeAddress = 0;
+    bno.getSensor(&sensor);
+    bnoID = sensor.sensor_id;
+
+    EEPROM.put(eeAddress, bnoID);
+
+    eeAddress += sizeof(long);
+    EEPROM.put(eeAddress, newCalib);
+    Serial.println("Data stored to EEPROM.");
+
+    Serial.println("\n--------------------------------\n");
+    delay(500);
+}
+
+void loop() {
+    /* Get a new sensor event */
+    sensors_event_t event;
+    bno.getEvent(&event);
+
+    /* Display the floating point data */
+    Serial.print("X: ");
+    Serial.print(event.orientation.x, 4);
+    Serial.print("\tY: ");
+    Serial.print(event.orientation.y, 4);
+    Serial.print("\tZ: ");
+    Serial.print(event.orientation.z, 4);
+
+    /* Optional: Display calibration status */
+    displayCalStatus();
+
+    /* Optional: Display sensor status (debug only) */
+    //displaySensorStatus();
+
+    /* New line for the next sample */
+    Serial.println("");
+
+    /* Wait the specified delay before requesting new data */
+    delay(BNO055_SAMPLERATE_DELAY_MS);
+}

Embedded/libraries/Adafruit_BNO055/examples/sensorapi/sensorapi.ino

@@ -0,0 +1,176 @@
+#include <Wire.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_BNO055.h>
+#include <utility/imumaths.h>
+
+/* This driver uses the Adafruit unified sensor library (Adafruit_Sensor),
+   which provides a common 'type' for sensor data and some helper functions.
+
+   To use this driver you will also need to download the Adafruit_Sensor
+   library and include it in your libraries folder.
+
+   You should also assign a unique ID to this sensor for use with
+   the Adafruit Sensor API so that you can identify this particular
+   sensor in any data logs, etc.  To assign a unique ID, simply
+   provide an appropriate value in the constructor below (12345
+   is used by default in this example).
+
+   Connections
+   ===========
+   Connect SCL to SCL pin (analog 5 on Arduino UNO)
+   Connect SDA to SDA pin (analog 4 on Arduino UNO)
+   Connect VDD to 3-5V DC (depending on your board's logic level)
+   Connect GROUND to common ground
+
+   History
+   =======
+   2015/MAR/03  - First release (KTOWN)
+   2015/AUG/27  - Added calibration and system status helpers
+*/
+
+/* Set the delay between fresh samples */
+#define BNO055_SAMPLERATE_DELAY_MS (100)
+
+// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
+//                                   id, address
+Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);
+
+/**************************************************************************/
+/*
+    Displays some basic information on this sensor from the unified
+    sensor API sensor_t type (see Adafruit_Sensor for more information)
+*/
+/**************************************************************************/
+void displaySensorDetails(void)
+{
+  sensor_t sensor;
+  bno.getSensor(&sensor);
+  Serial.println("------------------------------------");
+  Serial.print  ("Sensor:       "); Serial.println(sensor.name);
+  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
+  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
+  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" xxx");
+  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" xxx");
+  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" xxx");
+  Serial.println("------------------------------------");
+  Serial.println("");
+  delay(500);
+}
+
+/**************************************************************************/
+/*
+    Display some basic info about the sensor status
+*/
+/**************************************************************************/
+void displaySensorStatus(void)
+{
+  /* Get the system status values (mostly for debugging purposes) */
+  uint8_t system_status, self_test_results, system_error;
+  system_status = self_test_results = system_error = 0;
+  bno.getSystemStatus(&system_status, &self_test_results, &system_error);
+
+  /* Display the results in the Serial Monitor */
+  Serial.println("");
+  Serial.print("System Status: 0x");
+  Serial.println(system_status, HEX);
+  Serial.print("Self Test:     0x");
+  Serial.println(self_test_results, HEX);
+  Serial.print("System Error:  0x");
+  Serial.println(system_error, HEX);
+  Serial.println("");
+  delay(500);
+}
+
+/**************************************************************************/
+/*
+    Display sensor calibration status
+*/
+/**************************************************************************/
+void displayCalStatus(void)
+{
+  /* Get the four calibration values (0..3) */
+  /* Any sensor data reporting 0 should be ignored, */
+  /* 3 means 'fully calibrated" */
+  uint8_t system, gyro, accel, mag;
+  system = gyro = accel = mag = 0;
+  bno.getCalibration(&system, &gyro, &accel, &mag);
+
+  /* The data should be ignored until the system calibration is > 0 */
+  Serial.print("\t");
+  if (!system)
+  {
+    Serial.print("! ");
+  }
+
+  /* Display the individual values */
+  Serial.print("Sys:");
+  Serial.print(system, DEC);
+  Serial.print(" G:");
+  Serial.print(gyro, DEC);
+  Serial.print(" A:");
+  Serial.print(accel, DEC);
+  Serial.print(" M:");
+  Serial.print(mag, DEC);
+}
+
+/**************************************************************************/
+/*
+    Arduino setup function (automatically called at startup)
+*/
+/**************************************************************************/
+void setup(void)
+{
+  Serial.begin(115200);
+  Serial.println("Orientation Sensor Test"); Serial.println("");
+
+  /* Initialise the sensor */
+  if(!bno.begin())
+  {
+    /* There was a problem detecting the BNO055 ... check your connections */
+    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
+    while(1);
+  }
+
+  delay(1000);
+
+  /* Display some basic information on this sensor */
+  displaySensorDetails();
+
+  /* Optional: Display current status */
+  displaySensorStatus();
+
+  bno.setExtCrystalUse(true);
+}
+
+/**************************************************************************/
+/*
+    Arduino loop function, called once 'setup' is complete (your own code
+    should go here)
+*/
+/**************************************************************************/
+void loop(void)
+{
+  /* Get a new sensor event */
+  sensors_event_t event;
+  bno.getEvent(&event);
+
+  /* Display the floating point data */
+  Serial.print("X: ");
+  Serial.print(event.orientation.x, 4);
+  Serial.print("\tY: ");
+  Serial.print(event.orientation.y, 4);
+  Serial.print("\tZ: ");
+  Serial.print(event.orientation.z, 4);
+
+  /* Optional: Display calibration status */
+  displayCalStatus();
+
+  /* Optional: Display sensor status (debug only) */
+  //displaySensorStatus();
+
+  /* New line for the next sample */
+  Serial.println("");
+
+  /* Wait the specified delay before requesting nex data */
+  delay(BNO055_SAMPLERATE_DELAY_MS);
+}

Embedded/libraries/Adafruit_BNO055/examples/webserial_3d/webserial_3d.ino

@@ -0,0 +1,148 @@
+#include <Wire.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_BNO055.h>
+#include <utility/imumaths.h>
+
+/* Returns the IMU data as both a euler angles and quaternions as the WebSerial
+   3D Model viewer at https://adafruit-3dmodel-viewer.glitch.me/ expects.
+ 
+   This driver uses the Adafruit unified sensor library (Adafruit_Sensor),
+   which provides a common 'type' for sensor data and some helper functions.
+
+   To use this driver you will also need to download the Adafruit_Sensor
+   library and include it in your libraries folder.
+
+   You should also assign a unique ID to this sensor for use with
+   the Adafruit Sensor API so that you can identify this particular
+   sensor in any data logs, etc.  To assign a unique ID, simply
+   provide an appropriate value in the constructor below (12345
+   is used by default in this example).
+
+   Connections
+   ===========
+   Connect SCL to analog 5
+   Connect SDA to analog 4
+   Connect VDD to 3.3-5V DC
+   Connect GROUND to common ground
+
+   History
+   =======
+   2020/JUN/01  - First release (Melissa LeBlanc-Williams)
+*/
+
+/* Set the delay between fresh samples */
+#define BNO055_SAMPLERATE_DELAY_MS (100)
+
+// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
+//                                   id, address
+Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);
+
+/**************************************************************************/
+/*
+    Displays some basic information on this sensor from the unified
+    sensor API sensor_t type (see Adafruit_Sensor for more information)
+*/
+/**************************************************************************/
+void displaySensorDetails(void)
+{
+  sensor_t sensor;
+  bno.getSensor(&sensor);
+  Serial.println("------------------------------------");
+  Serial.print  ("Sensor:       "); Serial.println(sensor.name);
+  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
+  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
+  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" xxx");
+  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" xxx");
+  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" xxx");
+  Serial.println("------------------------------------");
+  Serial.println("");
+  delay(500);
+}
+
+/**************************************************************************/
+/*
+    Arduino setup function (automatically called at startup)
+*/
+/**************************************************************************/
+void setup(void)
+{
+  Serial.begin(9600);
+  Serial.println("WebSerial 3D Firmware"); Serial.println("");
+
+  /* Initialise the sensor */
+  if(!bno.begin())
+  {
+    /* There was a problem detecting the BNO055 ... check your connections */
+    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
+    while(1);
+  }
+   
+  delay(1000);
+
+  /* Use external crystal for better accuracy */
+  bno.setExtCrystalUse(true);
+   
+  /* Display some basic information on this sensor */
+  displaySensorDetails();
+}
+
+/**************************************************************************/
+/*
+    Arduino loop function, called once 'setup' is complete (your own code
+    should go here)
+*/
+/**************************************************************************/
+void loop(void)
+{
+  /* Get a new sensor event */
+  sensors_event_t event;
+  bno.getEvent(&event);
+
+  /* Board layout:
+         +----------+
+         |         *| RST   PITCH  ROLL  HEADING
+     ADR |*        *| SCL
+     INT |*        *| SDA     ^            /->
+     PS1 |*        *| GND     |            |
+     PS0 |*        *| 3VO     Y    Z-->    \-X
+         |         *| VIN
+         +----------+
+  */
+
+  /* The WebSerial 3D Model Viewer expects data as heading, pitch, roll */
+  Serial.print(F("Orientation: "));
+  Serial.print(360 - (float)event.orientation.x);
+  Serial.print(F(", "));
+  Serial.print((float)event.orientation.y);
+  Serial.print(F(", "));
+  Serial.print((float)event.orientation.z);
+  Serial.println(F(""));
+
+  /* The WebSerial 3D Model Viewer also expects data as roll, pitch, heading */
+  imu::Quaternion quat = bno.getQuat();
+  
+  Serial.print(F("Quaternion: "));
+  Serial.print((float)quat.w());
+  Serial.print(F(", "));
+  Serial.print((float)quat.x());
+  Serial.print(F(", "));
+  Serial.print((float)quat.y());
+  Serial.print(F(", "));
+  Serial.print((float)quat.z());
+  Serial.println(F(""));
+
+  /* Also send calibration data for each sensor. */
+  uint8_t sys, gyro, accel, mag = 0;
+  bno.getCalibration(&sys, &gyro, &accel, &mag);
+  Serial.print(F("Calibration: "));
+  Serial.print(sys, DEC);
+  Serial.print(F(", "));
+  Serial.print(gyro, DEC);
+  Serial.print(F(", "));
+  Serial.print(accel, DEC);
+  Serial.print(F(", "));
+  Serial.print(mag, DEC);
+  Serial.println(F(""));
+
+  delay(BNO055_SAMPLERATE_DELAY_MS);
+}

Embedded/libraries/Adafruit_BNO055/examples_processing/cuberotate/cuberotate.pde

@@ -0,0 +1,181 @@
+import processing.serial.*;
+import java.awt.datatransfer.*;
+import java.awt.Toolkit;
+import processing.opengl.*;
+import saito.objloader.*;
+import g4p_controls.*;
+
+float roll  = 0.0F;
+float pitch = 0.0F;
+float yaw   = 0.0F;
+float temp  = 0.0F;
+float alt   = 0.0F;
+
+OBJModel model;
+
+// Serial port state.
+Serial       port;
+final String serialConfigFile = "serialconfig.txt";
+boolean      printSerial = false;
+
+// UI controls.
+GPanel    configPanel;
+GDropList serialList;
+GLabel    serialLabel;
+GLabel    calLabel;
+GCheckbox printSerialCheckbox;
+
+void setup()
+{
+  size(640, 480, OPENGL);
+  frameRate(30);
+  model = new OBJModel(this);
+  model.load("bunny.obj");
+  model.scale(20);
+  
+  // Serial port setup.
+  // Grab list of serial ports and choose one that was persisted earlier or default to the first port.
+  int selectedPort = 0;
+  String[] availablePorts = Serial.list();
+  if (availablePorts == null) {
+    println("ERROR: No serial ports available!");
+    exit();
+  }
+  String[] serialConfig = loadStrings(serialConfigFile);
+  if (serialConfig != null && serialConfig.length > 0) {
+    String savedPort = serialConfig[0];
+    // Check if saved port is in available ports.
+    for (int i = 0; i < availablePorts.length; ++i) {
+      if (availablePorts[i].equals(savedPort)) {
+        selectedPort = i;
+      } 
+    }
+  }
+  // Build serial config UI.
+  configPanel = new GPanel(this, 10, 10, width-20, 90, "Configuration (click to hide/show)");
+  serialLabel = new GLabel(this,  0, 20, 80, 25, "Serial port:");
+  configPanel.addControl(serialLabel);
+  serialList = new GDropList(this, 90, 20, 200, 200, 6);
+  serialList.setItems(availablePorts, selectedPort);
+  configPanel.addControl(serialList);
+  calLabel = new GLabel(this, 300, 20, 350, 25, "Calibration: Sys=? Gyro=? Accel=? Mag=?");
+  configPanel.addControl(calLabel); 
+  printSerialCheckbox = new GCheckbox(this, 5, 50, 200, 20, "Print serial data");
+  printSerialCheckbox.setSelected(printSerial);
+  configPanel.addControl(printSerialCheckbox);
+  // Set serial port.
+  setSerialPort(serialList.getSelectedText());
+}
+ 
+void draw()
+{
+  background(0,0,0);
+
+  // Set a new co-ordinate space
+  pushMatrix();
+
+  // Simple 3 point lighting for dramatic effect.
+  // Slightly red light in upper right, slightly blue light in upper left, and white light from behind.
+  pointLight(255, 200, 200,  400, 400,  500);
+  pointLight(200, 200, 255, -400, 400,  500);
+  pointLight(255, 255, 255,    0,   0, -500);
+  
+  // Move bunny from 0,0 in upper left corner to roughly center of screen.
+  translate(300, 380, 0);
+  
+  // Rotate shapes around the X/Y/Z axis (values in radians, 0..Pi*2)
+  //rotateZ(radians(roll));
+  //rotateX(radians(pitch)); // extrinsic rotation
+  //rotateY(radians(yaw));
+  float c1 = cos(radians(roll));
+  float s1 = sin(radians(roll));
+  float c2 = cos(radians(pitch)); // intrinsic rotation
+  float s2 = sin(radians(pitch));
+  float c3 = cos(radians(yaw));
+  float s3 = sin(radians(yaw));
+  applyMatrix( c2*c3, s1*s3+c1*c3*s2, c3*s1*s2-c1*s3, 0,
+               -s2, c1*c2, c2*s1, 0,
+               c2*s3, c1*s2*s3-c3*s1, c1*c3+s1*s2*s3, 0,
+               0, 0, 0, 1);
+
+  pushMatrix();
+  noStroke();
+  model.draw();
+  popMatrix();
+  popMatrix();
+  //print("draw");
+}
+
+void serialEvent(Serial p) 
+{
+  String incoming = p.readString();
+  if (printSerial) {
+    println(incoming);
+  }
+  
+  if ((incoming.length() > 8))
+  {
+    String[] list = split(incoming, " ");
+    if ( (list.length > 0) && (list[0].equals("Orientation:")) ) 
+    {
+      roll  = float(list[3]); // Roll = Z
+      pitch = float(list[2]); // Pitch = Y 
+      yaw   = float(list[1]); // Yaw/Heading = X
+    }
+    if ( (list.length > 0) && (list[0].equals("Alt:")) ) 
+    {
+      alt  = float(list[1]);
+    }
+    if ( (list.length > 0) && (list[0].equals("Temp:")) ) 
+    {
+      temp  = float(list[1]);
+    }
+    if ( (list.length > 0) && (list[0].equals("Calibration:")) )
+    {
+      int sysCal   = int(list[1]);
+      int gyroCal  = int(list[2]);
+      int accelCal = int(list[3]);
+      int magCal   = int(trim(list[4]));
+      calLabel.setText("Calibration: Sys=" + sysCal + " Gyro=" + gyroCal + " Accel=" + accelCal + " Mag=" + magCal);
+    }
+  }
+}
+
+// Set serial port to desired value.
+void setSerialPort(String portName) {
+  // Close the port if it's currently open.
+  if (port != null) {
+    port.stop();
+  }
+  try {
+    // Open port.
+    port = new Serial(this, portName, 115200);
+    port.bufferUntil('\n');
+    // Persist port in configuration.
+    saveStrings(serialConfigFile, new String[] { portName });
+  }
+  catch (RuntimeException ex) {
+    // Swallow error if port can't be opened, keep port closed.
+    port = null; 
+  }
+}
+
+// UI event handlers
+
+void handlePanelEvents(GPanel panel, GEvent event) {
+  // Panel events, do nothing.
+}
+
+void handleDropListEvents(GDropList list, GEvent event) { 
+  // Drop list events, check if new serial port is selected.
+  if (list == serialList) {
+    setSerialPort(serialList.getSelectedText()); 
+  }
+}
+
+void handleToggleControlEvents(GToggleControl checkbox, GEvent event) { 
+  // Checkbox toggle events, check if print events is toggled.
+  if (checkbox == printSerialCheckbox) {
+    printSerial = printSerialCheckbox.isSelected(); 
+  }
+}

Embedded/libraries/Adafruit_BNO055/examples_processing/cuberotate/data/bunny.mtl

@@ -0,0 +1,13 @@
+# 3ds Max Wavefront OBJ Exporter v0.94b - (c)2007 guruware
+# File Created: 04.07.2010 10:41:39
+
+newmtl Body
+Ns 32
+d 1
+Tr 1
+Tf 1 1 1
+illum 2
+Ka 0.0000 0.0000 0.0000
+Kd 0.7412 0.4784 0.4765
+Ks 0.3500 0.3500 0.6500
+

Embedded/libraries/Adafruit_BNO055/examples_processing/cuberotate/serialconfig.txt

@@ -0,0 +1 @@
+/dev/tty.usbmodem141121

Embedded/libraries/Adafruit_BNO055/library.properties

@@ -0,0 +1,10 @@
+name=Adafruit BNO055
+version=1.5.0
+author=Adafruit <info@adafruit.com>
+maintainer=Adafruit <info@adafruit.com>
+sentence=Library for the Adafruit BNO055 Absolute Orientation Sensor.
+paragraph=Designed specifically to work with the Adafruit BNO055 Breakout, and is based on Adafruit's Unified Sensor Library.
+category=Sensors
+url=https://github.com/adafruit/Adafruit_BNO055
+architectures=*
+depends=Adafruit Unified Sensor, Adafruit BusIO

Embedded/libraries/Adafruit_BNO055/utility/imumaths.h

@@ -0,0 +1,27 @@
+/*
+    Inertial Measurement Unit Maths Library
+    Copyright (C) 2013-2014  Samuel Cowen
+        www.camelsoftware.com
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef IMUMATH_H
+#define IMUMATH_H
+
+#include "matrix.h"
+#include "quaternion.h"
+#include "vector.h"
+
+#endif

Embedded/libraries/Adafruit_BNO055/utility/matrix.h

@@ -0,0 +1,183 @@
+/*
+    Inertial Measurement Unit Maths Library
+    Copyright (C) 2013-2014  Samuel Cowen
+    www.camelsoftware.com
+
+    Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com)
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef IMUMATH_MATRIX_HPP
+#define IMUMATH_MATRIX_HPP
+
+#include <stdint.h>
+#include <string.h>
+
+#include "vector.h"
+
+namespace imu {
+
+template <uint8_t N> class Matrix {
+public:
+  Matrix() { memset(_cell_data, 0, N * N * sizeof(double)); }
+
+  Matrix(const Matrix &m) {
+    for (int ij = 0; ij < N * N; ++ij) {
+      _cell_data[ij] = m._cell_data[ij];
+    }
+  }
+
+  ~Matrix() {}
+
+  Matrix &operator=(const Matrix &m) {
+    for (int ij = 0; ij < N * N; ++ij) {
+      _cell_data[ij] = m._cell_data[ij];
+    }
+    return *this;
+  }
+
+  Vector<N> row_to_vector(int i) const {
+    Vector<N> ret;
+    for (int j = 0; j < N; j++) {
+      ret[j] = cell(i, j);
+    }
+    return ret;
+  }
+
+  Vector<N> col_to_vector(int j) const {
+    Vector<N> ret;
+    for (int i = 0; i < N; i++) {
+      ret[i] = cell(i, j);
+    }
+    return ret;
+  }
+
+  void vector_to_row(const Vector<N> &v, int i) {
+    for (int j = 0; j < N; j++) {
+      cell(i, j) = v[j];
+    }
+  }
+
+  void vector_to_col(const Vector<N> &v, int j) {
+    for (int i = 0; i < N; i++) {
+      cell(i, j) = v[i];
+    }
+  }
+
+  double operator()(int i, int j) const { return cell(i, j); }
+  double &operator()(int i, int j) { return cell(i, j); }
+
+  double cell(int i, int j) const { return _cell_data[i * N + j]; }
+  double &cell(int i, int j) { return _cell_data[i * N + j]; }
+
+  Matrix operator+(const Matrix &m) const {
+    Matrix ret;
+    for (int ij = 0; ij < N * N; ++ij) {
+      ret._cell_data[ij] = _cell_data[ij] + m._cell_data[ij];
+    }
+    return ret;
+  }
+
+  Matrix operator-(const Matrix &m) const {
+    Matrix ret;
+    for (int ij = 0; ij < N * N; ++ij) {
+      ret._cell_data[ij] = _cell_data[ij] - m._cell_data[ij];
+    }
+    return ret;
+  }
+
+  Matrix operator*(double scalar) const {
+    Matrix ret;
+    for (int ij = 0; ij < N * N; ++ij) {
+      ret._cell_data[ij] = _cell_data[ij] * scalar;
+    }
+    return ret;
+  }
+
+  Matrix operator*(const Matrix &m) const {
+    Matrix ret;
+    for (int i = 0; i < N; i++) {
+      Vector<N> row = row_to_vector(i);
+      for (int j = 0; j < N; j++) {
+        ret(i, j) = row.dot(m.col_to_vector(j));
+      }
+    }
+    return ret;
+  }
+
+  Matrix transpose() const {
+    Matrix ret;
+    for (int i = 0; i < N; i++) {
+      for (int j = 0; j < N; j++) {
+        ret(j, i) = cell(i, j);
+      }
+    }
+    return ret;
+  }
+
+  Matrix<N - 1> minor_matrix(int row, int col) const {
+    Matrix<N - 1> ret;
+    for (int i = 0, im = 0; i < N; i++) {
+      if (i == row)
+        continue;
+
+      for (int j = 0, jm = 0; j < N; j++) {
+        if (j != col) {
+          ret(im, jm++) = cell(i, j);
+        }
+      }
+      im++;
+    }
+    return ret;
+  }
+
+  double determinant() const {
+    // specialization for N == 1 given below this class
+    double det = 0.0, sign = 1.0;
+    for (int i = 0; i < N; ++i, sign = -sign)
+      det += sign * cell(0, i) * minor_matrix(0, i).determinant();
+    return det;
+  }
+
+  Matrix invert() const {
+    Matrix ret;
+    double det = determinant();
+
+    for (int i = 0; i < N; i++) {
+      for (int j = 0; j < N; j++) {
+        ret(i, j) = minor_matrix(j, i).determinant() / det;
+        if ((i + j) % 2 == 1)
+          ret(i, j) = -ret(i, j);
+      }
+    }
+    return ret;
+  }
+
+  double trace() const {
+    double tr = 0.0;
+    for (int i = 0; i < N; ++i)
+      tr += cell(i, i);
+    return tr;
+  }
+
+private:
+  double _cell_data[N * N];
+};
+
+template <> inline double Matrix<1>::determinant() const { return cell(0, 0); }
+
+}; // namespace imu
+
+#endif

Embedded/libraries/Adafruit_BNO055/utility/quaternion.h

@@ -0,0 +1,212 @@
+/*
+    Inertial Measurement Unit Maths Library
+    Copyright (C) 2013-2014  Samuel Cowen
+        www.camelsoftware.com
+
+    Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com)
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef IMUMATH_QUATERNION_HPP
+#define IMUMATH_QUATERNION_HPP
+
+#include <math.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "matrix.h"
+
+namespace imu {
+
+class Quaternion {
+public:
+  Quaternion() : _w(1.0), _x(0.0), _y(0.0), _z(0.0) {}
+
+  Quaternion(double w, double x, double y, double z)
+      : _w(w), _x(x), _y(y), _z(z) {}
+
+  Quaternion(double w, Vector<3> vec)
+      : _w(w), _x(vec.x()), _y(vec.y()), _z(vec.z()) {}
+
+  double &w() { return _w; }
+  double &x() { return _x; }
+  double &y() { return _y; }
+  double &z() { return _z; }
+
+  double w() const { return _w; }
+  double x() const { return _x; }
+  double y() const { return _y; }
+  double z() const { return _z; }
+
+  double magnitude() const {
+    return sqrt(_w * _w + _x * _x + _y * _y + _z * _z);
+  }
+
+  void normalize() {
+    double mag = magnitude();
+    *this = this->scale(1 / mag);
+  }
+
+  Quaternion conjugate() const { return Quaternion(_w, -_x, -_y, -_z); }
+
+  void fromAxisAngle(const Vector<3> &axis, double theta) {
+    _w = cos(theta / 2);
+    // only need to calculate sine of half theta once
+    double sht = sin(theta / 2);
+    _x = axis.x() * sht;
+    _y = axis.y() * sht;
+    _z = axis.z() * sht;
+  }
+
+  void fromMatrix(const Matrix<3> &m) {
+    double tr = m.trace();
+
+    double S;
+    if (tr > 0) {
+      S = sqrt(tr + 1.0) * 2;
+      _w = 0.25 * S;
+      _x = (m(2, 1) - m(1, 2)) / S;
+      _y = (m(0, 2) - m(2, 0)) / S;
+      _z = (m(1, 0) - m(0, 1)) / S;
+    } else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2)) {
+      S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2;
+      _w = (m(2, 1) - m(1, 2)) / S;
+      _x = 0.25 * S;
+      _y = (m(0, 1) + m(1, 0)) / S;
+      _z = (m(0, 2) + m(2, 0)) / S;
+    } else if (m(1, 1) > m(2, 2)) {
+      S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2;
+      _w = (m(0, 2) - m(2, 0)) / S;
+      _x = (m(0, 1) + m(1, 0)) / S;
+      _y = 0.25 * S;
+      _z = (m(1, 2) + m(2, 1)) / S;
+    } else {
+      S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2;
+      _w = (m(1, 0) - m(0, 1)) / S;
+      _x = (m(0, 2) + m(2, 0)) / S;
+      _y = (m(1, 2) + m(2, 1)) / S;
+      _z = 0.25 * S;
+    }
+  }
+
+  void toAxisAngle(Vector<3> &axis, double &angle) const {
+    double sqw = sqrt(1 - _w * _w);
+    if (sqw == 0) // it's a singularity and divide by zero, avoid
+      return;
+
+    angle = 2 * acos(_w);
+    axis.x() = _x / sqw;
+    axis.y() = _y / sqw;
+    axis.z() = _z / sqw;
+  }
+
+  Matrix<3> toMatrix() const {
+    Matrix<3> ret;
+    ret.cell(0, 0) = 1 - 2 * _y * _y - 2 * _z * _z;
+    ret.cell(0, 1) = 2 * _x * _y - 2 * _w * _z;
+    ret.cell(0, 2) = 2 * _x * _z + 2 * _w * _y;
+
+    ret.cell(1, 0) = 2 * _x * _y + 2 * _w * _z;
+    ret.cell(1, 1) = 1 - 2 * _x * _x - 2 * _z * _z;
+    ret.cell(1, 2) = 2 * _y * _z - 2 * _w * _x;
+
+    ret.cell(2, 0) = 2 * _x * _z - 2 * _w * _y;
+    ret.cell(2, 1) = 2 * _y * _z + 2 * _w * _x;
+    ret.cell(2, 2) = 1 - 2 * _x * _x - 2 * _y * _y;
+    return ret;
+  }
+
+  // Returns euler angles that represent the quaternion.  Angles are
+  // returned in rotation order and right-handed about the specified
+  // axes:
+  //
+  //   v[0] is applied 1st about z (ie, roll)
+  //   v[1] is applied 2nd about y (ie, pitch)
+  //   v[2] is applied 3rd about x (ie, yaw)
+  //
+  // Note that this means result.x() is not a rotation about x;
+  // similarly for result.z().
+  //
+  Vector<3> toEuler() const {
+    Vector<3> ret;
+    double sqw = _w * _w;
+    double sqx = _x * _x;
+    double sqy = _y * _y;
+    double sqz = _z * _z;
+
+    ret.x() = atan2(2.0 * (_x * _y + _z * _w), (sqx - sqy - sqz + sqw));
+    ret.y() = asin(-2.0 * (_x * _z - _y * _w) / (sqx + sqy + sqz + sqw));
+    ret.z() = atan2(2.0 * (_y * _z + _x * _w), (-sqx - sqy + sqz + sqw));
+
+    return ret;
+  }
+
+  Vector<3> toAngularVelocity(double dt) const {
+    Vector<3> ret;
+    Quaternion one(1.0, 0.0, 0.0, 0.0);
+    Quaternion delta = one - *this;
+    Quaternion r = (delta / dt);
+    r = r * 2;
+    r = r * one;
+
+    ret.x() = r.x();
+    ret.y() = r.y();
+    ret.z() = r.z();
+    return ret;
+  }
+
+  Vector<3> rotateVector(const Vector<2> &v) const {
+    return rotateVector(Vector<3>(v.x(), v.y()));
+  }
+
+  Vector<3> rotateVector(const Vector<3> &v) const {
+    Vector<3> qv(_x, _y, _z);
+    Vector<3> t = qv.cross(v) * 2.0;
+    return v + t * _w + qv.cross(t);
+  }
+
+  Quaternion operator*(const Quaternion &q) const {
+    return Quaternion(_w * q._w - _x * q._x - _y * q._y - _z * q._z,
+                      _w * q._x + _x * q._w + _y * q._z - _z * q._y,
+                      _w * q._y - _x * q._z + _y * q._w + _z * q._x,
+                      _w * q._z + _x * q._y - _y * q._x + _z * q._w);
+  }
+
+  Quaternion operator+(const Quaternion &q) const {
+    return Quaternion(_w + q._w, _x + q._x, _y + q._y, _z + q._z);
+  }
+
+  Quaternion operator-(const Quaternion &q) const {
+    return Quaternion(_w - q._w, _x - q._x, _y - q._y, _z - q._z);
+  }
+
+  Quaternion operator/(double scalar) const {
+    return Quaternion(_w / scalar, _x / scalar, _y / scalar, _z / scalar);
+  }
+
+  Quaternion operator*(double scalar) const { return scale(scalar); }
+
+  Quaternion scale(double scalar) const {
+    return Quaternion(_w * scalar, _x * scalar, _y * scalar, _z * scalar);
+  }
+
+private:
+  double _w, _x, _y, _z;
+};
+
+} // namespace imu
+
+#endif

Embedded/libraries/Adafruit_BNO055/utility/vector.h

@@ -0,0 +1,182 @@
+/*
+    Inertial Measurement Unit Maths Library
+    Copyright (C) 2013-2014  Samuel Cowen
+    www.camelsoftware.com
+
+    Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com)
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef IMUMATH_VECTOR_HPP
+#define IMUMATH_VECTOR_HPP
+
+#include <math.h>
+#include <stdint.h>
+#include <string.h>
+
+namespace imu {
+
+template <uint8_t N> class Vector {
+public:
+  Vector() { memset(p_vec, 0, sizeof(double) * N); }
+
+  Vector(double a) {
+    memset(p_vec, 0, sizeof(double) * N);
+    p_vec[0] = a;
+  }
+
+  Vector(double a, double b) {
+    memset(p_vec, 0, sizeof(double) * N);
+    p_vec[0] = a;
+    p_vec[1] = b;
+  }
+
+  Vector(double a, double b, double c) {
+    memset(p_vec, 0, sizeof(double) * N);
+    p_vec[0] = a;
+    p_vec[1] = b;
+    p_vec[2] = c;
+  }
+
+  Vector(double a, double b, double c, double d) {
+    memset(p_vec, 0, sizeof(double) * N);
+    p_vec[0] = a;
+    p_vec[1] = b;
+    p_vec[2] = c;
+    p_vec[3] = d;
+  }
+
+  Vector(const Vector<N> &v) {
+    for (int x = 0; x < N; x++)
+      p_vec[x] = v.p_vec[x];
+  }
+
+  ~Vector() {}
+
+  uint8_t n() { return N; }
+
+  double magnitude() const {
+    double res = 0;
+    for (int i = 0; i < N; i++)
+      res += p_vec[i] * p_vec[i];
+
+    return sqrt(res);
+  }
+
+  void normalize() {
+    double mag = magnitude();
+    if (isnan(mag) || mag == 0.0)
+      return;
+
+    for (int i = 0; i < N; i++)
+      p_vec[i] /= mag;
+  }
+
+  double dot(const Vector &v) const {
+    double ret = 0;
+    for (int i = 0; i < N; i++)
+      ret += p_vec[i] * v.p_vec[i];
+
+    return ret;
+  }
+
+  // The cross product is only valid for vectors with 3 dimensions,
+  // with the exception of higher dimensional stuff that is beyond
+  // the intended scope of this library.
+  // Only a definition for N==3 is given below this class, using
+  // cross() with another value for N will result in a link error.
+  Vector cross(const Vector &v) const;
+
+  Vector scale(double scalar) const {
+    Vector ret;
+    for (int i = 0; i < N; i++)
+      ret.p_vec[i] = p_vec[i] * scalar;
+    return ret;
+  }
+
+  Vector invert() const {
+    Vector ret;
+    for (int i = 0; i < N; i++)
+      ret.p_vec[i] = -p_vec[i];
+    return ret;
+  }
+
+  Vector &operator=(const Vector &v) {
+    for (int x = 0; x < N; x++)
+      p_vec[x] = v.p_vec[x];
+    return *this;
+  }
+
+  double &operator[](int n) { return p_vec[n]; }
+
+  double operator[](int n) const { return p_vec[n]; }
+
+  double &operator()(int n) { return p_vec[n]; }
+
+  double operator()(int n) const { return p_vec[n]; }
+
+  Vector operator+(const Vector &v) const {
+    Vector ret;
+    for (int i = 0; i < N; i++)
+      ret.p_vec[i] = p_vec[i] + v.p_vec[i];
+    return ret;
+  }
+
+  Vector operator-(const Vector &v) const {
+    Vector ret;
+    for (int i = 0; i < N; i++)
+      ret.p_vec[i] = p_vec[i] - v.p_vec[i];
+    return ret;
+  }
+
+  Vector operator*(double scalar) const { return scale(scalar); }
+
+  Vector operator/(double scalar) const {
+    Vector ret;
+    for (int i = 0; i < N; i++)
+      ret.p_vec[i] = p_vec[i] / scalar;
+    return ret;
+  }
+
+  void toDegrees() {
+    for (int i = 0; i < N; i++)
+      p_vec[i] *= 57.2957795131; // 180/pi
+  }
+
+  void toRadians() {
+    for (int i = 0; i < N; i++)
+      p_vec[i] *= 0.01745329251; // pi/180
+  }
+
+  double &x() { return p_vec[0]; }
+  double &y() { return p_vec[1]; }
+  double &z() { return p_vec[2]; }
+  double x() const { return p_vec[0]; }
+  double y() const { return p_vec[1]; }
+  double z() const { return p_vec[2]; }
+
+private:
+  double p_vec[N];
+};
+
+template <> inline Vector<3> Vector<3>::cross(const Vector &v) const {
+  return Vector(p_vec[1] * v.p_vec[2] - p_vec[2] * v.p_vec[1],
+                p_vec[2] * v.p_vec[0] - p_vec[0] * v.p_vec[2],
+                p_vec[0] * v.p_vec[1] - p_vec[1] * v.p_vec[0]);
+}
+
+} // namespace imu
+
+#endif

Embedded/libraries/Adafruit_BusIO/Adafruit_BusIO_Register.cpp

@@ -0,0 +1,365 @@
+#include <Adafruit_BusIO_Register.h>
+
+#if !defined(SPI_INTERFACES_COUNT) ||                                          \
+    (defined(SPI_INTERFACES_COUNT) && (SPI_INTERFACES_COUNT > 0))
+
+/*!
+ *    @brief  Create a register we access over an I2C Device (which defines the
+ * bus and address)
+ *    @param  i2cdevice The I2CDevice to use for underlying I2C access
+ *    @param  reg_addr The address pointer value for the I2C/SMBus register, can
+ * be 8 or 16 bits
+ *    @param  width    The width of the register data itself, defaults to 1 byte
+ *    @param  byteorder The byte order of the register (used when width is > 1),
+ * defaults to LSBFIRST
+ *    @param  address_width The width of the register address itself, defaults
+ * to 1 byte
+ */
+Adafruit_BusIO_Register::Adafruit_BusIO_Register(Adafruit_I2CDevice *i2cdevice,
+                                                 uint16_t reg_addr,
+                                                 uint8_t width,
+                                                 uint8_t byteorder,
+                                                 uint8_t address_width) {
+  _i2cdevice = i2cdevice;
+  _spidevice = NULL;
+  _addrwidth = address_width;
+  _address = reg_addr;
+  _byteorder = byteorder;
+  _width = width;
+}
+
+/*!
+ *    @brief  Create a register we access over an SPI Device (which defines the
+ * bus and CS pin)
+ *    @param  spidevice The SPIDevice to use for underlying SPI access
+ *    @param  reg_addr The address pointer value for the SPI register, can
+ * be 8 or 16 bits
+ *    @param  type     The method we use to read/write data to SPI (which is not
+ * as well defined as I2C)
+ *    @param  width    The width of the register data itself, defaults to 1 byte
+ *    @param  byteorder The byte order of the register (used when width is > 1),
+ * defaults to LSBFIRST
+ *    @param  address_width The width of the register address itself, defaults
+ * to 1 byte
+ */
+Adafruit_BusIO_Register::Adafruit_BusIO_Register(Adafruit_SPIDevice *spidevice,
+                                                 uint16_t reg_addr,
+                                                 Adafruit_BusIO_SPIRegType type,
+                                                 uint8_t width,
+                                                 uint8_t byteorder,
+                                                 uint8_t address_width) {
+  _spidevice = spidevice;
+  _spiregtype = type;
+  _i2cdevice = NULL;
+  _addrwidth = address_width;
+  _address = reg_addr;
+  _byteorder = byteorder;
+  _width = width;
+}
+
+/*!
+ *    @brief  Create a register we access over an I2C or SPI Device. This is a
+ * handy function because we can pass in NULL for the unused interface, allowing
+ * libraries to mass-define all the registers
+ *    @param  i2cdevice The I2CDevice to use for underlying I2C access, if NULL
+ * we use SPI
+ *    @param  spidevice The SPIDevice to use for underlying SPI access, if NULL
+ * we use I2C
+ *    @param  reg_addr The address pointer value for the I2C/SMBus/SPI register,
+ * can be 8 or 16 bits
+ *    @param  type     The method we use to read/write data to SPI (which is not
+ * as well defined as I2C)
+ *    @param  width    The width of the register data itself, defaults to 1 byte
+ *    @param  byteorder The byte order of the register (used when width is > 1),
+ * defaults to LSBFIRST
+ *    @param  address_width The width of the register address itself, defaults
+ * to 1 byte
+ */
+Adafruit_BusIO_Register::Adafruit_BusIO_Register(
+    Adafruit_I2CDevice *i2cdevice, Adafruit_SPIDevice *spidevice,
+    Adafruit_BusIO_SPIRegType type, uint16_t reg_addr, uint8_t width,
+    uint8_t byteorder, uint8_t address_width) {
+  _spidevice = spidevice;
+  _i2cdevice = i2cdevice;
+  _spiregtype = type;
+  _addrwidth = address_width;
+  _address = reg_addr;
+  _byteorder = byteorder;
+  _width = width;
+}
+
+/*!
+ *    @brief  Write a buffer of data to the register location
+ *    @param  buffer Pointer to data to write
+ *    @param  len Number of bytes to write
+ *    @return True on successful write (only really useful for I2C as SPI is
+ * uncheckable)
+ */
+bool Adafruit_BusIO_Register::write(uint8_t *buffer, uint8_t len) {
+
+  uint8_t addrbuffer[2] = {(uint8_t)(_address & 0xFF),
+                           (uint8_t)(_address >> 8)};
+
+  if (_i2cdevice) {
+    return _i2cdevice->write(buffer, len, true, addrbuffer, _addrwidth);
+  }
+  if (_spidevice) {
+    if (_spiregtype == ADDRESSED_OPCODE_BIT0_LOW_TO_WRITE) {
+      // very special case!
+
+      // pass the special opcode address which we set as the high byte of the
+      // regaddr
+      addrbuffer[0] =
+          (uint8_t)(_address >> 8) & ~0x01; // set bottom bit low to write
+      // the 'actual' reg addr is the second byte then
+      addrbuffer[1] = (uint8_t)(_address & 0xFF);
+      // the address appears to be a byte longer
+      return _spidevice->write(buffer, len, addrbuffer, _addrwidth + 1);
+    }
+
+    if (_spiregtype == ADDRBIT8_HIGH_TOREAD) {
+      addrbuffer[0] &= ~0x80;
+    }
+    if (_spiregtype == ADDRBIT8_HIGH_TOWRITE) {
+      addrbuffer[0] |= 0x80;
+    }
+    if (_spiregtype == AD8_HIGH_TOREAD_AD7_HIGH_TOINC) {
+      addrbuffer[0] &= ~0x80;
+      addrbuffer[0] |= 0x40;
+    }
+    return _spidevice->write(buffer, len, addrbuffer, _addrwidth);
+  }
+  return false;
+}
+
+/*!
+ *    @brief  Write up to 4 bytes of data to the register location
+ *    @param  value Data to write
+ *    @param  numbytes How many bytes from 'value' to write
+ *    @return True on successful write (only really useful for I2C as SPI is
+ * uncheckable)
+ */
+bool Adafruit_BusIO_Register::write(uint32_t value, uint8_t numbytes) {
+  if (numbytes == 0) {
+    numbytes = _width;
+  }
+  if (numbytes > 4) {
+    return false;
+  }
+
+  // store a copy
+  _cached = value;
+
+  for (int i = 0; i < numbytes; i++) {
+    if (_byteorder == LSBFIRST) {
+      _buffer[i] = value & 0xFF;
+    } else {
+      _buffer[numbytes - i - 1] = value & 0xFF;
+    }
+    value >>= 8;
+  }
+  return write(_buffer, numbytes);
+}
+
+/*!
+ *    @brief  Read data from the register location. This does not do any error
+ * checking!
+ *    @return Returns 0xFFFFFFFF on failure, value otherwise
+ */
+uint32_t Adafruit_BusIO_Register::read(void) {
+  if (!read(_buffer, _width)) {
+    return -1;
+  }
+
+  uint32_t value = 0;
+
+  for (int i = 0; i < _width; i++) {
+    value <<= 8;
+    if (_byteorder == LSBFIRST) {
+      value |= _buffer[_width - i - 1];
+    } else {
+      value |= _buffer[i];
+    }
+  }
+
+  return value;
+}
+
+/*!
+ *    @brief  Read cached data from last time we wrote to this register
+ *    @return Returns 0xFFFFFFFF on failure, value otherwise
+ */
+uint32_t Adafruit_BusIO_Register::readCached(void) { return _cached; }
+
+/*!
+ *    @brief  Read a buffer of data from the register location
+ *    @param  buffer Pointer to data to read into
+ *    @param  len Number of bytes to read
+ *    @return True on successful write (only really useful for I2C as SPI is
+ * uncheckable)
+ */
+bool Adafruit_BusIO_Register::read(uint8_t *buffer, uint8_t len) {
+  uint8_t addrbuffer[2] = {(uint8_t)(_address & 0xFF),
+                           (uint8_t)(_address >> 8)};
+
+  if (_i2cdevice) {
+    return _i2cdevice->write_then_read(addrbuffer, _addrwidth, buffer, len);
+  }
+  if (_spidevice) {
+    if (_spiregtype == ADDRESSED_OPCODE_BIT0_LOW_TO_WRITE) {
+      // very special case!
+
+      // pass the special opcode address which we set as the high byte of the
+      // regaddr
+      addrbuffer[0] =
+          (uint8_t)(_address >> 8) | 0x01; // set bottom bit high to read
+      // the 'actual' reg addr is the second byte then
+      addrbuffer[1] = (uint8_t)(_address & 0xFF);
+      // the address appears to be a byte longer
+      return _spidevice->write_then_read(addrbuffer, _addrwidth + 1, buffer,
+                                         len);
+    }
+    if (_spiregtype == ADDRBIT8_HIGH_TOREAD) {
+      addrbuffer[0] |= 0x80;
+    }
+    if (_spiregtype == ADDRBIT8_HIGH_TOWRITE) {
+      addrbuffer[0] &= ~0x80;
+    }
+    if (_spiregtype == AD8_HIGH_TOREAD_AD7_HIGH_TOINC) {
+      addrbuffer[0] |= 0x80 | 0x40;
+    }
+    return _spidevice->write_then_read(addrbuffer, _addrwidth, buffer, len);
+  }
+  return false;
+}
+
+/*!
+ *    @brief  Read 2 bytes of data from the register location
+ *    @param  value Pointer to uint16_t variable to read into
+ *    @return True on successful write (only really useful for I2C as SPI is
+ * uncheckable)
+ */
+bool Adafruit_BusIO_Register::read(uint16_t *value) {
+  if (!read(_buffer, 2)) {
+    return false;
+  }
+
+  if (_byteorder == LSBFIRST) {
+    *value = _buffer[1];
+    *value <<= 8;
+    *value |= _buffer[0];
+  } else {
+    *value = _buffer[0];
+    *value <<= 8;
+    *value |= _buffer[1];
+  }
+  return true;
+}
+
+/*!
+ *    @brief  Read 1 byte of data from the register location
+ *    @param  value Pointer to uint8_t variable to read into
+ *    @return True on successful write (only really useful for I2C as SPI is
+ * uncheckable)
+ */
+bool Adafruit_BusIO_Register::read(uint8_t *value) {
+  if (!read(_buffer, 1)) {
+    return false;
+  }
+
+  *value = _buffer[0];
+  return true;
+}
+
+/*!
+ *    @brief  Pretty printer for this register
+ *    @param  s The Stream to print to, defaults to &Serial
+ */
+void Adafruit_BusIO_Register::print(Stream *s) {
+  uint32_t val = read();
+  s->print("0x");
+  s->print(val, HEX);
+}
+
+/*!
+ *    @brief  Pretty printer for this register
+ *    @param  s The Stream to print to, defaults to &Serial
+ */
+void Adafruit_BusIO_Register::println(Stream *s) {
+  print(s);
+  s->println();
+}
+
+/*!
+ *    @brief  Create a slice of the register that we can address without
+ * touching other bits
+ *    @param  reg The Adafruit_BusIO_Register which defines the bus/register
+ *    @param  bits The number of bits wide we are slicing
+ *    @param  shift The number of bits that our bit-slice is shifted from LSB
+ */
+Adafruit_BusIO_RegisterBits::Adafruit_BusIO_RegisterBits(
+    Adafruit_BusIO_Register *reg, uint8_t bits, uint8_t shift) {
+  _register = reg;
+  _bits = bits;
+  _shift = shift;
+}
+
+/*!
+ *    @brief  Read 4 bytes of data from the register
+ *    @return  data The 4 bytes to read
+ */
+uint32_t Adafruit_BusIO_RegisterBits::read(void) {
+  uint32_t val = _register->read();
+  val >>= _shift;
+  return val & ((1 << (_bits)) - 1);
+}
+
+/*!
+ *    @brief  Write 4 bytes of data to the register
+ *    @param  data The 4 bytes to write
+ *    @return True on successful write (only really useful for I2C as SPI is
+ * uncheckable)
+ */
+bool Adafruit_BusIO_RegisterBits::write(uint32_t data) {
+  uint32_t val = _register->read();
+
+  // mask off the data before writing
+  uint32_t mask = (1 << (_bits)) - 1;
+  data &= mask;
+
+  mask <<= _shift;
+  val &= ~mask;          // remove the current data at that spot
+  val |= data << _shift; // and add in the new data
+
+  return _register->write(val, _register->width());
+}
+
+/*!
+ *    @brief  The width of the register data, helpful for doing calculations
+ *    @returns The data width used when initializing the register
+ */
+uint8_t Adafruit_BusIO_Register::width(void) { return _width; }
+
+/*!
+ *    @brief  Set the default width of data
+ *    @param width the default width of data read from register
+ */
+void Adafruit_BusIO_Register::setWidth(uint8_t width) { _width = width; }
+
+/*!
+ *    @brief  Set register address
+ *    @param address the address from register
+ */
+void Adafruit_BusIO_Register::setAddress(uint16_t address) {
+  _address = address;
+}
+
+/*!
+ *    @brief  Set the width of register address
+ *    @param address_width the width for register address
+ */
+void Adafruit_BusIO_Register::setAddressWidth(uint16_t address_width) {
+  _addrwidth = address_width;
+}
+
+#endif // SPI exists

Embedded/libraries/Adafruit_BusIO/Adafruit_BusIO_Register.h

@@ -0,0 +1,105 @@
+#include <Adafruit_I2CDevice.h>
+#include <Adafruit_SPIDevice.h>
+#include <Arduino.h>
+
+#if !defined(SPI_INTERFACES_COUNT) ||                                          \
+    (defined(SPI_INTERFACES_COUNT) && (SPI_INTERFACES_COUNT > 0))
+
+#ifndef Adafruit_BusIO_Register_h
+#define Adafruit_BusIO_Register_h
+
+typedef enum _Adafruit_BusIO_SPIRegType {
+  ADDRBIT8_HIGH_TOREAD = 0,
+  /*!<
+   * ADDRBIT8_HIGH_TOREAD
+   * When reading a register you must actually send the value 0x80 + register
+   * address to the device. e.g. To read the register 0x0B the register value
+   * 0x8B is sent and to write 0x0B is sent.
+   */
+  AD8_HIGH_TOREAD_AD7_HIGH_TOINC = 1,
+
+  /*!<
+   * ADDRBIT8_HIGH_TOWRITE
+   * When writing to a register you must actually send the value 0x80 +
+   * the register address to the device. e.g. To write to the register 0x19 the
+   * register value 0x99 is sent and to read 0x19 is sent.
+   */
+  ADDRBIT8_HIGH_TOWRITE = 2,
+
+  /*!<
+   * ADDRESSED_OPCODE_LOWBIT_TO_WRITE
+   * Used by the MCP23S series, we send 0x40 |'rd with the opcode
+   * Then set the lowest bit to write
+   */
+  ADDRESSED_OPCODE_BIT0_LOW_TO_WRITE = 3,
+
+} Adafruit_BusIO_SPIRegType;
+
+/*!
+ * @brief The class which defines a device register (a location to read/write
+ * data from)
+ */
+class Adafruit_BusIO_Register {
+public:
+  Adafruit_BusIO_Register(Adafruit_I2CDevice *i2cdevice, uint16_t reg_addr,
+                          uint8_t width = 1, uint8_t byteorder = LSBFIRST,
+                          uint8_t address_width = 1);
+
+  Adafruit_BusIO_Register(Adafruit_SPIDevice *spidevice, uint16_t reg_addr,
+                          Adafruit_BusIO_SPIRegType type, uint8_t width = 1,
+                          uint8_t byteorder = LSBFIRST,
+                          uint8_t address_width = 1);
+
+  Adafruit_BusIO_Register(Adafruit_I2CDevice *i2cdevice,
+                          Adafruit_SPIDevice *spidevice,
+                          Adafruit_BusIO_SPIRegType type, uint16_t reg_addr,
+                          uint8_t width = 1, uint8_t byteorder = LSBFIRST,
+                          uint8_t address_width = 1);
+
+  bool read(uint8_t *buffer, uint8_t len);
+  bool read(uint8_t *value);
+  bool read(uint16_t *value);
+  uint32_t read(void);
+  uint32_t readCached(void);
+  bool write(uint8_t *buffer, uint8_t len);
+  bool write(uint32_t value, uint8_t numbytes = 0);
+
+  uint8_t width(void);
+
+  void setWidth(uint8_t width);
+  void setAddress(uint16_t address);
+  void setAddressWidth(uint16_t address_width);
+
+  void print(Stream *s = &Serial);
+  void println(Stream *s = &Serial);
+
+private:
+  Adafruit_I2CDevice *_i2cdevice;
+  Adafruit_SPIDevice *_spidevice;
+  Adafruit_BusIO_SPIRegType _spiregtype;
+  uint16_t _address;
+  uint8_t _width, _addrwidth, _byteorder;
+  uint8_t _buffer[4]; // we wont support anything larger than uint32 for
+                      // non-buffered read
+  uint32_t _cached = 0;
+};
+
+/*!
+ * @brief The class which defines a slice of bits from within a device register
+ * (a location to read/write data from)
+ */
+class Adafruit_BusIO_RegisterBits {
+public:
+  Adafruit_BusIO_RegisterBits(Adafruit_BusIO_Register *reg, uint8_t bits,
+                              uint8_t shift);
+  bool write(uint32_t value);
+  uint32_t read(void);
+
+private:
+  Adafruit_BusIO_Register *_register;
+  uint8_t _bits, _shift;
+};
+
+#endif // BusIO_Register_h
+
+#endif // SPI exists

Embedded/libraries/Adafruit_BusIO/Adafruit_I2CDevice.cpp

@@ -0,0 +1,269 @@
+#include <Adafruit_I2CDevice.h>
+#include <Arduino.h>
+
+//#define DEBUG_SERIAL Serial
+
+/*!
+ *    @brief  Create an I2C device at a given address
+ *    @param  addr The 7-bit I2C address for the device
+ *    @param  theWire The I2C bus to use, defaults to &Wire
+ */
+Adafruit_I2CDevice::Adafruit_I2CDevice(uint8_t addr, TwoWire *theWire) {
+  _addr = addr;
+  _wire = theWire;
+  _begun = false;
+#ifdef ARDUINO_ARCH_SAMD
+  _maxBufferSize = 250; // as defined in Wire.h's RingBuffer
+#else
+  _maxBufferSize = 32;
+#endif
+}
+
+/*!
+ *    @brief  Initializes and does basic address detection
+ *    @param  addr_detect Whether we should attempt to detect the I2C address
+ * with a scan. 99% of sensors/devices don't mind but once in a while, they spaz
+ * on a scan!
+ *    @return True if I2C initialized and a device with the addr found
+ */
+bool Adafruit_I2CDevice::begin(bool addr_detect) {
+  _wire->begin();
+  _begun = true;
+
+  if (addr_detect) {
+    return detected();
+  }
+  return true;
+}
+
+/*!
+ *    @brief  Scans I2C for the address - note will give a false-positive
+ *    if there's no pullups on I2C
+ *    @return True if I2C initialized and a device with the addr found
+ */
+bool Adafruit_I2CDevice::detected(void) {
+  // Init I2C if not done yet
+  if (!_begun && !begin()) {
+    return false;
+  }
+
+  // A basic scanner, see if it ACK's
+  _wire->beginTransmission(_addr);
+  if (_wire->endTransmission() == 0) {
+#ifdef DEBUG_SERIAL
+    DEBUG_SERIAL.println(F("Detected"));
+#endif
+    return true;
+  }
+#ifdef DEBUG_SERIAL
+  DEBUG_SERIAL.println(F("Not detected"));
+#endif
+  return false;
+}
+
+/*!
+ *    @brief  Write a buffer or two to the I2C device. Cannot be more than
+ * maxBufferSize() bytes.
+ *    @param  buffer Pointer to buffer of data to write. This is const to
+ *            ensure the content of this buffer doesn't change.
+ *    @param  len Number of bytes from buffer to write
+ *    @param  prefix_buffer Pointer to optional array of data to write before
+ * buffer. Cannot be more than maxBufferSize() bytes. This is const to
+ *            ensure the content of this buffer doesn't change.
+ *    @param  prefix_len Number of bytes from prefix buffer to write
+ *    @param  stop Whether to send an I2C STOP signal on write
+ *    @return True if write was successful, otherwise false.
+ */
+bool Adafruit_I2CDevice::write(const uint8_t *buffer, size_t len, bool stop,
+                               const uint8_t *prefix_buffer,
+                               size_t prefix_len) {
+  _wire->beginTransmission(_addr);
+
+  // Write the prefix data (usually an address)
+  // This is required to be less than _maxBufferSize, so no need to chunkify
+  if ((prefix_len != 0) && (prefix_buffer != NULL)) {
+    if (_wire->write(prefix_buffer, prefix_len) != prefix_len) {
+#ifdef DEBUG_SERIAL
+      DEBUG_SERIAL.println(F("\tI2CDevice failed to write"));
+#endif
+      return false;
+    }
+  }
+
+  // Write the data itself, chunkify if needed
+  size_t bufferSize = maxBufferSize();
+  if (bufferSize >= len) {
+    // can just write
+    if (_wire->write(buffer, len) != len) {
+#ifdef DEBUG_SERIAL
+      DEBUG_SERIAL.println(F("\tI2CDevice failed to write"));
+#endif
+      return false;
+    }
+  } else {
+    // must chunkify
+    size_t pos = 0;
+    uint8_t write_buffer[bufferSize];
+    while (pos < len) {
+      size_t write_len = len - pos > bufferSize ? bufferSize : len - pos;
+      for (size_t i = 0; i < write_len; i++)
+        write_buffer[i] = buffer[pos++];
+      if (_wire->write(write_buffer, write_len) != write_len) {
+#ifdef DEBUG_SERIAL
+        DEBUG_SERIAL.println(F("\tI2CDevice failed to write"));
+#endif
+        return false;
+      }
+    }
+  }
+#ifdef DEBUG_SERIAL
+
+  DEBUG_SERIAL.print(F("\tI2CWRITE @ 0x"));
+  DEBUG_SERIAL.print(_addr, HEX);
+  DEBUG_SERIAL.print(F(" :: "));
+  if ((prefix_len != 0) && (prefix_buffer != NULL)) {
+    for (uint16_t i = 0; i < prefix_len; i++) {
+      DEBUG_SERIAL.print(F("0x"));
+      DEBUG_SERIAL.print(prefix_buffer[i], HEX);
+      DEBUG_SERIAL.print(F(", "));
+    }
+  }
+  for (uint16_t i = 0; i < len; i++) {
+    DEBUG_SERIAL.print(F("0x"));
+    DEBUG_SERIAL.print(buffer[i], HEX);
+    DEBUG_SERIAL.print(F(", "));
+    if (i % 32 == 31) {
+      DEBUG_SERIAL.println();
+    }
+  }
+  DEBUG_SERIAL.println();
+#endif
+
+#ifdef DEBUG_SERIAL
+  DEBUG_SERIAL.print("Stop: ");
+  DEBUG_SERIAL.println(stop);
+#endif
+
+  if (_wire->endTransmission(stop) == 0) {
+#ifdef DEBUG_SERIAL
+    DEBUG_SERIAL.println("Sent!");
+#endif
+    return true;
+  } else {
+#ifdef DEBUG_SERIAL
+    DEBUG_SERIAL.println("Failed to send!");
+#endif
+    return false;
+  }
+}
+
+/*!
+ *    @brief  Read from I2C into a buffer from the I2C device.
+ *    Cannot be more than maxBufferSize() bytes.
+ *    @param  buffer Pointer to buffer of data to read into
+ *    @param  len Number of bytes from buffer to read.
+ *    @param  stop Whether to send an I2C STOP signal on read
+ *    @return True if read was successful, otherwise false.
+ */
+bool Adafruit_I2CDevice::read(uint8_t *buffer, size_t len, bool stop) {
+  size_t bufferSize = maxBufferSize();
+  if (bufferSize >= len) {
+    // can just read
+    return _read(buffer, len, stop);
+  } else {
+    // must chunkify
+    size_t pos = 0;
+    uint8_t read_buffer[bufferSize];
+    while (pos < len) {
+      size_t read_len = len - pos > bufferSize ? bufferSize : len - pos;
+      if (!_read(read_buffer, read_len, false)) {
+        return false;
+      }
+      for (size_t i = 0; i < read_len; i++)
+        buffer[pos++] = read_buffer[i];
+    }
+    return true;
+  }
+}
+
+bool Adafruit_I2CDevice::_read(uint8_t *buffer, size_t len, bool stop) {
+#if defined(TinyWireM_h)
+  size_t recv = _wire->requestFrom((uint8_t)_addr, (uint8_t)len);
+#else
+  size_t recv = _wire->requestFrom((uint8_t)_addr, (uint8_t)len, (uint8_t)stop);
+#endif
+
+  if (recv != len) {
+    // Not enough data available to fulfill our obligation!
+#ifdef DEBUG_SERIAL
+    DEBUG_SERIAL.print(F("\tI2CDevice did not receive enough data: "));
+    DEBUG_SERIAL.println(recv);
+#endif
+    return false;
+  }
+
+  for (uint16_t i = 0; i < len; i++) {
+    buffer[i] = _wire->read();
+  }
+
+#ifdef DEBUG_SERIAL
+  DEBUG_SERIAL.print(F("\tI2CREAD  @ 0x"));
+  DEBUG_SERIAL.print(_addr, HEX);
+  DEBUG_SERIAL.print(F(" :: "));
+  for (uint16_t i = 0; i < len; i++) {
+    DEBUG_SERIAL.print(F("0x"));
+    DEBUG_SERIAL.print(buffer[i], HEX);
+    DEBUG_SERIAL.print(F(", "));
+    if (len % 32 == 31) {
+      DEBUG_SERIAL.println();
+    }
+  }
+  DEBUG_SERIAL.println();
+#endif
+
+  return true;
+}
+
+/*!
+ *    @brief  Write some data, then read some data from I2C into another buffer.
+ *    Cannot be more than maxBufferSize() bytes. The buffers can point to
+ *    same/overlapping locations.
+ *    @param  write_buffer Pointer to buffer of data to write from
+ *    @param  write_len Number of bytes from buffer to write.
+ *    @param  read_buffer Pointer to buffer of data to read into.
+ *    @param  read_len Number of bytes from buffer to read.
+ *    @param  stop Whether to send an I2C STOP signal between the write and read
+ *    @return True if write & read was successful, otherwise false.
+ */
+bool Adafruit_I2CDevice::write_then_read(const uint8_t *write_buffer,
+                                         size_t write_len, uint8_t *read_buffer,
+                                         size_t read_len, bool stop) {
+  if (!write(write_buffer, write_len, stop)) {
+    return false;
+  }
+
+  return read(read_buffer, read_len);
+}
+
+/*!
+ *    @brief  Returns the 7-bit address of this device
+ *    @return The 7-bit address of this device
+ */
+uint8_t Adafruit_I2CDevice::address(void) { return _addr; }
+
+/*!
+ *    @brief  Change the I2C clock speed to desired (relies on
+ *    underlying Wire support!
+ *    @param desiredclk The desired I2C SCL frequency
+ *    @return True if this platform supports changing I2C speed.
+ *    Not necessarily that the speed was achieved!
+ */
+bool Adafruit_I2CDevice::setSpeed(uint32_t desiredclk) {
+#if (ARDUINO >= 157) && !defined(ARDUINO_STM32_FEATHER) && !defined(TinyWireM_h)
+  _wire->setClock(desiredclk);
+  return true;
+#else
+  (void)desiredclk;
+  return false;
+#endif
+}

Embedded/libraries/Adafruit_BusIO/Adafruit_I2CDevice.h

@@ -0,0 +1,34 @@
+#include <Wire.h>
+
+#ifndef Adafruit_I2CDevice_h
+#define Adafruit_I2CDevice_h
+
+///< The class which defines how we will talk to this device over I2C
+class Adafruit_I2CDevice {
+public:
+  Adafruit_I2CDevice(uint8_t addr, TwoWire *theWire = &Wire);
+  uint8_t address(void);
+  bool begin(bool addr_detect = true);
+  bool detected(void);
+
+  bool read(uint8_t *buffer, size_t len, bool stop = true);
+  bool write(const uint8_t *buffer, size_t len, bool stop = true,
+             const uint8_t *prefix_buffer = NULL, size_t prefix_len = 0);
+  bool write_then_read(const uint8_t *write_buffer, size_t write_len,
+                       uint8_t *read_buffer, size_t read_len,
+                       bool stop = false);
+  bool setSpeed(uint32_t desiredclk);
+
+  /*!   @brief  How many bytes we can read in a transaction
+   *    @return The size of the Wire receive/transmit buffer */
+  size_t maxBufferSize() { return _maxBufferSize; }
+
+private:
+  uint8_t _addr;
+  TwoWire *_wire;
+  bool _begun;
+  size_t _maxBufferSize;
+  bool _read(uint8_t *buffer, size_t len, bool stop);
+};
+
+#endif // Adafruit_I2CDevice_h

Embedded/libraries/Adafruit_BusIO/Adafruit_I2CRegister.h

@@ -0,0 +1,8 @@
+#include "Adafruit_BusIO_Register.h"
+#ifndef _ADAFRUIT_I2C_REGISTER_H_
+#define _ADAFRUIT_I2C_REGISTER_H_
+
+typedef Adafruit_BusIO_Register Adafruit_I2CRegister;
+typedef Adafruit_BusIO_RegisterBits Adafruit_I2CRegisterBits;
+
+#endif

Embedded/libraries/Adafruit_BusIO/Adafruit_SPIDevice.cpp

@@ -0,0 +1,444 @@
+#include <Adafruit_SPIDevice.h>
+#include <Arduino.h>
+
+#if !defined(SPI_INTERFACES_COUNT) ||                                          \
+    (defined(SPI_INTERFACES_COUNT) && (SPI_INTERFACES_COUNT > 0))
+
+//#define DEBUG_SERIAL Serial
+
+/*!
+ *    @brief  Create an SPI device with the given CS pin and settins
+ *    @param  cspin The arduino pin number to use for chip select
+ *    @param  freq The SPI clock frequency to use, defaults to 1MHz
+ *    @param  dataOrder The SPI data order to use for bits within each byte,
+ * defaults to SPI_BITORDER_MSBFIRST
+ *    @param  dataMode The SPI mode to use, defaults to SPI_MODE0
+ *    @param  theSPI The SPI bus to use, defaults to &theSPI
+ */
+Adafruit_SPIDevice::Adafruit_SPIDevice(int8_t cspin, uint32_t freq,
+                                       BusIOBitOrder dataOrder,
+                                       uint8_t dataMode, SPIClass *theSPI) {
+  _cs = cspin;
+  _sck = _mosi = _miso = -1;
+  _spi = theSPI;
+  _begun = false;
+  _spiSetting = new SPISettings(freq, dataOrder, dataMode);
+  _freq = freq;
+  _dataOrder = dataOrder;
+  _dataMode = dataMode;
+}
+
+/*!
+ *    @brief  Create an SPI device with the given CS pin and settins
+ *    @param  cspin The arduino pin number to use for chip select
+ *    @param  sckpin The arduino pin number to use for SCK
+ *    @param  misopin The arduino pin number to use for MISO, set to -1 if not
+ * used
+ *    @param  mosipin The arduino pin number to use for MOSI, set to -1 if not
+ * used
+ *    @param  freq The SPI clock frequency to use, defaults to 1MHz
+ *    @param  dataOrder The SPI data order to use for bits within each byte,
+ * defaults to SPI_BITORDER_MSBFIRST
+ *    @param  dataMode The SPI mode to use, defaults to SPI_MODE0
+ */
+Adafruit_SPIDevice::Adafruit_SPIDevice(int8_t cspin, int8_t sckpin,
+                                       int8_t misopin, int8_t mosipin,
+                                       uint32_t freq, BusIOBitOrder dataOrder,
+                                       uint8_t dataMode) {
+  _cs = cspin;
+  _sck = sckpin;
+  _miso = misopin;
+  _mosi = mosipin;
+
+#ifdef BUSIO_USE_FAST_PINIO
+  csPort = (BusIO_PortReg *)portOutputRegister(digitalPinToPort(cspin));
+  csPinMask = digitalPinToBitMask(cspin);
+  if (mosipin != -1) {
+    mosiPort = (BusIO_PortReg *)portOutputRegister(digitalPinToPort(mosipin));
+    mosiPinMask = digitalPinToBitMask(mosipin);
+  }
+  if (misopin != -1) {
+    misoPort = (BusIO_PortReg *)portInputRegister(digitalPinToPort(misopin));
+    misoPinMask = digitalPinToBitMask(misopin);
+  }
+  clkPort = (BusIO_PortReg *)portOutputRegister(digitalPinToPort(sckpin));
+  clkPinMask = digitalPinToBitMask(sckpin);
+#endif
+
+  _freq = freq;
+  _dataOrder = dataOrder;
+  _dataMode = dataMode;
+  _begun = false;
+  _spiSetting = new SPISettings(freq, dataOrder, dataMode);
+  _spi = NULL;
+}
+
+/*!
+ *    @brief  Release memory allocated in constructors
+ */
+Adafruit_SPIDevice::~Adafruit_SPIDevice() {
+  if (_spiSetting) {
+    delete _spiSetting;
+    _spiSetting = nullptr;
+  }
+}
+
+/*!
+ *    @brief  Initializes SPI bus and sets CS pin high
+ *    @return Always returns true because there's no way to test success of SPI
+ * init
+ */
+bool Adafruit_SPIDevice::begin(void) {
+  pinMode(_cs, OUTPUT);
+  digitalWrite(_cs, HIGH);
+
+  if (_spi) { // hardware SPI
+    _spi->begin();
+  } else {
+    pinMode(_sck, OUTPUT);
+
+    if ((_dataMode == SPI_MODE0) || (_dataMode == SPI_MODE1)) {
+      // idle low on mode 0 and 1
+      digitalWrite(_sck, LOW);
+    } else {
+      // idle high on mode 2 or 3
+      digitalWrite(_sck, HIGH);
+    }
+    if (_mosi != -1) {
+      pinMode(_mosi, OUTPUT);
+      digitalWrite(_mosi, HIGH);
+    }
+    if (_miso != -1) {
+      pinMode(_miso, INPUT);
+    }
+  }
+
+  _begun = true;
+  return true;
+}
+
+/*!
+ *    @brief  Transfer (send/receive) one byte over hard/soft SPI
+ *    @param  buffer The buffer to send and receive at the same time
+ *    @param  len    The number of bytes to transfer
+ */
+void Adafruit_SPIDevice::transfer(uint8_t *buffer, size_t len) {
+  if (_spi) {
+    // hardware SPI is easy
+
+#if defined(SPARK)
+    _spi->transfer(buffer, buffer, len, NULL);
+#elif defined(STM32)
+    for (size_t i = 0; i < len; i++) {
+      _spi->transfer(buffer[i]);
+    }
+#else
+    _spi->transfer(buffer, len);
+#endif
+    return;
+  }
+
+  uint8_t startbit;
+  if (_dataOrder == SPI_BITORDER_LSBFIRST) {
+    startbit = 0x1;
+  } else {
+    startbit = 0x80;
+  }
+
+  bool towrite, lastmosi = !(buffer[0] & startbit);
+  uint8_t bitdelay_us = (1000000 / _freq) / 2;
+
+  // for softSPI we'll do it by hand
+  for (size_t i = 0; i < len; i++) {
+    // software SPI
+    uint8_t reply = 0;
+    uint8_t send = buffer[i];
+
+    /*
+    Serial.print("\tSending software SPI byte 0x");
+    Serial.print(send, HEX);
+    Serial.print(" -> 0x");
+    */
+
+    // Serial.print(send, HEX);
+    for (uint8_t b = startbit; b != 0;
+         b = (_dataOrder == SPI_BITORDER_LSBFIRST) ? b << 1 : b >> 1) {
+
+      if (bitdelay_us) {
+        delayMicroseconds(bitdelay_us);
+      }
+
+      if (_dataMode == SPI_MODE0 || _dataMode == SPI_MODE2) {
+        towrite = send & b;
+        if ((_mosi != -1) && (lastmosi != towrite)) {
+#ifdef BUSIO_USE_FAST_PINIO
+          if (towrite)
+            *mosiPort |= mosiPinMask;
+          else
+            *mosiPort &= ~mosiPinMask;
+#else
+          digitalWrite(_mosi, towrite);
+#endif
+          lastmosi = towrite;
+        }
+
+#ifdef BUSIO_USE_FAST_PINIO
+        *clkPort |= clkPinMask; // Clock high
+#else
+        digitalWrite(_sck, HIGH);
+#endif
+
+        if (bitdelay_us) {
+          delayMicroseconds(bitdelay_us);
+        }
+
+        if (_miso != -1) {
+#ifdef BUSIO_USE_FAST_PINIO
+          if (*misoPort & misoPinMask) {
+#else
+          if (digitalRead(_miso)) {
+#endif
+            reply |= b;
+          }
+        }
+
+#ifdef BUSIO_USE_FAST_PINIO
+        *clkPort &= ~clkPinMask; // Clock low
+#else
+        digitalWrite(_sck, LOW);
+#endif
+      } else { // if (_dataMode == SPI_MODE1 || _dataMode == SPI_MODE3)
+
+#ifdef BUSIO_USE_FAST_PINIO
+        *clkPort |= clkPinMask; // Clock high
+#else
+        digitalWrite(_sck, HIGH);
+#endif
+
+        if (bitdelay_us) {
+          delayMicroseconds(bitdelay_us);
+        }
+
+        if (_mosi != -1) {
+#ifdef BUSIO_USE_FAST_PINIO
+          if (send & b)
+            *mosiPort |= mosiPinMask;
+          else
+            *mosiPort &= ~mosiPinMask;
+#else
+          digitalWrite(_mosi, send & b);
+#endif
+        }
+
+#ifdef BUSIO_USE_FAST_PINIO
+        *clkPort &= ~clkPinMask; // Clock low
+#else
+        digitalWrite(_sck, LOW);
+#endif
+
+        if (_miso != -1) {
+#ifdef BUSIO_USE_FAST_PINIO
+          if (*misoPort & misoPinMask) {
+#else
+          if (digitalRead(_miso)) {
+#endif
+            reply |= b;
+          }
+        }
+      }
+      if (_miso != -1) {
+        buffer[i] = reply;
+      }
+    }
+  }
+  return;
+}
+
+/*!
+ *    @brief  Transfer (send/receive) one byte over hard/soft SPI
+ *    @param  send The byte to send
+ *    @return The byte received while transmitting
+ */
+uint8_t Adafruit_SPIDevice::transfer(uint8_t send) {
+  uint8_t data = send;
+  transfer(&data, 1);
+  return data;
+}
+
+/*!
+ *    @brief  Manually begin a transaction (calls beginTransaction if hardware
+ * SPI)
+ */
+void Adafruit_SPIDevice::beginTransaction(void) {
+  if (_spi) {
+    _spi->beginTransaction(*_spiSetting);
+  }
+}
+
+/*!
+ *    @brief  Manually end a transaction (calls endTransaction if hardware SPI)
+ */
+void Adafruit_SPIDevice::endTransaction(void) {
+  if (_spi) {
+    _spi->endTransaction();
+  }
+}
+
+/*!
+ *    @brief  Write a buffer or two to the SPI device.
+ *    @param  buffer Pointer to buffer of data to write
+ *    @param  len Number of bytes from buffer to write
+ *    @param  prefix_buffer Pointer to optional array of data to write before
+ * buffer.
+ *    @param  prefix_len Number of bytes from prefix buffer to write
+ *    @return Always returns true because there's no way to test success of SPI
+ * writes
+ */
+bool Adafruit_SPIDevice::write(uint8_t *buffer, size_t len,
+                               uint8_t *prefix_buffer, size_t prefix_len) {
+  if (_spi) {
+    _spi->beginTransaction(*_spiSetting);
+  }
+
+  digitalWrite(_cs, LOW);
+  // do the writing
+  for (size_t i = 0; i < prefix_len; i++) {
+    transfer(prefix_buffer[i]);
+  }
+  for (size_t i = 0; i < len; i++) {
+    transfer(buffer[i]);
+  }
+  digitalWrite(_cs, HIGH);
+
+  if (_spi) {
+    _spi->endTransaction();
+  }
+
+#ifdef DEBUG_SERIAL
+  DEBUG_SERIAL.print(F("\tSPIDevice Wrote: "));
+  if ((prefix_len != 0) && (prefix_buffer != NULL)) {
+    for (uint16_t i = 0; i < prefix_len; i++) {
+      DEBUG_SERIAL.print(F("0x"));
+      DEBUG_SERIAL.print(prefix_buffer[i], HEX);
+      DEBUG_SERIAL.print(F(", "));
+    }
+  }
+  for (uint16_t i = 0; i < len; i++) {
+    DEBUG_SERIAL.print(F("0x"));
+    DEBUG_SERIAL.print(buffer[i], HEX);
+    DEBUG_SERIAL.print(F(", "));
+    if (i % 32 == 31) {
+      DEBUG_SERIAL.println();
+    }
+  }
+  DEBUG_SERIAL.println();
+#endif
+
+  return true;
+}
+
+/*!
+ *    @brief  Read from SPI into a buffer from the SPI device.
+ *    @param  buffer Pointer to buffer of data to read into
+ *    @param  len Number of bytes from buffer to read.
+ *    @param  sendvalue The 8-bits of data to write when doing the data read,
+ * defaults to 0xFF
+ *    @return Always returns true because there's no way to test success of SPI
+ * writes
+ */
+bool Adafruit_SPIDevice::read(uint8_t *buffer, size_t len, uint8_t sendvalue) {
+  memset(buffer, sendvalue, len); // clear out existing buffer
+  if (_spi) {
+    _spi->beginTransaction(*_spiSetting);
+  }
+  digitalWrite(_cs, LOW);
+  transfer(buffer, len);
+  digitalWrite(_cs, HIGH);
+
+  if (_spi) {
+    _spi->endTransaction();
+  }
+
+#ifdef DEBUG_SERIAL
+  DEBUG_SERIAL.print(F("\tSPIDevice Read: "));
+  for (uint16_t i = 0; i < len; i++) {
+    DEBUG_SERIAL.print(F("0x"));
+    DEBUG_SERIAL.print(buffer[i], HEX);
+    DEBUG_SERIAL.print(F(", "));
+    if (len % 32 == 31) {
+      DEBUG_SERIAL.println();
+    }
+  }
+  DEBUG_SERIAL.println();
+#endif
+
+  return true;
+}
+
+/*!
+ *    @brief  Write some data, then read some data from SPI into another buffer.
+ * The buffers can point to same/overlapping locations. This does not
+ * transmit-receive at the same time!
+ *    @param  write_buffer Pointer to buffer of data to write from
+ *    @param  write_len Number of bytes from buffer to write.
+ *    @param  read_buffer Pointer to buffer of data to read into.
+ *    @param  read_len Number of bytes from buffer to read.
+ *    @param  sendvalue The 8-bits of data to write when doing the data read,
+ * defaults to 0xFF
+ *    @return Always returns true because there's no way to test success of SPI
+ * writes
+ */
+bool Adafruit_SPIDevice::write_then_read(uint8_t *write_buffer,
+                                         size_t write_len, uint8_t *read_buffer,
+                                         size_t read_len, uint8_t sendvalue) {
+  if (_spi) {
+    _spi->beginTransaction(*_spiSetting);
+  }
+
+  digitalWrite(_cs, LOW);
+  // do the writing
+  for (size_t i = 0; i < write_len; i++) {
+    transfer(write_buffer[i]);
+  }
+
+#ifdef DEBUG_SERIAL
+  DEBUG_SERIAL.print(F("\tSPIDevice Wrote: "));
+  for (uint16_t i = 0; i < write_len; i++) {
+    DEBUG_SERIAL.print(F("0x"));
+    DEBUG_SERIAL.print(write_buffer[i], HEX);
+    DEBUG_SERIAL.print(F(", "));
+    if (write_len % 32 == 31) {
+      DEBUG_SERIAL.println();
+    }
+  }
+  DEBUG_SERIAL.println();
+#endif
+
+  // do the reading
+  for (size_t i = 0; i < read_len; i++) {
+    read_buffer[i] = transfer(sendvalue);
+  }
+
+#ifdef DEBUG_SERIAL
+  DEBUG_SERIAL.print(F("\tSPIDevice Read: "));
+  for (uint16_t i = 0; i < read_len; i++) {
+    DEBUG_SERIAL.print(F("0x"));
+    DEBUG_SERIAL.print(read_buffer[i], HEX);
+    DEBUG_SERIAL.print(F(", "));
+    if (read_len % 32 == 31) {
+      DEBUG_SERIAL.println();
+    }
+  }
+  DEBUG_SERIAL.println();
+#endif
+
+  digitalWrite(_cs, HIGH);
+
+  if (_spi) {
+    _spi->endTransaction();
+  }
+
+  return true;
+}
+
+#endif // SPI exists

Embedded/libraries/Adafruit_BusIO/Adafruit_SPIDevice.h

@@ -0,0 +1,105 @@
+#if !defined(SPI_INTERFACES_COUNT) ||                                          \
+    (defined(SPI_INTERFACES_COUNT) && (SPI_INTERFACES_COUNT > 0))
+
+#include <SPI.h>
+
+#ifndef Adafruit_SPIDevice_h
+#define Adafruit_SPIDevice_h
+
+// some modern SPI definitions don't have BitOrder enum
+#if (defined(__AVR__) && !defined(ARDUINO_ARCH_MEGAAVR)) ||                    \
+    defined(ESP8266) || defined(TEENSYDUINO) || defined(SPARK) ||              \
+    defined(ARDUINO_ARCH_SPRESENSE) || defined(MEGATINYCORE) ||                \
+    defined(DXCORE) || defined(ARDUINO_AVR_ATmega4809) ||                      \
+    defined(ARDUINO_AVR_ATmega4808) || defined(ARDUINO_AVR_ATmega3209) ||      \
+    defined(ARDUINO_AVR_ATmega3208) || defined(ARDUINO_AVR_ATmega1609) ||      \
+    defined(ARDUINO_AVR_ATmega1608) || defined(ARDUINO_AVR_ATmega809) ||       \
+    defined(ARDUINO_AVR_ATmega808) || defined(ARDUINO_ARCH_ARC32)
+
+typedef enum _BitOrder {
+  SPI_BITORDER_MSBFIRST = MSBFIRST,
+  SPI_BITORDER_LSBFIRST = LSBFIRST,
+} BusIOBitOrder;
+
+#elif defined(ESP32) || defined(__ASR6501__)
+
+// some modern SPI definitions don't have BitOrder enum and have different SPI
+// mode defines
+typedef enum _BitOrder {
+  SPI_BITORDER_MSBFIRST = SPI_MSBFIRST,
+  SPI_BITORDER_LSBFIRST = SPI_LSBFIRST,
+} BusIOBitOrder;
+
+#else
+// Some platforms have a BitOrder enum but its named MSBFIRST/LSBFIRST
+#define SPI_BITORDER_MSBFIRST MSBFIRST
+#define SPI_BITORDER_LSBFIRST LSBFIRST
+typedef BitOrder BusIOBitOrder;
+#endif
+
+#if defined(__AVR__) || defined(TEENSYDUINO)
+typedef volatile uint8_t BusIO_PortReg;
+typedef uint8_t BusIO_PortMask;
+#define BUSIO_USE_FAST_PINIO
+
+#elif defined(ESP8266) || defined(ESP32) || defined(__SAM3X8E__) ||            \
+    defined(ARDUINO_ARCH_SAMD)
+typedef volatile uint32_t BusIO_PortReg;
+typedef uint32_t BusIO_PortMask;
+#define BUSIO_USE_FAST_PINIO
+
+#elif (defined(__arm__) || defined(ARDUINO_FEATHER52)) &&                      \
+    !defined(ARDUINO_ARCH_MBED) && !defined(ARDUINO_ARCH_RP2040)
+typedef volatile uint32_t BusIO_PortReg;
+typedef uint32_t BusIO_PortMask;
+#if not defined(__ASR6501__)
+#define BUSIO_USE_FAST_PINIO
+#endif
+
+#else
+#undef BUSIO_USE_FAST_PINIO
+#endif
+
+/**! The class which defines how we will talk to this device over SPI **/
+class Adafruit_SPIDevice {
+public:
+  Adafruit_SPIDevice(int8_t cspin, uint32_t freq = 1000000,
+                     BusIOBitOrder dataOrder = SPI_BITORDER_MSBFIRST,
+                     uint8_t dataMode = SPI_MODE0, SPIClass *theSPI = &SPI);
+
+  Adafruit_SPIDevice(int8_t cspin, int8_t sck, int8_t miso, int8_t mosi,
+                     uint32_t freq = 1000000,
+                     BusIOBitOrder dataOrder = SPI_BITORDER_MSBFIRST,
+                     uint8_t dataMode = SPI_MODE0);
+  ~Adafruit_SPIDevice();
+
+  bool begin(void);
+  bool read(uint8_t *buffer, size_t len, uint8_t sendvalue = 0xFF);
+  bool write(uint8_t *buffer, size_t len, uint8_t *prefix_buffer = NULL,
+             size_t prefix_len = 0);
+  bool write_then_read(uint8_t *write_buffer, size_t write_len,
+                       uint8_t *read_buffer, size_t read_len,
+                       uint8_t sendvalue = 0xFF);
+
+  uint8_t transfer(uint8_t send);
+  void transfer(uint8_t *buffer, size_t len);
+  void beginTransaction(void);
+  void endTransaction(void);
+
+private:
+  SPIClass *_spi;
+  SPISettings *_spiSetting;
+  uint32_t _freq;
+  BusIOBitOrder _dataOrder;
+  uint8_t _dataMode;
+
+  int8_t _cs, _sck, _mosi, _miso;
+#ifdef BUSIO_USE_FAST_PINIO
+  BusIO_PortReg *mosiPort, *clkPort, *misoPort, *csPort;
+  BusIO_PortMask mosiPinMask, misoPinMask, clkPinMask, csPinMask;
+#endif
+  bool _begun;
+};
+
+#endif // Adafruit_SPIDevice_h
+#endif // has SPI defined

Embedded/libraries/Adafruit_BusIO/LICENSE

@@ -0,0 +1,21 @@
+The MIT License (MIT)
+
+Copyright (c) 2017 Adafruit Industries
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

Embedded/libraries/Adafruit_BusIO/README.md

@@ -0,0 +1,8 @@
+# Adafruit Bus IO Library [![Build Status](https://github.com/adafruit/Adafruit_BusIO/workflows/Arduino%20Library%20CI/badge.svg)](https://github.com/adafruit/Adafruit_BusIO/actions)
+
+
+This is a helper libary to abstract away I2C & SPI transactions and registers 
+
+Adafruit invests time and resources providing this open source code, please support Adafruit and open-source hardware by purchasing products from Adafruit!
+
+MIT license, all text above must be included in any redistribution

Embedded/libraries/Adafruit_BusIO/examples/i2c_address_detect/i2c_address_detect.ino

@@ -0,0 +1,21 @@
+#include <Adafruit_I2CDevice.h>
+
+Adafruit_I2CDevice i2c_dev = Adafruit_I2CDevice(0x10);
+
+void setup() {
+  while (!Serial) { delay(10); }
+  Serial.begin(115200);
+  Serial.println("I2C address detection test");
+
+  if (!i2c_dev.begin()) {
+    Serial.print("Did not find device at 0x");
+    Serial.println(i2c_dev.address(), HEX);
+    while (1);
+  }
+  Serial.print("Device found on address 0x");
+  Serial.println(i2c_dev.address(), HEX);
+}
+
+void loop() {
+  
+}

Embedded/libraries/Adafruit_BusIO/examples/i2c_readwrite/i2c_readwrite.ino

@@ -0,0 +1,41 @@
+#include <Adafruit_I2CDevice.h>
+
+#define I2C_ADDRESS 0x60
+Adafruit_I2CDevice i2c_dev = Adafruit_I2CDevice(I2C_ADDRESS);
+
+
+void setup() {
+  while (!Serial) { delay(10); }
+  Serial.begin(115200);
+  Serial.println("I2C device read and write test");
+
+  if (!i2c_dev.begin()) {
+    Serial.print("Did not find device at 0x");
+    Serial.println(i2c_dev.address(), HEX);
+    while (1);
+  }
+  Serial.print("Device found on address 0x");
+  Serial.println(i2c_dev.address(), HEX);
+
+  uint8_t buffer[32];
+  // Try to read 32 bytes
+  i2c_dev.read(buffer, 32);
+  Serial.print("Read: ");
+  for (uint8_t i=0; i<32; i++) {
+    Serial.print("0x"); Serial.print(buffer[i], HEX); Serial.print(", ");
+  }
+  Serial.println();
+
+  // read a register by writing first, then reading
+  buffer[0] = 0x0C;  // we'll reuse the same buffer
+  i2c_dev.write_then_read(buffer, 1, buffer, 2, false);
+  Serial.print("Write then Read: ");
+  for (uint8_t i=0; i<2; i++) {
+    Serial.print("0x"); Serial.print(buffer[i], HEX); Serial.print(", ");
+  }
+  Serial.println();
+}
+
+void loop() {
+  
+}

Embedded/libraries/Adafruit_BusIO/examples/i2c_registers/i2c_registers.ino

@@ -0,0 +1,38 @@
+#include <Adafruit_I2CDevice.h>
+#include <Adafruit_BusIO_Register.h>
+
+#define I2C_ADDRESS 0x60
+Adafruit_I2CDevice i2c_dev = Adafruit_I2CDevice(I2C_ADDRESS);
+
+
+void setup() {
+  while (!Serial) { delay(10); }
+  Serial.begin(115200);
+  Serial.println("I2C device register test");
+
+  if (!i2c_dev.begin()) {
+    Serial.print("Did not find device at 0x");
+    Serial.println(i2c_dev.address(), HEX);
+    while (1);
+  }
+  Serial.print("Device found on address 0x");
+  Serial.println(i2c_dev.address(), HEX);
+
+  Adafruit_BusIO_Register id_reg = Adafruit_BusIO_Register(&i2c_dev, 0x0C, 2, LSBFIRST);
+  uint16_t id;
+  id_reg.read(&id);
+  Serial.print("ID register = 0x"); Serial.println(id, HEX);
+
+  Adafruit_BusIO_Register thresh_reg = Adafruit_BusIO_Register(&i2c_dev, 0x01, 2, LSBFIRST);
+  uint16_t thresh;
+  thresh_reg.read(&thresh);
+  Serial.print("Initial threshold register = 0x"); Serial.println(thresh, HEX);
+
+  thresh_reg.write(~thresh);
+
+  Serial.print("Post threshold register = 0x"); Serial.println(thresh_reg.read(), HEX);
+}
+
+void loop() {
+  
+}

Embedded/libraries/Adafruit_BusIO/examples/i2corspi_register/i2corspi_register.ino

@@ -0,0 +1,38 @@
+#include <Adafruit_BusIO_Register.h>
+
+// Define which interface to use by setting the unused interface to NULL!
+
+#define SPIDEVICE_CS 10
+Adafruit_SPIDevice *spi_dev = NULL; // new Adafruit_SPIDevice(SPIDEVICE_CS);
+
+#define I2C_ADDRESS 0x5D
+Adafruit_I2CDevice *i2c_dev = new Adafruit_I2CDevice(I2C_ADDRESS);
+
+void setup() {
+  while (!Serial) { delay(10); }
+  Serial.begin(115200);
+  Serial.println("I2C or SPI device register test");
+
+  if (spi_dev && !spi_dev->begin()) {
+    Serial.println("Could not initialize SPI device");
+  }
+
+  if (i2c_dev) {
+    if (i2c_dev->begin()) {
+      Serial.print("Device found on I2C address 0x");
+      Serial.println(i2c_dev->address(), HEX);
+    } else {
+      Serial.print("Did not find I2C device at 0x");
+      Serial.println(i2c_dev->address(), HEX);
+    }
+  }
+
+  Adafruit_BusIO_Register id_reg = Adafruit_BusIO_Register(i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, 0x0F);
+  uint8_t id=0;
+  id_reg.read(&id);
+  Serial.print("ID register = 0x"); Serial.println(id, HEX);
+}
+
+void loop() {
+  
+}

Embedded/libraries/Adafruit_BusIO/examples/spi_modetest/spi_modetest.ino

@@ -0,0 +1,29 @@
+#include <Adafruit_SPIDevice.h>
+
+#define SPIDEVICE_CS 10
+Adafruit_SPIDevice spi_dev = Adafruit_SPIDevice(SPIDEVICE_CS, 100000, SPI_BITORDER_MSBFIRST, SPI_MODE1);
+//Adafruit_SPIDevice spi_dev = Adafruit_SPIDevice(SPIDEVICE_CS, 13, 12, 11, 100000, SPI_BITORDER_MSBFIRST, SPI_MODE1);
+
+
+void setup() {
+  while (!Serial) { delay(10); }
+  Serial.begin(115200);
+  Serial.println("SPI device mode test");
+
+  if (!spi_dev.begin()) {
+    Serial.println("Could not initialize SPI device");
+    while (1);
+  }
+}
+
+void loop() {
+  Serial.println("\n\nTransfer test");
+  for (uint16_t x=0; x<=0xFF; x++) {
+    uint8_t i = x;   
+    Serial.print("0x"); Serial.print(i, HEX); 
+    spi_dev.read(&i, 1, i);
+    Serial.print("/"); Serial.print(i, HEX);
+    Serial.print(", ");
+    delay(25);
+  }
+}

Embedded/libraries/Adafruit_BusIO/examples/spi_readwrite/spi_readwrite.ino

@@ -0,0 +1,39 @@
+#include <Adafruit_SPIDevice.h>
+
+#define SPIDEVICE_CS 10
+Adafruit_SPIDevice spi_dev = Adafruit_SPIDevice(SPIDEVICE_CS);
+
+
+void setup() {
+  while (!Serial) { delay(10); }
+  Serial.begin(115200);
+  Serial.println("SPI device read and write test");
+
+  if (!spi_dev.begin()) {
+    Serial.println("Could not initialize SPI device");
+    while (1);
+  }
+
+  uint8_t buffer[32];
+
+  // Try to read 32 bytes
+  spi_dev.read(buffer, 32);
+  Serial.print("Read: ");
+  for (uint8_t i=0; i<32; i++) {
+    Serial.print("0x"); Serial.print(buffer[i], HEX); Serial.print(", ");
+  }
+  Serial.println();
+
+  // read a register by writing first, then reading
+  buffer[0] = 0x8F;  // we'll reuse the same buffer
+  spi_dev.write_then_read(buffer, 1, buffer, 2, false);
+  Serial.print("Write then Read: ");
+  for (uint8_t i=0; i<2; i++) {
+    Serial.print("0x"); Serial.print(buffer[i], HEX); Serial.print(", ");
+  }
+  Serial.println();
+}
+
+void loop() {
+  
+}

Embedded/libraries/Adafruit_BusIO/examples/spi_register_bits/spi_register_bits.ino

@@ -0,0 +1,192 @@
+/*************************************************** 
+
+  This is an example for how to use Adafruit_BusIO_RegisterBits from Adafruit_BusIO library.
+
+  Designed specifically to work with the Adafruit RTD Sensor
+  ----> https://www.adafruit.com/products/3328
+  uisng a MAX31865 RTD-to-Digital Converter
+  ----> https://datasheets.maximintegrated.com/en/ds/MAX31865.pdf
+
+  This sensor uses SPI to communicate, 4 pins are required to  
+  interface.
+  A fifth pin helps to detect when a new conversion is ready.
+
+  Adafruit invests time and resources providing this open source code, 
+  please support Adafruit and open-source hardware by purchasing 
+  products from Adafruit!
+
+  Example written (2020/3) by Andreas Hardtung/AnHard.  
+  BSD license, all text above must be included in any redistribution
+ ****************************************************/
+
+#include <Adafruit_BusIO_Register.h>
+#include <Adafruit_SPIDevice.h>
+
+#define MAX31865_SPI_SPEED    (5000000)
+#define MAX31865_SPI_BITORDER (SPI_BITORDER_MSBFIRST)
+#define MAX31865_SPI_MODE     (SPI_MODE1)
+
+#define MAX31865_SPI_CS       (10)
+#define MAX31865_READY_PIN    (2)
+
+
+Adafruit_SPIDevice spi_dev = Adafruit_SPIDevice( MAX31865_SPI_CS, MAX31865_SPI_SPEED, MAX31865_SPI_BITORDER, MAX31865_SPI_MODE, &SPI); // Hardware SPI
+// Adafruit_SPIDevice spi_dev = Adafruit_SPIDevice( MAX31865_SPI_CS, 13, 12, 11, MAX31865_SPI_SPEED, MAX31865_SPI_BITORDER, MAX31865_SPI_MODE); // Software SPI
+
+// MAX31865 chip related *********************************************************************************************
+Adafruit_BusIO_Register     config_reg  = Adafruit_BusIO_Register(&spi_dev, 0x00, ADDRBIT8_HIGH_TOWRITE, 1, MSBFIRST);
+Adafruit_BusIO_RegisterBits   bias_bit  = Adafruit_BusIO_RegisterBits(&config_reg, 1, 7);
+Adafruit_BusIO_RegisterBits   auto_bit  = Adafruit_BusIO_RegisterBits(&config_reg, 1, 6);
+Adafruit_BusIO_RegisterBits   oneS_bit  = Adafruit_BusIO_RegisterBits(&config_reg, 1, 5);
+Adafruit_BusIO_RegisterBits   wire_bit  = Adafruit_BusIO_RegisterBits(&config_reg, 1, 4);
+Adafruit_BusIO_RegisterBits faultT_bits = Adafruit_BusIO_RegisterBits(&config_reg, 2, 2);
+Adafruit_BusIO_RegisterBits faultR_bit  = Adafruit_BusIO_RegisterBits(&config_reg, 1, 1);
+Adafruit_BusIO_RegisterBits fi50hz_bit  = Adafruit_BusIO_RegisterBits(&config_reg, 1, 0);
+
+Adafruit_BusIO_Register     rRatio_reg  = Adafruit_BusIO_Register(&spi_dev, 0x01, ADDRBIT8_HIGH_TOWRITE, 2, MSBFIRST);
+Adafruit_BusIO_RegisterBits rRatio_bits = Adafruit_BusIO_RegisterBits(&rRatio_reg, 15, 1);
+Adafruit_BusIO_RegisterBits  fault_bit  = Adafruit_BusIO_RegisterBits(&rRatio_reg,  1, 0);
+
+Adafruit_BusIO_Register      maxRratio_reg = Adafruit_BusIO_Register(&spi_dev, 0x03, ADDRBIT8_HIGH_TOWRITE, 2, MSBFIRST);
+Adafruit_BusIO_RegisterBits maxRratio_bits = Adafruit_BusIO_RegisterBits(&maxRratio_reg, 15, 1);
+
+Adafruit_BusIO_Register      minRratio_reg = Adafruit_BusIO_Register(&spi_dev, 0x05, ADDRBIT8_HIGH_TOWRITE, 2, MSBFIRST);
+Adafruit_BusIO_RegisterBits minRratio_bits = Adafruit_BusIO_RegisterBits(&minRratio_reg, 15, 1);
+
+Adafruit_BusIO_Register                fault_reg = Adafruit_BusIO_Register(&spi_dev, 0x07, ADDRBIT8_HIGH_TOWRITE, 1, MSBFIRST);
+Adafruit_BusIO_RegisterBits range_high_fault_bit = Adafruit_BusIO_RegisterBits(&fault_reg, 1, 7);
+Adafruit_BusIO_RegisterBits  range_low_fault_bit = Adafruit_BusIO_RegisterBits(&fault_reg, 1, 6);
+Adafruit_BusIO_RegisterBits refin_high_fault_bit = Adafruit_BusIO_RegisterBits(&fault_reg, 1, 5);
+Adafruit_BusIO_RegisterBits  refin_low_fault_bit = Adafruit_BusIO_RegisterBits(&fault_reg, 1, 4);
+Adafruit_BusIO_RegisterBits  rtdin_low_fault_bit = Adafruit_BusIO_RegisterBits(&fault_reg, 1, 3);
+Adafruit_BusIO_RegisterBits    voltage_fault_bit = Adafruit_BusIO_RegisterBits(&fault_reg, 1, 2);
+
+// Print the details of the configuration register.
+void printConfig( void ) {
+  Serial.print("BIAS: ");         if (bias_bit.read() )   Serial.print("ON");   else Serial.print("OFF");
+  Serial.print(", AUTO: ");       if (auto_bit.read() )   Serial.print("ON");   else Serial.print("OFF");
+  Serial.print(", ONES: ");       if (oneS_bit.read() )   Serial.print("ON");   else Serial.print("OFF");
+  Serial.print(", WIRE: ");       if (wire_bit.read() )   Serial.print("3");    else Serial.print("2/4");
+  Serial.print(", FAULTCLEAR: "); if (faultR_bit.read() ) Serial.print("ON");   else Serial.print("OFF");
+  Serial.print(", ");             if (fi50hz_bit.read() ) Serial.print("50HZ"); else Serial.print("60HZ");
+  Serial.println();
+}
+
+// Check and print faults. Then clear them.
+void checkFaults( void ) {
+  if (fault_bit.read()) {
+    Serial.print("MAX: "); Serial.println(maxRratio_bits.read());
+    Serial.print("VAL: "); Serial.println(   rRatio_bits.read());
+    Serial.print("MIN: "); Serial.println(minRratio_bits.read());
+
+    if (range_high_fault_bit.read() ) Serial.println("Range high fault");
+    if ( range_low_fault_bit.read() ) Serial.println("Range low fault");
+    if (refin_high_fault_bit.read() ) Serial.println("REFIN high fault");
+    if ( refin_low_fault_bit.read() ) Serial.println("REFIN low fault");
+    if ( rtdin_low_fault_bit.read() ) Serial.println("RTDIN low fault");
+    if (   voltage_fault_bit.read() ) Serial.println("Voltage fault");
+
+    faultR_bit.write(1); // clear fault
+  }
+}
+
+void setup() {
+  #if (MAX31865_1_READY_PIN != -1)
+    pinMode(MAX31865_READY_PIN ,INPUT_PULLUP);
+  #endif
+
+  while (!Serial) { delay(10); }
+  Serial.begin(115200);
+  Serial.println("SPI Adafruit_BusIO_RegisterBits test on MAX31865");
+
+  if (!spi_dev.begin()) {
+    Serial.println("Could not initialize SPI device");
+    while (1);
+  }
+
+  // Set up for automode 50Hz. We don't care about selfheating. We want the highest possible sampling rate.
+  auto_bit.write(0);   // Don't switch filtermode while auto_mode is on.
+  fi50hz_bit.write(1); // Set filter to 50Hz mode.
+  faultR_bit.write(1); // Clear faults.
+  bias_bit.write(1);   // In automode we want to have the bias current always on.
+  delay(5);            // Wait until bias current settles down.
+                       // 10.5 time constants of the input RC network is required.
+                       // 10ms worst case for 10kω reference resistor and a 0.1µF capacitor across the RTD inputs.
+                       // Adafruit Module has 0.1µF and only 430/4300ω So here 0.43/4.3ms
+  auto_bit.write(1);   // Now we can set automode. Automatically starting first conversion.
+
+  // Test the READY_PIN
+  #if (defined( MAX31865_READY_PIN ) && (MAX31865_READY_PIN != -1))
+    int i = 0;
+    while (digitalRead(MAX31865_READY_PIN) && i++ <= 100) { delay(1); }
+    if (i >= 100) {
+      Serial.print("ERROR: Max31865 Pin detection does not work. PIN:");
+      Serial.println(MAX31865_READY_PIN);
+    }
+  #else
+    delay(100);
+  #endif
+
+  // Set ratio range.
+  // Setting the temperatures would need some more calculation - not related to Adafruit_BusIO_RegisterBits.
+  uint16_t ratio = rRatio_bits.read();
+  maxRratio_bits.write( (ratio < 0x8fffu-1000u) ? ratio + 1000u : 0x8fffu ); 
+  minRratio_bits.write( (ratio >         1000u) ? ratio - 1000u : 0u      );
+
+  printConfig();
+  checkFaults();
+}
+
+void loop() {
+  #if (defined( MAX31865_READY_PIN ) && (MAX31865_1_READY_PIN != -1))
+    // Is converstion ready?
+    if (!digitalRead(MAX31865_READY_PIN))
+  #else
+    // Warant conversion is ready.
+    delay(21); // 21ms for 50Hz-mode. 19ms in 60Hz-mode.
+  #endif
+    {
+      // Read ratio, calculate temperature, scale, filter and print.
+      Serial.println( rRatio2C( rRatio_bits.read() ) * 100.0f, 0); // Temperature scaled by 100
+      // Check, print, clear faults.
+      checkFaults();
+    }
+
+  // Do something else.
+  //delay(15000);
+}
+
+
+// Module/Sensor related. Here Adafruit PT100 module with a 2_Wire PT100 Class C *****************************
+float rRatio2C(uint16_t ratio) {
+  // A simple linear conversion.
+  const float R0 = 100.0f;
+  const float Rref = 430.0f;
+  const float alphaPT = 0.003850f;
+  const float ADCmax = (1u << 15) - 1.0f;
+  const float rscale = Rref / ADCmax;
+  // Measured temperature in boiling water 101.08°C with factor a = 1 and b = 0. Rref and MAX at about 22±2°C.
+  // Measured temperature in ice/water bath 0.76°C with factor a = 1 and b = 0. Rref and MAX at about 22±2°C.
+  //const float a = 1.0f / (alphaPT * R0);
+  const float a = (100.0f/101.08f) / (alphaPT * R0);
+  //const float b = 0.0f;  // 101.08
+  const float b = -0.76f;  // 100.32 > 101.08
+
+  return filterRing( ((ratio * rscale) - R0) * a + b );
+}
+
+// General purpose *********************************************************************************************
+#define RINGLENGTH 250
+float filterRing( float newVal ) {
+  static float ring[RINGLENGTH] = { 0.0 };
+  static uint8_t ringIndex = 0;
+  static bool ringFull = false;
+
+  if ( ringIndex == RINGLENGTH ) { ringFull = true; ringIndex = 0; }
+  ring[ringIndex] = newVal;
+  uint8_t loopEnd = (ringFull) ? RINGLENGTH : ringIndex + 1;
+  float ringSum = 0.0f;
+  for (uint8_t i = 0;  i < loopEnd; i++) ringSum += ring[i];
+  ringIndex++;
+  return ringSum / loopEnd;
+}

Embedded/libraries/Adafruit_BusIO/examples/spi_registers/spi_registers.ino

@@ -0,0 +1,34 @@
+#include <Adafruit_BusIO_Register.h>
+#include <Adafruit_SPIDevice.h>
+
+#define SPIDEVICE_CS 10
+Adafruit_SPIDevice spi_dev = Adafruit_SPIDevice(SPIDEVICE_CS);
+
+void setup() {
+  while (!Serial) { delay(10); }
+  Serial.begin(115200);
+  Serial.println("SPI device register test");
+
+  if (!spi_dev.begin()) {
+    Serial.println("Could not initialize SPI device");
+    while (1);
+  }
+
+  Adafruit_BusIO_Register id_reg = Adafruit_BusIO_Register(&spi_dev, 0x0F, ADDRBIT8_HIGH_TOREAD);
+  uint8_t id = 0;
+  id_reg.read(&id);
+  Serial.print("ID register = 0x"); Serial.println(id, HEX);
+
+  Adafruit_BusIO_Register thresh_reg = Adafruit_BusIO_Register(&spi_dev, 0x0C, ADDRBIT8_HIGH_TOREAD, 2, LSBFIRST);
+  uint16_t thresh = 0;
+  thresh_reg.read(&thresh);
+  Serial.print("Initial threshold register = 0x"); Serial.println(thresh, HEX);
+
+  thresh_reg.write(~thresh);
+
+  Serial.print("Post threshold register = 0x"); Serial.println(thresh_reg.read(), HEX);
+}
+
+void loop() {
+  
+}

Embedded/libraries/Adafruit_BusIO/library.properties

@@ -0,0 +1,9 @@
+name=Adafruit BusIO
+version=1.9.1
+author=Adafruit
+maintainer=Adafruit <info@adafruit.com>
+sentence=This is a library for abstracting away UART, I2C and SPI interfacing
+paragraph=This is a library for abstracting away UART, I2C and SPI interfacing
+category=Signal Input/Output
+url=https://github.com/adafruit/Adafruit_BusIO
+architectures=*

Embedded/libraries/Adafruit_ICM20X/Adafruit_ICM20649.cpp

@@ -0,0 +1,118 @@
+/*!   @file Adafruit_ICM20649.cpp
+ */
+#include "Arduino.h"
+#include <Wire.h>
+
+#include "Adafruit_ICM20649.h"
+#include "Adafruit_ICM20X.h"
+
+/*!
+ *    @brief  Instantiates a new ICM20649 class!
+ */
+Adafruit_ICM20649::Adafruit_ICM20649(void) {}
+
+/*!
+ *    @brief  Sets up the hardware and initializes I2C
+ *    @param  i2c_address
+ *            The I2C address to be used.
+ *    @param  wire
+ *            The Wire object to be used for I2C connections.
+ *    @param  sensor_id
+ *            An optional parameter to set the sensor ids to differentiate
+ * similar sensors The passed value is assigned to the accelerometer and the
+ * gyro get +1 and the temperature sensor +2.
+ *    @return True if initialization was successful, otherwise false.
+ */
+bool Adafruit_ICM20649::begin_I2C(uint8_t i2c_address, TwoWire *wire,
+                                  int32_t sensor_id) {
+
+  if (i2c_dev) {
+    delete i2c_dev; // remove old interface
+  }
+
+  i2c_dev = new Adafruit_I2CDevice(i2c_address, wire);
+
+  if (!i2c_dev->begin()) {
+    Serial.println("I2C begin Failed");
+    return false;
+  }
+
+  return _init(sensor_id);
+}
+
+void Adafruit_ICM20649::scaleValues(void) {
+
+  icm20649_gyro_range_t gyro_range = (icm20649_gyro_range_t)current_gyro_range;
+  icm20649_accel_range_t accel_range =
+      (icm20649_accel_range_t)current_accel_range;
+  float accel_scale = 1.0;
+  float gyro_scale = 1.0;
+
+  if (gyro_range == ICM20649_GYRO_RANGE_500_DPS)
+    gyro_scale = 65.5;
+  if (gyro_range == ICM20649_GYRO_RANGE_1000_DPS)
+    gyro_scale = 32.8;
+  if (gyro_range == ICM20649_GYRO_RANGE_2000_DPS)
+    gyro_scale = 16.4;
+  if (gyro_range == ICM20649_GYRO_RANGE_4000_DPS)
+    gyro_scale = 8.2;
+
+  if (accel_range == ICM20649_ACCEL_RANGE_4_G)
+    accel_scale = 8192.0;
+  if (accel_range == ICM20649_ACCEL_RANGE_8_G)
+    accel_scale = 4096.0;
+  if (accel_range == ICM20649_ACCEL_RANGE_16_G)
+    accel_scale = 2048.0;
+  if (accel_range == ICM20649_ACCEL_RANGE_30_G)
+    accel_scale = 1024.0;
+
+  gyroX = rawGyroX / gyro_scale;
+  gyroY = rawGyroY / gyro_scale;
+  gyroZ = rawGyroZ / gyro_scale;
+
+  accX = rawAccX / accel_scale;
+  accY = rawAccY / accel_scale;
+  accZ = rawAccZ / accel_scale;
+}
+
+/**************************************************************************/
+/*!
+    @brief Get the accelerometer's measurement range.
+    @returns The accelerometer's measurement range (`icm20649_accel_range_t`).
+*/
+icm20649_accel_range_t Adafruit_ICM20649::getAccelRange(void) {
+  return (icm20649_accel_range_t)readAccelRange();
+}
+
+/**************************************************************************/
+/*!
+
+    @brief Sets the accelerometer's measurement range.
+    @param  new_accel_range
+            Measurement range to be set. Must be an
+            `icm20649_accel_range_t`.
+*/
+void Adafruit_ICM20649::setAccelRange(icm20649_accel_range_t new_accel_range) {
+  writeAccelRange((uint8_t)new_accel_range);
+}
+
+/**************************************************************************/
+/*!
+    @brief Get the gyro's measurement range.
+    @returns The gyro's measurement range (`icm20649_gyro_range_t`).
+*/
+icm20649_gyro_range_t Adafruit_ICM20649::getGyroRange(void) {
+  return (icm20649_gyro_range_t)readGyroRange();
+}
+
+/**************************************************************************/
+/*!
+
+    @brief Sets the gyro's measurement range.
+    @param  new_gyro_range
+            Measurement range to be set. Must be an
+            `icm20649_gyro_range_t`.
+*/
+void Adafruit_ICM20649::setGyroRange(icm20649_gyro_range_t new_gyro_range) {
+  writeGyroRange((uint8_t)new_gyro_range);
+}

Embedded/libraries/Adafruit_ICM20X/Adafruit_ICM20649.h

@@ -0,0 +1,62 @@
+/*!
+ *  @file Adafruit_ICM20649.h
+ *
+ * 	I2C Driver for the Adafruit ICM20649 6-DoF Wide-Range Accelerometer and
+ *Gyro library
+ *
+ * 	This is a library for the Adafruit ICM20649 breakout:
+ * 	https://www.adafruit.com/products/4464
+ *
+ * 	Adafruit invests time and resources providing this open source code,
+ *  please support Adafruit and open-source hardware by purchasing products from
+ * 	Adafruit!
+ *
+ *
+ *	BSD license (see license.txt)
+ */
+
+#ifndef _ADAFRUIT_ICM20649_H
+#define _ADAFRUIT_ICM20649_H
+
+#include "Adafruit_ICM20X.h"
+
+#define ICM20649_I2CADDR_DEFAULT 0x68 ///< ICM20X default i2c address
+
+/** The accelerometer data range */
+typedef enum {
+  ICM20649_ACCEL_RANGE_4_G,
+  ICM20649_ACCEL_RANGE_8_G,
+  ICM20649_ACCEL_RANGE_16_G,
+  ICM20649_ACCEL_RANGE_30_G,
+} icm20649_accel_range_t;
+
+/** The gyro data range */
+typedef enum {
+  ICM20649_GYRO_RANGE_500_DPS,
+  ICM20649_GYRO_RANGE_1000_DPS,
+  ICM20649_GYRO_RANGE_2000_DPS,
+  ICM20649_GYRO_RANGE_4000_DPS,
+} icm20649_gyro_range_t;
+
+/*!
+ *    @brief  Class that stores state and functions for interacting with
+ *            the ST ICM20649 6-DoF Accelerometer and Gyro
+ */
+class Adafruit_ICM20649 : public Adafruit_ICM20X {
+public:
+  Adafruit_ICM20649();
+  virtual ~Adafruit_ICM20649(){};
+  bool begin_I2C(uint8_t i2c_addr = ICM20649_I2CADDR_DEFAULT,
+                 TwoWire *wire = &Wire, int32_t sensor_id = 0);
+
+  icm20649_accel_range_t getAccelRange(void);
+  void setAccelRange(icm20649_accel_range_t new_accel_range);
+
+  icm20649_gyro_range_t getGyroRange(void);
+  void setGyroRange(icm20649_gyro_range_t new_gyro_range);
+
+private:
+  void scaleValues(void);
+};
+
+#endif

Embedded/libraries/Adafruit_ICM20X/Adafruit_ICM20948.cpp

@@ -0,0 +1,243 @@
+/*!   @file Adafruit_ICM20948.cpp
+ */
+#include "Arduino.h"
+#include <Wire.h>
+
+#include "Adafruit_ICM20948.h"
+#include "Adafruit_ICM20X.h"
+
+/*!
+ *    @brief  Instantiates a new ICM20948 class!
+ */
+
+Adafruit_ICM20948::Adafruit_ICM20948(void) {}
+/*!
+ *    @brief  Sets up the hardware and initializes I2C
+ *    @param  i2c_address
+ *            The I2C address to be used.
+ *    @param  wire
+ *            The Wire object to be used for I2C connections.
+ *    @param  sensor_id
+ *            An optional parameter to set the sensor ids to differentiate
+ * similar sensors The passed value is assigned to the accelerometer and the
+ * gyro get +1 and the temperature sensor +2.
+ *    @return True if initialization was successful, otherwise false.
+ */
+bool Adafruit_ICM20948::begin_I2C(uint8_t i2c_address, TwoWire *wire,
+                                  int32_t sensor_id) {
+
+  if (i2c_dev) {
+    delete i2c_dev; // remove old interface
+  }
+
+  i2c_dev = new Adafruit_I2CDevice(i2c_address, wire);
+
+  if (!i2c_dev->begin()) {
+    Serial.println("I2C begin Failed");
+    return false;
+  }
+  bool init_success = _init(sensor_id);
+  if (!setupMag()) {
+    Serial.println("failed to setup mag");
+    return false;
+  }
+
+  return init_success;
+}
+
+// A million thanks to the SparkFun folks for their library that I pillaged to
+// write this method! See their Arduino library here:
+// https://github.com/sparkfun/SparkFun_ICM-20948_ArduinoLibrary
+bool Adafruit_ICM20948::auxI2CBusSetupFailed(void) {
+  // check aux I2C bus connection by reading the magnetometer chip ID
+  bool aux_i2c_setup_failed = true;
+  for (int i = 0; i < I2C_MASTER_RESETS_BEFORE_FAIL; i++) {
+    if (getMagId() != ICM20948_MAG_ID) {
+      resetI2CMaster();
+    } else {
+      aux_i2c_setup_failed = false;
+      break;
+    }
+  }
+  return aux_i2c_setup_failed;
+}
+
+uint8_t Adafruit_ICM20948::getMagId(void) {
+  // verify the magnetometer id
+  return readExternalRegister(0x8C, 0x01);
+}
+
+bool Adafruit_ICM20948::setupMag(void) {
+  uint8_t buffer[2];
+
+  setI2CBypass(false);
+
+  configureI2CMaster();
+
+  enableI2CMaster(true);
+
+  if (auxI2CBusSetupFailed()) {
+    return false;
+  }
+
+  // set mag data rate
+  if (!setMagDataRate(AK09916_MAG_DATARATE_100_HZ)) {
+    Serial.println("Error setting magnetometer data rate on external bus");
+    return false;
+  }
+
+  // TODO: extract method
+  // Set up Slave0 to proxy Mag readings
+  _setBank(3);
+  // set up slave0 to proxy reads to mag
+  buffer[0] = ICM20X_B3_I2C_SLV0_ADDR;
+  buffer[1] = 0x8C;
+  if (!i2c_dev->write(buffer, 2)) {
+    return false;
+  }
+
+  buffer[0] = ICM20X_B3_I2C_SLV0_REG;
+  buffer[1] = 0x10;
+  if (!i2c_dev->write(buffer, 2)) {
+    return false;
+  }
+
+  buffer[0] = ICM20X_B3_I2C_SLV0_CTRL;
+  buffer[1] = 0x89; // enable, read 9 bytes
+  if (!i2c_dev->write(buffer, 2)) {
+    return false;
+  }
+
+  return true;
+}
+
+/**
+ * @brief
+ *
+ * @param slv_addr
+ * @param mag_reg_addr
+ * @param num_finished_checks
+ * @return uint8_t
+ */
+uint8_t Adafruit_ICM20948::readMagRegister(uint8_t mag_reg_addr) {
+  return readExternalRegister(0x8C, mag_reg_addr);
+}
+
+bool Adafruit_ICM20948::writeMagRegister(uint8_t mag_reg_addr, uint8_t value) {
+  return writeExternalRegister(0x0C, mag_reg_addr, value);
+}
+
+void Adafruit_ICM20948::scaleValues(void) {
+
+  icm20948_gyro_range_t gyro_range = (icm20948_gyro_range_t)current_gyro_range;
+  icm20948_accel_range_t accel_range =
+      (icm20948_accel_range_t)current_accel_range;
+
+  float accel_scale = 1.0;
+  float gyro_scale = 1.0;
+
+  if (gyro_range == ICM20948_GYRO_RANGE_250_DPS)
+    gyro_scale = 131.0;
+  if (gyro_range == ICM20948_GYRO_RANGE_500_DPS)
+    gyro_scale = 65.5;
+  if (gyro_range == ICM20948_GYRO_RANGE_1000_DPS)
+    gyro_scale = 32.8;
+  if (gyro_range == ICM20948_GYRO_RANGE_2000_DPS)
+    gyro_scale = 16.4;
+
+  if (accel_range == ICM20948_ACCEL_RANGE_2_G)
+    accel_scale = 16384.0;
+  if (accel_range == ICM20948_ACCEL_RANGE_4_G)
+    accel_scale = 8192.0;
+  if (accel_range == ICM20948_ACCEL_RANGE_8_G)
+    accel_scale = 4096.0;
+  if (accel_range == ICM20948_ACCEL_RANGE_16_G)
+    accel_scale = 2048.0;
+
+  gyroX = rawGyroX / gyro_scale;
+  gyroY = rawGyroY / gyro_scale;
+  gyroZ = rawGyroZ / gyro_scale;
+
+  accX = rawAccX / accel_scale;
+  accY = rawAccY / accel_scale;
+  accZ = rawAccZ / accel_scale;
+
+  magX = rawMagX * ICM20948_UT_PER_LSB;
+  magY = rawMagY * ICM20948_UT_PER_LSB;
+  magZ = rawMagZ * ICM20948_UT_PER_LSB;
+}
+
+/**************************************************************************/
+/*!
+    @brief Get the accelerometer's measurement range.
+    @returns The accelerometer's measurement range (`icm20948_accel_range_t`).
+*/
+icm20948_accel_range_t Adafruit_ICM20948::getAccelRange(void) {
+  return (icm20948_accel_range_t)readAccelRange();
+}
+
+/**************************************************************************/
+/*!
+
+    @brief Sets the accelerometer's measurement range.
+    @param  new_accel_range
+            Measurement range to be set. Must be an
+            `icm20948_accel_range_t`.
+*/
+void Adafruit_ICM20948::setAccelRange(icm20948_accel_range_t new_accel_range) {
+  writeAccelRange((uint8_t)new_accel_range);
+}
+
+/**************************************************************************/
+/*!
+    @brief Get the gyro's measurement range.
+    @returns The gyro's measurement range (`icm20948_gyro_range_t`).
+*/
+icm20948_gyro_range_t Adafruit_ICM20948::getGyroRange(void) {
+  return (icm20948_gyro_range_t)readGyroRange();
+}
+
+/**************************************************************************/
+/*!
+
+    @brief Sets the gyro's measurement range.
+    @param  new_gyro_range
+            Measurement range to be set. Must be an
+            `icm20948_gyro_range_t`.
+*/
+void Adafruit_ICM20948::setGyroRange(icm20948_gyro_range_t new_gyro_range) {
+  writeGyroRange((uint8_t)new_gyro_range);
+}
+
+/**
+ * @brief Get the current magnetometer measurement rate
+ *
+ * @return ak09916_data_rate_t the current rate
+ */
+ak09916_data_rate_t Adafruit_ICM20948::getMagDataRate(void) {
+
+  uint8_t raw_mag_rate = readMagRegister(AK09916_CNTL2);
+  return (ak09916_data_rate_t)(raw_mag_rate);
+}
+/**
+ * @brief Set the magnetometer measurement rate
+ *
+ * @param rate The rate to set.
+ *
+ * @return true: success false: failure
+ */
+bool Adafruit_ICM20948::setMagDataRate(ak09916_data_rate_t rate) {
+  /*
+   * Following the datasheet, the sensor will be set to
+   * AK09916_MAG_DATARATE_SHUTDOWN followed by a 100ms delay, followed by
+   * setting the new data rate.
+   *
+   * See page 9 of https://www.y-ic.es/datasheet/78/SMDSW.020-2OZ.pdf
+   */
+
+  // don't need to read/mask because there's nothing else in the register and
+  // it's right justified
+  bool success = writeMagRegister(AK09916_CNTL2, AK09916_MAG_DATARATE_SHUTDOWN);
+  delay(1);
+  return writeMagRegister(AK09916_CNTL2, rate) && success;
+}

Embedded/libraries/Adafruit_ICM20X/Adafruit_ICM20948.h

@@ -0,0 +1,103 @@
+/*!
+ *  @file Adafruit_ICM20948.h
+ *
+ * 	I2C Driver for the Adafruit ICM20948 9-DoF Accelerometer, Gyro, and
+ *Magnetometer library
+ *
+ * 	This is a library for the Adafruit ICM20948 breakout:
+ * 	https://www.adafruit.com/products/4554
+ *
+ * 	Adafruit invests time and resources providing this open source code,
+ *  please support Adafruit and open-source hardware by purchasing products from
+ * 	Adafruit!
+ *
+ *
+ *	BSD license (see license.txt)
+ */
+
+#ifndef _ADAFRUIT_ICM20948_H
+#define _ADAFRUIT_ICM20948_H
+
+#include "Adafruit_ICM20X.h"
+
+#define ICM20948_I2CADDR_DEFAULT 0x69 ///< ICM20948 default i2c address
+#define ICM20948_MAG_ID 0x09          ///< The chip ID for the magnetometer
+
+#define ICM20948_UT_PER_LSB 0.15 ///< mag data LSB value (fixed)
+
+#define AK09916_WIA2 0x01  ///< Magnetometer
+#define AK09916_ST1 0x10   ///< Magnetometer
+#define AK09916_HXL 0x11   ///< Magnetometer
+#define AK09916_HXH 0x12   ///< Magnetometer
+#define AK09916_HYL 0x13   ///< Magnetometer
+#define AK09916_HYH 0x14   ///< Magnetometer
+#define AK09916_HZL 0x15   ///< Magnetometer
+#define AK09916_HZH 0x16   ///< Magnetometer
+#define AK09916_ST2 0x18   ///< Magnetometer
+#define AK09916_CNTL2 0x31 ///< Magnetometer
+#define AK09916_CNTL3 0x32 ///< Magnetometer
+
+/** The accelerometer data range */
+typedef enum {
+  ICM20948_ACCEL_RANGE_2_G,
+  ICM20948_ACCEL_RANGE_4_G,
+  ICM20948_ACCEL_RANGE_8_G,
+  ICM20948_ACCEL_RANGE_16_G,
+} icm20948_accel_range_t;
+
+/** The gyro data range */
+typedef enum {
+  ICM20948_GYRO_RANGE_250_DPS,
+  ICM20948_GYRO_RANGE_500_DPS,
+  ICM20948_GYRO_RANGE_1000_DPS,
+  ICM20948_GYRO_RANGE_2000_DPS,
+} icm20948_gyro_range_t;
+
+/**
+ * @brief Data rates/modes for the embedded AsahiKASEI AK09916 3-axis
+ * magnetometer
+ *
+ */
+typedef enum {
+  AK09916_MAG_DATARATE_SHUTDOWN = 0x0, ///< Stops measurement updates
+  AK09916_MAG_DATARATE_SINGLE =
+      0x1, ///< Takes a single measurement then switches to
+           ///< AK09916_MAG_DATARATE_SHUTDOWN
+  AK09916_MAG_DATARATE_10_HZ = 0x2,  ///< updates at 10Hz
+  AK09916_MAG_DATARATE_20_HZ = 0x4,  ///< updates at 20Hz
+  AK09916_MAG_DATARATE_50_HZ = 0x6,  ///< updates at 50Hz
+  AK09916_MAG_DATARATE_100_HZ = 0x8, ///< updates at 100Hz
+} ak09916_data_rate_t;
+
+/*!
+ *    @brief  Class that stores state and functions for interacting with
+ *            the ST ICM2948 9-DoF Accelerometer, gyro, and magnetometer
+ */
+class Adafruit_ICM20948 : public Adafruit_ICM20X {
+public:
+  Adafruit_ICM20948();
+  ~Adafruit_ICM20948(){};
+  bool begin_I2C(uint8_t i2c_addr = ICM20948_I2CADDR_DEFAULT,
+                 TwoWire *wire = &Wire, int32_t sensor_id = 0);
+
+  icm20948_accel_range_t getAccelRange(void);
+  void setAccelRange(icm20948_accel_range_t new_accel_range);
+
+  icm20948_gyro_range_t getGyroRange(void);
+  void setGyroRange(icm20948_gyro_range_t new_gyro_range);
+
+  ak09916_data_rate_t getMagDataRate(void);
+  bool setMagDataRate(ak09916_data_rate_t rate);
+
+private:
+  uint8_t readMagRegister(uint8_t reg_addr);
+  bool writeMagRegister(uint8_t reg_addr, uint8_t value);
+
+  uint8_t getMagId(void);
+  bool auxI2CBusSetupFailed(void);
+
+  bool setupMag(void);
+  void scaleValues(void);
+};
+
+#endif

Embedded/libraries/Adafruit_ICM20X/Adafruit_ICM20X.cpp

@@ -0,0 +1,979 @@
+
+/*!
+ *  @file Adafruit_ICM20X.cpp
+ *
+ *  @mainpage Adafruit ICM20X family motion sensor library
+ *  @see Adafruit_ICM20649, Adafruit_ICM20948
+ *
+ *  @section intro_sec Introduction
+ *
+ * 	I2C Driver for the Adafruit ICM20X Family of motion sensors
+ *
+ * 	This is a library for the Adafruit ICM20X breakouts:
+ * 	* https://www.adafruit.com/product/4464
+ *
+ * 	* https://www.adafruit.com/product/4554
+ *
+ * 	Adafruit invests time and resources providing this open source code,
+ *  please support Adafruit and open-source hardware by purchasing products from
+ * 	Adafruit!
+ *
+ *  @section dependencies Dependencies
+ * * [Adafruit BusIO](https://github.com/adafruit/Adafruit_BusIO)
+ *
+ * * [Adafruit Unified Sensor
+ * Driver](https://github.com/adafruit/Adafruit_Sensor)
+ *
+ *  @section author Author
+ *
+ *  Bryan Siepert for Adafruit Industries
+ *
+ * 	@section license License
+ *
+ * 	BSD (see license.txt)
+ *
+ * 	@section  HISTORY
+ *
+ *    See [the repository's  release page for the release
+ * history](https://github.com/adafruit/Adafruit_ICM20X/releases)
+ */
+
+#include "Arduino.h"
+#include <Wire.h>
+
+#include "Adafruit_ICM20X.h"
+
+/*!
+ *    @brief  Instantiates a new ICM20X class!
+ */
+Adafruit_ICM20X::Adafruit_ICM20X(void) {}
+
+/*!
+ *    @brief  Cleans up the ICM20X
+ */
+Adafruit_ICM20X::~Adafruit_ICM20X(void) {
+  if (accel_sensor)
+    delete accel_sensor;
+  if (gyro_sensor)
+    delete gyro_sensor;
+  if (mag_sensor)
+    delete mag_sensor;
+  if (temp_sensor)
+    delete temp_sensor;
+}
+
+/*!
+ *    @brief  Sets up the hardware and initializes I2C
+ *    @param  i2c_address
+ *            The I2C address to be used.
+ *    @param  wire
+ *            The Wire object to be used for I2C connections.
+ *    @param  sensor_id
+ *            An optional parameter to set the sensor ids to differentiate
+ * similar sensors The passed value is assigned to the accelerometer, the gyro
+ * gets +1, the magnetometer +2, and the temperature sensor +3.
+ *    @return True if initialization was successful, otherwise false.
+ */
+bool Adafruit_ICM20X::begin_I2C(uint8_t i2c_address, TwoWire *wire,
+                                int32_t sensor_id) {
+  (void)i2c_address;
+  (void)wire;
+  (void)sensor_id;
+  return false;
+}
+
+/*!
+ *    @brief  Sets up the hardware and initializes hardware SPI
+ *    @param  cs_pin The arduino pin # connected to chip select
+ *    @param  theSPI The SPI object to be used for SPI connections.
+ *    @param  sensor_id An optional parameter to set the sensor ids to
+ * differentiate similar sensors The passed value is assigned to the
+ * accelerometer, the gyro gets +1, the magnetometer +2, and the temperature
+ * sensor +3.
+ *    @return True if initialization was successful, otherwise false.
+ */
+bool Adafruit_ICM20X::begin_SPI(uint8_t cs_pin, SPIClass *theSPI,
+                                int32_t sensor_id) {
+  i2c_dev = NULL;
+
+  if (spi_dev) {
+    delete spi_dev; // remove old interface
+  }
+
+  spi_dev = new Adafruit_SPIDevice(cs_pin,
+                                   1000000,               // frequency
+                                   SPI_BITORDER_MSBFIRST, // bit order
+                                   SPI_MODE0,             // data mode
+                                   theSPI);
+
+  if (!spi_dev->begin()) {
+    return false;
+  }
+
+  return _init(sensor_id);
+}
+
+/*!
+ *    @brief  Sets up the hardware and initializes software SPI
+ *    @param  cs_pin The arduino pin # connected to chip select
+ *    @param  sck_pin The arduino pin # connected to SPI clock
+ *    @param  miso_pin The arduino pin # connected to SPI MISO
+ *    @param  mosi_pin The arduino pin # connected to SPI MOSI
+ *    @param  sensor_id An optional parameter to set the sensor ids to
+ * differentiate similar sensors The passed value is assigned to the
+ * accelerometer, the gyro gets +1, the magnetometer +2, and the temperature
+ * sensor +3.
+ *    @return True if initialization was successful, otherwise false.
+ */
+bool Adafruit_ICM20X::begin_SPI(int8_t cs_pin, int8_t sck_pin, int8_t miso_pin,
+                                int8_t mosi_pin, int32_t sensor_id) {
+  i2c_dev = NULL;
+
+  if (spi_dev) {
+    delete spi_dev; // remove old interface
+  }
+  spi_dev = new Adafruit_SPIDevice(cs_pin, sck_pin, miso_pin, mosi_pin,
+                                   1000000,               // frequency
+                                   SPI_BITORDER_MSBFIRST, // bit order
+                                   SPI_MODE0);            // data mode
+  if (!spi_dev->begin()) {
+    return false;
+  }
+
+  return _init(sensor_id);
+}
+
+/*!
+ * @brief Reset the internal registers and restores the default settings
+ *
+ */
+void Adafruit_ICM20X::reset(void) {
+  _setBank(0);
+
+  Adafruit_BusIO_Register pwr_mgmt1 = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B0_PWR_MGMT_1, 1);
+
+  Adafruit_BusIO_RegisterBits reset_bit =
+      Adafruit_BusIO_RegisterBits(&pwr_mgmt1, 1, 7);
+
+  reset_bit.write(1);
+  delay(20);
+
+  while (reset_bit.read()) {
+    delay(10);
+  };
+  delay(50);
+}
+
+/*!  @brief Initilizes the sensor
+ *   @param sensor_id Optional unique ID for the sensor set
+ *   @returns True if chip identified and initialized
+ */
+bool Adafruit_ICM20X::_init(int32_t sensor_id) {
+  Adafruit_BusIO_Register chip_id = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B0_WHOAMI);
+
+  _setBank(0);
+  uint8_t chip_id_ = chip_id.read();
+  // This returns true when using a 649 lib with a 948
+  if ((chip_id_ != ICM20649_CHIP_ID) && (chip_id_ != ICM20948_CHIP_ID)) {
+    return false;
+  }
+
+  _sensorid_accel = sensor_id;
+  _sensorid_gyro = sensor_id + 1;
+  _sensorid_mag = sensor_id + 2;
+  _sensorid_temp = sensor_id + 3;
+
+  reset();
+
+  Adafruit_BusIO_Register pwr_mgmt_1 = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B0_PWR_MGMT_1);
+
+  Adafruit_BusIO_RegisterBits sleep =
+      Adafruit_BusIO_RegisterBits(&pwr_mgmt_1, 1, 6);
+  sleep.write(false); // take out of default sleep state
+
+  // 3 will be the largest range for either sensor
+  writeGyroRange(3);
+  writeAccelRange(3);
+
+  // 1100Hz/(1+10) = 100Hz
+  setGyroRateDivisor(10);
+
+  // # 1125Hz/(1+20) = 53.57Hz
+  setAccelRateDivisor(20);
+
+  temp_sensor = new Adafruit_ICM20X_Temp(this);
+  accel_sensor = new Adafruit_ICM20X_Accelerometer(this);
+  gyro_sensor = new Adafruit_ICM20X_Gyro(this);
+  mag_sensor = new Adafruit_ICM20X_Magnetometer(this);
+  delay(20);
+
+  return true;
+}
+
+/**************************************************************************/
+/*!
+    @brief  Gets the most recent sensor event, Adafruit Unified Sensor format
+    @param  accel
+            Pointer to an Adafruit Unified sensor_event_t object to be filled
+            with acceleration event data.
+
+    @param  gyro
+            Pointer to an Adafruit Unified sensor_event_t object to be filled
+            with gyro event data.
+
+    @param  mag
+            Pointer to an Adafruit Unified sensor_event_t object to be filled
+            with magnetometer event data.
+
+    @param  temp
+            Pointer to an Adafruit Unified sensor_event_t object to be filled
+            with temperature event data.
+
+    @return True on successful read
+*/
+/**************************************************************************/
+bool Adafruit_ICM20X::getEvent(sensors_event_t *accel, sensors_event_t *gyro,
+                               sensors_event_t *temp, sensors_event_t *mag) {
+  uint32_t t = millis();
+  _read();
+
+  // use helpers to fill in the events
+  fillAccelEvent(accel, t);
+  fillGyroEvent(gyro, t);
+  fillTempEvent(temp, t);
+  if (mag) {
+    fillMagEvent(mag, t);
+  }
+
+  return true;
+}
+
+void Adafruit_ICM20X::fillAccelEvent(sensors_event_t *accel,
+                                     uint32_t timestamp) {
+  memset(accel, 0, sizeof(sensors_event_t));
+  accel->version = 1;
+  accel->sensor_id = _sensorid_accel;
+  accel->type = SENSOR_TYPE_ACCELEROMETER;
+  accel->timestamp = timestamp;
+
+  accel->acceleration.x = accX * SENSORS_GRAVITY_EARTH;
+  accel->acceleration.y = accY * SENSORS_GRAVITY_EARTH;
+  accel->acceleration.z = accZ * SENSORS_GRAVITY_EARTH;
+}
+
+void Adafruit_ICM20X::fillGyroEvent(sensors_event_t *gyro, uint32_t timestamp) {
+  memset(gyro, 0, sizeof(sensors_event_t));
+  gyro->version = 1;
+  gyro->sensor_id = _sensorid_gyro;
+  gyro->type = SENSOR_TYPE_GYROSCOPE;
+  gyro->timestamp = timestamp;
+  gyro->gyro.x = gyroX * SENSORS_DPS_TO_RADS;
+  gyro->gyro.y = gyroY * SENSORS_DPS_TO_RADS;
+  gyro->gyro.z = gyroZ * SENSORS_DPS_TO_RADS;
+}
+
+void Adafruit_ICM20X::fillMagEvent(sensors_event_t *mag, uint32_t timestamp) {
+  memset(mag, 0, sizeof(sensors_event_t));
+  mag->version = 1;
+  mag->sensor_id = _sensorid_mag;
+  mag->type = SENSOR_TYPE_MAGNETIC_FIELD;
+  mag->timestamp = timestamp;
+  mag->magnetic.x = magX; // magic number!
+  mag->magnetic.y = magY;
+  mag->magnetic.z = magZ;
+}
+
+void Adafruit_ICM20X::fillTempEvent(sensors_event_t *temp, uint32_t timestamp) {
+
+  memset(temp, 0, sizeof(sensors_event_t));
+  temp->version = sizeof(sensors_event_t);
+  temp->sensor_id = _sensorid_temp;
+  temp->type = SENSOR_TYPE_AMBIENT_TEMPERATURE;
+  temp->timestamp = timestamp;
+  temp->temperature = (temperature / 333.87) + 21.0;
+}
+/******************* Adafruit_Sensor functions *****************/
+/*!
+ *     @brief  Updates the measurement data for all sensors simultaneously
+ */
+/**************************************************************************/
+void Adafruit_ICM20X::_read(void) {
+
+  _setBank(0);
+
+  // reading 9 bytes of mag data to fetch the register that tells the mag we've
+  // read all the data
+  const uint8_t numbytes = 14 + 9; // Read Accel, gyro, temp, and 9 bytes of mag
+
+  Adafruit_BusIO_Register data_reg = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B0_ACCEL_XOUT_H, numbytes);
+
+  uint8_t buffer[numbytes];
+  data_reg.read(buffer, numbytes);
+
+  rawAccX = buffer[0] << 8 | buffer[1];
+  rawAccY = buffer[2] << 8 | buffer[3];
+  rawAccZ = buffer[4] << 8 | buffer[5];
+
+  rawGyroX = buffer[6] << 8 | buffer[7];
+  rawGyroY = buffer[8] << 8 | buffer[9];
+  rawGyroZ = buffer[10] << 8 | buffer[11];
+
+  temperature = buffer[12] << 8 | buffer[13];
+
+  rawMagX = ((buffer[16] << 8) |
+             (buffer[15] & 0xFF)); // Mag data is read little endian
+  rawMagY = ((buffer[18] << 8) | (buffer[17] & 0xFF));
+  rawMagZ = ((buffer[20] << 8) | (buffer[19] & 0xFF));
+
+  scaleValues();
+  _setBank(0);
+}
+/*!
+ * @brief Scales the raw variables based on the current measurement range
+ *
+ */
+void Adafruit_ICM20X::scaleValues(void) {}
+
+/*!
+    @brief  Gets an Adafruit Unified Sensor object for the accelerometer
+    sensor component
+    @return Adafruit_Sensor pointer to accelerometer sensor
+ */
+Adafruit_Sensor *Adafruit_ICM20X::getAccelerometerSensor(void) {
+  return accel_sensor;
+}
+
+/*!
+    @brief  Gets an Adafruit Unified Sensor object for the gyro sensor component
+    @return Adafruit_Sensor pointer to gyro sensor
+ */
+Adafruit_Sensor *Adafruit_ICM20X::getGyroSensor(void) { return gyro_sensor; }
+
+/*!
+    @brief  Gets an Adafruit Unified Sensor object for the magnetometer sensor
+   component
+    @return Adafruit_Sensor pointer to magnetometer sensor
+ */
+Adafruit_Sensor *Adafruit_ICM20X::getMagnetometerSensor(void) {
+  return mag_sensor;
+}
+
+/*!
+    @brief  Gets an Adafruit Unified Sensor object for the temp sensor component
+    @return Adafruit_Sensor pointer to temperature sensor
+ */
+Adafruit_Sensor *Adafruit_ICM20X::getTemperatureSensor(void) {
+  return temp_sensor;
+}
+/**************************************************************************/
+/*!
+    @brief Sets register bank.
+    @param  bank_number
+          The bank to set to active
+*/
+void Adafruit_ICM20X::_setBank(uint8_t bank_number) {
+
+  Adafruit_BusIO_Register reg_bank_sel = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B0_REG_BANK_SEL);
+
+  reg_bank_sel.write((bank_number & 0b11) << 4);
+}
+
+/**************************************************************************/
+/*!
+    @brief Get the accelerometer's measurement range.
+    @returns The accelerometer's measurement range (`icm20x_accel_range_t`).
+*/
+uint8_t Adafruit_ICM20X::readAccelRange(void) {
+  _setBank(2);
+
+  Adafruit_BusIO_Register accel_config_1 = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B2_ACCEL_CONFIG_1);
+
+  Adafruit_BusIO_RegisterBits accel_range =
+      Adafruit_BusIO_RegisterBits(&accel_config_1, 2, 1);
+
+  uint8_t range = accel_range.read();
+  _setBank(0);
+  return range;
+}
+
+/**************************************************************************/
+/*!
+
+    @brief Sets the accelerometer's measurement range.
+    @param  new_accel_range
+            Measurement range to be set. Must be an
+            `icm20x_accel_range_t`.
+*/
+void Adafruit_ICM20X::writeAccelRange(uint8_t new_accel_range) {
+  _setBank(2);
+
+  Adafruit_BusIO_Register accel_config_1 = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B2_ACCEL_CONFIG_1);
+
+  Adafruit_BusIO_RegisterBits accel_range =
+      Adafruit_BusIO_RegisterBits(&accel_config_1, 2, 1);
+
+  accel_range.write(new_accel_range);
+  current_accel_range = new_accel_range;
+
+  _setBank(0);
+}
+
+/**************************************************************************/
+/*!
+    @brief Get the gyro's measurement range.
+    @returns The gyro's measurement range (`icm20x_gyro_range_t`).
+*/
+uint8_t Adafruit_ICM20X::readGyroRange(void) {
+  _setBank(2);
+
+  Adafruit_BusIO_Register gyro_config_1 = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B2_GYRO_CONFIG_1);
+
+  Adafruit_BusIO_RegisterBits gyro_range =
+      Adafruit_BusIO_RegisterBits(&gyro_config_1, 2, 1);
+
+  uint8_t range = gyro_range.read();
+  _setBank(0);
+  return range;
+}
+
+/**************************************************************************/
+/*!
+
+    @brief Sets the gyro's measurement range.
+    @param  new_gyro_range
+            Measurement range to be set. Must be an
+            `icm20x_gyro_range_t`.
+*/
+void Adafruit_ICM20X::writeGyroRange(uint8_t new_gyro_range) {
+  _setBank(2);
+
+  Adafruit_BusIO_Register gyro_config_1 = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B2_GYRO_CONFIG_1);
+
+  Adafruit_BusIO_RegisterBits gyro_range =
+      Adafruit_BusIO_RegisterBits(&gyro_config_1, 2, 1);
+
+  gyro_range.write(new_gyro_range);
+  current_gyro_range = new_gyro_range;
+  _setBank(0);
+}
+
+/**************************************************************************/
+/*!
+    @brief Get the accelerometer's data rate divisor.
+    @returns The accelerometer's data rate divisor (`uint8_t`).
+*/
+uint16_t Adafruit_ICM20X::getAccelRateDivisor(void) {
+  _setBank(2);
+
+  Adafruit_BusIO_Register accel_rate_divisor =
+      Adafruit_BusIO_Register(i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD,
+                              ICM20X_B2_ACCEL_SMPLRT_DIV_1, 2, MSBFIRST);
+
+  uint16_t divisor_val = accel_rate_divisor.read();
+
+  _setBank(0);
+  return divisor_val;
+}
+
+/**************************************************************************/
+/*!
+
+    @brief Sets the accelerometer's data rate divisor.
+    @param  new_accel_divisor
+            The accelerometer's data rate divisor (`uint16_t`). This 12-bit
+   value must be <= 4095
+*/
+void Adafruit_ICM20X::setAccelRateDivisor(uint16_t new_accel_divisor) {
+  _setBank(2);
+
+  Adafruit_BusIO_Register accel_rate_divisor =
+      Adafruit_BusIO_Register(i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD,
+                              ICM20X_B2_ACCEL_SMPLRT_DIV_1, 2, MSBFIRST);
+
+  accel_rate_divisor.write(new_accel_divisor);
+  _setBank(0);
+}
+
+/**************************************************************************/
+/*!
+    @brief Get the gyro's data rate divisor.
+    @returns The gyro's data rate divisor (`uint8_t`).
+*/
+uint8_t Adafruit_ICM20X::getGyroRateDivisor(void) {
+  _setBank(2);
+
+  Adafruit_BusIO_Register gyro_rate_divisor = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B2_GYRO_SMPLRT_DIV, 1);
+
+  uint8_t divisor_val = gyro_rate_divisor.read();
+
+  _setBank(0);
+  return divisor_val;
+}
+
+/**************************************************************************/
+/*!
+
+    @brief Sets the gyro's data rate divisor.
+    @param  new_gyro_divisor
+            The gyro's data rate divisor (`uint8_t`).
+*/
+void Adafruit_ICM20X::setGyroRateDivisor(uint8_t new_gyro_divisor) {
+  _setBank(2);
+
+  Adafruit_BusIO_Register gyro_rate_divisor = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B2_GYRO_SMPLRT_DIV, 1);
+
+  gyro_rate_divisor.write(new_gyro_divisor);
+  _setBank(0);
+}
+
+/**************************************************************************/
+/*!
+ * @brief Enable or disable the accelerometer's Digital Low Pass Filter
+ *
+ * @param enable true: enable false: disable
+ * @param cutoff_freq Signals changing at a rate higher than the given cutoff
+ * frequency will be filtered out
+ * @return true: success false: failure
+ */
+bool Adafruit_ICM20X::enableAccelDLPF(bool enable,
+                                      icm20x_accel_cutoff_t cutoff_freq) {
+  _setBank(2);
+  Adafruit_BusIO_Register accel_config1 = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B2_ACCEL_CONFIG_1);
+
+  Adafruit_BusIO_RegisterBits dlpf_enable =
+      Adafruit_BusIO_RegisterBits(&accel_config1, 1, 0);
+  if (!dlpf_enable.write(enable)) {
+    return false;
+  }
+
+  if (!enable) {
+    return true;
+  }
+
+  Adafruit_BusIO_RegisterBits dlpf_config =
+      Adafruit_BusIO_RegisterBits(&accel_config1, 3, 3);
+
+  if (!dlpf_config.write(cutoff_freq)) {
+    return false;
+  }
+  return true;
+}
+
+/**************************************************************************/
+/*!
+ * @brief Enable or disable the gyro's Digital Low Pass Filter
+ *
+ * @param enable true: enable false: disable
+ * @param cutoff_freq Signals changing at a rate higher than the given cutoff
+ * frequency will be filtered out
+ * @return true: success false: failure
+ */
+bool Adafruit_ICM20X::enableGyrolDLPF(bool enable,
+                                      icm20x_gyro_cutoff_t cutoff_freq) {
+  _setBank(2);
+  Adafruit_BusIO_Register gyro_config1 = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B2_ACCEL_CONFIG_1);
+
+  Adafruit_BusIO_RegisterBits dlpf_enable =
+      Adafruit_BusIO_RegisterBits(&gyro_config1, 1, 0);
+  if (!dlpf_enable.write(enable)) {
+    return false;
+  }
+
+  if (!enable) {
+    return true;
+  }
+
+  Adafruit_BusIO_RegisterBits dlpf_config =
+      Adafruit_BusIO_RegisterBits(&gyro_config1, 3, 3);
+
+  if (!dlpf_config.write(cutoff_freq)) {
+    return false;
+  }
+  return true;
+}
+
+/**************************************************************************/
+/*!
+ * @brief Sets the polarity of the int1 pin
+ *
+ * @param active_low Set to true to make INT1 active low, false to make it
+ * active high
+ */
+void Adafruit_ICM20X::setInt1ActiveLow(bool active_low) {
+
+  _setBank(0);
+
+  Adafruit_BusIO_Register int_pin_cfg = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B0_REG_INT_PIN_CFG);
+
+  Adafruit_BusIO_RegisterBits int1_polarity =
+      Adafruit_BusIO_RegisterBits(&int_pin_cfg, 1, 7);
+
+  Adafruit_BusIO_RegisterBits int1_open_drain =
+      Adafruit_BusIO_RegisterBits(&int_pin_cfg, 1, 6);
+
+  int1_open_drain.write(true);
+  int1_polarity.write(active_low);
+}
+/*!
+ * @brief Sets the polarity of the INT2 pin
+ *
+ * @param active_low Set to true to make INT1 active low, false to make it
+ * active high
+ */
+void Adafruit_ICM20X::setInt2ActiveLow(bool active_low) {
+
+  _setBank(0);
+
+  Adafruit_BusIO_Register int_enable_1 = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B0_REG_INT_ENABLE_1);
+
+  Adafruit_BusIO_RegisterBits int2_polarity =
+      Adafruit_BusIO_RegisterBits(&int_enable_1, 1, 7);
+
+  Adafruit_BusIO_RegisterBits int2_open_drain =
+      Adafruit_BusIO_RegisterBits(&int_enable_1, 1, 6);
+
+  int2_open_drain.write(true);
+  int2_polarity.write(active_low);
+}
+
+/**************************************************************************/
+/*!
+ * @brief Sets the bypass status of the I2C master bus support.
+ *
+ * @param bypass_i2c Set to true to bypass the internal I2C master circuitry,
+ * connecting the external I2C bus to the main I2C bus. Set to false to
+ * re-connect
+ */
+void Adafruit_ICM20X::setI2CBypass(bool bypass_i2c) {
+  _setBank(0);
+
+  Adafruit_BusIO_Register int_enable_1 = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B0_REG_INT_PIN_CFG);
+
+  Adafruit_BusIO_RegisterBits i2c_bypass_enable =
+      Adafruit_BusIO_RegisterBits(&int_enable_1, 1, 1);
+
+  i2c_bypass_enable.write(bypass_i2c);
+}
+
+/**************************************************************************/
+/*!
+ * @brief Enable or disable the I2C mastercontroller
+ *
+ * @param enable_i2c_master true: enable false: disable
+ *
+ * @return true: success false: error
+ */
+bool Adafruit_ICM20X::enableI2CMaster(bool enable_i2c_master) {
+  _setBank(0);
+  Adafruit_BusIO_Register user_ctrl_reg = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B0_USER_CTRL);
+
+  Adafruit_BusIO_RegisterBits i2c_master_enable_bit =
+      Adafruit_BusIO_RegisterBits(&user_ctrl_reg, 1, 5);
+  return i2c_master_enable_bit.write(enable_i2c_master);
+}
+
+// TODO: add params
+/**************************************************************************/
+/*!
+ * @brief Set the I2C clock rate for the auxillary I2C bus to 345.60kHz and
+ * disable repeated start
+ *
+ * @return true: success false: failure
+ */
+bool Adafruit_ICM20X::configureI2CMaster(void) {
+
+  _setBank(3);
+  Adafruit_BusIO_Register i2c_master_ctrl_reg = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B3_I2C_MST_CTRL);
+
+  return i2c_master_ctrl_reg.write(0x17);
+}
+
+/**************************************************************************/
+/*!
+ * @brief Read a single byte from a given register address for an I2C slave
+ * device on the auxiliary I2C bus
+ *
+ * @param slv_addr the 7-bit I2C address of the slave device
+ * @param reg_addr the register address to read from
+ * @return the requested register value
+ */
+uint8_t Adafruit_ICM20X::readExternalRegister(uint8_t slv_addr,
+                                              uint8_t reg_addr) {
+
+  return auxillaryRegisterTransaction(true, slv_addr, reg_addr);
+}
+
+/**************************************************************************/
+/*!
+ * @brief Write a single byte to a given register address for an I2C slave
+ * device on the auxiliary I2C bus
+ *
+ * @param slv_addr the 7-bit I2C address of the slave device
+ * @param reg_addr the register address to write to
+ * @param value the value to write
+ * @return true
+ * @return false
+ */
+bool Adafruit_ICM20X::writeExternalRegister(uint8_t slv_addr, uint8_t reg_addr,
+                                            uint8_t value) {
+
+  return (bool)auxillaryRegisterTransaction(false, slv_addr, reg_addr, value);
+}
+
+/**************************************************************************/
+/*!
+ * @brief Write a single byte to a given register address for an I2C slave
+ * device on the auxiliary I2C bus
+ *
+ * @param slv_addr the 7-bit I2C address of the slave device
+ * @param reg_addr the register address to write to
+ * @param value the value to write
+ * @return true
+ * @return false
+ */
+uint8_t Adafruit_ICM20X::auxillaryRegisterTransaction(bool read,
+                                                      uint8_t slv_addr,
+                                                      uint8_t reg_addr,
+                                                      uint8_t value) {
+
+  _setBank(3);
+
+  Adafruit_BusIO_Register *slv4_di_reg;
+
+  Adafruit_BusIO_Register *slv4_do_reg;
+  Adafruit_BusIO_Register slv4_addr_reg = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B3_I2C_SLV4_ADDR);
+
+  Adafruit_BusIO_Register slv4_reg_reg = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B3_I2C_SLV4_REG);
+
+  Adafruit_BusIO_Register slv4_ctrl_reg = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B3_I2C_SLV4_CTRL);
+
+  Adafruit_BusIO_Register i2c_master_status_reg = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B0_I2C_MST_STATUS);
+
+  Adafruit_BusIO_RegisterBits slave_finished_bit =
+      Adafruit_BusIO_RegisterBits(&i2c_master_status_reg, 1, 6);
+
+  if (read) {
+    slv_addr |= 0x80; // set high bit for read, presumably for multi-byte reads
+
+    slv4_di_reg = new Adafruit_BusIO_Register(
+        i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B3_I2C_SLV4_DI);
+  } else {
+
+    slv4_do_reg = new Adafruit_BusIO_Register(
+        i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B3_I2C_SLV4_DO);
+
+    if (!slv4_do_reg->write(value)) {
+      return (uint8_t) false;
+    }
+  }
+
+  if (!slv4_addr_reg.write(slv_addr)) {
+    return (uint8_t) false;
+  }
+  if (!slv4_reg_reg.write(reg_addr)) {
+    return (uint8_t) false;
+  }
+
+  if (!slv4_ctrl_reg.write(0x80)) {
+    return (uint8_t) false;
+  }
+
+  _setBank(0);
+  uint8_t tries = 0;
+  // wait until the operation is finished
+  while (slave_finished_bit.read() != true) {
+    tries++;
+    if (tries >= NUM_FINISHED_CHECKS) {
+      return (uint8_t) false;
+    }
+  }
+  if (read) {
+    _setBank(3);
+    return slv4_di_reg->read();
+  }
+  return (uint8_t) true;
+}
+
+/**************************************************************************/
+/*!
+ * @brief Reset the I2C master
+ *
+ */
+void Adafruit_ICM20X::resetI2CMaster(void) {
+
+  _setBank(0);
+  Adafruit_BusIO_Register user_ctrl = Adafruit_BusIO_Register(
+      i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, ICM20X_B0_USER_CTRL);
+
+  Adafruit_BusIO_RegisterBits i2c_master_reset_bit =
+      Adafruit_BusIO_RegisterBits(&user_ctrl, 1, 1);
+
+  i2c_master_reset_bit.write(true);
+  while (i2c_master_reset_bit.read()) {
+    delay(10);
+  }
+  delay(100);
+}
+
+/**************************************************************************/
+/*!
+    @brief  Gets the sensor_t data for the ICM20X's accelerometer
+*/
+/**************************************************************************/
+void Adafruit_ICM20X_Accelerometer::getSensor(sensor_t *sensor) {
+  /* Clear the sensor_t object */
+  memset(sensor, 0, sizeof(sensor_t));
+
+  /* Insert the sensor name in the fixed length char array */
+  strncpy(sensor->name, "ICM20X_A", sizeof(sensor->name) - 1);
+  sensor->name[sizeof(sensor->name) - 1] = 0;
+  sensor->version = 1;
+  sensor->sensor_id = _sensorID;
+  sensor->type = SENSOR_TYPE_ACCELEROMETER;
+  sensor->min_delay = 0;
+  sensor->min_value = -294.1995F; /*  -30g = 294.1995 m/s^2  */
+  sensor->max_value = 294.1995F;  /* 30g = 294.1995 m/s^2  */
+  sensor->resolution =
+      0.122; /* 8192LSB/1000 mG -> 8.192 LSB/ mG => 0.122 mG/LSB at +-4g */
+}
+
+/**************************************************************************/
+/*!
+    @brief  Gets the accelerometer as a standard sensor event
+    @param  event Sensor event object that will be populated
+    @returns True
+*/
+/**************************************************************************/
+bool Adafruit_ICM20X_Accelerometer::getEvent(sensors_event_t *event) {
+  _theICM20X->_read();
+  _theICM20X->fillAccelEvent(event, millis());
+
+  return true;
+}
+
+/**************************************************************************/
+/*!
+    @brief  Gets the sensor_t data for the ICM20X's gyroscope sensor
+*/
+/**************************************************************************/
+void Adafruit_ICM20X_Gyro::getSensor(sensor_t *sensor) {
+  /* Clear the sensor_t object */
+  memset(sensor, 0, sizeof(sensor_t));
+
+  /* Insert the sensor name in the fixed length char array */
+  strncpy(sensor->name, "ICM20X_G", sizeof(sensor->name) - 1);
+  sensor->name[sizeof(sensor->name) - 1] = 0;
+  sensor->version = 1;
+  sensor->sensor_id = _sensorID;
+  sensor->type = SENSOR_TYPE_GYROSCOPE;
+  sensor->min_delay = 0;
+  sensor->min_value = -69.81; /* -4000 dps -> rad/s (radians per second) */
+  sensor->max_value = +69.81;
+  sensor->resolution = 2.665e-7; /* 65.5 LSB/DPS */
+}
+
+/**************************************************************************/
+/*!
+    @brief  Gets the gyroscope as a standard sensor event
+    @param  event Sensor event object that will be populated
+    @returns True
+*/
+/**************************************************************************/
+bool Adafruit_ICM20X_Gyro::getEvent(sensors_event_t *event) {
+  _theICM20X->_read();
+  _theICM20X->fillGyroEvent(event, millis());
+
+  return true;
+}
+
+/**************************************************************************/
+/*!
+    @brief  Gets the sensor_t data for the ICM20X's magnetometer sensor
+*/
+/**************************************************************************/
+void Adafruit_ICM20X_Magnetometer::getSensor(sensor_t *sensor) {
+  /* Clear the sensor_t object */
+  memset(sensor, 0, sizeof(sensor_t));
+
+  /* Insert the sensor name in the fixed length char array */
+  strncpy(sensor->name, "ICM20X_M", sizeof(sensor->name) - 1);
+  sensor->name[sizeof(sensor->name) - 1] = 0;
+  sensor->version = 1;
+  sensor->sensor_id = _sensorID;
+  sensor->type = SENSOR_TYPE_MAGNETIC_FIELD;
+  sensor->min_delay = 0;
+  sensor->min_value = -4900;
+  sensor->max_value = 4900;
+  sensor->resolution = 0.6667;
+}
+
+/**************************************************************************/
+/*!
+    @brief  Gets the magnetometer as a standard sensor event
+    @param  event Sensor event object that will be populated
+    @returns True
+*/
+/**************************************************************************/
+bool Adafruit_ICM20X_Magnetometer::getEvent(sensors_event_t *event) {
+  _theICM20X->_read();
+  _theICM20X->fillMagEvent(event, millis());
+
+  return true;
+}
+
+/**************************************************************************/
+/*!
+    @brief  Gets the sensor_t data for the ICM20X's tenperature
+*/
+/**************************************************************************/
+void Adafruit_ICM20X_Temp::getSensor(sensor_t *sensor) {
+  /* Clear the sensor_t object */
+  memset(sensor, 0, sizeof(sensor_t));
+
+  /* Insert the sensor name in the fixed length char array */
+  strncpy(sensor->name, "ICM20X_T", sizeof(sensor->name) - 1);
+  sensor->name[sizeof(sensor->name) - 1] = 0;
+  sensor->version = 1;
+  sensor->sensor_id = _sensorID;
+  sensor->type = SENSOR_TYPE_AMBIENT_TEMPERATURE;
+  sensor->min_delay = 0;
+  sensor->min_value = -40;
+  sensor->max_value = 85;
+  sensor->resolution = 0.0029952; /* 333.87 LSB/C => 1/333.87 C/LSB */
+}
+
+/**************************************************************************/
+/*!
+    @brief  Gets the temperature as a standard sensor event
+    @param  event Sensor event object that will be populated
+    @returns True
+*/
+/**************************************************************************/
+bool Adafruit_ICM20X_Temp::getEvent(sensors_event_t *event) {
+  _theICM20X->_read();
+  _theICM20X->fillTempEvent(event, millis());
+
+  return true;
+}

Embedded/libraries/Adafruit_ICM20X/Adafruit_ICM20X.h

@@ -0,0 +1,282 @@
+/*!
+ *  @file Adafruit_ICM20X.h
+ *
+ * 	I2C Driver for the Adafruit ICM20X 6-DoF Wide-Range Accelerometer and
+ *Gyro library
+ *
+ * 	This is a library for the Adafruit ICM20X breakouts:
+ * 	https://www.adafruit.com/product/4464
+ * 	https://www.adafruit.com/product/4554
+ *
+ * 	Adafruit invests time and resources providing this open source code,
+ *  please support Adafruit and open-source hardware by purchasing products from
+ * 	Adafruit!
+ *
+ *
+ *	BSD license (see license.txt)
+ */
+
+#ifndef _ADAFRUIT_ICM20X_H
+#define _ADAFRUIT_ICM20X_H
+
+#include "Arduino.h"
+#include <Adafruit_BusIO_Register.h>
+#include <Adafruit_I2CDevice.h>
+#include <Adafruit_Sensor.h>
+#include <Wire.h>
+
+// Misc configuration macros
+#define I2C_MASTER_RESETS_BEFORE_FAIL                                          \
+  5 ///< The number of times to try resetting a stuck I2C master before giving
+    ///< up
+#define NUM_FINISHED_CHECKS                                                    \
+  100 ///< How many times to poll I2C_SLV4_DONE before giving up and resetting
+
+// Bank 0
+#define ICM20X_B0_WHOAMI 0x00         ///< Chip ID register
+#define ICM20X_B0_USER_CTRL 0x03      ///< User Control Reg. Includes I2C Master
+#define ICM20X_B0_LP_CONFIG 0x05      ///< Low Power config
+#define ICM20X_B0_REG_INT_PIN_CFG 0xF ///< Interrupt config register
+#define ICM20X_B0_REG_INT_ENABLE 0x10 ///< Interrupt enable register 0
+#define ICM20X_B0_REG_INT_ENABLE_1 0x11 ///< Interrupt enable register 1
+#define ICM20X_B0_I2C_MST_STATUS                                               \
+  0x17 ///< Records if I2C master bus data is finished
+#define ICM20X_B0_REG_BANK_SEL 0x7F ///< register bank selection register
+#define ICM20X_B0_PWR_MGMT_1 0x06   ///< primary power management register
+#define ICM20X_B0_ACCEL_XOUT_H 0x2D ///< first byte of accel data
+#define ICM20X_B0_GYRO_XOUT_H 0x33  ///< first byte of accel data
+
+// Bank 2
+#define ICM20X_B2_GYRO_SMPLRT_DIV 0x00    ///< Gyroscope data rate divisor
+#define ICM20X_B2_GYRO_CONFIG_1 0x01      ///< Gyro config for range setting
+#define ICM20X_B2_ACCEL_SMPLRT_DIV_1 0x10 ///< Accel data rate divisor MSByte
+#define ICM20X_B2_ACCEL_SMPLRT_DIV_2 0x11 ///< Accel data rate divisor LSByte
+#define ICM20X_B2_ACCEL_CONFIG_1 0x14     ///< Accel config for setting range
+
+// Bank 3
+#define ICM20X_B3_I2C_MST_ODR_CONFIG 0x0 ///< Sets ODR for I2C master bus
+#define ICM20X_B3_I2C_MST_CTRL 0x1       ///< I2C master bus config
+#define ICM20X_B3_I2C_MST_DELAY_CTRL 0x2 ///< I2C master bus config
+#define ICM20X_B3_I2C_SLV0_ADDR                                                \
+  0x3 ///< Sets I2C address for I2C master bus slave 0
+#define ICM20X_B3_I2C_SLV0_REG                                                 \
+  0x4 ///< Sets register address for I2C master bus slave 0
+#define ICM20X_B3_I2C_SLV0_CTRL 0x5 ///< Controls for I2C master bus slave 0
+#define ICM20X_B3_I2C_SLV0_DO 0x6   ///< Sets I2C master bus slave 0 data out
+
+#define ICM20X_B3_I2C_SLV4_ADDR                                                \
+  0x13 ///< Sets I2C address for I2C master bus slave 4
+#define ICM20X_B3_I2C_SLV4_REG                                                 \
+  0x14 ///< Sets register address for I2C master bus slave 4
+#define ICM20X_B3_I2C_SLV4_CTRL 0x15 ///< Controls for I2C master bus slave 4
+#define ICM20X_B3_I2C_SLV4_DO 0x16   ///< Sets I2C master bus slave 4 data out
+#define ICM20X_B3_I2C_SLV4_DI 0x17   ///< Sets I2C master bus slave 4 data in
+
+#define ICM20948_CHIP_ID 0xEA ///< ICM20948 default device id from WHOAMI
+#define ICM20649_CHIP_ID 0xE1 ///< ICM20649 default device id from WHOAMI
+
+/** Options for `enableAccelDLPF` */
+typedef enum {
+  ICM20X_ACCEL_FREQ_246_0_HZ = 0x1,
+  ICM20X_ACCEL_FREQ_111_4_HZ = 0x2,
+  ICM20X_ACCEL_FREQ_50_4_HZ = 0x3,
+  ICM20X_ACCEL_FREQ_23_9_HZ = 0x4,
+  ICM20X_ACCEL_FREQ_11_5_HZ = 0x5,
+  ICM20X_ACCEL_FREQ_5_7_HZ = 0x6,
+  ICM20X_ACCEL_FREQ_473_HZ = 0x7,
+} icm20x_accel_cutoff_t;
+
+/** Options for `enableGyroDLPF` */
+typedef enum {
+  ICM20X_GYRO_FREQ_196_6_HZ = 0x0,
+  ICM20X_GYRO_FREQ_151_8_HZ = 0x1,
+  ICM20X_GYRO_FREQ_119_5_HZ = 0x2,
+  ICM20X_GYRO_FREQ_51_2_HZ = 0x3,
+  ICM20X_GYRO_FREQ_23_9_HZ = 0x4,
+  ICM20X_GYRO_FREQ_11_6_HZ = 0x5,
+  ICM20X_GYRO_FREQ_5_7_HZ = 0x6,
+  ICM20X_GYRO_FREQ_361_4_HZ = 0x7,
+
+} icm20x_gyro_cutoff_t;
+
+class Adafruit_ICM20X;
+
+/** Adafruit Unified Sensor interface for accelerometer component of ICM20X */
+class Adafruit_ICM20X_Accelerometer : public Adafruit_Sensor {
+public:
+  /** @brief Create an Adafruit_Sensor compatible object for the accelerometer
+     sensor
+      @param parent A pointer to the ICM20X class */
+  Adafruit_ICM20X_Accelerometer(Adafruit_ICM20X *parent) {
+    _theICM20X = parent;
+  }
+  bool getEvent(sensors_event_t *);
+  void getSensor(sensor_t *);
+
+private:
+  int _sensorID = 0x20A;
+  Adafruit_ICM20X *_theICM20X = NULL;
+};
+
+/** Adafruit Unified Sensor interface for gyro component of ICM20X */
+class Adafruit_ICM20X_Gyro : public Adafruit_Sensor {
+public:
+  /** @brief Create an Adafruit_Sensor compatible object for the gyro sensor
+      @param parent A pointer to the ICM20X class */
+  Adafruit_ICM20X_Gyro(Adafruit_ICM20X *parent) { _theICM20X = parent; }
+  bool getEvent(sensors_event_t *);
+  void getSensor(sensor_t *);
+
+private:
+  int _sensorID = 0x20B;
+  Adafruit_ICM20X *_theICM20X = NULL;
+};
+
+/** Adafruit Unified Sensor interface for magnetometer component of ICM20X */
+class Adafruit_ICM20X_Magnetometer : public Adafruit_Sensor {
+public:
+  /** @brief Create an Adafruit_Sensor compatible object for the magnetometer
+     sensor
+      @param parent A pointer to the ICM20X class */
+  Adafruit_ICM20X_Magnetometer(Adafruit_ICM20X *parent) { _theICM20X = parent; }
+  bool getEvent(sensors_event_t *);
+  void getSensor(sensor_t *);
+
+private:
+  int _sensorID = 0x20C;
+  Adafruit_ICM20X *_theICM20X = NULL;
+};
+
+/** Adafruit Unified Sensor interface for temperature component of ICM20X */
+class Adafruit_ICM20X_Temp : public Adafruit_Sensor {
+public:
+  /** @brief Create an Adafruit_Sensor compatible object for the temp sensor
+      @param parent A pointer to the ICM20X class */
+  Adafruit_ICM20X_Temp(Adafruit_ICM20X *parent) { _theICM20X = parent; }
+  bool getEvent(sensors_event_t *);
+  void getSensor(sensor_t *);
+
+private:
+  int _sensorID = 0x20D;
+  Adafruit_ICM20X *_theICM20X = NULL;
+};
+
+/*!
+ *    @brief  Class that stores state and functions for interacting with
+ *            the ST ICM20X 6-DoF Accelerometer and Gyro
+ */
+class Adafruit_ICM20X {
+public:
+  Adafruit_ICM20X();
+  ~Adafruit_ICM20X();
+
+  bool begin_SPI(uint8_t cs_pin, SPIClass *theSPI = &SPI,
+                 int32_t sensor_id = 0);
+  bool begin_SPI(int8_t cs_pin, int8_t sck_pin, int8_t miso_pin,
+                 int8_t mosi_pin, int32_t sensor_id = 0);
+
+  uint8_t getGyroRateDivisor(void);
+  void setGyroRateDivisor(uint8_t new_gyro_divisor);
+
+  uint16_t getAccelRateDivisor(void);
+  void setAccelRateDivisor(uint16_t new_accel_divisor);
+
+  bool enableAccelDLPF(bool enable, icm20x_accel_cutoff_t cutoff_freq);
+  bool enableGyrolDLPF(bool enable, icm20x_gyro_cutoff_t cutoff_freq);
+
+  void reset(void);
+
+  // TODO: bool-ify
+  void setInt1ActiveLow(bool active_low);
+  void setInt2ActiveLow(bool active_low);
+
+  Adafruit_Sensor *getAccelerometerSensor(void);
+  Adafruit_Sensor *getGyroSensor(void);
+  Adafruit_Sensor *getMagnetometerSensor(void);
+  Adafruit_Sensor *getTemperatureSensor(void);
+
+  bool getEvent(sensors_event_t *accel, sensors_event_t *gyro,
+                sensors_event_t *temp, sensors_event_t *mag = NULL);
+
+  uint8_t readExternalRegister(uint8_t slv_addr, uint8_t reg_addr);
+  bool writeExternalRegister(uint8_t slv_addr, uint8_t reg_addr, uint8_t value);
+  bool configureI2CMaster(void);
+  bool enableI2CMaster(bool enable_i2c_master);
+  void resetI2CMaster(void);
+  void setI2CBypass(bool bypass_i2c);
+
+protected:
+  float temperature, ///< Last reading's temperature (C)
+      accX,          ///< Last reading's accelerometer X axis m/s^2
+      accY,          ///< Last reading's accelerometer Y axis m/s^2
+      accZ,          ///< Last reading's accelerometer Z axis m/s^2
+      gyroX,         ///< Last reading's gyro X axis in rad/s
+      gyroY,         ///< Last reading's gyro Y axis in rad/s
+      gyroZ,         ///< Last reading's gyro Z axis in rad/s
+      magX,          ///< Last reading's mag X axis in rad/s
+      magY,          ///< Last reading's mag Y axis in rad/s
+      magZ;          ///< Last reading's mag Z axis in rad/s
+
+  Adafruit_I2CDevice *i2c_dev = NULL; ///< Pointer to I2C bus interface
+  Adafruit_SPIDevice *spi_dev = NULL; ///< Pointer to SPI bus interface
+
+  Adafruit_ICM20X_Accelerometer *accel_sensor =
+      NULL;                                 ///< Accelerometer data object
+  Adafruit_ICM20X_Gyro *gyro_sensor = NULL; ///< Gyro data object
+  Adafruit_ICM20X_Magnetometer *mag_sensor =
+      NULL;                                 ///< Magnetometer sensor data object
+  Adafruit_ICM20X_Temp *temp_sensor = NULL; ///< Temp sensor data object
+  uint16_t _sensorid_accel,                 ///< ID number for accelerometer
+      _sensorid_gyro,                       ///< ID number for gyro
+      _sensorid_mag,                        ///< ID number for mag
+      _sensorid_temp;                       ///< ID number for temperature
+
+  void _read(void);
+  virtual void scaleValues(void);
+  virtual bool begin_I2C(uint8_t i2c_add, TwoWire *wire, int32_t sensor_id);
+  // virtual bool _init(int32_t sensor_id);
+  bool _init(int32_t sensor_id);
+  int16_t rawAccX, ///< temp variables
+      rawAccY,     ///< temp variables
+      rawAccZ,     ///< temp variables
+      rawTemp,     ///< temp variables
+      rawGyroX,    ///< temp variables
+      rawGyroY,    ///< temp variables
+      rawGyroZ,    ///< temp variables
+      rawMagX,     ///< temp variables
+      rawMagY,     ///< temp variables
+      rawMagZ;     ///< temp variables
+
+  uint8_t current_accel_range; ///< accelerometer range cache
+  uint8_t current_gyro_range;  ///< gyro range cache
+  // virtual void _setBank(uint8_t bank_number);
+  void _setBank(uint8_t bank_number);
+
+  uint8_t readAccelRange(void);
+  void writeAccelRange(uint8_t new_accel_range);
+
+  uint8_t readGyroRange(void);
+  void writeGyroRange(uint8_t new_gyro_range);
+
+private:
+  friend class Adafruit_ICM20X_Accelerometer; ///< Gives access to private
+                                              ///< members to Accelerometer
+                                              ///< data object
+  friend class Adafruit_ICM20X_Gyro; ///< Gives access to private members to
+                                     ///< Gyro data object
+  friend class Adafruit_ICM20X_Magnetometer; ///< Gives access to private
+                                             ///< members to Magnetometer data
+                                             ///< object
+
+  friend class Adafruit_ICM20X_Temp; ///< Gives access to private members to
+                                     ///< Temp data object
+
+  void fillAccelEvent(sensors_event_t *accel, uint32_t timestamp);
+  void fillGyroEvent(sensors_event_t *gyro, uint32_t timestamp);
+  void fillTempEvent(sensors_event_t *temp, uint32_t timestamp);
+  void fillMagEvent(sensors_event_t *mag, uint32_t timestamp);
+  uint8_t auxillaryRegisterTransaction(bool read, uint8_t slv_addr,
+                                       uint8_t reg_addr, uint8_t value = -1);
+};
+
+#endif

Embedded/libraries/Adafruit_ICM20X/README.md

@@ -0,0 +1,58 @@
+# Adafruit ICM20X [![Build Status](https://github.com/adafruit/Adafruit_ICM20X/workflows/Arduino%20Library%20CI/badge.svg)](https://github.com/adafruit/Adafruit_ICM20X/actions)[![Documentation](https://github.com/adafruit/ci-arduino/blob/master/assets/doxygen_badge.svg)](http://adafruit.github.io/Adafruit_ICM20X/html/index.html)
+
+This is the Adafruit ICM20X Arduino Library for use with the TDK Invensense ICM20649 and ICM20948 motion sensors.
+
+
+### Tested and works great with the following Adafruit ICM20X Family Breakout Boards:
+
+[<img src="assets/649.jpg?raw=true" width="300px">](https://www.adafruit.com/products/4464)
+[<img src="assets/948.png?raw=true" width="300px">](https://www.adafruit.com/products/4554)
+
+
+#### Compatible sensor breakouts available in the Adafruit store:
+* [Adafruit ICM-20649 Wide Range ±30g ±4000dps 6-DoF IMU](https://www.adafruit.com/products/4464)
+* [Adafruit ICM-20948 9-DoF IMU](https://www.adafruit.com/products/4554)
+
+Adafruit invests time and resources providing this open source code, please support Adafruit and open-source hardware by purchasing products from Adafruit!
+
+# Installation
+To install, use the Arduino Library Manager and search for "Adafruit ICM20X" and install the library.
+
+## Dependencies
+ * [Adafruit BusIO](https://github.com/adafruit/Adafruit_BusIO)
+ * [Adafruit Unified Sensor Driver](https://github.com/adafruit/Adafruit_Sensor)
+
+# Contributing
+
+Contributions are welcome! Please read our [Code of Conduct](https://github.com/adafruit/Adafruit_ICM20X/blob/master/CODE_OF_CONDUCT.md>)
+before contributing to help this project stay welcoming.
+
+## Documentation and doxygen
+Documentation is produced by doxygen. Contributions should include documentation for any new code added.
+
+Some examples of how to use doxygen can be found in these guide pages:
+
+https://learn.adafruit.com/the-well-automated-arduino-library/doxygen
+
+https://learn.adafruit.com/the-well-automated-arduino-library/doxygen-tips
+
+## Formatting and clang-format
+This library uses [`clang-format`](https://releases.llvm.org/download.html) to standardize the formatting of `.cpp` and `.h` files. 
+Contributions should be formatted using `clang-format`:
+
+The `-i` flag will make the changes to the file.
+```bash
+clang-format -i *.cpp *.h
+```
+If you prefer to make the changes yourself, running `clang-format` without the `-i` flag will print out a formatted version of the file. You can save this to a file and diff it against the original to see the changes.
+
+Note that the formatting output by `clang-format` is what the automated formatting checker will expect. Any diffs from this formatting will result in a failed build until they are addressed. Using the `-i` flag is highly recommended.
+
+### clang-format resources
+  * [Binary builds and source available on the LLVM downloads page](https://releases.llvm.org/download.html)
+  * [Documentation and IDE integration](https://clang.llvm.org/docs/ClangFormat.html)
+
+## About this Driver
+Written by Bryan Siepert for Adafruit Industries.
+BSD license, check license.txt for more information
+All text above must be included in any redistribution

Embedded/libraries/Adafruit_ICM20X/code-of-conduct.md

@@ -0,0 +1,127 @@
+# Adafruit Community Code of Conduct
+
+## Our Pledge
+
+In the interest of fostering an open and welcoming environment, we as
+contributors and leaders pledge to making participation in our project and
+our community a harassment-free experience for everyone, regardless of age, body
+size, disability, ethnicity, gender identity and expression, level or type of
+experience, education, socio-economic status, nationality, personal appearance,
+race, religion, or sexual identity and orientation.
+
+## Our Standards
+
+We are committed to providing a friendly, safe and welcoming environment for
+all.
+
+Examples of behavior that contributes to creating a positive environment
+include:
+
+* Be kind and courteous to others
+* Using welcoming and inclusive language
+* Being respectful of differing viewpoints and experiences
+* Collaborating with other community members
+* Gracefully accepting constructive criticism
+* Focusing on what is best for the community
+* Showing empathy towards other community members
+
+Examples of unacceptable behavior by participants include:
+
+* The use of sexualized language or imagery and sexual attention or advances
+* The use of inappropriate images, including in a community member's avatar
+* The use of inappropriate language, including in a community member's nickname
+* Any spamming, flaming, baiting or other attention-stealing behavior
+* Excessive or unwelcome helping; answering outside the scope of the question
+  asked
+* Trolling, insulting/derogatory comments, and personal or political attacks
+* Public or private harassment
+* Publishing others' private information, such as a physical or electronic
+  address, without explicit permission
+* Other conduct which could reasonably be considered inappropriate
+
+The goal of the standards and moderation guidelines outlined here is to build
+and maintain a respectful community. We ask that you don’t just aim to be
+"technically unimpeachable", but rather try to be your best self. 
+
+We value many things beyond technical expertise, including collaboration and
+supporting others within our community. Providing a positive experience for
+other community members can have a much more significant impact than simply
+providing the correct answer.
+
+## Our Responsibilities
+
+Project leaders are responsible for clarifying the standards of acceptable
+behavior and are expected to take appropriate and fair corrective action in
+response to any instances of unacceptable behavior.
+
+Project leaders have the right and responsibility to remove, edit, or
+reject messages, comments, commits, code, issues, and other contributions
+that are not aligned to this Code of Conduct, or to ban temporarily or
+permanently any community member for other behaviors that they deem
+inappropriate, threatening, offensive, or harmful.
+
+## Moderation
+
+Instances of behaviors that violate the Adafruit Community Code of Conduct
+may be reported by any member of the community. Community members are
+encouraged to report these situations, including situations they witness
+involving other community members.
+
+You may report in the following ways:
+
+In any situation, you may send an email to <support@adafruit.com>.
+
+On the Adafruit Discord, you may send an open message from any channel
+to all Community Helpers by tagging @community helpers. You may also send an
+open message from any channel, or a direct message to @kattni#1507,
+@tannewt#4653, @Dan Halbert#1614, @cater#2442, @sommersoft#0222, or
+@Andon#8175.
+
+Email and direct message reports will be kept confidential.
+
+In situations on Discord where the issue is particularly egregious, possibly
+illegal, requires immediate action, or violates the Discord terms of service,
+you should also report the message directly to Discord.
+
+These are the steps for upholding our community’s standards of conduct.
+
+1. Any member of the community may report any situation that violates the
+Adafruit Community Code of Conduct. All reports will be reviewed and
+investigated.
+2. If the behavior is an egregious violation, the community member who
+committed the violation may be banned immediately, without warning.
+3. Otherwise, moderators will first respond to such behavior with a warning.
+4. Moderators follow a soft "three strikes" policy - the community member may
+be given another chance, if they are receptive to the warning and change their
+behavior.
+5. If the community member is unreceptive or unreasonable when warned by a
+moderator, or the warning goes unheeded, they may be banned for a first or
+second offense. Repeated offenses will result in the community member being
+banned.
+
+## Scope
+
+This Code of Conduct and the enforcement policies listed above apply to all
+Adafruit Community venues. This includes but is not limited to any community
+spaces (both public and private), the entire Adafruit Discord server, and
+Adafruit GitHub repositories. Examples of Adafruit Community spaces include
+but are not limited to meet-ups, audio chats on the Adafruit Discord, or
+interaction at a conference.
+
+This Code of Conduct applies both within project spaces and in public spaces
+when an individual is representing the project or its community. As a community
+member, you are representing our community, and are expected to behave
+accordingly.
+
+## Attribution
+
+This Code of Conduct is adapted from the [Contributor Covenant][homepage],
+version 1.4, available at
+<https://www.contributor-covenant.org/version/1/4/code-of-conduct.html>,
+and the [Rust Code of Conduct](https://www.rust-lang.org/en-US/conduct.html).
+
+For other projects adopting the Adafruit Community Code of
+Conduct, please contact the maintainers of those projects for enforcement.
+If you wish to use this code of conduct for your own project, consider
+explicitly mentioning your moderation policy or making a copy with your
+own moderation policy so as to avoid confusion.

Embedded/libraries/Adafruit_ICM20X/examples/adafruit_icm20649_test/adafruit_icm20649_test.ino

@@ -0,0 +1,138 @@
+// Basic demo for accelerometer readings from Adafruit ICM20649
+
+#include <Adafruit_ICM20X.h>
+#include <Adafruit_ICM20649.h>
+#include <Adafruit_Sensor.h>
+#include <Wire.h>
+
+Adafruit_ICM20649 icm;
+uint16_t measurement_delay_us = 65535; // Delay between measurements for testing
+// For SPI mode, we need a CS pin
+#define ICM_CS 10
+// For software-SPI mode we need SCK/MOSI/MISO pins
+#define ICM_SCK 13
+#define ICM_MISO 12
+#define ICM_MOSI 11
+
+void setup(void) {
+  Serial.begin(115200);
+  while (!Serial)
+    delay(10); // will pause Zero, Leonardo, etc until serial console opens
+
+  Serial.println("Adafruit ICM20649 test!");
+
+  // Try to initialize!
+  if (!icm.begin_I2C()) {
+    // if (!icm.begin_SPI(ICM_CS)) {
+    // if (!icm.begin_SPI(ICM_CS, ICM_SCK, ICM_MISO, ICM_MOSI)) {
+
+    Serial.println("Failed to find ICM20649 chip");
+    while (1) {
+      delay(10);
+    }
+  }
+  Serial.println("ICM20649 Found!");
+  // icm.setAccelRange(ICM20649_ACCEL_RANGE_4_G);
+  Serial.print("Accelerometer range set to: ");
+  switch (icm.getAccelRange()) {
+  case ICM20649_ACCEL_RANGE_4_G:
+    Serial.println("+-4G");
+    break;
+  case ICM20649_ACCEL_RANGE_8_G:
+    Serial.println("+-8G");
+    break;
+  case ICM20649_ACCEL_RANGE_16_G:
+    Serial.println("+-16G");
+    break;
+  case ICM20649_ACCEL_RANGE_30_G:
+    Serial.println("+-30G");
+    break;
+  }
+
+  // icm.setGyroRange(ICM20649_GYRO_RANGE_500_DPS);
+  Serial.print("Gyro range set to: ");
+  switch (icm.getGyroRange()) {
+  case ICM20649_GYRO_RANGE_500_DPS:
+    Serial.println("500 degrees/s");
+    break;
+  case ICM20649_GYRO_RANGE_1000_DPS:
+    Serial.println("1000 degrees/s");
+    break;
+  case ICM20649_GYRO_RANGE_2000_DPS:
+    Serial.println("2000 degrees/s");
+    break;
+  case ICM20649_GYRO_RANGE_4000_DPS:
+    Serial.println("4000 degrees/s");
+    break;
+  }
+
+  //  icm.setAccelRateDivisor(4095);
+  uint16_t accel_divisor = icm.getAccelRateDivisor();
+  float accel_rate = 1125 / (1.0 + accel_divisor);
+
+  Serial.print("Accelerometer data rate divisor set to: ");
+  Serial.println(accel_divisor);
+  Serial.print("Accelerometer data rate (Hz) is approximately: ");
+  Serial.println(accel_rate);
+
+  //  icm.setGyroRateDivisor(255);
+  uint8_t gyro_divisor = icm.getGyroRateDivisor();
+  float gyro_rate = 1100 / (1.0 + gyro_divisor);
+
+  Serial.print("Gyro data rate divisor set to: ");
+  Serial.println(gyro_divisor);
+  Serial.print("Gyro data rate (Hz) is approximately: ");
+  Serial.println(gyro_rate);
+  Serial.println();
+}
+
+void loop() {
+
+  //  /* Get a new normalized sensor event */
+  sensors_event_t accel;
+  sensors_event_t gyro;
+  sensors_event_t temp;
+  icm.getEvent(&accel, &gyro, &temp);
+
+  Serial.print("\t\tTemperature ");
+  Serial.print(temp.temperature);
+  Serial.println(" deg C");
+
+  /* Display the results (acceleration is measured in m/s^2) */
+  Serial.print("\t\tAccel X: ");
+  Serial.print(accel.acceleration.x);
+  Serial.print(" \tY: ");
+  Serial.print(accel.acceleration.y);
+  Serial.print(" \tZ: ");
+  Serial.print(accel.acceleration.z);
+  Serial.println(" m/s^2 ");
+
+  /* Display the results (acceleration is measured in m/s^2) */
+  Serial.print("\t\tGyro X: ");
+  Serial.print(gyro.gyro.x);
+  Serial.print(" \tY: ");
+  Serial.print(gyro.gyro.y);
+  Serial.print(" \tZ: ");
+  Serial.print(gyro.gyro.z);
+  Serial.println(" radians/s ");
+  Serial.println();
+
+  delay(100);
+
+  //  Serial.print(temp.temperature);
+  //
+  //  Serial.print(",");
+  //
+  //  Serial.print(accel.acceleration.x);
+  //  Serial.print(","); Serial.print(accel.acceleration.y);
+  //  Serial.print(","); Serial.print(accel.acceleration.z);
+  //
+  //  Serial.print(",");
+  //  Serial.print(gyro.gyro.x);
+  //  Serial.print(","); Serial.print(gyro.gyro.y);
+  //  Serial.print(","); Serial.print(gyro.gyro.z);
+
+  //  Serial.println();
+  //
+  //  delayMicroseconds(measurement_delay_us);
+}

Embedded/libraries/Adafruit_ICM20X/examples/adafruit_icm20948_test/adafruit_icm20948_test.ino

@@ -0,0 +1,177 @@
+// Basic demo for accelerometer readings from Adafruit ICM20948
+
+#include <Adafruit_ICM20X.h>
+#include <Adafruit_ICM20948.h>
+#include <Adafruit_Sensor.h>
+#include <Wire.h>
+
+Adafruit_ICM20948 icm;
+uint16_t measurement_delay_us = 65535; // Delay between measurements for testing
+// For SPI mode, we need a CS pin
+#define ICM_CS 10
+// For software-SPI mode we need SCK/MOSI/MISO pins
+#define ICM_SCK 13
+#define ICM_MISO 12
+#define ICM_MOSI 11
+
+void setup(void) {
+  Serial.begin(115200);
+  while (!Serial)
+    delay(10); // will pause Zero, Leonardo, etc until serial console opens
+
+  Serial.println("Adafruit ICM20948 test!");
+
+  // Try to initialize!
+  if (!icm.begin_I2C()) {
+    // if (!icm.begin_SPI(ICM_CS)) {
+    // if (!icm.begin_SPI(ICM_CS, ICM_SCK, ICM_MISO, ICM_MOSI)) {
+
+    Serial.println("Failed to find ICM20948 chip");
+    while (1) {
+      delay(10);
+    }
+  }
+  Serial.println("ICM20948 Found!");
+  // icm.setAccelRange(ICM20948_ACCEL_RANGE_16_G);
+  Serial.print("Accelerometer range set to: ");
+  switch (icm.getAccelRange()) {
+  case ICM20948_ACCEL_RANGE_2_G:
+    Serial.println("+-2G");
+    break;
+  case ICM20948_ACCEL_RANGE_4_G:
+    Serial.println("+-4G");
+    break;
+  case ICM20948_ACCEL_RANGE_8_G:
+    Serial.println("+-8G");
+    break;
+  case ICM20948_ACCEL_RANGE_16_G:
+    Serial.println("+-16G");
+    break;
+  }
+  Serial.println("OK");
+
+  // icm.setGyroRange(ICM20948_GYRO_RANGE_2000_DPS);
+  Serial.print("Gyro range set to: ");
+  switch (icm.getGyroRange()) {
+  case ICM20948_GYRO_RANGE_250_DPS:
+    Serial.println("250 degrees/s");
+    break;
+  case ICM20948_GYRO_RANGE_500_DPS:
+    Serial.println("500 degrees/s");
+    break;
+  case ICM20948_GYRO_RANGE_1000_DPS:
+    Serial.println("1000 degrees/s");
+    break;
+  case ICM20948_GYRO_RANGE_2000_DPS:
+    Serial.println("2000 degrees/s");
+    break;
+  }
+
+  //  icm.setAccelRateDivisor(4095);
+  uint16_t accel_divisor = icm.getAccelRateDivisor();
+  float accel_rate = 1125 / (1.0 + accel_divisor);
+
+  Serial.print("Accelerometer data rate divisor set to: ");
+  Serial.println(accel_divisor);
+  Serial.print("Accelerometer data rate (Hz) is approximately: ");
+  Serial.println(accel_rate);
+
+  //  icm.setGyroRateDivisor(255);
+  uint8_t gyro_divisor = icm.getGyroRateDivisor();
+  float gyro_rate = 1100 / (1.0 + gyro_divisor);
+
+  Serial.print("Gyro data rate divisor set to: ");
+  Serial.println(gyro_divisor);
+  Serial.print("Gyro data rate (Hz) is approximately: ");
+  Serial.println(gyro_rate);
+
+  // icm.setMagDataRate(AK09916_MAG_DATARATE_10_HZ);
+  Serial.print("Magnetometer data rate set to: ");
+  switch (icm.getMagDataRate()) {
+  case AK09916_MAG_DATARATE_SHUTDOWN:
+    Serial.println("Shutdown");
+    break;
+  case AK09916_MAG_DATARATE_SINGLE:
+    Serial.println("Single/One shot");
+    break;
+  case AK09916_MAG_DATARATE_10_HZ:
+    Serial.println("10 Hz");
+    break;
+  case AK09916_MAG_DATARATE_20_HZ:
+    Serial.println("20 Hz");
+    break;
+  case AK09916_MAG_DATARATE_50_HZ:
+    Serial.println("50 Hz");
+    break;
+  case AK09916_MAG_DATARATE_100_HZ:
+    Serial.println("100 Hz");
+    break;
+  }
+  Serial.println();
+
+}
+
+void loop() {
+
+  //  /* Get a new normalized sensor event */
+  sensors_event_t accel;
+  sensors_event_t gyro;
+  sensors_event_t mag;
+  sensors_event_t temp;
+  icm.getEvent(&accel, &gyro, &temp, &mag);
+
+  Serial.print("\t\tTemperature ");
+  Serial.print(temp.temperature);
+  Serial.println(" deg C");
+
+  /* Display the results (acceleration is measured in m/s^2) */
+  Serial.print("\t\tAccel X: ");
+  Serial.print(accel.acceleration.x);
+  Serial.print(" \tY: ");
+  Serial.print(accel.acceleration.y);
+  Serial.print(" \tZ: ");
+  Serial.print(accel.acceleration.z);
+  Serial.println(" m/s^2 ");
+
+  Serial.print("\t\tMag X: ");
+  Serial.print(mag.magnetic.x);
+  Serial.print(" \tY: ");
+  Serial.print(mag.magnetic.y);
+  Serial.print(" \tZ: ");
+  Serial.print(mag.magnetic.z);
+  Serial.println(" uT");
+
+  /* Display the results (acceleration is measured in m/s^2) */
+  Serial.print("\t\tGyro X: ");
+  Serial.print(gyro.gyro.x);
+  Serial.print(" \tY: ");
+  Serial.print(gyro.gyro.y);
+  Serial.print(" \tZ: ");
+  Serial.print(gyro.gyro.z);
+  Serial.println(" radians/s ");
+  Serial.println();
+
+  delay(100);
+
+  //  Serial.print(temp.temperature);
+  //
+  //  Serial.print(",");
+  //
+  //  Serial.print(accel.acceleration.x);
+  //  Serial.print(","); Serial.print(accel.acceleration.y);
+  //  Serial.print(","); Serial.print(accel.acceleration.z);
+  //
+  //  Serial.print(",");
+  //  Serial.print(gyro.gyro.x);
+  //  Serial.print(","); Serial.print(gyro.gyro.y);
+  //  Serial.print(","); Serial.print(gyro.gyro.z);
+  //
+  //  Serial.print(",");
+  //  Serial.print(mag.magnetic.x);
+  //  Serial.print(","); Serial.print(mag.magnetic.y);
+  //  Serial.print(","); Serial.print(mag.magnetic.z);
+
+  //  Serial.println();
+  //
+  //  delayMicroseconds(measurement_delay_us);
+}