qtosd: port in pyqt5
[fpv.git] / qtosd / serialfpv.py
1 # -*- coding: utf-8 -*-
2
3 # Get IMU, GPS, misc info from serial logs (or from a file)
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 import serial
20 import math
21 import sys
22 import re
23 import time
24
25
26 INT = "([-+]?[0-9][0-9]*)"
27 FLOAT = "([-+]?[0-9]*\.?[0-9]+)"
28
29 class SerialFPV():
30     def __init__(self):
31         self.f = None
32         self.roll = 0
33         self.pitch = 0
34         self.yaw = 0
35         self.prev_time = None
36         self.prev_reftime = None
37         self.realtime = False
38         self.data = ""
39
40     def open_serial(self, filename = "/dev/ttyUSB0", baudrate = 57600):
41         self.f = serial.Serial(port = filename, baudrate = baudrate)
42         self.realtime = True
43         self.data = ""
44
45     def open_file(self, filename):
46         self.f = open(filename)
47         self.realtime = False
48         self.prev_time = None
49         self.prev_reftime = None
50         self.data = ""
51
52     def update_state(self):
53         # read from file/serial
54         while len(self.data) < 16384:
55             data = self.f.read(1024)
56             if data == "":
57                 break
58             self.data += data
59
60         lines = self.data.split("\n")
61
62         # try to match imu/gps infos for all lines
63         while len(lines) > 0:
64             l = lines.pop(0)
65
66             m = re.match(".*IMU received %s %s %s"%(INT, INT, INT), l)
67             if m:
68                 self.roll, self.pitch, self.yaw = (
69                     map(lambda x: float(x)*(math.pi/18000), m.groups()))
70                 continue
71
72             m = re.match("%s "%INT +
73                          "\| gyro %s %s %s " % (FLOAT, FLOAT, FLOAT) +
74                          "\| accel %s %s %s " % (FLOAT, FLOAT, FLOAT) +
75                          "\| magnet %s %s %s " % (FLOAT, FLOAT, FLOAT) +
76                          "\| angles %s %s %s"% (FLOAT, FLOAT, FLOAT), l)
77             if m:
78                 cur_time = float(m.groups()[0]) / 1000.
79                 # first time, init prev_time and prev_reftime
80                 if self.realtime == False and self.prev_time == None:
81                     self.prev_time = cur_time
82                     self.prev_reftime = time.time()
83                 # if it's a replay, check current time first
84                 if self.realtime == False:
85                     # too early, break
86                     if cur_time - self.prev_time > time.time() - self.prev_reftime:
87                         lines.insert(0, l)
88                         break
89                 roll, pitch, yaw = (
90                     map(lambda x: float(x), m.groups()[10:13]))
91                 self.roll = math.degrees(roll) + 180.
92                 self.pitch = math.degrees(pitch)
93                 self.yaw = math.degrees(yaw)
94                 continue
95
96         self.data = reduce(lambda x,y: x+"\n"+y, lines, "")