2026-07-06 17:31:02 -06:00
|
|
|
import busio
|
|
|
|
|
import adafruit_bno055
|
|
|
|
|
import time
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class ImuMode:
|
|
|
|
|
CONFIG_MODE = 0x00
|
|
|
|
|
ACCONLY_MODE = 0x01
|
|
|
|
|
MAGONLY_MODE = 0x02
|
|
|
|
|
GYRONLY_MODE = 0x03
|
|
|
|
|
ACCMAG_MODE = 0x04
|
|
|
|
|
ACCGYRO_MODE = 0x05
|
|
|
|
|
MAGGYRO_MODE = 0x06
|
|
|
|
|
AMG_MODE = 0x07
|
|
|
|
|
IMUPLUS_MODE = 0x08
|
|
|
|
|
COMPASS_MODE = 0x09
|
|
|
|
|
M4G_MODE = 0x0A
|
|
|
|
|
NDOF_FMC_OFF_MODE = 0x0B
|
|
|
|
|
NDOF_MODE = 0x0C
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class IMU:
|
|
|
|
|
def __init__(self, scl_pin, sda_pin, mode, mag_offset=None, gyro_offset=None, accel_offset=None):
|
|
|
|
|
i2c = busio.I2C(scl_pin, sda_pin)
|
|
|
|
|
self._imu = adafruit_bno055.BNO055_I2C(i2c)
|
|
|
|
|
print(self._imu.axis_remap)
|
|
|
|
|
self._imu.axis_remap = (0, 1, 2, 0, -1, -1)
|
|
|
|
|
self._imu.mode = adafruit_bno055.ACCMAG_MODE
|
|
|
|
|
time.sleep(1)
|
|
|
|
|
self._imu.mode = adafruit_bno055.NDOF_MODE
|
|
|
|
|
if mag_offset:
|
|
|
|
|
self._imu.offsets_magnetometer = mag_offset
|
|
|
|
|
if gyro_offset:
|
|
|
|
|
self._imu.offsets_gyroscope = gyro_offset
|
|
|
|
|
if accel_offset:
|
|
|
|
|
self._imu.offsets_accelerometer = accel_offset
|
|
|
|
|
self._accel_update_time = [0, 0, 0, 0]
|
|
|
|
|
self.accel_updated = False
|
|
|
|
|
self._acceleration = [0, 0, 0]
|
|
|
|
|
self._linear_acceleration = [0, 0, 0]
|
|
|
|
|
self._gravity = [0, 0, 0]
|
|
|
|
|
|
|
|
|
|
self._mag_update_time = [0, 0, 0, 0]
|
|
|
|
|
self.mag_updated = False
|
|
|
|
|
self._mag = [0, 0, 0]
|
|
|
|
|
|
|
|
|
|
self._gyro_update_time = [0, 0, 0, 0]
|
|
|
|
|
self.gyro_updated = False
|
|
|
|
|
self._gyro = [0, 0, 0]
|
|
|
|
|
|
|
|
|
|
self._euler_update_time = [0, 0, 0, 0]
|
|
|
|
|
self.euler_updated = False
|
|
|
|
|
self._euler = [0, 0, 0]
|
|
|
|
|
|
|
|
|
|
self._quat_update_time = [0, 0, 0, 0]
|
|
|
|
|
self.quat_updated = False
|
|
|
|
|
self._quat = [0, 0, 0, 0]
|
|
|
|
|
#self._do_calibration()
|
|
|
|
|
|
|
|
|
|
def _do_calibration(self):
|
|
|
|
|
print("Magnetometer: Perform the figure-eight calibration dance.")
|
|
|
|
|
while not self._imu.calibration_status[3] == 3:
|
|
|
|
|
# Calibration Dance Step One: Magnetometer
|
|
|
|
|
# Move sensor away from magnetic interference or shields
|
|
|
|
|
# Perform the figure-eight until calibrated
|
|
|
|
|
print(f"Mag Calib Status: {100 / 3 * self._imu.calibration_status[3]:3.0f}%")
|
|
|
|
|
time.sleep(1)
|
|
|
|
|
print("... CALIBRATED")
|
|
|
|
|
time.sleep(1)
|
|
|
|
|
|
|
|
|
|
print("Accelerometer: Perform the six-step calibration dance.")
|
|
|
|
|
while not self._imu.calibration_status[2] == 3:
|
|
|
|
|
# Calibration Dance Step Two: Accelerometer
|
|
|
|
|
# Place sensor board into six stable positions for a few seconds each:
|
|
|
|
|
# 1) x-axis right, y-axis up, z-axis away
|
|
|
|
|
# 2) x-axis up, y-axis left, z-axis away
|
|
|
|
|
# 3) x-axis left, y-axis down, z-axis away
|
|
|
|
|
# 4) x-axis down, y-axis right, z-axis away
|
|
|
|
|
# 5) x-axis left, y-axis right, z-axis up
|
|
|
|
|
# 6) x-axis right, y-axis left, z-axis down
|
|
|
|
|
# Repeat the steps until calibrated
|
|
|
|
|
print(f"Accel Calib Status: {100 / 3 * self._imu.calibration_status[2]:3.0f}%")
|
|
|
|
|
time.sleep(1)
|
|
|
|
|
print("... CALIBRATED")
|
|
|
|
|
time.sleep(1)
|
|
|
|
|
|
|
|
|
|
print("Gyroscope: Perform the hold-in-place calibration dance.")
|
|
|
|
|
while not self._imu.calibration_status[1] == 3:
|
|
|
|
|
# Calibration Dance Step Three: Gyroscope
|
|
|
|
|
# Place sensor in any stable position for a few seconds
|
|
|
|
|
# (Accelerometer calibration may also calibrate the gyro)
|
|
|
|
|
print(f"Gyro Calib Status: {100 / 3 * self._imu.calibration_status[1]:3.0f}%")
|
|
|
|
|
time.sleep(1)
|
|
|
|
|
print("... CALIBRATED")
|
|
|
|
|
time.sleep(1)
|
|
|
|
|
|
|
|
|
|
print("\nCALIBRATION COMPLETED")
|
|
|
|
|
print("Insert these preset offset values into project code:")
|
|
|
|
|
print(f" Offsets_Magnetometer: {self._imu.offsets_magnetometer}")
|
|
|
|
|
print(f" Offsets_Gyroscope: {self._imu.offsets_gyroscope}")
|
|
|
|
|
print(f" Offsets_Accelerometer: {self._imu.offsets_accelerometer}")
|
|
|
|
|
|
|
|
|
|
def update(self, utc):
|
|
|
|
|
if self._acceleration != self._imu.acceleration:
|
2026-07-06 18:53:28 -06:00
|
|
|
self._acceleration = self._imu.acceleration[:]
|
|
|
|
|
self._linear_acceleration = self._imu.linear_acceleration[:]
|
|
|
|
|
self._gravity = self._imu.gravity[:]
|
2026-07-06 17:31:02 -06:00
|
|
|
self._accel_update_time = utc
|
|
|
|
|
self.accel_updated = True
|
|
|
|
|
|
|
|
|
|
def get_accel_message(self):
|
|
|
|
|
if not self.accel_updated:
|
|
|
|
|
return
|
|
|
|
|
self.accel_updated = False
|
|
|
|
|
message = bytearray(b'\xd4\x53\x2a\xfa')
|
|
|
|
|
message += self._accel_update_time[0].to_bytes(1, 'little')
|
|
|
|
|
message += self._accel_update_time[1].to_bytes(1, 'little')
|
|
|
|
|
message += self._accel_update_time[2].to_bytes(1, 'little')
|
|
|
|
|
message += self._accel_update_time[3].to_bytes(4, 'little')
|
|
|
|
|
value_list = list(self._acceleration)
|
|
|
|
|
for idx in range(len(value_list)):
|
|
|
|
|
neg = int(value_list[idx] < 0)
|
|
|
|
|
val = abs(round(value_list[idx]*1000000))
|
|
|
|
|
message += neg.to_bytes(1, 'little')
|
|
|
|
|
message += val.to_bytes(4, 'little')
|
|
|
|
|
value_list = list(self._linear_acceleration)
|
|
|
|
|
for idx in range(len(value_list)):
|
|
|
|
|
neg = int(value_list[idx] < 0)
|
|
|
|
|
val = abs(round(value_list[idx]*1000000))
|
|
|
|
|
message += neg.to_bytes(1, 'little')
|
|
|
|
|
message += val.to_bytes(4, 'little')
|
|
|
|
|
value_list = list(self._gravity)
|
|
|
|
|
for idx in range(len(value_list)):
|
|
|
|
|
neg = int(value_list[idx] < 0)
|
|
|
|
|
val = abs(round(value_list[idx]*1000000))
|
|
|
|
|
message += neg.to_bytes(1, 'little')
|
|
|
|
|
message += val.to_bytes(4, 'little')
|
|
|
|
|
message += b'\x00\x00'
|
|
|
|
|
return message
|
|
|
|
|
|
|
|
|
|
def get_mag_message(self):
|
|
|
|
|
if not self.mag_updated:
|
|
|
|
|
return
|
|
|
|
|
self.mag_updated = False
|
|
|
|
|
message = bytearray(b'\xd4\x53\x2a\x39')
|
|
|
|
|
message += self._mag_update_time[0].to_bytes(1, 'little')
|
|
|
|
|
message += self._mag_update_time[1].to_bytes(1, 'little')
|
|
|
|
|
message += self._mag_update_time[2].to_bytes(1, 'little')
|
|
|
|
|
message += self._mag_update_time[3].to_bytes(4, 'little')
|
|
|
|
|
value_list = list(self._mag)
|
|
|
|
|
for idx in range(len(value_list)):
|
|
|
|
|
neg = int(value_list[idx] < 0)
|
|
|
|
|
message += neg.to_bytes(1, 'little')
|
|
|
|
|
val = abs(round(value_list[idx]*1000))
|
|
|
|
|
message += val.to_bytes(4, 'little')
|
|
|
|
|
message += b'\x00\x00'
|
|
|
|
|
return message
|
|
|
|
|
|
|
|
|
|
def get_gyro_message(self):
|
|
|
|
|
if not self.gyro_updated:
|
|
|
|
|
return
|
|
|
|
|
self.gyro_updated = False
|
|
|
|
|
message = bytearray(b'\xd4\x53\x2a\xa7')
|
|
|
|
|
message += self._gyro_update_time[0].to_bytes(1, 'little')
|
|
|
|
|
message += self._gyro_update_time[1].to_bytes(1, 'little')
|
|
|
|
|
message += self._gyro_update_time[2].to_bytes(1, 'little')
|
|
|
|
|
message += self._gyro_update_time[3].to_bytes(4, 'little')
|
|
|
|
|
value_list = list(self._gyro)
|
|
|
|
|
for idx in range(len(value_list)):
|
|
|
|
|
neg = int(value_list[idx] < 0)
|
|
|
|
|
message += neg.to_bytes(1, 'little')
|
|
|
|
|
val = abs(round(value_list[idx]*1000000))
|
|
|
|
|
message += val.to_bytes(8, 'little')
|
|
|
|
|
message += b'\x00\x00'
|
|
|
|
|
return message
|
|
|
|
|
|
|
|
|
|
def get_euler_message(self):
|
|
|
|
|
if not self.euler_updated:
|
|
|
|
|
return
|
|
|
|
|
self.euler_updated = False
|
|
|
|
|
message = bytearray(b'\xd4\x53\x2a\x5e')
|
|
|
|
|
message += self._euler_update_time[0].to_bytes(1, 'little')
|
|
|
|
|
message += self._euler_update_time[1].to_bytes(1, 'little')
|
|
|
|
|
message += self._euler_update_time[2].to_bytes(1, 'little')
|
|
|
|
|
message += self._euler_update_time[3].to_bytes(4, 'little')
|
|
|
|
|
value_list = list(self._euler)
|
|
|
|
|
for idx in range(len(value_list)):
|
|
|
|
|
neg = int(value_list[idx] < 0)
|
|
|
|
|
message += neg.to_bytes(1, 'little')
|
|
|
|
|
val = abs(round(value_list[idx]*1000))
|
|
|
|
|
message += val.to_bytes(4, 'little')
|
|
|
|
|
message += b'\x00\x00'
|
|
|
|
|
return message
|
|
|
|
|
|
|
|
|
|
def get_quaternion_message(self):
|
|
|
|
|
if not self.quat_updated:
|
|
|
|
|
return
|
|
|
|
|
self.quat_updated = False
|
|
|
|
|
message = bytearray(b'\xd4\x53\x2a\xba')
|
|
|
|
|
message += self._quat_update_time[0].to_bytes(1, 'little')
|
|
|
|
|
message += self._quat_update_time[1].to_bytes(1, 'little')
|
|
|
|
|
message += self._quat_update_time[2].to_bytes(1, 'little')
|
|
|
|
|
message += self._quat_update_time[3].to_bytes(4, 'little')
|
|
|
|
|
value_list = list(self._quat)
|
|
|
|
|
for idx in range(len(value_list)):
|
|
|
|
|
neg = int(value_list[idx] < 0)
|
|
|
|
|
val = abs(round(value_list[idx]*1000000))
|
|
|
|
|
message += neg.to_bytes(1, 'little')
|
|
|
|
|
message += val.to_bytes(4, 'little')
|
|
|
|
|
message += b'\x00\x00'
|
|
|
|
|
return message
|
