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, "")