librem5-gts: gnss interface for GTS1800

This will format the gnss output for the GTS1800 GPS tuning equipement

http://www.generaltest.com



It will output a UDP datastream on the port and IP specified
Signed-off-by: Angus Ainslie <angus@akkea.ca>

test_scripts/librem5-gts.py

+#!/usr/bin/python
+
+import pynmea2
+import os
+import threading
+import socket
+import time
+import datetime
+
+port = "/dev/gnss0"
+udpIP = "192.168.0.2"
+udpPort = 8080
+NMEAudpPort = None
+VERSION = "0.1"
+debug = False
+
+
+class GtsData:
+
+    def __init__(self):
+        self.lat = 0.0
+        self.lon = 0.0
+        self.altitude = 0.0
+        self.timestamp = None
+        self.datestamp = None
+        self.sats = {}
+        self.gpSats = []
+        self.glSats = []
+        self.leap_seconds = 18
+        self.accuracy = '99.0'
+        self.pos_status = 'V'
+
+    @property
+    def solSats(self):
+        return self.gpSats + self.glSats
+
+    @property
+    def datetime(self):
+        if self.datestamp and self.timestamp:
+            return datetime.datetime.combine(self.datestamp, self.timestamp)
+
+        return None
+
+    @property
+    def antenna(self):
+        ret = 0
+        if self.pos_status == 'A':
+            ret = ret | 1
+        if len(self.solSats) > 0:
+            ret = ret | 2
+
+        return ret
+
+    @property
+    def fixsuc(self):
+        if self.pos_status == 'A':
+            return 1
+
+        return 0
+
+    @property
+    def fixtime(self):
+        if self.datetime:
+            return (self.datetime - datetime.datetime(1980, 1, 6)).total_seconds() + self.leap_seconds
+
+        return 0
+
+    @property
+    def utctime(self):
+        if self.datetime:
+            return int(time.mktime(self.datetime.timetuple()))
+
+        return 0
+
+    @property
+    def timems(self):
+        if not self.timestamp:
+            return 0
+
+        # we need to spoof milliseconds because the module doesn't provide that
+        timesec = time.time()
+        return int((timesec - int(timesec)) * 1000)
+
+
+class GnssThread(threading.Thread):
+
+    def __init__(self, port=None):
+        threading.Thread.__init__(self)
+        self.running = True
+        self.gts = GtsData()
+        self.NMEAdata = []
+        if port:
+            self.gnss = open(port)
+
+    def open(self, port):
+        self.gnss.open(port)
+
+    def run(self):
+        while self.running:
+            data = self.gnss.readline()
+            msg = None
+            if NMEAudpPort:
+                self.NMEAdata.append(data)
+            try:
+                msg = pynmea2.parse(data)
+                if debug:
+                    print(repr(msg))
+                if isinstance(msg, pynmea2.types.talker.GSV):
+                    if msg.snr_1:
+                        self.gts.sats[msg.sv_prn_num_1] = (msg.elevation_deg_1, msg.azimuth_1, msg.snr_1)
+                    elif len(msg.sv_prn_num_1) and msg.sv_prn_num_1 in self.gts.sats:
+                        del self.gts.sats[msg.sv_prn_num_1]
+                    if msg.snr_2:
+                        self.gts.sats[msg.sv_prn_num_2] = (msg.elevation_deg_2, msg.azimuth_2, msg.snr_2)
+                    elif len(msg.sv_prn_num_2) and msg.sv_prn_num_2 in self.gts.sats:
+                        del self.gts.sats[msg.sv_prn_num_2]
+                    if msg.snr_3:
+                        self.gts.sats[msg.sv_prn_num_3] = (msg.elevation_deg_3, msg.azimuth_3, msg.snr_3)
+                    elif len(msg.sv_prn_num_3) and msg.sv_prn_num_3 in self.gts.sats:
+                        del self.gts.sats[msg.sv_prn_num_3]
+                    if msg.snr_4:
+                        self.gts.sats[msg.sv_prn_num_4] = (msg.elevation_deg_4, msg.azimuth_4, msg.snr_4)
+                    elif len(msg.sv_prn_num_4) and msg.sv_prn_num_4 in self.gts.sats:
+                        del self.gts.sats[msg.sv_prn_num_4]
+                elif isinstance(msg, pynmea2.types.talker.GGA):
+                    self.gts.timestamp = msg.timestamp
+                    self.gts.lon = msg.longitude
+                    self.gts.lat = msg.latitude
+                    self.gts.altitude = msg.altitude
+                    self.gts.accuracy = msg.horizontal_dil
+                elif isinstance(msg, pynmea2.types.talker.RMC):
+                    self.gts.datestamp = msg.datestamp
+                    self.gts.timestamp = msg.timestamp
+                    self.gts.pos_status = msg.status
+                elif isinstance(msg, pynmea2.types.talker.GSA):
+                    solSats = []
+                    if len(msg.sv_id01):
+                        solSats.append(msg.sv_id01)
+                    if len(msg.sv_id02):
+                        solSats.append(msg.sv_id02)
+                    if len(msg.sv_id03):
+                        solSats.append(msg.sv_id03)
+                    if len(msg.sv_id04):
+                        solSats.append(msg.sv_id04)
+                    if len(msg.sv_id05):
+                        solSats.append(msg.sv_id05)
+                    if len(msg.sv_id06):
+                        solSats.append(msg.sv_id06)
+                    if len(msg.sv_id07):
+                        solSats.append(msg.sv_id07)
+                    if len(msg.sv_id08):
+                        solSats.append(msg.sv_id08)
+                    if len(msg.sv_id09):
+                        solSats.append(msg.sv_id09)
+                    if len(msg.sv_id10):
+                        solSats.append(msg.sv_id10)
+                    if len(msg.sv_id11):
+                        solSats.append(msg.sv_id11)
+                    if len(msg.sv_id12):
+                        solSats.append(msg.sv_id12)
+                    if len(solSats):
+                        if int(solSats[0]) > 64:
+                            # GLONASS sats
+                            self.gts.glSats = solSats
+                        else:
+                            self.gts.gpSats = solSats
+            except Exception as e:
+                print("exception: ", e, "\nNMEA read failed ", data)
+                if msg:
+                    print(repr(msg))
+
+
+if __name__ == '__main__':
+    if NMEAudpPort == udpPort:
+        print('Disabling NMEA transfers')
+        NMEAudpPort = None
+    gnsst = GnssThread(port)
+    gnsst.start()
+    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    count = 0
+    print('Fix time      ', gnsst.gts.fixtime)
+    try:
+        while True:
+            count = count + 1
+            if count > 9999:
+                count = 1
+            if not debug:
+                os.system('clear')
+
+            print
+            print('GTS-GPS %s %s:%d' % (VERSION, udpIP, udpPort))
+            print('----------------------------------------')
+            print('longitude        %3.8f' % gnsst.gts.lon)
+            print('latitude         %2.8f' % gnsst.gts.lat)
+            print('altitude (m)     %.2f' % gnsst.gts.altitude)
+            print('accuracy         %s' % gnsst.gts.accuracy)
+            print('sats             ')
+            print(gnsst.gts.solSats)
+            print('timestamp        %s.%03d' % (gnsst.gts.datetime, gnsst.gts.timems))
+            print('Fix time         %.3f' % gnsst.gts.utctime)
+            print
+            # print 'gnsst info', gnsst.gt
+
+            sats = b''
+            for sat in gnsst.gts.sats:
+                if len(sats) > 0:
+                    sats = sats + b'&'
+                sats = sats + b'S%s:%.1f' % (sat, float(gnsst.gts.sats[sat][2]))
+            msg = b'%s.%03d,%d,0,%d,%d,%s,%3.8f,%2.8f,%.3f,' % (gnsst.gts.datetime, gnsst.gts.timems, gnsst.gts.antenna, gnsst.gts.fixsuc, gnsst.gts.utctime, gnsst.gts.accuracy, gnsst.gts.lon, gnsst.gts.lat, gnsst.gts.altitude) + sats
+            msg = b'%04d%s\x00' % (len(msg), msg)
+            print('msg: %d %s' % (len(msg), msg))
+            sock.sendto(msg, (udpIP, udpPort))
+            while (len(gnsst.NMEAdata)):
+                data = gnsst.NMEAdata.pop()
+                if debug:
+                    print("nmea send: ", data)
+                if NMEAudpPort:
+                    sock.sendto(data, (udpIP, NMEAudpPort))
+
+            time.sleep(0.5)
+
+    except Exception as err:
+        print("\nKilling Thread: ", format(err))
+    except KeyboardInterrupt:
+        print("\nKilling Thread")
+    finally:
+        gnsst.running = False
+        gnsst.join()
+
+    print("Done.\nExiting.")