def __init__(self): """ """ # update from the IMU. self.spatial = Spatial() imu_handlers = Controller.IMU_Handlers() self.prev = 0 # attach the event handlers. try: self.spatial.setOnAttachHandler(imu_handlers.on_attach) self.spatial.setOnDetachHandler(imu_handlers.on_detach) self.spatial.setOnErrorhandler(imu_handlers.on_error) self.spatial.setOnSpatialDataHandler(imu_handlers.on_data) self.spatial.openPhidget() self.spatial.waitForAttach(10000) self.spatial.setDataRate(4) except: print("Error connecting to IMU, I cannot handle this. " + \ "I will just go die now!") exit(1) samplePeriod = 1 / 256 self.ahrs = MadgwickAHRS(samplePeriod) #t = Thread(target=self.update_head) #t.daemon = True #t.start() self.x = 0 self.y = 0 self.z = 0 self.update_head()
def __init__(self): """ """ # update from the IMU. self.spatial = Spatial() imu_handlers = Controller.IMU_Handlers() self.prev = 0 # attach the event handlers. try: self.spatial.setOnAttachHandler(imu_handlers.on_attach) self.spatial.setOnDetachHandler(imu_handlers.on_detach) self.spatial.setOnErrorhandler(imu_handlers.on_error) self.spatial.setOnSpatialDataHandler(imu_handlers.on_data) self.spatial.openPhidget() self.spatial.waitForAttach(10000) self.spatial.setDataRate(4) except: print("Error connecting to IMU, I cannot handle this. " + \ "I will just go die now!") exit(1) samplePeriod = 1/256 self.ahrs = MadgwickAHRS(samplePeriod) #t = Thread(target=self.update_head) #t.daemon = True #t.start() self.x = 0 self.y = 0 self.z = 0 self.update_head()
class Transformer(): def __init__(self): self.filter = MadgwickAHRS() #self.filter = MahonyAHRS() def transform(self, events): angles = [] angles.append(self.filter.processingEvent(events[len(events) - 1])) # for i in range(0, len(events)): # angles.append(self.filter.processingEvent(events[i], False)) return angles
class Controller(object): """ """ # stores acceleration gyroscope, magnetic, time. imu_measurements = [[], [], [], 0] class IMU_Handlers(object): """ """ def on_data(self, e): """ """ source = e.device for index, spatialData in enumerate(e.spatialData): if (len(spatialData.Acceleration) == 3) and (len(spatialData.AngularRate) == 3) \ and (len(spatialData.MagneticField) == 3): acc = [spatialData.Acceleration[0], spatialData.Acceleration[1], spatialData.Acceleration[2]] gyr = [spatialData.AngularRate[0], spatialData.AngularRate[1], spatialData.AngularRate[2]] mag = [spatialData.MagneticField[0], spatialData.MagneticField[1], spatialData.MagneticField[2]] Controller.imu_measurements[0] = gyr Controller.imu_measurements[1] = acc Controller.imu_measurements[2] = mag Controller.imu_measurements[3] = spatialData.Timestamp.seconds break; def on_attach(self, e): """ """ return def on_detach(self, e): """ """ return def on_error(self, e): """ """ try: source = e.device print(("Spatial %i: Phidget Error %i: %s" % \ (source.getSerialNum(), e.eCode, e.description))) except PhidgetException as e: print(("Phidget Exception %i: %s" % (e.code, e.details))) def __init__(self): """ """ # update from the IMU. self.spatial = Spatial() imu_handlers = Controller.IMU_Handlers() self.prev = 0 # attach the event handlers. try: self.spatial.setOnAttachHandler(imu_handlers.on_attach) self.spatial.setOnDetachHandler(imu_handlers.on_detach) self.spatial.setOnErrorhandler(imu_handlers.on_error) self.spatial.setOnSpatialDataHandler(imu_handlers.on_data) self.spatial.openPhidget() self.spatial.waitForAttach(10000) self.spatial.setDataRate(4) except: print("Error connecting to IMU, I cannot handle this. " + \ "I will just go die now!") exit(1) samplePeriod = 1/256 self.ahrs = MadgwickAHRS(samplePeriod) #t = Thread(target=self.update_head) #t.daemon = True #t.start() self.x = 0 self.y = 0 self.z = 0 self.update_head() def update_head(self): """ """ while True: data = copy.copy(Controller.imu_measurements) if (len(data[0]) + len(data[1]) + len(data[2]) == 9): self.ahrs.update( data[0][0], data[0][1], data[0][2], data[1][0], data[1][1], data[1][2], data[2][0], data[2][1], data[2][2] ) x, y, z = self.ahrs.getEulerAngles() self.x = x self.y = y self.z = z if self.prev != data[3]: print ("") print(self.x , self.y , self.z) #print (self.ahrs.quatern2rotMat()) print (data) print ("") #print (data[3]) self.prev = data[3]
def __init__(self): self.filter = MadgwickAHRS()
layout.addWidget(self.p1, 0, 0) layout.addWidget(self.p2, 1, 0) layout.addWidget(self.p3, 2, 0) streamer = SensorStreamer() app = QtGui.QApplication(sys.argv) #Sensor views w = Window() w.show() #Cube view c = CubeView(title="Magdwick", position=[1000, 100, 400, 400]) c.box.transformer.filter = MadgwickAHRS() c.show() # c1 = CubeView(title = "Mahony", position = [1000, 500, 400, 400]) # c1.box.transformer.filter = MahonyAHRS() # c1.show() # # c2 = CubeView(title = "Magdwick9D", position = [1400, 100, 400, 400]) # c2.box.transformer.filter = MadgwickAHRS(True) # c2.show() # # c3 = CubeView(title = "Mahony9D", position = [1400, 500, 400, 400]) # c3.box.transformer.filter = MahonyAHRS(True) # c3.show() streamer.start(40, PORT=5556)
class Controller(object): """ """ # stores acceleration gyroscope, magnetic, time. imu_measurements = [[], [], [], 0] class IMU_Handlers(object): """ """ def on_data(self, e): """ """ source = e.device for index, spatialData in enumerate(e.spatialData): if (len(spatialData.Acceleration) == 3) and (len(spatialData.AngularRate) == 3) \ and (len(spatialData.MagneticField) == 3): acc = [ spatialData.Acceleration[0], spatialData.Acceleration[1], spatialData.Acceleration[2] ] gyr = [ spatialData.AngularRate[0], spatialData.AngularRate[1], spatialData.AngularRate[2] ] mag = [ spatialData.MagneticField[0], spatialData.MagneticField[1], spatialData.MagneticField[2] ] Controller.imu_measurements[0] = gyr Controller.imu_measurements[1] = acc Controller.imu_measurements[2] = mag Controller.imu_measurements[ 3] = spatialData.Timestamp.seconds break def on_attach(self, e): """ """ return def on_detach(self, e): """ """ return def on_error(self, e): """ """ try: source = e.device print(("Spatial %i: Phidget Error %i: %s" % \ (source.getSerialNum(), e.eCode, e.description))) except PhidgetException as e: print(("Phidget Exception %i: %s" % (e.code, e.details))) def __init__(self): """ """ # update from the IMU. self.spatial = Spatial() imu_handlers = Controller.IMU_Handlers() self.prev = 0 # attach the event handlers. try: self.spatial.setOnAttachHandler(imu_handlers.on_attach) self.spatial.setOnDetachHandler(imu_handlers.on_detach) self.spatial.setOnErrorhandler(imu_handlers.on_error) self.spatial.setOnSpatialDataHandler(imu_handlers.on_data) self.spatial.openPhidget() self.spatial.waitForAttach(10000) self.spatial.setDataRate(4) except: print("Error connecting to IMU, I cannot handle this. " + \ "I will just go die now!") exit(1) samplePeriod = 1 / 256 self.ahrs = MadgwickAHRS(samplePeriod) #t = Thread(target=self.update_head) #t.daemon = True #t.start() self.x = 0 self.y = 0 self.z = 0 self.update_head() def update_head(self): """ """ while True: data = copy.copy(Controller.imu_measurements) if (len(data[0]) + len(data[1]) + len(data[2]) == 9): self.ahrs.update(data[0][0], data[0][1], data[0][2], data[1][0], data[1][1], data[1][2], data[2][0], data[2][1], data[2][2]) x, y, z = self.ahrs.getEulerAngles() self.x = x self.y = y self.z = z if self.prev != data[3]: print("") print(self.x, self.y, self.z) #print (self.ahrs.quatern2rotMat()) print(data) print("") #print (data[3]) self.prev = data[3]