def __init__(self, ReadFromArduino_instance, dt, offset_GYRO, verbose=0):
self.dt = dt
self.verbose = verbose
self.ReadFromArduino_instance = ReadFromArduino_instance
self.list_measurements = []
self.offset_GYRO = offset_GYRO
# initial state: assume resting, Z downwards, still --------------------
# no rotation to Earth referential
self.o = qt.normalise_quaternion(
qt.Quaternion(1, 0.0001, 0.0001, 0.0001))
# reference vectors; can be used to print assumed orientation ----------
self.X_IMU_ref_IMU = qt.Vector(1, 0, 0)
self.Y_IMU_ref_IMU = qt.Vector(0, 1, 0)
self.Z_IMU_ref_IMU = qt.Vector(0, 0, 1)
self.read_and_update_measurement()
self.cycle = 0
def read_and_update_measurement(self):
"""Read and update measurement from Arduino."""
self.ReadFromArduino_instance.read_one_value()
self.latest_measurement = self.ReadFromArduino_instance.latest_values
self.wm = qt.Vector(self.latest_measurement[3] - self.offset_GYRO[0],
self.latest_measurement[4] - self.offset_GYRO[1],
self.latest_measurement[5] - self.offset_GYRO[2])
if self.verbose > 2:
print("Obtained measurements in SI:")
print(self.latest_measurement)
self.list_measurements.append(self.latest_measurement)
def __init__(self, x=0, y=0, z=0):
self.x, self.y, self.z = float(x), float(y), float(z)
self.v = qt.Vector(self.x, self.y, self.z)
