From 844bf63c7f0b676932a04c3e3c7060abf90faea4 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Tue, 2 Jun 2015 18:46:01 +0200 Subject: [PATCH 1/1] add first version of qt-osd Signed-off-by: Olivier Matz --- qtosd/qtosd.py | 749 +++++++++++++++++++++++++++++++++++++++++++++ qtosd/qtosd.ui | 85 +++++ qtosd/serialfpv.py | 97 ++++++ 3 files changed, 931 insertions(+) create mode 100644 qtosd/qtosd.py create mode 100644 qtosd/qtosd.ui create mode 100644 qtosd/serialfpv.py diff --git a/qtosd/qtosd.py b/qtosd/qtosd.py new file mode 100644 index 0000000..dd2a6ee --- /dev/null +++ b/qtosd/qtosd.py @@ -0,0 +1,749 @@ +# -*- coding: utf-8 -*- + +# OSD (on screen display) written in Qt +# Copyright (C) 2015 Olivier Matz +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +# Inspired from QcGauge +# Copyright (C) 2015 Hadj Tahar Berrima +# http://pytricity.com/qt-artificial-horizon-custom-widget/ + +import math +from PyQt4 import QtCore, QtGui + +import serialfpv + +try: + _fromUtf8 = QtCore.QString.fromUtf8 +except AttributeError: + def _fromUtf8(s): + return s + +try: + _encoding = QtGui.QApplication.UnicodeUTF8 + def _translate(context, text, disambig): + return QtGui.QApplication.translate(context, text, disambig, _encoding) +except AttributeError: + def _translate(context, text, disambig): + return QtGui.QApplication.translate(context, text, disambig) + +class OSDWidget(QtGui.QWidget): + def __init__(self, roundWidget = False): + super(OSDWidget, self).__init__() + # init parameters + self.roundWidget = roundWidget + # parameters that will be modified by the user + self.user_pitch = 0 + self.user_roll = 0 + self.user_yaw = 0 + self.user_speed = 0 + self.user_alt = 0 + self.user_rthome = 0 + self.left_txt = "14.8v / 34A\n1.5Ah" # XXX + self.right_txt = "23:03 since take-off\n1.4 km to home\nRSSI 60dB" # XXX + self.user_frame_cb = None + # filtered parameters + self.pitch = 0 + self.roll = 0 + self.yaw = 0 + self.speed = 0 + self.alt = 0 + self.rthome = 0 + # filtered parameters (0 = no filter, 1 = infinite filter) + self.pitch_filter_coef = 0.8 + self.roll_filter_coef = 0.8 + self.yaw_filter_coef = 0.8 + self.speed_filter_coef = 0.8 + self.alt_filter_coef = 0.8 + self.rthome_filter_coef = 0.8 + # QRect representing the viewport + self.dev = None + # QRect representing the viewport, adjusted to a square + self.adjdev = None + self.setMinimumSize(250, 250) + + # how many degrees between pitch reference lines + if roundWidget: + self.CAM_ANGLE = 90. + self.PITCH_REFLINES_STEP_ANGLE = 10. + self.PITCH_REFLINES_BOLD_STEP_ANGLE = 30. + self.PITCH_REFLINES_NUM_LINES = 6 + else: + self.CAM_ANGLE = 40. + self.PITCH_REFLINES_STEP_ANGLE = 5. + self.PITCH_REFLINES_BOLD_STEP_ANGLE = 10. + self.PITCH_REFLINES_NUM_LINES = 4 + + self.PITCH_REFLINES_TOTAL_ANGLE = (self.PITCH_REFLINES_STEP_ANGLE * + self.PITCH_REFLINES_NUM_LINES) + # in fraction of radius + self.FONT_SIZE = 0.06 + # how many degrees between yaw reference lines + self.YAW_REFLINES_STEP_ANGLE = 15. + self.YAW_REFLINES_NUM_LINES = 12 + self.YAW_REFLINES_TOTAL_ANGLE = (self.YAW_REFLINES_STEP_ANGLE * + self.YAW_REFLINES_NUM_LINES) + self.YAW_REFLINES_SIZE_FACTOR = 0.5 + + self.SPEED_REFLINES_STEP = 10. + self.SPEED_REFLINES_BOLD_STEP = 50. + self.SPEED_REFLINES_NUM_LINES = 10 + self.SPEED_REFLINES_TOTAL = (self.SPEED_REFLINES_STEP * + self.SPEED_REFLINES_NUM_LINES) + self.SPEED_REFLINES_SIZE_FACTOR = 0.7 + + self.ALT_REFLINES_STEP = 100. + self.ALT_REFLINES_BOLD_STEP = 500. + self.ALT_REFLINES_NUM_LINES = 10 + self.ALT_REFLINES_TOTAL = (self.ALT_REFLINES_STEP * + self.ALT_REFLINES_NUM_LINES) + self.ALT_REFLINES_SIZE_FACTOR = 0.7 + + self.FPS = 50. + self.timer = QtCore.QTimer(self) + self.connect(self.timer, QtCore.SIGNAL("timeout()"), self.frameTimerCb) + self.timer.start(1000. / self.FPS) + + def paintEvent(self, evt): + """Paint callback, this is the entry point for all drawings.""" + painter = QtGui.QPainter() + painter.begin(self) + painter.setRenderHint(QtGui.QPainter.Antialiasing) + self.dev = evt.rect() + self.min_dim = min(self.dev.right(), self.dev.bottom()) + self.max_dim = max(self.dev.right(), self.dev.bottom()) + self.adjdev = QtCore.QRect(0, 0, self.min_dim, self.min_dim) + self.adjdev.moveCenter(self.dev.center()) + self.draw(painter) + painter.end() + + def frameTimerCb(self): + """called periodically, every frame, it calls the user_frame_cb function, + updates the filter and updates the widget""" + if self.user_frame_cb != None: + self.user_frame_cb() + + # avoid filter bugs when changing between 180 and -180 + self.pitch += round((self.user_pitch - self.pitch) / 360.) * 360 + self.pitch = (self.pitch * self.pitch_filter_coef + + self.user_pitch * (1. - self.pitch_filter_coef)) + self.roll += round((self.user_roll - self.roll) / 360.) * 360 + self.roll = (self.roll * self.roll_filter_coef + + self.user_roll * (1. - self.roll_filter_coef)) + self.yaw += round((self.user_yaw - self.yaw) / 360.) * 360 + self.yaw = (self.yaw * self.yaw_filter_coef + + self.user_yaw * (1. - self.yaw_filter_coef)) + self.speed = (self.speed * self.speed_filter_coef + + self.user_speed * (1. - self.speed_filter_coef)) + self.alt = (self.alt * self.alt_filter_coef + + self.user_alt * (1. - self.alt_filter_coef)) + self.rthome += round((self.user_rthome - self.rthome) / 360.) * 360 + self.rthome = (self.rthome * self.rthome_filter_coef + + self.user_rthome * (1. - self.rthome_filter_coef)) + self.update() + + def draw(self, painter): + """Draw the widget.""" + if self.roundWidget: + self.drawHorizonRound(painter) + else: + self.drawHorizon(painter) + self.drawPitchGraduation(painter) + self.drawRollGraduation(painter) + self.drawYaw(painter) + self.drawCenterRef(painter) + if self.roundWidget == False: + self.drawSpeed(painter) + self.drawAlt(painter) + self.drawReturnToHome(painter) + self.drawTxtInfo(painter) + + def getSkyBrush(self): + """return the color gradient for the sky (blue)""" + sky_gradient = QtGui.QLinearGradient(self.adjdev.topLeft(), + self.adjdev.bottomRight()) + color1 = QtCore.Qt.blue + color2 = QtCore.Qt.darkBlue + sky_gradient.setColorAt(0, color1) + sky_gradient.setColorAt(.8, color2) + return sky_gradient + + def getGroundBrush(self): + """return the color gradient for the ground (marron)""" + ground_gradient = QtGui.QLinearGradient(self.adjdev.topLeft(), + self.adjdev.bottomRight()) + color1 = QtGui.QColor(140, 100, 80) + color2 = QtGui.QColor(140, 100, 40) + ground_gradient.setColorAt(0, color1) + ground_gradient.setColorAt(.8, color2) + return ground_gradient + + def getCenterRadius(self): + """Return the radius of the widget circle""" + if self.roundWidget: + return self.adjdev.width() / 2. + else: + return self.adjdev.width() / 3.5 + + def drawHorizonRound(self, painter): + """Draw the horizon for round widget: the sky in blue, + the ground in marron.""" + + # set local pitch and roll + pitch = self.pitch + roll = self.roll + if pitch > 90.: + pitch = 180. - pitch + roll += 180. + if roll > 180.: + roll -= 360. + if pitch < -90.: + pitch = -180. - pitch + roll += 180. + if roll > 180.: + roll -= 360. + + # we have to draw a partial circle delimited by its chord, define + # where the chord starts + start_angle = math.degrees(math.asin(pitch / 90.)) - roll + span = 2 * math.degrees(math.asin(pitch / 90.)) + + # draw the sky + painter.setBrush(self.getSkyBrush()) + # startAngle and spanAngle must be specified in 1/16th of a degree + painter.drawChord(self.adjdev, 16 * start_angle, 16 * (180. - span)) + + # draw the ground + painter.setBrush(self.getGroundBrush()) + # startAngle and spanAngle must be specified in 1/16th of a degree + painter.drawChord(self.adjdev, 16 * start_angle, -16 * (180. + span)) + + def drawHorizon(self, painter): + """Draw the horizon: the sky in blue, the ground in marron.""" + painter.save() + painter.setBrush(self.getSkyBrush()) + painter.drawRect(self.dev) + center = self.adjdev.center() + r = self.getCenterRadius() + # radius of the adjusted screen (same than r if roundWidget = True) + dev_r = self.adjdev.width() / 2. + roll = self.roll + if self.pitch < -90.: + pitch = -180 - self.pitch + elif self.pitch < 90.: + pitch = self.pitch + else: + pitch = 180 - self.pitch + y_off = (pitch / self.CAM_ANGLE) * dev_r + painter.translate(center.x(), center.y()) + painter.rotate(roll) + ground_rect = QtCore.QRect(0, 0, self.max_dim * 5., self.max_dim * 5.) + if self.pitch < 90. and self.pitch > -90.: + ground_rect.moveCenter(QtCore.QPoint(0, -y_off + ground_rect.width()/2.)) + else: + ground_rect.moveCenter(QtCore.QPoint(0, y_off - ground_rect.width()/2.)) + painter.setBrush(self.getGroundBrush()) + painter.drawRect(ground_rect) + painter.restore() + + def drawCenterRef(self, painter): + """Draw the cross on the middle of the OSD""" + center = self.adjdev.center() + r = self.getCenterRadius() + pen = QtGui.QPen(QtCore.Qt.red, r / 100., QtCore.Qt.SolidLine) + painter.setPen(pen) + pt1 = QtCore.QPoint(center.x() - 0.05 * r, center.y()) + pt2 = QtCore.QPoint(center.x() + 0.05 * r, center.y()) + painter.drawLine(pt1, pt2) + pt1 = QtCore.QPoint(center.x(), center.y() + -0.025 * r) + pt2 = QtCore.QPoint(center.x(), center.y() + 0.025 * r) + painter.drawLine(pt1, pt2) + + def drawPitchGraduation(self, painter): + """Draw the pitch graduations.""" + # change the reference + painter.save() + center = self.adjdev.center() + r = self.getCenterRadius() + # radius of the adjusted screen (same than r if roundWidget = True) + dev_r = self.adjdev.width() / 2. + roll = self.roll + x_off = (self.pitch / self.CAM_ANGLE) * dev_r * math.sin(math.radians(roll)) + y_off = (self.pitch / self.CAM_ANGLE) * dev_r * math.cos(math.radians(roll)) + painter.translate(center.x() + x_off, center.y() - y_off) + painter.rotate(roll) + + # set font and pen + pen = QtGui.QPen(QtCore.Qt.white, r / 100., QtCore.Qt.SolidLine) + painter.setPen(pen) + font = QtGui.QFont("Meiryo UI", 0, QtGui.QFont.Bold) + font.setPointSizeF(self.FONT_SIZE * r) + painter.setFont(font) + + # round to nearest angle that is a multiple of step + a = self.pitch / self.PITCH_REFLINES_STEP_ANGLE + a = round(a) + a = int(a * self.PITCH_REFLINES_STEP_ANGLE) + a -= self.PITCH_REFLINES_STEP_ANGLE * (self.PITCH_REFLINES_NUM_LINES / 2.) + angles = [ a + i * self.PITCH_REFLINES_STEP_ANGLE + for i in range(self.PITCH_REFLINES_NUM_LINES + 1) ] + for a in angles: + # thin line + if int(a) % int(self.PITCH_REFLINES_BOLD_STEP_ANGLE) != 0: + pen = QtGui.QPen(QtCore.Qt.white, r / 100., QtCore.Qt.SolidLine) + painter.setPen(pen) + pt1 = QtCore.QPoint(-0.05 * r, dev_r / self.CAM_ANGLE * a) + pt2 = QtCore.QPoint(0.05 * r, dev_r / self.CAM_ANGLE * a) + painter.drawLine(pt1, pt2) + continue + + # bold line + pen = QtGui.QPen(QtCore.Qt.white, r / 50., QtCore.Qt.SolidLine) + painter.setPen(pen) + pt1 = QtCore.QPoint(-0.2 * r, dev_r / self.CAM_ANGLE * a) + pt2 = QtCore.QPoint(0.2 * r, dev_r / self.CAM_ANGLE * a) + painter.drawLine(pt1, pt2) + + # the left text + disp_val = -a + if disp_val > 90.: + disp_val = 180. - disp_val + if disp_val < -90.: + disp_val = -180. - disp_val + disp_val = str(int(disp_val)) + metrics = painter.fontMetrics() + sz = metrics.size(QtCore.Qt.TextSingleLine, disp_val) + lefttxt = QtCore.QRect(QtCore.QPoint(0, 0), sz) + lefttxt.moveCenter(pt1 - QtCore.QPoint(0.2 * r, 0)) + #pen.setWidth(1); + #brush = QtGui.QBrush(QtCore.Qt.white) + #painter.setBrush(brush) + #pen.setColor(QtCore.Qt.black); + #painter.setPen(pen) + #path = QtGui.QPainterPath() + #path.addText(lefttxt.center(), font, disp_val) + #painter.drawPath(path) + painter.drawText(lefttxt, QtCore.Qt.TextSingleLine, disp_val) + + # flip the right text + painter.save() + painter.translate(pt2 + QtCore.QPoint(0.2 * r, 0)) + painter.rotate(180.) + painter.translate(-pt2 - QtCore.QPoint(0.2 * r, 0)) + righttxt = QtCore.QRect(QtCore.QPoint(0, 0), sz) + righttxt.moveCenter(pt2 + QtCore.QPoint(0.2 * r, 0)) + #path = QtGui.QPainterPath() + #path.addText(righttxt.center(), font, disp_val) + #painter.drawPath(path) + painter.drawText(righttxt, QtCore.Qt.TextSingleLine, disp_val) + painter.restore() + + painter.restore() + + def drawOneRollGraduation(self, painter, deg, disp_text): + # draw the graduiation + r = self.getCenterRadius() + center = self.adjdev.center() + x = center.x() - math.cos(math.radians(deg)) * r + y = center.y() - math.sin(math.radians(deg)) * r + pt = QtCore.QPoint(x, y) + path = QtGui.QPainterPath() + path.moveTo(pt) + path.lineTo(center) + pt2 = path.pointAtPercent(0.075) # graduation len is 7.5% of the radius + painter.drawLine(pt, pt2) + # then draw the text + if disp_text == True: + pt_txt = path.pointAtPercent(0.2) + font = QtGui.QFont("Meiryo UI", 0, QtGui.QFont.Bold) + font.setPointSizeF(self.FONT_SIZE * r) + painter.setFont(font) + disp_val = deg + if disp_val > 90: + disp_val = 180. - disp_val + disp_val = str(int(disp_val)) + metrics = painter.fontMetrics() + sz = metrics.size(QtCore.Qt.TextSingleLine, disp_val) + txt = QtCore.QRect(QtCore.QPoint(0, 0), sz) + txt.moveCenter(pt_txt.toPoint()) + painter.drawText(txt, QtCore.Qt.TextSingleLine, disp_val) + + def drawRollGraduation(self, painter): + """Draw the roll graduations.""" + center = self.adjdev.center() + r = self.getCenterRadius() + + # draw the red reference lines (pitch 0) + painter.save() + painter.translate(center.x(), center.y()) + painter.rotate(self.roll) + pen = QtGui.QPen(QtCore.Qt.red, r / 100., QtCore.Qt.SolidLine) + painter.setPen(pen) + pt1 = QtCore.QPoint(-0.925 * r, 0) + pt2 = QtCore.QPoint(-0.85 * r, 0) + painter.drawLine(pt1, pt2) + pt1 = QtCore.QPoint(0.925 * r, 0) + pt2 = QtCore.QPoint(0.85 * r, 0) + painter.drawLine(pt1, pt2) + painter.restore() + + pen = QtGui.QPen(QtCore.Qt.white, r / 50., QtCore.Qt.SolidLine) + painter.setPen(pen) + deg = 0 + while deg <= 180: + if deg % 30 == 0: + w = r / 50. + disp_text = True + else: + w = r / 100. + disp_text = False + pen.setWidth(w) + painter.setPen(pen) + self.drawOneRollGraduation(painter, deg, disp_text) + deg += 10 + + def drawYaw(self, painter): + center = self.adjdev.center() + r = self.getCenterRadius() + pen = QtGui.QPen(QtCore.Qt.red, r / 100., QtCore.Qt.SolidLine) + painter.setPen(pen) + font = QtGui.QFont("Meiryo UI", 0, QtGui.QFont.Bold) + font.setPointSizeF(self.FONT_SIZE * r) + painter.setFont(font) + if self.roundWidget == True: + y_txt = center.y() + r * 0.6 + y1 = center.y() + r * 0.7 + y2 = center.y() + r * 0.8 + y3 = center.y() + r * 0.85 + else: + y_txt = center.y() + r * 1. + y1 = center.y() + r * 1.1 + y2 = center.y() + r * 1.2 + y3 = center.y() + r * 1.25 + pt1 = QtCore.QPoint(center.x(), y2) + pt2 = QtCore.QPoint(center.x(), y3) + painter.drawLine(pt1, pt2) + + # round to nearest angle multiple of step + a = self.yaw / self.YAW_REFLINES_STEP_ANGLE + a = round(a) + a = int(a * self.YAW_REFLINES_STEP_ANGLE) + a -= self.YAW_REFLINES_STEP_ANGLE * (self.YAW_REFLINES_NUM_LINES / 2.) + angles = [ a + i * self.YAW_REFLINES_STEP_ANGLE + for i in range(self.YAW_REFLINES_NUM_LINES + 1) ] + for a in angles: + # text (N, S, E, W) + if int(a) % 90 == 0: + disp_text = True + pen = QtGui.QPen(QtCore.Qt.white, r / 50., QtCore.Qt.SolidLine) + painter.setPen(pen) + else: + disp_text = False + pen = QtGui.QPen(QtCore.Qt.white, r / 100., QtCore.Qt.SolidLine) + painter.setPen(pen) + # the line + x = center.x() - ((r / (self.YAW_REFLINES_TOTAL_ANGLE / 2.)) * + (self.yaw - a) * self.YAW_REFLINES_SIZE_FACTOR) + pt_txt = QtCore.QPoint(x, y_txt) + pt1 = QtCore.QPoint(x, y1) + pt2 = QtCore.QPoint(x, y2) + painter.drawLine(pt1, pt2) + if disp_text == False: + continue + disp_val = ["N", "E", "S", "W"][(int(a)/90)%4] + metrics = painter.fontMetrics() + sz = metrics.size(QtCore.Qt.TextSingleLine, disp_val) + txt = QtCore.QRect(QtCore.QPoint(0, 0), sz) + txt.moveCenter(pt_txt) + painter.drawText(txt, QtCore.Qt.TextSingleLine, disp_val) + + def drawSpeed(self, painter): + center = self.adjdev.center() + r = self.getCenterRadius() + pen = QtGui.QPen(QtCore.Qt.red, r / 100., QtCore.Qt.SolidLine) + painter.setPen(pen) + font = QtGui.QFont("Meiryo UI", 0, QtGui.QFont.Bold) + font.setPointSizeF(self.FONT_SIZE * r) + painter.setFont(font) + x1 = center.x() - 1.5 * r + x2 = center.x() - 1.6 * r + pt1 = QtCore.QPoint(center.x() - 1.45 * r, center.y()) + pt2 = QtCore.QPoint(x2, center.y()) + painter.drawLine(pt1, pt2) + + # round to nearest angle multiple of step + s = self.speed / self.SPEED_REFLINES_STEP + s = round(s) + s = int(s * self.SPEED_REFLINES_STEP) + s -= self.SPEED_REFLINES_STEP * (self.SPEED_REFLINES_NUM_LINES / 2.) + speeds = [ s + i * self.SPEED_REFLINES_STEP + for i in range(self.SPEED_REFLINES_NUM_LINES + 1) ] + for s in speeds: + if int(s) % int(self.SPEED_REFLINES_BOLD_STEP) == 0: + disp_text = True + pen = QtGui.QPen(QtCore.Qt.white, r / 50., QtCore.Qt.SolidLine) + painter.setPen(pen) + else: + disp_text = False + pen = QtGui.QPen(QtCore.Qt.white, r / 100., QtCore.Qt.SolidLine) + painter.setPen(pen) + # the line + y = center.y() + ((r / (self.SPEED_REFLINES_TOTAL/2.)) * + (self.speed - s) * self.SPEED_REFLINES_SIZE_FACTOR) + pt_txt = QtCore.QPoint(center.x() + r * -1.75, y) + pt1 = QtCore.QPoint(x1, y) + pt2 = QtCore.QPoint(x2, y) + painter.drawLine(pt1, pt2) + if disp_text == False: + continue + disp_val = str(int(s)) + metrics = painter.fontMetrics() + sz = metrics.size(QtCore.Qt.TextSingleLine, disp_val) + txt = QtCore.QRect(QtCore.QPoint(0, 0), sz) + txt.moveCenter(pt_txt) + painter.drawText(txt, QtCore.Qt.TextSingleLine, disp_val) + + def drawAlt(self, painter): + center = self.adjdev.center() + r = self.getCenterRadius() + pen = QtGui.QPen(QtCore.Qt.red, r / 100., QtCore.Qt.SolidLine) + painter.setPen(pen) + font = QtGui.QFont("Meiryo UI", 0, QtGui.QFont.Bold) + font.setPointSizeF(self.FONT_SIZE * r) + painter.setFont(font) + x1 = center.x() + 1.5 * r + x2 = center.x() + 1.6 * r + pt1 = QtCore.QPoint(center.x() + 1.45 * r, center.y()) + pt2 = QtCore.QPoint(x2, center.y()) + painter.drawLine(pt1, pt2) + + # round to nearest angle multiple of step + a = self.alt / self.ALT_REFLINES_STEP + a = round(a) + a = int(a * self.ALT_REFLINES_STEP) + a -= self.ALT_REFLINES_STEP * (self.ALT_REFLINES_NUM_LINES / 2.) + alts = [ a + i * self.ALT_REFLINES_STEP + for i in range(self.ALT_REFLINES_NUM_LINES + 1) ] + for a in alts: + if int(a) % int(self.ALT_REFLINES_BOLD_STEP) == 0: + disp_text = True + pen = QtGui.QPen(QtCore.Qt.white, r / 50., QtCore.Qt.SolidLine) + painter.setPen(pen) + else: + disp_text = False + pen = QtGui.QPen(QtCore.Qt.white, r / 100., QtCore.Qt.SolidLine) + painter.setPen(pen) + # the line + y = center.y() + ((r / (self.ALT_REFLINES_TOTAL / 2.)) * + (self.alt - a) * self.ALT_REFLINES_SIZE_FACTOR) + pt_txt = QtCore.QPoint(center.x() + r * 1.75, y) + pt1 = QtCore.QPoint(x1, y) + pt2 = QtCore.QPoint(x2, y) + painter.drawLine(pt1, pt2) + if disp_text == False: + continue + disp_val = str(int(a)) + metrics = painter.fontMetrics() + sz = metrics.size(QtCore.Qt.TextSingleLine, disp_val) + txt = QtCore.QRect(QtCore.QPoint(0, 0), sz) + txt.moveCenter(pt_txt) + painter.drawText(txt, QtCore.Qt.TextSingleLine, disp_val) + + def drawReturnToHome(self, painter): + center = self.adjdev.center() + r = self.getCenterRadius() + dev_r = self.adjdev.width() / 2. + painter.save() + painter.translate(center.x(), center.y() - 0.9 * dev_r) + painter.rotate(self.yaw) # XXX + pen = QtGui.QPen(QtCore.Qt.white, r / 100., QtCore.Qt.SolidLine) + painter.setPen(pen) + poly = QtGui.QPolygonF() + poly.append(QtCore.QPoint(0., -0.08 * r)) + poly.append(QtCore.QPoint(0.04 * r, 0.08 * r)) + poly.append(QtCore.QPoint(-0.04 * r, 0.08 * r)) + poly.append(QtCore.QPoint(0., -0.08 * r)) + path = QtGui.QPainterPath() + path.addPolygon(poly) + brush = QtGui.QBrush(QtCore.Qt.darkGray) + painter.setBrush(brush) + painter.drawPath(path) + painter.restore() + + def drawTxtInfo(self, painter): + center = self.adjdev.center() + dev_r = self.adjdev.width() / 2. + r = self.getCenterRadius() + pen = QtGui.QPen(QtCore.Qt.white, r / 100., QtCore.Qt.SolidLine) + painter.setPen(pen) + font = QtGui.QFont("Meiryo UI", 0, QtGui.QFont.Bold) + font.setPointSizeF(self.FONT_SIZE * r) + metrics = painter.fontMetrics() + + sz = metrics.size(QtCore.Qt.AlignLeft, self.left_txt) + txt = QtCore.QRect(QtCore.QPoint(0, 0), sz) + pt_txt = QtCore.QPoint(center.x() + dev_r * -0.95, + center.y() + dev_r * -0.95) + txt.moveTopLeft(pt_txt) + painter.drawText(txt, QtCore.Qt.AlignLeft, self.left_txt) + + sz = metrics.size(QtCore.Qt.AlignRight, self.right_txt) + txt = QtCore.QRect(QtCore.QPoint(0, 0), sz) + pt_txt = QtCore.QPoint(center.x() + dev_r * 0.95, + center.y() + dev_r * -0.95) + txt.moveTopRight(pt_txt) + painter.drawText(txt, QtCore.Qt.AlignRight, self.right_txt) + + def setPitch(self, pitch): + """set the pitch in degrees, between -180 and 180""" + pitch = pitch % 360. + if pitch > 180.: + pitch -= 360. + self.user_pitch = pitch + + def setRoll(self, roll): + """set the roll in degrees, between -180 and 180""" + roll = roll % 360. + if roll > 180.: + roll -= 360. + self.user_roll = roll + + def setYaw(self, yaw): + """set the yaw in degrees, between 0 and 360""" + yaw = yaw % 360. + self.user_yaw = yaw + + def setSpeed(self, speed): + """set the speed""" + self.user_speed = speed + + def setAlt(self, alt): + """set the alt""" + self.user_alt = alt + + def setReturnToHomeAngle(self, angle): + """set the left text""" + self.user_rthome = angle + + def setLeftTxt(self, txt): + """set the right text""" + self.left_txt = txt + + def setRightTxt(self, txt): + """set the left text""" + self.right_txt = txt + + def setFrameCb(self, user_frame_cb): + """set a function that is called every frame, before updating the widget""" + self.user_frame_cb = user_frame_cb + +class Ui_MainWindow(object): + def __init__(self, filename = None, roundWidget = False): + self.roundWidget = roundWidget + self.filename = filename + + def update_fpv_info(self): + self.fpv.update() + self.osd.user_speed = self.osd.user_speed + 1 + self.osd.setRoll(self.fpv.roll) + self.osd.setPitch(self.fpv.pitch) + self.osd.setYaw(self.fpv.yaw) + + def setupUi(self, MainWindow): + MainWindow.setObjectName(_fromUtf8("MainWindow")) + MainWindow.resize(400, 300) + self.centralWidget = QtGui.QWidget(MainWindow) + self.centralWidget.setObjectName(_fromUtf8("centralWidget")) + self.gridLayout = QtGui.QGridLayout(self.centralWidget) + self.gridLayout.setObjectName(_fromUtf8("gridLayout")) + + self.pitchSlider = QtGui.QSlider(self.centralWidget) + self.pitchSlider.setMinimum(-180) + self.pitchSlider.setMaximum(180) + self.pitchSlider.setOrientation(QtCore.Qt.Vertical) + self.pitchSlider.setObjectName(_fromUtf8("pitchSlider")) + self.pitchSlider.valueChanged[int].connect(self.changePitch) + self.gridLayout.addWidget(self.pitchSlider, 0, 0) + + self.osd = OSDWidget(self.roundWidget) + if self.filename: + self.fpv = serialfpv.SerialFPV() + self.fpv.open_file(self.filename) + self.osd.setFrameCb(self.update_fpv_info) + self.gridLayout.addWidget(self.osd, 0, 1) + + self.rollSlider = QtGui.QSlider(self.centralWidget) + self.rollSlider.setMinimum(-180) + self.rollSlider.setMaximum(180) + self.rollSlider.setOrientation(QtCore.Qt.Horizontal) + self.rollSlider.setObjectName(_fromUtf8("rollSlider")) + self.rollSlider.valueChanged[int].connect(self.changeRoll) + self.gridLayout.addWidget(self.rollSlider, 1, 1) + + self.yawSlider = QtGui.QSlider(self.centralWidget) + self.yawSlider.setMaximum(360) + self.yawSlider.setProperty("value", 180) + self.osd.setYaw(180) + self.yawSlider.setOrientation(QtCore.Qt.Horizontal) + self.yawSlider.setObjectName(_fromUtf8("yawSlider")) + self.yawSlider.valueChanged[int].connect(self.changeYaw) + self.gridLayout.addWidget(self.yawSlider, 2, 1) + + MainWindow.setCentralWidget(self.centralWidget) + self.menuBar = QtGui.QMenuBar(MainWindow) + self.menuBar.setGeometry(QtCore.QRect(0, 0, 400, 23)) + self.menuBar.setObjectName(_fromUtf8("menuBar")) + MainWindow.setMenuBar(self.menuBar) + self.mainToolBar = QtGui.QToolBar(MainWindow) + self.mainToolBar.setObjectName(_fromUtf8("mainToolBar")) + MainWindow.addToolBar(QtCore.Qt.TopToolBarArea, self.mainToolBar) + self.statusBar = QtGui.QStatusBar(MainWindow) + self.statusBar.setObjectName(_fromUtf8("statusBar")) + MainWindow.setStatusBar(self.statusBar) + + self.retranslateUi(MainWindow) + QtCore.QMetaObject.connectSlotsByName(MainWindow) + + def retranslateUi(self, MainWindow): + MainWindow.setWindowTitle(_translate("MainWindow", "MainWindow", None)) + + def changePitch(self, value): + self.osd.setPitch(value) + self.osd.update() + + def changeRoll(self, value): + self.osd.setRoll(value) + self.osd.update() + + def changeYaw(self, value): + self.osd.setYaw(value) + self.osd.update() + +if __name__ == "__main__": + import sys + app = QtGui.QApplication(sys.argv) + if "round" in sys.argv: + roundWidget = True + else: + roundWidget = False + MainWindow = QtGui.QMainWindow() + if len(sys.argv) > 1: + ui = Ui_MainWindow(sys.argv[1], roundWidget) + else: + ui = Ui_MainWindow(roundWidget = roundWidget) + ui.setupUi(MainWindow) + MainWindow.show() + sys.exit(app.exec_()) + diff --git a/qtosd/qtosd.ui b/qtosd/qtosd.ui new file mode 100644 index 0000000..05f288e --- /dev/null +++ b/qtosd/qtosd.ui @@ -0,0 +1,85 @@ + + + MainWindow + + + + 0 + 0 + 400 + 300 + + + + MainWindow + + + + + + + -180 + + + 180 + + + Qt::Vertical + + + + + + + + + + -180 + + + 180 + + + Qt::Horizontal + + + + + + + 360 + + + 180 + + + Qt::Horizontal + + + + + + + + + 0 + 0 + 400 + 23 + + + + + + TopToolBarArea + + + false + + + + + + + + diff --git a/qtosd/serialfpv.py b/qtosd/serialfpv.py new file mode 100644 index 0000000..6153a3d --- /dev/null +++ b/qtosd/serialfpv.py @@ -0,0 +1,97 @@ +# -*- coding: utf-8 -*- + +# Get IMU, GPS, misc info from serial logs (or from a file) +# Copyright (C) 2015 Olivier Matz +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import serial +import math +import sys +import re +import time + + +INT = "([-+]?[0-9][0-9]*)" +FLOAT = "([-+]?[0-9]*\.?[0-9]+)" + +class SerialFPV(): + def __init__(self): + self.f = None + self.roll = 0 + self.pitch = 0 + self.yaw = 0 + self.prev_time = None + self.prev_reftime = None + self.realtime = False + self.data = "" + + def open_serial(self, filename = "/dev/ttyUSB0", baudrate = 57600): + self.f = serial.Serial(port = filename, baudrate = baudrate) + self.realtime = True + self.data = "" + + def open_file(self, filename): + self.f = open(filename) + self.realtime = False + self.prev_time = None + self.prev_reftime = None + self.data = "" + + def update(self): + # read from file/serial + while len(self.data) < 16384: + data = self.f.read(1024) + if data == "": + break + self.data += data + + lines = self.data.split("\n") + + # try to match imu/gps infos for all lines + while len(lines) > 0: + l = lines.pop(0) + + m = re.match(".*IMU received %s %s %s"%(INT, INT, INT), l) + if m: + self.roll, self.pitch, self.yaw = ( + map(lambda x: float(x)*(math.pi/18000), m.groups())) + continue + + m = re.match("%s "%INT + + "\| gyro %s %s %s " % (FLOAT, FLOAT, FLOAT) + + "\| accel %s %s %s " % (FLOAT, FLOAT, FLOAT) + + "\| magnet %s %s %s " % (FLOAT, FLOAT, FLOAT) + + "\| angles %s %s %s"% (FLOAT, FLOAT, FLOAT), l) + if m: + cur_time = float(m.groups()[0]) / 20. + # first time, init prev_time and prev_reftime + if self.realtime == False and self.prev_time == None: + self.prev_time = cur_time + self.prev_reftime = time.time() + # if it's a replay, check current time first + if self.realtime == False: + # too early, break + if cur_time - self.prev_time > time.time() - self.prev_reftime: + lines.insert(0, l) + break + roll, pitch, yaw = ( + map(lambda x: float(x), m.groups()[10:13])) + print roll, pitch, yaw + self.roll = math.degrees(roll) + 180. + self.pitch = math.degrees(pitch) + self.yaw = math.degrees(yaw) + continue + + self.data = reduce(lambda x,y: x+"\n"+y, lines, "") -- 2.20.1