qtosd/qtosd.py

   1 # -*- coding: utf-8 -*-
   2
   3 # OSD (on screen display) written in Qt
   4 # Copyright (C) 2015 Olivier Matz <zer0@droids-corp.org>
   5 #
   6 # This program is free software: you can redistribute it and/or modify
   7 # it under the terms of the GNU General Public License as published by
   8 # the Free Software Foundation, either version 3 of the License, or
   9 # (at your option) any later version.
  10 #
  11 # This program is distributed in the hope that it will be useful,
  12 # but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 # GNU General Public License for more details.
  15 #
  16 # You should have received a copy of the GNU General Public License
  17 # along with this program.  If not, see <http://www.gnu.org/licenses/>.
  18
  19 # Inspired from QcGauge
  20 # Copyright (C) 2015 Hadj Tahar Berrima
  21 # http://pytricity.com/qt-artificial-horizon-custom-widget/
  22
  23 import math
  24 import argparse
  25
  26 from PyQt5 import QtCore
  27 from PyQt5.QtCore import (pyqtSlot, QTimer, QRect, QPoint, Qt, QByteArray,
  28                           QSizeF, QRectF)
  29 from PyQt5.QtGui import (QPainter, QColor, QPen, QBrush, QLinearGradient, QFont,
  30                          QPainterPath, QPolygonF)
  31 from PyQt5.QtWidgets import (QApplication, QMainWindow, QWidget, QAction,
  32                              QActionGroup, QGraphicsScene, QGraphicsView)
  33 from PyQt5.QtMultimediaWidgets import (QGraphicsVideoItem)
  34 from PyQt5.QtMultimedia import (QCamera, QAbstractVideoSurface,
  35                                 QAbstractVideoBuffer)
  36
  37 import serialfpv
  38 import qtosd_ui
  39
  40 try:
  41     _fromUtf8 = QtCore.QString.fromUtf8
  42 except AttributeError:
  43     def _fromUtf8(s):
  44         return s
  45
  46 try:
  47     _encoding = QApplication.UnicodeUTF8
  48     def _translate(context, text, disambig):
  49         return QApplication.translate(context, text, disambig, _encoding)
  50 except AttributeError:
  51     def _translate(context, text, disambig):
  52         return QApplication.translate(context, text, disambig)
  53
  54 class OSDWidget(QWidget):
  55     def __init__(self, mode = "camera", filename = None):
  56         super(OSDWidget, self).__init__()
  57         # init parameters
  58         self.mode = mode
  59         self.fpv = None
  60         # parameters that will be modified by the user
  61         self.user_pitch = 0
  62         self.user_roll = 0
  63         self.user_yaw = 0
  64         self.user_speed = 0
  65         self.user_alt = 0
  66         self.user_rthome = 0
  67         self.left_txt = "14.8v / 34A\n1.5Ah" # XXX
  68         self.right_txt = "23:03 since take-off\n1.4 km to home\nRSSI 60dB" # XXX
  69         self.user_frame_cb = None
  70         # filtered parameters
  71         self.pitch = 0
  72         self.roll = 0
  73         self.yaw = 0
  74         self.speed = 0
  75         self.alt = 0
  76         self.rthome = 0
  77         # filtered parameters (0 = no filter, 1 = infinite filter)
  78         self.pitch_filter_coef = 0.8
  79         self.roll_filter_coef = 0.8
  80         self.yaw_filter_coef = 0.8
  81         self.speed_filter_coef = 0.8
  82         self.alt_filter_coef = 0.8
  83         self.rthome_filter_coef = 0.8
  84         # QRect representing the viewport
  85         self.dev = None
  86         # QRect representing the viewport, adjusted to a square
  87         self.adjdev = None
  88         self.setMinimumSize(250, 250)
  89
  90         self.setStyleSheet("background-color: transparent;")
  91
  92         # how many degrees between pitch reference lines
  93         if mode == "round":
  94             self.CAM_ANGLE = 90.
  95             self.PITCH_REFLINES_STEP_ANGLE = 10.
  96             self.PITCH_REFLINES_BOLD_STEP_ANGLE = 30.
  97             self.PITCH_REFLINES_NUM_LINES = 6
  98         else:
  99             self.CAM_ANGLE = 40.
 100             self.PITCH_REFLINES_STEP_ANGLE = 5.
 101             self.PITCH_REFLINES_BOLD_STEP_ANGLE = 10.
 102             self.PITCH_REFLINES_NUM_LINES = 4
 103
 104         self.PITCH_REFLINES_TOTAL_ANGLE = (self.PITCH_REFLINES_STEP_ANGLE *
 105                                            self.PITCH_REFLINES_NUM_LINES)
 106         # in fraction of radius
 107         self.FONT_SIZE = 0.06
 108         # how many degrees between yaw reference lines
 109         self.YAW_REFLINES_STEP_ANGLE = 15.
 110         self.YAW_REFLINES_NUM_LINES = 12
 111         self.YAW_REFLINES_TOTAL_ANGLE = (self.YAW_REFLINES_STEP_ANGLE *
 112                                          self.YAW_REFLINES_NUM_LINES)
 113         self.YAW_REFLINES_SIZE_FACTOR = 0.5
 114
 115         self.SPEED_REFLINES_STEP = 10.
 116         self.SPEED_REFLINES_BOLD_STEP = 50.
 117         self.SPEED_REFLINES_NUM_LINES = 10
 118         self.SPEED_REFLINES_TOTAL = (self.SPEED_REFLINES_STEP *
 119                                      self.SPEED_REFLINES_NUM_LINES)
 120         self.SPEED_REFLINES_SIZE_FACTOR = 0.7
 121
 122         self.ALT_REFLINES_STEP = 100.
 123         self.ALT_REFLINES_BOLD_STEP = 500.
 124         self.ALT_REFLINES_NUM_LINES = 10
 125         self.ALT_REFLINES_TOTAL = (self.ALT_REFLINES_STEP *
 126                                    self.ALT_REFLINES_NUM_LINES)
 127         self.ALT_REFLINES_SIZE_FACTOR = 0.7
 128
 129         if filename:
 130             self.fpv = serialfpv.SerialFPV()
 131             self.fpv.open_file(filename)
 132
 133         self.FPS = 50.
 134         self.timer = QTimer(self)
 135         self.timer.timeout.connect(self.frameTimerCb)
 136         self.timer.start(1000. / self.FPS)
 137
 138     @pyqtSlot(name = "paintEvent")
 139     def paintEvent(self, evt):
 140         """Paint callback, this is the entry point for all drawings."""
 141         painter = QPainter()
 142         painter.begin(self)
 143         painter.setRenderHint(QPainter.Antialiasing)
 144         self.dev = evt.rect()
 145         self.min_dim = min(self.dev.right(), self.dev.bottom())
 146         self.max_dim = max(self.dev.right(), self.dev.bottom())
 147         self.adjdev = QRect(0, 0, self.min_dim, self.min_dim)
 148         self.adjdev.moveCenter(self.dev.center())
 149         self.draw(painter)
 150         painter.end()
 151
 152     @pyqtSlot(name = "frameTimerCb")
 153     def frameTimerCb(self):
 154         # read from SerialFPV object
 155         if self.fpv:
 156             self.fpv.update_state()
 157             self.user_speed = self.user_speed + 1
 158             self.setRoll(self.fpv.roll)
 159             self.setPitch(self.fpv.pitch)
 160             self.setYaw(self.fpv.yaw)
 161
 162         # avoid filter bugs when changing between 180 and -180
 163         self.pitch += round((self.user_pitch - self.pitch) / 360.) * 360
 164         self.pitch = (self.pitch * self.pitch_filter_coef +
 165                     self.user_pitch * (1. - self.pitch_filter_coef))
 166         self.roll += round((self.user_roll - self.roll) / 360.) * 360
 167         self.roll = (self.roll * self.roll_filter_coef +
 168                     self.user_roll * (1. - self.roll_filter_coef))
 169         self.yaw += round((self.user_yaw - self.yaw) / 360.) * 360
 170         self.yaw = (self.yaw * self.yaw_filter_coef +
 171                     self.user_yaw * (1. - self.yaw_filter_coef))
 172         self.speed = (self.speed * self.speed_filter_coef +
 173                     self.user_speed * (1. - self.speed_filter_coef))
 174         self.alt = (self.alt * self.alt_filter_coef +
 175                     self.user_alt * (1. - self.alt_filter_coef))
 176         self.rthome += round((self.user_rthome - self.rthome) / 360.) * 360
 177         self.rthome = (self.rthome * self.rthome_filter_coef +
 178                     self.user_rthome * (1. - self.rthome_filter_coef))
 179         self.update()
 180
 181     def draw(self, painter):
 182         """Draw the widget."""
 183         if self.mode == "round":
 184             self.drawHorizonRound(painter)
 185         elif self.mode == "rectangle":
 186             self.drawHorizon(painter)
 187         self.drawPitchGraduation(painter)
 188         self.drawRollGraduation(painter)
 189         self.drawYaw(painter)
 190         self.drawCenterRef(painter)
 191         if self.mode != "round":
 192             self.drawSpeed(painter)
 193             self.drawAlt(painter)
 194             self.drawReturnToHome(painter)
 195             self.drawTxtInfo(painter)
 196
 197     def getSkyBrush(self):
 198         """return the color gradient for the sky (blue)"""
 199         sky_gradient = QLinearGradient(self.adjdev.topLeft(),
 200                                        self.adjdev.bottomRight())
 201         color1 = Qt.blue
 202         color2 = Qt.darkBlue
 203         sky_gradient.setColorAt(0, color1)
 204         sky_gradient.setColorAt(.8, color2)
 205         return sky_gradient
 206
 207     def getGroundBrush(self):
 208         """return the color gradient for the ground (marron)"""
 209         ground_gradient = QLinearGradient(self.adjdev.topLeft(),
 210                                           self.adjdev.bottomRight())
 211         color1 = QColor(140, 100, 80)
 212         color2 = QColor(140, 100, 40)
 213         ground_gradient.setColorAt(0, color1)
 214         ground_gradient.setColorAt(.8, color2)
 215         return ground_gradient
 216
 217     def getCenterRadius(self):
 218         """Return the radius of the widget circle"""
 219         if self.mode == "round":
 220             return self.adjdev.width() / 2.
 221         else:
 222             return self.adjdev.width() / 3.5
 223
 224     def drawHorizonRound(self, painter):
 225         """Draw the horizon for round widget: the sky in blue,
 226            the ground in marron."""
 227
 228         # set local pitch and roll
 229         pitch = self.pitch
 230         roll = self.roll
 231         if pitch > 90.:
 232             pitch = 180. - pitch
 233             roll += 180.
 234             if roll > 180.:
 235                 roll -= 360.
 236         if pitch < -90.:
 237             pitch = -180. - pitch
 238             roll += 180.
 239             if roll > 180.:
 240                 roll -= 360.
 241
 242         # we have to draw a partial circle delimited by its chord, define
 243         # where the chord starts
 244         start_angle = math.degrees(math.asin(pitch / 90.)) - roll
 245         span = 2 * math.degrees(math.asin(pitch / 90.))
 246
 247         # draw the sky
 248         painter.setBrush(self.getSkyBrush())
 249         # startAngle and spanAngle must be specified in 1/16th of a degree
 250         painter.drawChord(self.adjdev, 16 * start_angle, 16 * (180. - span))
 251
 252         # draw the ground
 253         painter.setBrush(self.getGroundBrush())
 254         # startAngle and spanAngle must be specified in 1/16th of a degree
 255         painter.drawChord(self.adjdev, 16 * start_angle, -16 * (180. + span))
 256
 257     def drawHorizon(self, painter):
 258         """Draw the horizon: the sky in blue, the ground in marron."""
 259         painter.save()
 260         painter.setBrush(self.getSkyBrush())
 261         painter.drawRect(self.dev)
 262         center = self.adjdev.center()
 263         r = self.getCenterRadius()
 264         # radius of the adjusted screen (same than r if roundWidget = True)
 265         dev_r = self.adjdev.width() / 2.
 266         roll = self.roll
 267         if self.pitch < -90.:
 268             pitch = -180 - self.pitch
 269         elif self.pitch < 90.:
 270             pitch = self.pitch
 271         else:
 272             pitch = 180 - self.pitch
 273         y_off = (pitch / self.CAM_ANGLE) * dev_r
 274         painter.translate(center.x(), center.y())
 275         painter.rotate(roll)
 276         ground_rect = QRect(0, 0, self.max_dim * 5., self.max_dim * 5.)
 277         if self.pitch < 90. and self.pitch > -90.:
 278             ground_rect.moveCenter(QPoint(0, -y_off + ground_rect.width()/2.))
 279         else:
 280             ground_rect.moveCenter(QPoint(0, y_off - ground_rect.width()/2.))
 281         painter.setBrush(self.getGroundBrush())
 282         painter.drawRect(ground_rect)
 283         painter.restore()
 284
 285     def drawCenterRef(self, painter):
 286         """Draw the cross on the middle of the OSD"""
 287         center = self.adjdev.center()
 288         r = self.getCenterRadius()
 289         pen = QPen(Qt.red, r / 100., Qt.SolidLine)
 290         painter.setPen(pen)
 291         pt1 = QPoint(center.x() - 0.05 * r, center.y())
 292         pt2 = QPoint(center.x() + 0.05 * r, center.y())
 293         painter.drawLine(pt1, pt2)
 294         pt1 = QPoint(center.x(), center.y() + -0.025 * r)
 295         pt2 = QPoint(center.x(), center.y() + 0.025 * r)
 296         painter.drawLine(pt1, pt2)
 297
 298     def drawPitchGraduation(self, painter):
 299         """Draw the pitch graduations."""
 300         # change the reference
 301         painter.save()
 302         center = self.adjdev.center()
 303         r = self.getCenterRadius()
 304         # radius of the adjusted screen (same than r if roundWidget = True)
 305         dev_r = self.adjdev.width() / 2.
 306         roll = self.roll
 307         x_off = (self.pitch / self.CAM_ANGLE) * dev_r * math.sin(math.radians(roll))
 308         y_off = (self.pitch / self.CAM_ANGLE) * dev_r * math.cos(math.radians(roll))
 309         painter.translate(center.x() + x_off, center.y() - y_off)
 310         painter.rotate(roll)
 311
 312         # set font and pen
 313         pen = QPen(Qt.white, r / 100., Qt.SolidLine)
 314         painter.setPen(pen)
 315         font = QFont("Meiryo UI", 0, QFont.Bold)
 316         font.setPointSizeF(self.FONT_SIZE * r)
 317         painter.setFont(font)
 318
 319         # round to nearest angle that is a multiple of step
 320         a = self.pitch / self.PITCH_REFLINES_STEP_ANGLE
 321         a = round(a)
 322         a = int(a * self.PITCH_REFLINES_STEP_ANGLE)
 323         a -= self.PITCH_REFLINES_STEP_ANGLE * (self.PITCH_REFLINES_NUM_LINES / 2.)
 324         angles = [ a + i * self.PITCH_REFLINES_STEP_ANGLE
 325                    for i in range(self.PITCH_REFLINES_NUM_LINES + 1) ]
 326         for a in angles:
 327             # thin line
 328             if int(a) % int(self.PITCH_REFLINES_BOLD_STEP_ANGLE) != 0:
 329                 pen = QPen(Qt.white, r / 100., Qt.SolidLine)
 330                 painter.setPen(pen)
 331                 pt1 = QPoint(-0.05 * r, dev_r / self.CAM_ANGLE * a)
 332                 pt2 = QPoint(0.05 * r, dev_r / self.CAM_ANGLE * a)
 333                 painter.drawLine(pt1, pt2)
 334                 continue
 335
 336             # bold line
 337             pen = QPen(Qt.white, r / 50., Qt.SolidLine)
 338             painter.setPen(pen)
 339             pt1 = QPoint(-0.2 * r, dev_r / self.CAM_ANGLE * a)
 340             pt2 = QPoint(0.2 * r, dev_r / self.CAM_ANGLE * a)
 341             painter.drawLine(pt1, pt2)
 342
 343             # the left text
 344             disp_val = -a
 345             if disp_val > 90.:
 346                 disp_val = 180. - disp_val
 347             if disp_val < -90.:
 348                 disp_val = -180. - disp_val
 349             disp_val = str(int(disp_val))
 350             metrics = painter.fontMetrics()
 351             sz = metrics.size(Qt.TextSingleLine, disp_val)
 352             lefttxt = QRect(QPoint(0, 0), sz)
 353             lefttxt.moveCenter(pt1 - QPoint(0.2 * r, 0))
 354             # XXX
 355             #pen.setWidth(1);
 356             #brush = QBrush(Qt.white)
 357             #painter.setBrush(brush)
 358             #pen.setColor(Qt.black);
 359             #painter.setPen(pen)
 360             #path = QPainterPath()
 361             #path.addText(lefttxt.center(), font, disp_val)
 362             #painter.drawPath(path)
 363             painter.drawText(lefttxt, Qt.TextSingleLine, disp_val)
 364
 365             # flip the right text
 366             painter.save()
 367             painter.translate(pt2 + QPoint(0.2 * r, 0))
 368             painter.rotate(180.)
 369             painter.translate(-pt2 - QPoint(0.2 * r, 0))
 370             righttxt = QRect(QPoint(0, 0), sz)
 371             righttxt.moveCenter(pt2 + QPoint(0.2 * r, 0))
 372             #path = QPainterPath()
 373             #path.addText(righttxt.center(), font, disp_val)
 374             #painter.drawPath(path)
 375             painter.drawText(righttxt, Qt.TextSingleLine, disp_val)
 376             painter.restore()
 377
 378         painter.restore()
 379
 380     def drawOneRollGraduation(self, painter, deg, disp_text):
 381         # draw the graduiation
 382         r = self.getCenterRadius()
 383         center = self.adjdev.center()
 384         x = center.x() - math.cos(math.radians(deg)) * r
 385         y = center.y() - math.sin(math.radians(deg)) * r
 386         pt = QPoint(x, y)
 387         path = QPainterPath()
 388         path.moveTo(pt)
 389         path.lineTo(center)
 390         pt2 = path.pointAtPercent(0.075) # graduation len is 7.5% of the radius
 391         painter.drawLine(pt, pt2)
 392         # then draw the text
 393         if disp_text == True:
 394             pt_txt = path.pointAtPercent(0.2)
 395             font = QFont("Meiryo UI", 0, QFont.Bold)
 396             font.setPointSizeF(self.FONT_SIZE * r)
 397             painter.setFont(font)
 398             disp_val = deg
 399             if disp_val > 90:
 400                 disp_val = 180. - disp_val
 401             disp_val = str(int(disp_val))
 402             metrics = painter.fontMetrics()
 403             sz = metrics.size(Qt.TextSingleLine, disp_val)
 404             txt = QRect(QPoint(0, 0), sz)
 405             txt.moveCenter(pt_txt.toPoint())
 406             painter.drawText(txt, Qt.TextSingleLine, disp_val)
 407
 408     def drawRollGraduation(self, painter):
 409         """Draw the roll graduations."""
 410         center = self.adjdev.center()
 411         r = self.getCenterRadius()
 412
 413         # draw the red reference lines (pitch 0)
 414         painter.save()
 415         painter.translate(center.x(), center.y())
 416         painter.rotate(self.roll)
 417         pen = QPen(Qt.red, r / 100., Qt.SolidLine)
 418         painter.setPen(pen)
 419         pt1 = QPoint(-0.925 * r, 0)
 420         pt2 = QPoint(-0.85 * r, 0)
 421         painter.drawLine(pt1, pt2)
 422         pt1 = QPoint(0.925 * r, 0)
 423         pt2 = QPoint(0.85 * r, 0)
 424         painter.drawLine(pt1, pt2)
 425         painter.restore()
 426
 427         pen = QPen(Qt.white, r / 50., Qt.SolidLine)
 428         painter.setPen(pen)
 429         deg = 0
 430         while deg <= 180:
 431             if deg % 30 == 0:
 432                 w = r / 50.
 433                 disp_text = True
 434             else:
 435                 w = r / 100.
 436                 disp_text = False
 437             pen.setWidth(w)
 438             painter.setPen(pen)
 439             self.drawOneRollGraduation(painter, deg, disp_text)
 440             deg += 10
 441
 442     def drawYaw(self, painter):
 443         center = self.adjdev.center()
 444         r = self.getCenterRadius()
 445         pen = QPen(Qt.red, r / 100., Qt.SolidLine)
 446         painter.setPen(pen)
 447         font = QFont("Meiryo UI", 0, QFont.Bold)
 448         font.setPointSizeF(self.FONT_SIZE * r)
 449         painter.setFont(font)
 450         if self.mode == "round":
 451             y_txt = center.y() + r * 0.6
 452             y1 = center.y() + r * 0.7
 453             y2 = center.y() + r * 0.8
 454             y3 = center.y() + r * 0.85
 455         else:
 456             y_txt = center.y() + r * 1.
 457             y1 = center.y() + r * 1.1
 458             y2 = center.y() + r * 1.2
 459             y3 = center.y() + r * 1.25
 460         pt1 = QPoint(center.x(), y2)
 461         pt2 = QPoint(center.x(), y3)
 462         painter.drawLine(pt1, pt2)
 463
 464         # round to nearest angle multiple of step
 465         a = self.yaw / self.YAW_REFLINES_STEP_ANGLE
 466         a = round(a)
 467         a = int(a * self.YAW_REFLINES_STEP_ANGLE)
 468         a -= self.YAW_REFLINES_STEP_ANGLE * (self.YAW_REFLINES_NUM_LINES / 2.)
 469         angles = [ a + i * self.YAW_REFLINES_STEP_ANGLE
 470                    for i in range(self.YAW_REFLINES_NUM_LINES + 1) ]
 471         for a in angles:
 472             # text (N, S, E, W)
 473             if int(a) % 90 == 0:
 474                 disp_text = True
 475                 pen = QPen(Qt.white, r / 50., Qt.SolidLine)
 476                 painter.setPen(pen)
 477             else:
 478                 disp_text = False
 479                 pen = QPen(Qt.white, r / 100., Qt.SolidLine)
 480                 painter.setPen(pen)
 481             # the line
 482             x = center.x() - ((r / (self.YAW_REFLINES_TOTAL_ANGLE / 2.)) *
 483                               (self.yaw - a) * self.YAW_REFLINES_SIZE_FACTOR)
 484             pt_txt = QPoint(x, y_txt)
 485             pt1 = QPoint(x, y1)
 486             pt2 = QPoint(x, y2)
 487             painter.drawLine(pt1, pt2)
 488             if disp_text == False:
 489                 continue
 490             disp_val = ["N", "E", "S", "W"][(int(a)/90)%4]
 491             metrics = painter.fontMetrics()
 492             sz = metrics.size(Qt.TextSingleLine, disp_val)
 493             txt = QRect(QPoint(0, 0), sz)
 494             txt.moveCenter(pt_txt)
 495             painter.drawText(txt, Qt.TextSingleLine, disp_val)
 496
 497     def drawSpeed(self, painter):
 498         center = self.adjdev.center()
 499         r = self.getCenterRadius()
 500         pen = QPen(Qt.red, r / 100., Qt.SolidLine)
 501         painter.setPen(pen)
 502         font = QFont("Meiryo UI", 0, QFont.Bold)
 503         font.setPointSizeF(self.FONT_SIZE * r)
 504         painter.setFont(font)
 505         x1 =  center.x() - 1.5 * r
 506         x2 =  center.x() - 1.6 * r
 507         pt1 = QPoint(center.x() - 1.45 * r, center.y())
 508         pt2 = QPoint(x2, center.y())
 509         painter.drawLine(pt1, pt2)
 510
 511         # round to nearest angle multiple of step
 512         s = self.speed / self.SPEED_REFLINES_STEP
 513         s = round(s)
 514         s = int(s * self.SPEED_REFLINES_STEP)
 515         s -= self.SPEED_REFLINES_STEP * (self.SPEED_REFLINES_NUM_LINES / 2.)
 516         speeds = [ s + i * self.SPEED_REFLINES_STEP
 517                    for i in range(self.SPEED_REFLINES_NUM_LINES + 1) ]
 518         for s in speeds:
 519             if int(s) % int(self.SPEED_REFLINES_BOLD_STEP) == 0:
 520                 disp_text = True
 521                 pen = QPen(Qt.white, r / 50., Qt.SolidLine)
 522                 painter.setPen(pen)
 523             else:
 524                 disp_text = False
 525                 pen = QPen(Qt.white, r / 100., Qt.SolidLine)
 526                 painter.setPen(pen)
 527             # the line
 528             y = center.y() + ((r / (self.SPEED_REFLINES_TOTAL/2.)) *
 529                               (self.speed - s) * self.SPEED_REFLINES_SIZE_FACTOR)
 530             pt_txt = QPoint(center.x() + r * -1.75, y)
 531             pt1 = QPoint(x1, y)
 532             pt2 = QPoint(x2, y)
 533             painter.drawLine(pt1, pt2)
 534             if disp_text == False:
 535                 continue
 536             disp_val = str(int(s))
 537             metrics = painter.fontMetrics()
 538             sz = metrics.size(Qt.TextSingleLine, disp_val)
 539             txt = QRect(QPoint(0, 0), sz)
 540             txt.moveCenter(pt_txt)
 541             painter.drawText(txt, Qt.TextSingleLine, disp_val)
 542
 543     def drawAlt(self, painter):
 544         center = self.adjdev.center()
 545         r = self.getCenterRadius()
 546         pen = QPen(Qt.red, r / 100., Qt.SolidLine)
 547         painter.setPen(pen)
 548         font = QFont("Meiryo UI", 0, QFont.Bold)
 549         font.setPointSizeF(self.FONT_SIZE * r)
 550         painter.setFont(font)
 551         x1 =  center.x() + 1.5 * r
 552         x2 =  center.x() + 1.6 * r
 553         pt1 = QPoint(center.x() + 1.45 * r, center.y())
 554         pt2 = QPoint(x2, center.y())
 555         painter.drawLine(pt1, pt2)
 556
 557         # round to nearest angle multiple of step
 558         a = self.alt / self.ALT_REFLINES_STEP
 559         a = round(a)
 560         a = int(a * self.ALT_REFLINES_STEP)
 561         a -= self.ALT_REFLINES_STEP * (self.ALT_REFLINES_NUM_LINES / 2.)
 562         alts = [ a + i * self.ALT_REFLINES_STEP
 563                    for i in range(self.ALT_REFLINES_NUM_LINES + 1) ]
 564         for a in alts:
 565             if int(a) % int(self.ALT_REFLINES_BOLD_STEP) == 0:
 566                 disp_text = True
 567                 pen = QPen(Qt.white, r / 50., Qt.SolidLine)
 568                 painter.setPen(pen)
 569             else:
 570                 disp_text = False
 571                 pen = QPen(Qt.white, r / 100., Qt.SolidLine)
 572                 painter.setPen(pen)
 573             # the line
 574             y = center.y() + ((r / (self.ALT_REFLINES_TOTAL / 2.)) *
 575                               (self.alt - a) * self.ALT_REFLINES_SIZE_FACTOR)
 576             pt_txt = QPoint(center.x() + r * 1.75, y)
 577             pt1 = QPoint(x1, y)
 578             pt2 = QPoint(x2, y)
 579             painter.drawLine(pt1, pt2)
 580             if disp_text == False:
 581                 continue
 582             disp_val = str(int(a))
 583             metrics = painter.fontMetrics()
 584             sz = metrics.size(Qt.TextSingleLine, disp_val)
 585             txt = QRect(QPoint(0, 0), sz)
 586             txt.moveCenter(pt_txt)
 587             painter.drawText(txt, Qt.TextSingleLine, disp_val)
 588
 589     def drawReturnToHome(self, painter):
 590         center = self.adjdev.center()
 591         r = self.getCenterRadius()
 592         dev_r = self.adjdev.width() / 2.
 593         painter.save()
 594         painter.translate(center.x(), center.y() - 0.9 * dev_r)
 595         painter.rotate(self.yaw) # XXX
 596         pen = QPen(Qt.white, r / 100., Qt.SolidLine)
 597         painter.setPen(pen)
 598         poly = QPolygonF()
 599         poly.append(QPoint(0., -0.08 * r))
 600         poly.append(QPoint(0.04 * r, 0.08 * r))
 601         poly.append(QPoint(-0.04 * r, 0.08 * r))
 602         poly.append(QPoint(0., -0.08 * r))
 603         path = QPainterPath()
 604         path.addPolygon(poly)
 605         brush = QBrush(Qt.darkGray)
 606         painter.setBrush(brush)
 607         painter.drawPath(path)
 608         painter.restore()
 609
 610     def drawTxtInfo(self, painter):
 611         center = self.adjdev.center()
 612         dev_r = self.adjdev.width() / 2.
 613         r = self.getCenterRadius()
 614         pen = QPen(Qt.white, r / 100., Qt.SolidLine)
 615         painter.setPen(pen)
 616         font = QFont("Meiryo UI", 0, QFont.Bold)
 617         font.setPointSizeF(self.FONT_SIZE * r)
 618         metrics = painter.fontMetrics()
 619
 620         sz = metrics.size(Qt.AlignLeft, self.left_txt)
 621         txt = QRect(QPoint(0, 0), sz)
 622         pt_txt = QPoint(center.x() + dev_r * -0.95,
 623                                center.y() + dev_r * -0.95)
 624         txt.moveTopLeft(pt_txt)
 625         painter.drawText(txt, Qt.AlignLeft, self.left_txt)
 626
 627         sz = metrics.size(Qt.AlignRight, self.right_txt)
 628         txt = QRect(QPoint(0, 0), sz)
 629         pt_txt = QPoint(center.x() + dev_r * 0.95,
 630                                center.y() + dev_r * -0.95)
 631         txt.moveTopRight(pt_txt)
 632         painter.drawText(txt, Qt.AlignRight, self.right_txt)
 633
 634     def setPitch(self, pitch):
 635         """set the pitch in degrees, between -180 and 180"""
 636         pitch = pitch % 360.
 637         if pitch > 180.:
 638             pitch -= 360.
 639         self.user_pitch = pitch
 640
 641     def setRoll(self, roll):
 642         """set the roll in degrees, between -180 and 180"""
 643         roll = roll % 360.
 644         if roll > 180.:
 645             roll -= 360.
 646         self.user_roll = roll
 647
 648     def setYaw(self, yaw):
 649         """set the yaw in degrees, between 0 and 360"""
 650         yaw = yaw % 360.
 651         self.user_yaw = yaw
 652
 653     def setSpeed(self, speed):
 654         """set the speed"""
 655         self.user_speed = speed
 656
 657     def setAlt(self, alt):
 658         """set the alt"""
 659         self.user_alt = alt
 660
 661     def setReturnToHomeAngle(self, angle):
 662         """set the left text"""
 663         self.user_rthome = angle
 664
 665     def setLeftTxt(self, txt):
 666         """set the right text"""
 667         self.left_txt = txt
 668
 669     def setRightTxt(self, txt):
 670         """set the left text"""
 671         self.right_txt = txt
 672
 673 class OSDGraphicsView(QGraphicsView):
 674     def __init__(self, parent=None):
 675         super(OSDGraphicsView, self).__init__(parent)
 676         self.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
 677         self.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
 678         self.setMinimumSize(200, 150)
 679     def resizeEvent(self, event):
 680         # use item 0 to resize automatically
 681         self.fitInView(self.items()[0], Qt.KeepAspectRatio)
 682         super(OSDGraphicsView, self).resizeEvent(event)
 683
 684 class Ui_OSD(QMainWindow):
 685     def __init__(self, parent = None, mode = "camera", filename = None):
 686         super(Ui_OSD, self).__init__(parent)
 687         self.ui = qtosd_ui.Ui_MainWindow()
 688         self.ui.setupUi(self)
 689
 690         self.mode = mode
 691         self.filename = filename
 692
 693         self.osd = OSDWidget(mode = self.mode,
 694                              filename = self.filename)
 695         self.osd.setObjectName("osd")
 696         self.ui.pitchSlider.valueChanged[int].connect(self.changePitch)
 697         self.ui.rollSlider.valueChanged[int].connect(self.changeRoll)
 698         self.ui.yawSlider.valueChanged[int].connect(self.changeYaw)
 699         self.ui.actionExit.triggered.connect(self.close)
 700
 701         self.scene = QGraphicsScene(self)
 702         self.graphicsView = OSDGraphicsView(self.scene)
 703
 704         if self.mode == "camera":
 705             self.videoItem = QGraphicsVideoItem()
 706             self.videoItem.setSize(QSizeF(640, 480)) # XXX
 707             self.scene.addItem(self.videoItem)
 708
 709             x = self.videoItem.boundingRect().width() / 2.0
 710             y = self.videoItem.boundingRect().height() / 2.0
 711             #self.videoItem.setTransform(
 712             #        QTransform().translate(x, y).rotate(70).translate(-x, -y))
 713
 714             self.initCamera()
 715
 716         self.scene.addWidget(self.osd)
 717         self.ui.gridLayout.addWidget(self.graphicsView, 0, 1)
 718
 719     def initCamera(self):
 720         # find camera devices and add them in the menu
 721         cameraDevice = QByteArray()
 722         videoDevicesGroup = QActionGroup(self)
 723         videoDevicesGroup.setExclusive(True)
 724         for deviceName in QCamera.availableDevices():
 725             description = QCamera.deviceDescription(deviceName)
 726             videoDeviceAction = QAction(description, videoDevicesGroup)
 727             videoDeviceAction.setCheckable(True)
 728             videoDeviceAction.setData(deviceName)
 729             if cameraDevice.isEmpty():
 730                 cameraDevice = deviceName
 731                 videoDeviceAction.setChecked(True)
 732             self.ui.menuDevices.addAction(videoDeviceAction)
 733         videoDevicesGroup.triggered.connect(self.updateCameraDevice)
 734         # select the first camera
 735         self.setCamera(cameraDevice)
 736
 737     def setCamera(self, cameraDevice):
 738         if cameraDevice.isEmpty():
 739             self.camera = QCamera()
 740         else:
 741             self.camera = QCamera(cameraDevice)
 742
 743         self.camera.stateChanged.connect(self.updateCameraState)
 744         self.camera.error.connect(self.displayCameraError)
 745
 746         #self.ui.exposureCompensation.valueChanged.connect(
 747         #        self.setExposureCompensation)
 748
 749         self.camera.setCaptureMode(QCamera.CaptureViewfinder)
 750         self.camera.setViewfinder(self.videoItem)
 751         self.updateCameraState(self.camera.state())
 752         self.camera.start()
 753
 754     #XXX stop camera? remove from scene?
 755
 756     def updateCameraDevice(self, action):
 757         print "updateCameraDevice"
 758         self.setCamera(action.data())
 759
 760     def updateCameraState(self, state):
 761         print "updateCameraState %s"%(str(state))
 762
 763     def displayCameraError(self):
 764         print "displayCameraError"
 765         QMessageBox.warning(self, "Camera error", self.camera.errorString())
 766
 767     def keyPressEvent(self, event):
 768         key = event.key()
 769         if key == Qt.Key_J:
 770             self.osd.setRoll(self.osd.user_roll + 2)
 771             event.accept()
 772         elif key == Qt.Key_L:
 773             self.osd.setRoll(self.osd.user_roll - 2)
 774             event.accept()
 775         elif key == Qt.Key_I:
 776             self.osd.setPitch(self.osd.user_pitch + 2)
 777             event.accept()
 778         elif key == Qt.Key_K:
 779             self.osd.setPitch(self.osd.user_pitch - 2)
 780             event.accept()
 781         elif key == Qt.Key_Q:
 782             self.close()
 783         elif not event.isAutoRepeat():
 784             if key == Qt.Key_CameraFocus:
 785                 self.camera.searchAndLock()
 786                 event.accept()
 787             else:
 788                 super(Ui_OSD, self).keyPressEvent(event)
 789
 790     def keyReleaseEvent(self, event):
 791         key = event.key()
 792         if event.isAutoRepeat():
 793             return
 794         if key == Qt.Key_CameraFocus:
 795             self.camera.unlock()
 796         else:
 797             super(Ui_OSD, self).keyReleaseEvent(event)
 798
 799     def retranslateUi(self, MainWindow):
 800         MainWindow.setWindowTitle(_translate("MainWindow", "MainWindow", None))
 801
 802     @pyqtSlot(int, name = "changePitch")
 803     def changePitch(self, value):
 804         self.osd.setPitch(value)
 805
 806     @pyqtSlot(int, name = "changeRoll")
 807     def changeRoll(self, value):
 808         self.osd.setRoll(value)
 809
 810     @pyqtSlot(int, name = "changeYaw")
 811     def changeYaw(self, value):
 812         self.osd.setYaw(value)
 813
 814 if __name__ == "__main__":
 815     import sys
 816
 817     parser = argparse.ArgumentParser(description='OSD written in Qt.')
 818     parser.add_argument('--mode', '-m', action="store",
 819                         choices=["round", "rectangle", "camera"],
 820                         help='display the widget as a round attitude meter')
 821     parser.add_argument('--filename', '-f',
 822                         help='specify the log file')
 823     args = parser.parse_args()
 824
 825     app = QApplication(sys.argv)
 826     ui = Ui_OSD(filename = args.filename, mode = args.mode)
 827     ui.show()
 828     sys.exit(app.exec_())
 829