def __init__(self): self.Acceleration = Vector() self.Omega = Vector() self.Quat = Quaternion() self.Alpha = Vector() self.time = 0 self.sensorTest = PrecisionTest(Params.dt) self.v_time = [] self.true_omega = [] self.v_omega =[] self.diff_omega = []
class SensorValues: def __init__(self): self.Acceleration = Vector() self.Omega = Vector() self.Quat = Quaternion() self.Alpha = Vector() self.time = 0 self.sensorTest = PrecisionTest(Params.dt) self.v_time = [] self.true_omega = [] self.v_omega =[] self.diff_omega = [] def setValues(self, acceleration, gyroscope, quat, alpha): #self.Acceleration = acceleration #self.Omega = gyroscope #self.Quat = quat #self.Alpha = alpha #return self.time += Params.dt oldGyro = Vector(self.Omega) self.Acceleration[0] = int(acceleration[0]*8192*2)/(8192.*2.) self.Acceleration[1] = int(acceleration[1]*8192*2)/(8192.*2.) self.Acceleration[2] = int(acceleration[2]*8192*2)/(8192.*2.) self.Omega[0] = int(gyroscope[0]*131*2)/(131.*2.) self.Omega[1] = int(gyroscope[1]*131*2)/(131.*2.) self.Omega[2] = int(gyroscope[2]*131*2)/(131.*2.) self.Quat = quat self.computeAlpha(oldGyro) self.sensorTest.setAngularMeasure(self.Quat, self.Omega, self.time) #self.test.setControl(self.b.CtrlInput) self.sensorTest.computeValues() self.sensorTest.compare(gyroscope, alpha) self.compare(gyroscope) self.Acceleration = acceleration self.Omega = gyroscope self.Quat = quat self.Alpha = alpha def computeAlpha(self, oldGyroscope): dt = Params.dt self.Alpha = (self.Omega - oldGyroscope)/dt; def compare(self, omega): self.v_time.append(self.time) self.true_omega.append(omega[0]) self.v_omega.append(self.Omega[0]) self.diff_omega.append(self.Omega[0]-omega[0]) def plot(self): plt.subplot(4, 1, 1) plt.title("omega") plt.plot(self.v_time, self.v_omega, "r", self.v_time, self.true_omega, "b") plt.subplot(4, 1, 2) plt.title("omega_diff") plt.plot(self.v_time, self.diff_omega, "r") plt.show()