pbio/imu: Implement 3D attitude and fusion of IMU data.

.vscode/tasks.json

@@ -18,6 +18,34 @@
             "type": "shell",
             "command": "make movehub -j"
         },
+        {
+            "label": "build and deploy primehub",
+            "type": "shell",
+            "command": "make",
+            "args": ["-j", "deploy"],
+            "group": {
+                "kind": "build",
+                "isDefault": true
+            },
+            "options": {
+                "cwd": "${workspaceFolder}/bricks/primehub"
+              },
+            "problemMatcher": [
+                {
+                    "owner": "cpp",
+                    "fileLocation": ["relative", "${workspaceFolder}/bricks/primehub"],
+                    "pattern": {
+                        "regexp": "^(.*?):(\\d+):(\\d+):\\s+(warning|error):\\s+(.*)$",
+                        "file": 1,
+                        "line": 2,
+                        "column": 3,
+                        "severity": 4,
+                        "message": 5
+                    }
+                }
+            ],
+            "detail": "Build the PrimeHub project"
+        },
         {
             "label": "build virtualhub",
             "type": "shell",

CHANGELOG.md

@@ -4,17 +4,33 @@
 
 ## [Unreleased]
 
+### Added
+
+- Added optional `calibrated=True` parameter to `acceleration()` and `up()` and
+  `angular_velocity()` methods of the IMU ([support#943]).
+- Implemented `hub.imu.orientation()` to give the rotation matrix of the hub or
+  robot with respect to the inertial frame.
+- Added calibration parameters that can be set for angular velocity offset and
+  scale and acceleration offset and scale.
+
 ### Changed
 
 - The method `DriveBase.angle()` now returns a float ([support#1844]). This
   makes it properly equivalent to `hub.imu.heading`.
+- Re-implemented tilt using the gyro data by default. Pure accelerometer tilt
+  can still be obtained with `hub.imu.tilt(use_gyro=False)`.
+- Re-implemented `hub.imu.heading()` to use optionally use the projection of 3D
+  orientation to improve performance when the hub is lifted off the ground.
+  The 1D-based heading remains the default for now.
 
 ### Fixed
 - Fixed `DriveBase.angle()` getting an incorrectly rounded gyro value, which
   could cause `turn(360)` to be off by a degree ([support#1844]).
+- Fixed `hub` silently ignoring non-orthogonal base axis when it should raise.
 
-[support#1844]: https://github.com/pybricks/support/issues/1844
+[support#943]: https://github.com/pybricks/support/issues/943
 [support#1886]: https://github.com/pybricks/support/issues/1886
+[support#1844]: https://github.com/pybricks/support/issues/1844
 
 ## [3.6.0b2] - 2024-10-15
 

bricks/_common/arm_none_eabi.mk

@@ -78,9 +78,7 @@ include $(PBTOP)/micropython/py/mkenv.mk
 
 # Include common frozen modules.
 ifeq ($(PB_FROZEN_MODULES),1)
-ifneq ("$(wildcard $(PBTOP)/bricks/_common/modules/*.py)","")
-FROZEN_MANIFEST ?= ../_common/manifest.py
-endif
+FROZEN_MANIFEST ?= manifest.py
 endif
 
 # qstr definitions (must come before including py.mk)

bricks/_common/manifest.py

@@ -2,8 +2,5 @@
 
 modules = list(pathlib.Path("./modules").glob("*.py"))
 
-if any(modules):
-    for m in modules:
-        path, file = m.parts
-        print(f"Including {m.stem} as a module.")
-        freeze_as_mpy(path, file)
+for m in modules:
+    freeze_as_mpy(str(m.parent), m.name)

bricks/_common/modules/.gitignore

@@ -1,3 +1,2 @@
 *.py
-!_hub_extra.py
 !_builtin_port_view.py

bricks/cityhub/manifest.py

@@ -0,0 +1 @@
+include("../_common/manifest.py")

bricks/debug/manifest.py

@@ -0,0 +1 @@
+include("../_common/manifest.py")

bricks/essentialhub/manifest.py

@@ -0,0 +1,5 @@
+# Common modules
+include("../_common/manifest.py")
+
+# Essential Hub IMU is the same as the one in Prime Hub
+freeze_as_mpy("../primehub/modules", "_imu_calibrate.py")

bricks/ev3/manifest.py

@@ -0,0 +1 @@
+include("../_common/manifest.py")

bricks/nxt/manifest.py

@@ -0,0 +1 @@
+include("../_common/manifest.py")

bricks/primehub/manifest.py

@@ -0,0 +1,5 @@
+# Common modules
+include("../_common/manifest.py")
+
+freeze_as_mpy("../primehub/modules", "_imu_calibrate.py")
+freeze_as_mpy("../primehub/modules", "_light_matrix.py")

bricks/primehub/modules/_imu_calibrate.py

@@ -0,0 +1,149 @@
+from pybricks.hubs import ThisHub
+from pybricks.parameters import Side, Axis
+from pybricks.tools import wait, vector
+
+
+hub = ThisHub()
+
+
+def beep(freq):
+    try:
+        hub.speaker.beep(freq, 100)
+    except AttributeError:
+        # Technic hub does not have a speaker.
+        pass
+    wait(10)
+
+
+def wait_for_stationary(side):
+    while not hub.imu.stationary() or hub.imu.up() != side:
+        wait(10)
+
+
+up_sides = {
+    Side.FRONT: (0, 0),
+    Side.BACK: (1, 0),
+    Side.LEFT: (2, 1),
+    Side.RIGHT: (3, 1),
+    Side.TOP: (4, 2),
+    Side.BOTTOM: (5, 2),
+}
+
+gravity = [0] * 6
+bias = vector(0, 0, 0)
+
+STATIONARY_COUNT = 1000
+
+
+def roll_over_axis(axis, new_side):
+
+    global bias, bias_count
+
+    print("Roll it towards you, without lifting the hub up!")
+
+    angle_start = hub.imu.rotation(axis, calibrated=False)
+    while hub.imu.up() != new_side or not hub.imu.stationary():
+
+        _, _, z = hub.imu.orientation() * axis
+        if abs(z) > 0.07:
+            print(hub.imu.orientation() * axis)
+            raise RuntimeError("Lifted it!")
+        wait(100)
+
+    uncalibrated_90_deg_rotation = abs(hub.imu.rotation(axis, calibrated=False) - angle_start)
+    if abs(uncalibrated_90_deg_rotation - 90) > 10:
+        raise RuntimeError("Not 90 deg!")
+
+    print("Calibrating...")
+    beep(1000)
+
+    rotation_start = vector(
+        hub.imu.rotation(Axis.X, calibrated=False),
+        hub.imu.rotation(Axis.Y, calibrated=False),
+        hub.imu.rotation(Axis.Z, calibrated=False),
+    )
+
+    acceleration = vector(0, 0, 0)
+
+    for i in range(STATIONARY_COUNT):
+        acceleration += hub.imu.acceleration(calibrated=False)
+        bias += hub.imu.angular_velocity(calibrated=False)
+        wait(1)
+
+    acceleration /= STATIONARY_COUNT
+
+    rotation_end = vector(
+        hub.imu.rotation(Axis.X, calibrated=False),
+        hub.imu.rotation(Axis.Y, calibrated=False),
+        hub.imu.rotation(Axis.Z, calibrated=False),
+    )
+    if abs(rotation_end - rotation_start) > 1:
+        raise RuntimeError("Moved it!")
+
+    side_index, axis_index = up_sides[new_side]
+
+    # Store the gravity value for the current side being up.
+    # We will visit each side several times. We'll divide by the number
+    # of visits later.
+    gravity[side_index] += acceleration[axis_index]
+
+    beep(500)
+
+    return uncalibrated_90_deg_rotation
+
+
+calibrate_x = """
+Going to calibrate X now!
+- Put the hub on the table in front of you.
+- top side (display) facing up
+- right side (ports BDF) towards you.
+"""
+
+calibrate_y = """
+Going to calibrate Y now
+- Put the hub on the table in front of you.
+- top side (display) facing up
+- back side (speaker) towards you.
+"""
+
+calibrate_z = """
+Going to calibrate Z now!
+- Put the hub on the table in front of you.
+- front side (USB port) facing up
+- left side (ports ACE) towards you
+"""
+
+REPEAT = 2
+
+# For each 3-axis run, we will visit each side twice.
+SIDE_COUNT = REPEAT * 2
+
+
+def roll_hub(axis, message, start_side, sides):
+    print(message)
+    wait_for_stationary(start_side)
+    beep()
+    rotation = 0
+    for _ in range(REPEAT):
+        for side in sides:
+            rotation += roll_over_axis(axis, side)
+    return rotation / REPEAT
+
+
+rotation_x = roll_hub(
+    Axis.X, calibrate_x, Side.TOP, [Side.LEFT, Side.BOTTOM, Side.RIGHT, Side.TOP]
+)
+rotation_y = roll_hub(
+    Axis.Y, calibrate_y, Side.TOP, [Side.FRONT, Side.BOTTOM, Side.BACK, Side.TOP]
+)
+rotation_z = roll_hub(
+    Axis.Z, calibrate_z, Side.FRONT, [Side.RIGHT, Side.BACK, Side.LEFT, Side.FRONT]
+)
+
+hub.imu.settings(
+    angular_velocity_bias=tuple(bias / SIDE_COUNT / STATIONARY_COUNT / 6),
+    angular_velocity_scale=(rotation_x, rotation_y, rotation_z),
+    acceleration_correction=[g / SIDE_COUNT for g in gravity],
+)
+
+print("Result: ", hub.imu.settings())

bricks/primehub/modules/_light_matrix.py

@@ -0,0 +1,9 @@
+from pybricks.tools import wait
+
+
+def light_matrix_text_async(display, text, on, off):
+    for char in text:
+        display.char(char)
+        yield from wait(on)
+        display.off()
+        yield from wait(off)

bricks/technichub/manifest.py

@@ -0,0 +1,5 @@
+# Common modules
+include("../_common/manifest.py")
+
+# Technic Hub IMU is the same as the one in Prime Hub
+freeze_as_mpy("../primehub/modules", "_imu_calibrate.py")