Split sensor types into files
This commit is contained in:
@@ -0,0 +1,211 @@
|
||||
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
|
||||
Reference in New Issue
Block a user