[Feature] User IMU calibration implementation #1907
Description
We are introducing a builtin routine to calibrate the IMU on all three axes. The user is instructed to place the hub in a particular orientation, and then to rotate the hub towards the user several times. This is repeated for each axis.
The instructions are currently text-only. Ultimately, this should be paired with a visual animation from the Pybricks Code IDE.
To try it out (updated 2024-12-03)
- Install the latest firmware.
- On the REPL, run
import _imu_calibrateand follow the instructions.
To see the results afterwards, do:
print(hub.imu.settings())The values remain on the hub until you install a new firmware. You can then run the calibration again, or simply restore the values you printed above.
Hub specifics
The SPIKE Prime Hub casing is close enough to a rectangular box. You can just rotate it in place, as in the video below.
The Technic Hub sides aren't really flat enough. You can add some Technic elements like Bert did below.
Video
This video is slightly outdated now, but it shows the gist of the procedure.
calibration2.mp4
Original post
Click to see original post
The technical aspects of calibration are being discussed in #943, this issue is more about the user routine: How can we make it easy for the user to calibrate their gyro? This is an alternative to #1678 to be ready for 3D. It is a bit simpler too since you don't need an external reference.
This would have to be done just once and the results would be saved persistently on the hub. You'd have to do it again if you updated the firmware. This will calibrate the accelerometer and gyro all in one go.
The procedure is as follows, three times. It takes a bit of practice, but it is easy to do.
- Place the hub in an indicated starting position.
- Roll it towards you 4 times without lifting it up.
We could animate this visually in a Pybricks Code pane. This is roughly what the user would have to do:
calibration2.mp4
(Video is cut off. This repeats one more time for the final axis.)
Instructions
- Install this firmware.
- Run the following. Rough instructions are shown in Pybricks Code. (We'll clean that up later.)
from pybricks.hubs import ThisHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop, Axis
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch, vector
hub = ThisHub()
def wait_for_stationary(side):
while not hub.imu.stationary() or hub.imu.up() != side:
wait(10)
up_sides = {
Side.FRONT: (0, 0, 1),
Side.BACK: (1, 0, -1),
Side.LEFT: (2, 1, 1),
Side.RIGHT: (3, 1, -1),
Side.TOP: (4, 2, 1),
Side.BOTTOM: (5, 2, -1),
}
gravity = [0] * 6
bias_count = 0
bias = vector(0, 0, 0)
_, _, _, old_scale, _, = hub.imu.settings()
def roll_over_axis(axis, new_side, first=False):
global bias, bias_count
if first:
print("Roll it over towards you, without lifting the hub up!")
else:
print("Roll towards you again!")
angle_start = hub.imu.rotation(axis)
while hub.imu.up() != new_side or not hub.imu.stationary():
x, y, z = hub.imu.orientation() * axis
if abs(z) > 0.05:
print(hub.imu.orientation() * axis)
raise RuntimeError("Oops, lifted it!")
wait(100)
rotation = abs(hub.imu.rotation(axis)-angle_start)
if abs(rotation - 90) > 10:
raise RuntimeError("Rotated too far or not far enough!")
print("Nice work. Calibrating now...")
hub.speaker.beep(1000)
wait(10)
rotation_start = vector(
hub.imu.rotation(Axis.X),
hub.imu.rotation(Axis.Y),
hub.imu.rotation(Axis.Z)
)
COUNT = 1000
acceleration = vector(0, 0, 0)
for i in range(COUNT):
acceleration += hub.imu.acceleration(calibrated=False)
bias += hub.imu.angular_velocity(calibrated=False)
bias_count += 1
wait(1)
acceleration /= COUNT
rotation_end = vector(
hub.imu.rotation(Axis.X),
hub.imu.rotation(Axis.Y),
hub.imu.rotation(Axis.Z)
)
if abs(rotation_end - rotation_start) > 1:
raise RuntimeError("Too much rotation while calibrating")
side_index, axis_index, axis_direction = up_sides[new_side]
# Store the gravity value for the current side being up.
# We will visit each side twice. The second time the gravity value will
# already be nonzero, so we can take the average between two to get
# slightly better results while we're here anyway.
acceleration_previous = gravity[side_index]
acceleration_now = acceleration[axis_index]
if abs(acceleration_previous) > 1:
acceleration_now = (acceleration_now + acceleration_previous) / 2
gravity[side_index] = acceleration_now
# Todo, expose unscaled version in api?
unadjusted_rotation = rotation * old_scale[axis_index] / 360
hub.speaker.beep(500)
return unadjusted_rotation
calibrate_x = """
We're 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 = """
We're 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 = """
We're 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
"""
print(calibrate_x)
wait_for_stationary(Side.TOP)
hub.speaker.beep()
rotation_x = roll_over_axis(Axis.X, Side.LEFT, first=True)
rotation_x += roll_over_axis(Axis.X, Side.BOTTOM)
rotation_x += roll_over_axis(Axis.X, Side.RIGHT)
rotation_x += roll_over_axis(Axis.X, Side.TOP)
print(calibrate_y)
wait_for_stationary(Side.TOP)
hub.speaker.beep()
rotation_y = roll_over_axis(Axis.Y, Side.FRONT, first=True)
rotation_y += roll_over_axis(Axis.Y, Side.BOTTOM)
rotation_y += roll_over_axis(Axis.Y, Side.BACK)
rotation_y += roll_over_axis(Axis.Y, Side.TOP)
print(calibrate_z)
wait_for_stationary(Side.FRONT)
hub.speaker.beep()
rotation_z = roll_over_axis(Axis.Z, Side.RIGHT, first=True)
rotation_z += roll_over_axis(Axis.Z, Side.BACK)
rotation_z += roll_over_axis(Axis.Z, Side.LEFT)
rotation_z += roll_over_axis(Axis.Z, Side.FRONT)
print("old: ", hub.imu.settings())
print("applying")
print(bias[0] / bias_count, bias[1] / bias_count, bias[2] / bias_count)
print(rotation_x, rotation_y, rotation_z)
print(gravity)
hub.imu.settings(
angular_velocity_bias = (bias[0] / bias_count, bias[1] / bias_count, bias[2] / bias_count),
angular_velocity_scale = (rotation_x, rotation_y, rotation_z),
acceleration_correction = gravity
)
print("new: ", hub.imu.settings())