Example #1
0
    def __createDial(self, pos):
        dial = None
        if pos == 0:
            self.__ai = AttitudeIndicator(self)
            dial = self.__ai
        elif pos == 1:
            self.__speedo = SpeedoMeter(self)
            self.__speedo.setRange(0.0, 8.0)
            self.__speedo.setLabel("0")
            self.__speedo.setOrigin(180)
            self.__speedo.setScaleArc(0.0,270.0)
            self.__speedo.setScale(-1, 2, 1.0)
            dial = self.__speedo   
        elif pos == 2:
            self.__speedo = SpeedoMeter(self)
            self.__speedo.setRange(0.0, 10.0)
            self.__speedo.setLabel("m")
            self.__speedo.setOrigin(-90)
            self.__speedo.setScaleArc(0.0,360.0)
            self.__speedo.setScale(-1, 2, 1)
            dial = self.__speedo                        

        if dial:
            dial.setReadOnly(True)
            dial.scaleDraw().setPenWidth(3)
            dial.setLineWidth(4)
            dial.setFrameShadow(Qwt.QwtDial.Sunken)

        return dial    
Example #2
0
    def __createDial(self, pos):
        dial = None
        if pos == 0:
            self.__ai = AttitudeIndicator(self)
            dial = self.__ai
        elif pos == 1:
            self.__speedo = SpeedoMeter(self)
            self.__speedo.setRange(0.0, 8.0)
            self.__speedo.setLabel("0")
            self.__speedo.setOrigin(180)
            self.__speedo.setScaleArc(0.0,270.0)
            self.__speedo.setScale(-1, 2, 1.0)
            dial = self.__speedo   
        elif pos == 2:
            self.__speedo = SpeedoMeter(self)
            self.__speedo.setRange(0.0, 10.0)
            self.__speedo.setLabel("m")
            self.__speedo.setOrigin(-90)
            self.__speedo.setScaleArc(0.0,360.0)
            self.__speedo.setScale(-1, 2, 1)
            dial = self.__speedo                        

        if dial:
            dial.setReadOnly(True)
            dial.scaleDraw().setPenWidth(3)
            dial.setLineWidth(4)
            dial.setFrameShadow(Qwt.QwtDial.Sunken)

        return dial    
Example #3
0
class SensorsWidget(QtGui.QWidget):
    
    sensorsUpdate=QtCore.pyqtSignal()
    
    def __init__(self,winParent):      
        super(SensorsWidget, self).__init__()
        self.winParent=winParent
        self.sensorsUpdate.connect(self.updateSensors)
        self.initUI()
        
    def initUI(self):
        
        self.setMinimumSize(660,450)
        self.setMaximumSize(660,450)
        
        self.setWindowTitle("Sensors")
        
        self.horizon=self.__createDial(0)
        self.horizon.resize(200,200)
        self.horizon.move(60,20) 

       	self.pitchLabel=QtGui.QLabel('Pitch:',self)
        self.pitchLabel.move(300,140)
        self.pitchValueLabel=QtGui.QLabel('-180',self)
        self.pitchValueLabel.resize(60,21)
        self.pitchValueLabel.move(340,140)
        
        self.rollLabel=QtGui.QLabel('Roll:',self)
        self.rollLabel.move(300,160)
        self.rollValueLabel=QtGui.QLabel('180',self)
        self.rollValueLabel.resize(60,21)
        self.rollValueLabel.move(340,160)

        self.compass=self.__createCompass(1)
        self.compass.resize(120,120)
        self.compass.move(280,10)
        self.compass.show()

        self.yawLabel=QtGui.QLabel('Yaw:',self)
        self.yawLabel.move(300,180)
        self.yawValueLabel=QtGui.QLabel('45',self)
        self.yawValueLabel.resize(60,21)
        self.yawValueLabel.move(340,180)

        self.altd=self.__createDial(2)
        self.altd.resize(150,150)
        self.altd.move(420,50)


        self.battery=Qwt.QwtThermo(self)
        self.battery.setMaxValue(100.0)
        self.battery.setMinValue(0.0)
        self.battery.setPipeWidth(10)
        
        self.battery.move(580,10)
        self.battery.resize(56,241)
        self.batteryLabel=QtGui.QLabel('Battery (%)',self)
        self.batteryLabel.move(580,251)

        self.velLinX=self.__createDial(1)
        self.velLinX.resize(150,150)
        self.velLinX.move(60,270)
        self.velXLabel = QtGui.QLabel('Linear X (m/s)',self)        
        self.velXLabel.move(95,420)

        
        self.velLinY=self.__createDial(1)
        self.velLinY.resize(150,150)
        self.velLinY.move(240,270)
        self.velYLabel = QtGui.QLabel('Linear Y (m/s)',self)
        self.velYLabel.move(275,420)             
        
        self.velLinZ=self.__createDial(1)
        self.velLinZ.resize(150,150)
        self.velLinZ.setLabel("8 m/s")
        self.velLinZ.move(420,270)
        self.velZLabel = QtGui.QLabel('Linear Z (m/s)',self)
        self.velZLabel.move(455,420)


        self.__speed_offset = 0.8
        self.__angle_offset = 0.05
        self.__gradient_offset = 0.005
        
        
    def updateSensors(self):
        pose=self.winParent.getSensor().getPose3D()

        if pose != None:
            qw=pose.q0
            qx=pose.q1
            qy=pose.q2
            qz=pose.q3
            self.drawAltd(pose.z)
            self.drawYawValues(self.quatToYaw(qw,qx,qy,qz)*180/math.pi)
            self.drawPitchRollValues(self.quatToPitch(qw,qx,qy,qz)*180/math.pi,self.quatToRoll(qw,qx,qy,qz)*180/math.pi)


        navdata=self.winParent.getSensor().getNavdata()

        if navdata != None:
            self.battery.setValue(navdata.batteryPercent)
            self.drawVelocities(navdata.vx,navdata.vy,navdata.vz)
    
  
    def drawYawValues(self,degress):
        value="{0:.2f}".format(degress)
        self.yawValueLabel.setText(unicode(value))
        self.compass.setValue(degress)

    def drawAltd(self, meters):

        if(meters>=10 and meters<100):
            self.altd.setValue(meters%10)
        elif (meters>=100 and meters<1000):
            self.altd.setValue(meters%100)          
        else:
            self.altd.setValue(meters)

        altLabel="{0:.0f}".format(meters)+' m'
        self.altd.setLabel(altLabel)

    def drawPitchRollValues(self,pitch,roll):
        if(pitch>0 and pitch<=90):
            result=pitch/90
            result=-result
        elif (pitch<0 and pitch>=-90):
            result=pitch/-90
        else:
            result=0.0
    
        self.horizon.setGradient(result)
        self.horizon.setAngle(roll)
        pitchValue="{0:.2f}".format(pitch)
        rollValue="{0:.2f}".format(roll)
        self.pitchValueLabel.setText(unicode(pitchValue))
        self.rollValueLabel.setText(unicode(rollValue))


    def drawVelocities(self,vx,vy,vz):

        vx=math.fabs(vx)
        vx/=1000.0
        self.velLinX.setValue(vx)
        vx=math.fabs(vx)
        vxLabel="{0:.0f}".format(vx)+' m/s'
        self.velLinX.setLabel(vxLabel)
        
        vy=math.fabs(vy)
        vy/=1000.0
        self.velLinY.setValue(vy)
        vyLabel="{0:.0f}".format(vy)+' m/s'
        self.velLinY.setLabel(vyLabel)

        vz=math.fabs(vz)
        vz/=1000.0
        self.velLinZ.setValue(vz)
        vzLabel="{0:.0f}".format(vz)+' m/s'
        self.velLinZ.setLabel(vzLabel)
        
   
    def quatToRoll(self,qw,qx,qy,qz):
        rotateXa0=2.0*(qy*qz + qw*qx)
        rotateXa1=qw*qw - qx*qx - qy*qy + qz*qz
        rotateX=0.0
        
        if(rotateXa0 != 0.0 and rotateXa1 !=0.0):
            rotateX=math.atan2(rotateXa0, rotateXa1)
            
        return rotateX

    def quatToPitch(self,qw,qx,qy,qz):
        rotateYa0=-2.0*(qx*qz - qw*qy)
        rotateY=0.0
        if(rotateYa0>=1.0):
            rotateY=math.pi/2.0
        elif(rotateYa0<=-1.0):
            rotateY=-math.pi/2.0
        else:
            rotateY=math.asin(rotateYa0)
        
        return rotateY


    def quatToYaw(self,qw,qx,qy,qz):
        rotateZa0=2.0*(qx*qy + qw*qz)
        rotateZa1=qw*qw + qx*qx - qy*qy - qz*qz
        rotateZ=0.0
        if(rotateZa0 != 0.0 and rotateZa1 != 0.0):
            rotateZ=math.atan2(rotateZa0,rotateZa1)
        
        return rotateZ                
        
    def closeEvent(self, event):
        self.winParent.closeSensorsWidget()
        
    def __createDial(self, pos):
        dial = None
        if pos == 0:
            self.__ai = AttitudeIndicator(self)
            dial = self.__ai
        elif pos == 1:
            self.__speedo = SpeedoMeter(self)
            self.__speedo.setRange(0.0, 8.0)
            self.__speedo.setLabel("0")
            self.__speedo.setOrigin(180)
            self.__speedo.setScaleArc(0.0,270.0)
            self.__speedo.setScale(-1, 2, 1.0)
            dial = self.__speedo   
        elif pos == 2:
            self.__speedo = SpeedoMeter(self)
            self.__speedo.setRange(0.0, 10.0)
            self.__speedo.setLabel("m")
            self.__speedo.setOrigin(-90)
            self.__speedo.setScaleArc(0.0,360.0)
            self.__speedo.setScale(-1, 2, 1)
            dial = self.__speedo                        

        if dial:
            dial.setReadOnly(True)
            dial.scaleDraw().setPenWidth(3)
            dial.setLineWidth(4)
            dial.setFrameShadow(Qwt.QwtDial.Sunken)

        return dial    
    
    def __createCompass(self, pos):

        palette = Qt.QPalette()
        for colorRole in colorRoleList:
            palette.setColor(colorRole, Qt.QColor())

        palette.setColor(
            Qt.QPalette.Base,
            self.palette().color(self.backgroundRole()).light(120))
        palette.setColor(
            Qt.QPalette.Foreground,
            palette.color(Qt.QPalette.Base))

        compass = Qwt.QwtCompass(self)
        compass.setLineWidth(4)
        if pos < 3:
            compass.setFrameShadow(Qwt.QwtCompass.Sunken)
        else:
            compass.setFrameShadow(Qwt.QwtCompass.Raised)

        if pos == 0:
            compass.setLabelMap({0.0: "N",
                                 90.0: "E",
                                 180.0: "S",
                                 270.0: "W"})
            
            rose = Qwt.QwtSimpleCompassRose(4, 1)
            compass.setRose(rose)
            compass.setNeedle(
                Qwt.QwtCompassWindArrow(Qwt.QwtCompassWindArrow.Style2))
            compass.setValue(60.0)
        elif pos == 1:
            compass.setLabelMap({0.0: "",
                                 90.0: "",
                                 180.0: "",
                                 270.0: ""})
            
            compass.setScaleOptions(Qwt.QwtDial.ScaleBackbone
                        | Qwt.QwtDial.ScaleTicks
                        | Qwt.QwtDial.ScaleLabel)
            compass.setScaleTicks(0, 0, 3)
            
            compass.setNeedle(Qwt.QwtCompassMagnetNeedle(
                Qwt.QwtCompassMagnetNeedle.TriangleStyle,
                Qt.Qt.white,
                Qt.Qt.red))
            compass.setValue(220.0)            
 
        newPalette = compass.palette()
        for colorRole in colorRoleList:
            if palette.color(colorRole).isValid():
                for colorGroup in colorGroupList:
                    newPalette.setColor(
                        colorGroup, colorRole, palette.color(colorRole))

        for colorGroup in colorGroupList:
            light = newPalette.color(
                colorGroup, Qt.QPalette.Base).light(170)
            dark = newPalette.color(
                colorGroup, Qt.QPalette.Base).dark(170)
            if compass.frameShadow() == Qwt.QwtDial.Raised:
                mid = newPalette.color(
                    colorGroup, Qt.QPalette.Base).dark(110)
            else:
                mid = newPalette.color(
                    colorGroup, Qt.QPalette.Base).light(110)

            newPalette.setColor(colorGroup, Qt.QPalette.Dark, dark)
            newPalette.setColor(colorGroup, Qt.QPalette.Mid, mid)
            newPalette.setColor(colorGroup, Qt.QPalette.Light, light)

        compass.setPalette(newPalette)
        compass.setReadOnly(True)
        return compass
Example #4
0
class SensorsWidget(QtGui.QWidget):
    
    sensorsUpdate=QtCore.pyqtSignal()
    
    def __init__(self,winParent):      
        super(SensorsWidget, self).__init__()
        self.winParent=winParent
        self.sensorsUpdate.connect(self.updateSensors)
        self.initUI()
        
    def initUI(self):
        
        self.setMinimumSize(660,450)
        self.setMaximumSize(660,450)
        
        self.setWindowTitle("Sensors")
        
        self.horizon=self.__createDial(0)
        self.horizon.resize(200,200)
        self.horizon.move(60,20) 

       	self.pitchLabel=QtGui.QLabel('Pitch:',self)
        self.pitchLabel.move(300,140)
        self.pitchValueLabel=QtGui.QLabel('-180',self)
        self.pitchValueLabel.resize(60,21)
        self.pitchValueLabel.move(340,140)
        
        self.rollLabel=QtGui.QLabel('Roll:',self)
        self.rollLabel.move(300,160)
        self.rollValueLabel=QtGui.QLabel('180',self)
        self.rollValueLabel.resize(60,21)
        self.rollValueLabel.move(340,160)

        self.compass=self.__createCompass(1)
        self.compass.resize(120,120)
        self.compass.move(280,10)
        self.compass.show()

        self.yawLabel=QtGui.QLabel('Yaw:',self)
        self.yawLabel.move(300,180)
        self.yawValueLabel=QtGui.QLabel('45',self)
        self.yawValueLabel.resize(60,21)
        self.yawValueLabel.move(340,180)

        self.altd=self.__createDial(2)
        self.altd.resize(150,150)
        self.altd.move(420,50)


        self.battery=Qwt.QwtThermo(self)
        self.battery.setMaxValue(100.0)
        self.battery.setMinValue(0.0)
        self.battery.setPipeWidth(10)
        
        self.battery.move(580,10)
        self.battery.resize(56,241)
        self.batteryLabel=QtGui.QLabel('Battery (%)',self)
        self.batteryLabel.move(580,251)

        self.velLinX=self.__createDial(1)
        self.velLinX.resize(150,150)
        self.velLinX.move(60,270)
        self.velXLabel = QtGui.QLabel('Linear X (m/s)',self)        
        self.velXLabel.move(95,420)

        
        self.velLinY=self.__createDial(1)
        self.velLinY.resize(150,150)
        self.velLinY.move(240,270)
        self.velYLabel = QtGui.QLabel('Linear Y (m/s)',self)
        self.velYLabel.move(275,420)             
        
        self.velLinZ=self.__createDial(1)
        self.velLinZ.resize(150,150)
        self.velLinZ.setLabel("8 m/s")
        self.velLinZ.move(420,270)
        self.velZLabel = QtGui.QLabel('Linear Z (m/s)',self)
        self.velZLabel.move(455,420)


        self.__speed_offset = 0.8
        self.__angle_offset = 0.05
        self.__gradient_offset = 0.005
        
        
    def updateSensors(self):
        pose=self.winParent.getSensor().getPose3D()

        if pose != None:
            qw=pose.q0
            qx=pose.q1
            qy=pose.q2
            qz=pose.q3
            self.drawAltd(pose.z)
            self.drawYawValues(self.quatToYaw(qw,qx,qy,qz)*180/math.pi)
            self.drawPitchRollValues(self.quatToPitch(qw,qx,qy,qz)*180/math.pi,self.quatToRoll(qw,qx,qy,qz)*180/math.pi)


        navdata=self.winParent.getSensor().getNavdata()

        if navdata != None:
            self.battery.setValue(navdata.batteryPercent)
            self.drawVelocities(navdata.vx,navdata.vy,navdata.vz)
    
  
    def drawYawValues(self,degress):
        value="{0:.2f}".format(degress)
        self.yawValueLabel.setText(unicode(value))
        self.compass.setValue(degress)

    def drawAltd(self, meters):

        if(meters>=10 and meters<100):
            self.altd.setValue(meters%10)
        elif (meters>=100 and meters<1000):
            self.altd.setValue(meters%100)          
        else:
            self.altd.setValue(meters)

        altLabel="{0:.0f}".format(meters)+' m'
        self.altd.setLabel(altLabel)

    def drawPitchRollValues(self,pitch,roll):
        if(pitch>0 and pitch<=90):
            result=pitch/90
            result=-result
        elif (pitch<0 and pitch>=-90):
            result=pitch/-90
        else:
            result=0.0
    
        self.horizon.setGradient(result)
        self.horizon.setAngle(-roll)
        pitchValue="{0:.2f}".format(pitch)
        rollValue="{0:.2f}".format(roll)
        self.pitchValueLabel.setText(unicode(pitchValue))
        self.rollValueLabel.setText(unicode(rollValue))


    def drawVelocities(self,vx,vy,vz):

        vx=math.fabs(vx)
        vx/=1000.0
        self.velLinX.setValue(vx)
        vx=math.fabs(vx)
        vxLabel="{0:.0f}".format(vx)+' m/s'
        self.velLinX.setLabel(vxLabel)
        
        vy=math.fabs(vy)
        vy/=1000.0
        self.velLinY.setValue(vy)
        vyLabel="{0:.0f}".format(vy)+' m/s'
        self.velLinY.setLabel(vyLabel)

        vz=math.fabs(vz)
        vz/=1000.0
        self.velLinZ.setValue(vz)
        vzLabel="{0:.0f}".format(vz)+' m/s'
        self.velLinZ.setLabel(vzLabel)
        
   
    def quatToRoll(self,qw,qx,qy,qz):
        rotateXa0=2.0*(qy*qz + qw*qx)
        rotateXa1=qw*qw - qx*qx - qy*qy + qz*qz
        rotateX=0.0
        
        if(rotateXa0 != 0.0 and rotateXa1 !=0.0):
            rotateX=math.atan2(rotateXa0, rotateXa1)
            
        return rotateX

    def quatToPitch(self,qw,qx,qy,qz):
        rotateYa0=-2.0*(qx*qz - qw*qy)
        rotateY=0.0
        if(rotateYa0>=1.0):
            rotateY=math.pi/2.0
        elif(rotateYa0<=-1.0):
            rotateY=-math.pi/2.0
        else:
            rotateY=math.asin(rotateYa0)
        
        return rotateY


    def quatToYaw(self,qw,qx,qy,qz):
        rotateZa0=2.0*(qx*qy + qw*qz)
        rotateZa1=qw*qw + qx*qx - qy*qy - qz*qz
        rotateZ=0.0
        if(rotateZa0 != 0.0 and rotateZa1 != 0.0):
            rotateZ=math.atan2(rotateZa0,rotateZa1)
        
        return rotateZ                
        
    def closeEvent(self, event):
        self.winParent.closeSensorsWidget()
        
    def __createDial(self, pos):
        dial = None
        if pos == 0:
            self.__ai = AttitudeIndicator(self)
            dial = self.__ai
        elif pos == 1:
            self.__speedo = SpeedoMeter(self)
            self.__speedo.setRange(0.0, 8.0)
            self.__speedo.setLabel("0")
            self.__speedo.setOrigin(180)
            self.__speedo.setScaleArc(0.0,270.0)
            self.__speedo.setScale(-1, 2, 1.0)
            dial = self.__speedo   
        elif pos == 2:
            self.__speedo = SpeedoMeter(self)
            self.__speedo.setRange(0.0, 10.0)
            self.__speedo.setLabel("m")
            self.__speedo.setOrigin(-90)
            self.__speedo.setScaleArc(0.0,360.0)
            self.__speedo.setScale(-1, 2, 1)
            dial = self.__speedo                        

        if dial:
            dial.setReadOnly(True)
            dial.scaleDraw().setPenWidth(3)
            dial.setLineWidth(4)
            dial.setFrameShadow(Qwt.QwtDial.Sunken)

        return dial    
    
    def __createCompass(self, pos):

        palette = Qt.QPalette()
        for colorRole in colorRoleList:
            palette.setColor(colorRole, Qt.QColor())

        palette.setColor(
            Qt.QPalette.Base,
            self.palette().color(self.backgroundRole()).light(120))
        palette.setColor(
            Qt.QPalette.Foreground,
            palette.color(Qt.QPalette.Base))

        compass = Qwt.QwtCompass(self)
        compass.setLineWidth(4)
        if pos < 3:
            compass.setFrameShadow(Qwt.QwtCompass.Sunken)
        else:
            compass.setFrameShadow(Qwt.QwtCompass.Raised)

        if pos == 0:
            compass.setLabelMap({0.0: "N",
                                 90.0: "E",
                                 180.0: "S",
                                 270.0: "W"})
            
            rose = Qwt.QwtSimpleCompassRose(4, 1)
            compass.setRose(rose)
            compass.setNeedle(
                Qwt.QwtCompassWindArrow(Qwt.QwtCompassWindArrow.Style2))
            compass.setValue(60.0)
        elif pos == 1:
            compass.setLabelMap({0.0: "",
                                 90.0: "",
                                 180.0: "",
                                 270.0: ""})
            
            compass.setScaleOptions(Qwt.QwtDial.ScaleBackbone
                        | Qwt.QwtDial.ScaleTicks
                        | Qwt.QwtDial.ScaleLabel)
            compass.setScaleTicks(0, 0, 3)
            
            compass.setNeedle(Qwt.QwtCompassMagnetNeedle(
                Qwt.QwtCompassMagnetNeedle.TriangleStyle,
                Qt.Qt.white,
                Qt.Qt.red))
            compass.setValue(220.0)            
 
        newPalette = compass.palette()
        for colorRole in colorRoleList:
            if palette.color(colorRole).isValid():
                for colorGroup in colorGroupList:
                    newPalette.setColor(
                        colorGroup, colorRole, palette.color(colorRole))

        for colorGroup in colorGroupList:
            light = newPalette.color(
                colorGroup, Qt.QPalette.Base).light(170)
            dark = newPalette.color(
                colorGroup, Qt.QPalette.Base).dark(170)
            if compass.frameShadow() == Qwt.QwtDial.Raised:
                mid = newPalette.color(
                    colorGroup, Qt.QPalette.Base).dark(110)
            else:
                mid = newPalette.color(
                    colorGroup, Qt.QPalette.Base).light(110)

            newPalette.setColor(colorGroup, Qt.QPalette.Dark, dark)
            newPalette.setColor(colorGroup, Qt.QPalette.Mid, mid)
            newPalette.setColor(colorGroup, Qt.QPalette.Light, light)

        compass.setPalette(newPalette)
        compass.setReadOnly(True)
        return compass