add first version of qt-osd

[fpv.git] / qtosd / serialfpv.py

diff --git a/qtosd/serialfpv.py b/qtosd/serialfpv.py
new file mode 100644 (file)
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 <zer0@droids-corp.org>
+#
+# 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 <http://www.gnu.org/licenses/>.
+
+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, "")