Exemple #1
0
    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)
Exemple #7
0
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]