def __init__(self, cam, imu, out_name):
super().__init__(cam, imu, out_name, menu_music, loop=True)
self.last_frame = None
self.imu_failure = None
self.menu_np = menu_np
self.out_name = out_name
try:
if isinstance(imu, IMU):
self.pro = imu
if imu is not None and imu is not False:
if imu is True:
com = get_coms_in_range()
if len(com) == 0:
raise RuntimeError("No COMs found.")
com = com[0]
else:
com = imu
con, self.pro = get_i2c_imu(com)
self.pro.start()
except Exception as e:
self.imu_failure = e
self.menu_np = cv2.putText(
self.menu_np,
f"IMU Failed to initialize: {e}",
(0, 24),
cv2.FONT_HERSHEY_SIMPLEX,
0.5,
(0, 0, 255, 255),
1,
)
def run_get_i2c_imu_orient_algo():
try:
nonlocal uvw
coms = get_coms_in_range()
con, pro = get_i2c_imu(coms[0])
pro.start()
acc = (float(0), ) * 3
while True:
grav = np.asarray(unit_vector(pro.protocol.imu.get_grav()))
mag = np.asarray(unit_vector(pro.protocol.imu.get_mag()))
east = unit_vector(np.cross(mag, grav))
south = unit_vector(np.cross(east, grav))
down = unit_vector(np.cross(south, east))
xyz = np.asarray(
[east * 0.1, east, south * 0.1, south, down * 0.1, down],
dtype=np.float64,
).transpose()
if np.isnan(acc).any():
acc = (float(0), ) * 3
if not np.isnan(xyz).any():
obj.mlab_source.set(x=xyz[0], y=xyz[1], z=xyz[2])
time.sleep(0)
yield
except:
e = traceback.format_exc()
print(e)
print("ded")
def run_get_i2c_imu():
try:
nonlocal uvw
coms = get_coms_in_range()
con, pro = get_i2c_imu(coms[0])
pro.start()
acc = (float(0), ) * 3
while True:
orient1 = pro.protocol.imu.get_instant_orientation()
if np.isnan(acc).any():
acc = (float(0), ) * 3
if not np.isnan(orient1).any():
qxyz = [
orient1 * Quaternion.from_axis(v) * ~orient1
for v in uvw.transpose()
]
xyz = np.asarray([np.asarray(x.xyz)
for x in qxyz]).transpose()
obj.mlab_source.set(x=xyz[0], y=xyz[1], z=xyz[2])
time.sleep(0)
yield
except:
e = traceback.format_exc()
print(e)
print("ded")
def test_get_i2c_imu():
coms = get_coms_in_range()
con, pro = get_i2c_imu(coms[0])
pro.start()
orient1 = (float("nan"),) * 3
while np.any(np.isnan(orient1)):
orient1 = pro.protocol.imu.get_instant_orientation()
while True:
# print(f"acc: {pro.protocol.imu.get_acc(Quaternion.from_euler(3*pi/2, pi/4, 0))}")
# pro.protocol.imu.get_grav(stationary=True)
# print(f"grav:{pro.protocol.imu.grav_magnitude}")
# print(f"gyro: {pro.protocol.imu.get_gyro()}")
# print(f"mag: {pro.protocol.imu.get_mag()}")
print(f"orient1: {orient1}")
print(f"orient2: {pro.protocol.imu.get_instant_orientation(orient1)}")
def run_get_i2c_imu_north_up():
try:
nonlocal uvw
coms = get_coms_in_range()
con, pro = get_i2c_imu(coms[0])
pro.start()
acc = (float(0),) * 3
while True:
n, u = pro.protocol.imu.get_north_up()
if np.isnan(acc).any():
acc = (float(0),) * 3
if not np.isnan(n).any() and not np.isnan(u).any():
xyz = np.asarray([n * 0.1, n, u * 0.1, u], dtype=np.float64).transpose()
obj.mlab_source.set(x=xyz[0], y=xyz[1], z=xyz[2])
time.sleep(0)
yield
except:
e = traceback.format_exc()
print(e)
print("ded")