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: self._acceleration = self._imu.acceleration[:] self._linear_acceleration = self._imu.linear_acceleration[:] self._gravity = self._imu.gravity[:] 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