Commit db408851 authored by Angus Ainslie's avatar Angus Ainslie Committed by Angus Ainslie
Browse files

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: default avatarAngus Ainslie <angus@akkea.ca>
parent ed6dfba7
#!/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.")
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment