#! /usr/bin/python ''' =============================================================================================== H O V E R C R A F T I N S T R U M E N T A T I O N GNSS / GPS DATA DISTRIBUTION OVER NETWORK This program reads data from GNSS/GPS receiver that appears as a COM-port. It auto-detects COM-port number and speed. GNSS/GPS receivers can transmit a various number of NMEA-telegrams that belong to the same time instance (normally, each second); also the first and last telegram in such a block can vary between different receivers --- so the program auto-detects last telegram in such a block. After finding last NMEA-telegram in block, it extracts information from different telegrams and forwards everything as a single JSON-formatted package. Data is tranmitted as UDP broadcast telegram. There is also a TCP server that transmits this JSON-package to clients (client can be a telnet session, like "telnet 127.0.0.01 20001"). An example client program is provided in project documentation at: http://www.polarhovercraft.no section "GPS/GNSS data distribution" Program includes GPS simullator, which is invoked by giving parameter SIM after program name. Simulator runs in two modes (that user selects): A = Great circle starting at 81degN/12degE to 82.5degN/-4degW (303.4km), travelling approx 1m/sec. B = Rose pattern centered on crossing of Greenwich and Equator, testing all four quadrants. ---------------------------------------------------------------------------------- Ver Date By Description ---------------------------------------------------------------------------------- 2.0 23 Mar 2020 O.M. Main new features: (a) Auto-detecting GNSS/GPS COM-port and speed. (b) Auto-detecting last NMEA telegram in block that is received at each time instance (normally every second). This is used when extracting data from various NMEA telegrams, to ensure all data belongs to the same time instance. (Last telegram in these blocks varies between different GNSS/GPS receivers). (c) TCP server features: - Accepts 5 simultaneous socket connections. - Transmits data as JSON-formatted string in PUSH mode. - Gracefully handles socket pull-down on client side. 0.1 ---- 2011 O.M. Initial version ---------------------------------------------------------------------------------- Dept. of Earth Science University of Bergen N O R W A Y =============================================================================================== ''' #-------------------------------------------------------------------------- # C O N S T A N T S #-------------------------------------------------------------------------- VERSION = ''' ======================================================================= HOVERCRAFT --- GNSS/GPS DATA DISTRIBUTION OVER NETWORK Ver. 2.0 Date: 23 March, 2020 By: O.M. Department of Earth Science, University of Bergen, Norway Terminate program by pressing key Start GPS simulation mode by giving parameter "SIM" after program name ======================================================================= ''' GPS_SERIAL_PORT = 'COM9' # From ver. 2 there is auto-detection on both COM-port and baudrate ... GPS_SERIAL_SPEED = 9600 GPS_UDP_PORT = 20001 #-------------------------------------------------------------------------- # I M P O R T S E C T I O N #-------------------------------------------------------------------------- import socket from string import split import select from time import time, gmtime, strftime, strptime, sleep from calendar import timegm import sys import thread import os import datetime import serial import json import LatLongUTMconversion import Queue import threading #=================================================================== #======== U T I L I T I E S #=================================================================== """ Colors text in console mode application (win32). Uses ctypes and Win32 methods SetConsoleTextAttribute and GetConsoleScreenBufferInfo. $Id: color_console.py 534 2009-05-10 04:00:59Z andre $ """ from ctypes import windll, Structure, c_short, c_ushort, byref SHORT = c_short WORD = c_ushort class COORD(Structure): """struct in wincon.h.""" _fields_ = [ ("X", SHORT), ("Y", SHORT)] class SMALL_RECT(Structure): """struct in wincon.h.""" _fields_ = [ ("Left", SHORT), ("Top", SHORT), ("Right", SHORT), ("Bottom", SHORT)] class CONSOLE_SCREEN_BUFFER_INFO(Structure): """struct in wincon.h.""" _fields_ = [ ("dwSize", COORD), ("dwCursorPosition", COORD), ("wAttributes", WORD), ("srWindow", SMALL_RECT), ("dwMaximumWindowSize", COORD)] # winbase.h STD_INPUT_HANDLE = -10 STD_OUTPUT_HANDLE = -11 STD_ERROR_HANDLE = -12 # wincon.h FOREGROUND_BLACK = 0x0000 FOREGROUND_BLUE = 0x0001 FOREGROUND_GREEN = 0x0002 FOREGROUND_CYAN = 0x0003 FOREGROUND_RED = 0x0004 FOREGROUND_MAGENTA = 0x0005 FOREGROUND_YELLOW = 0x0006 FOREGROUND_GREY = 0x0007 FOREGROUND_INTENSITY = 0x0008 # foreground color is intensified. BACKGROUND_BLACK = 0x0000 BACKGROUND_BLUE = 0x0010 BACKGROUND_GREEN = 0x0020 BACKGROUND_CYAN = 0x0030 BACKGROUND_RED = 0x0040 BACKGROUND_MAGENTA = 0x0050 BACKGROUND_YELLOW = 0x0060 BACKGROUND_GREY = 0x0070 BACKGROUND_INTENSITY = 0x0080 # background color is intensified. stdout_handle = windll.kernel32.GetStdHandle(STD_OUTPUT_HANDLE) SetConsoleTextAttribute = windll.kernel32.SetConsoleTextAttribute GetConsoleScreenBufferInfo = windll.kernel32.GetConsoleScreenBufferInfo def get_text_attr(): """Returns the character attributes (colors) of the console screen buffer.""" csbi = CONSOLE_SCREEN_BUFFER_INFO() GetConsoleScreenBufferInfo(stdout_handle, byref(csbi)) return csbi.wAttributes def set_text_attr(color): """Sets the character attributes (colors) of the console screen buffer. Color is a combination of foreground and background color, foreground and background intensity.""" SetConsoleTextAttribute(stdout_handle, color) default_colors = get_text_attr() default_bg = default_colors & 0x0070 def test(): """Simple test for color_console.""" default_colors = get_text_attr() default_bg = default_colors & 0x0070 set_text_attr(FOREGROUND_BLUE | default_bg | FOREGROUND_INTENSITY) print '===========================================' set_text_attr(FOREGROUND_BLUE | BACKGROUND_GREY | FOREGROUND_INTENSITY | BACKGROUND_INTENSITY) print 'And Now for Something', set_text_attr(FOREGROUND_RED | BACKGROUND_GREY | FOREGROUND_INTENSITY | BACKGROUND_INTENSITY) print 'Completely Different!', set_text_attr(default_colors) print set_text_attr(FOREGROUND_RED | default_bg | FOREGROUND_INTENSITY) print '===========================================' set_text_attr(default_colors) #------------------------------------------------------ # AUTO-DETECT SERIAL PORT WITH GNSS/GPS DATA STREAM #------------------------------------------------------ def check_serial_ports(): ports = ['COM%s' % (i + 1) for i in range (256)] result = [] for port in ports: try: s = serial.Serial(port) s.close() result.append(port) except (OSError, serial.SerialException): pass if result: print "Searching for COM port with GNSS/GPS data, also determining correct baudrate ..." else: set_text_attr(FOREGROUND_RED | default_bg | FOREGROUND_INTENSITY) print print "ERROR: No COM ports found on this computer" print set_text_attr(default_colors) answer = raw_input('Press to exit program') sys.exit() found = '' baudrates = [9600, 4800, 19200] for COM_port in result: data = '' print COM_port + ': ', for baudrate in baudrates: print '%d bits/s ' % (baudrate), s = serial.Serial(COM_port, baudrate, timeout=1.3) data = s.read(100) s.close() if (('$GP' in data) or ('$GN' in data)): set_text_attr(FOREGROUND_GREEN | default_bg | FOREGROUND_INTENSITY) print 'Found NMEA data:' set_text_attr(FOREGROUND_YELLOW | default_bg | FOREGROUND_INTENSITY) found = (COM_port, baudrate) print data set_text_attr(default_colors) break if not data: print if found: break if not found: set_text_attr(FOREGROUND_RED | default_bg | FOREGROUND_INTENSITY) print print "ERROR: No COM port with NMEA telegrams found" print set_text_attr(default_colors) answer = raw_input('Press to exit program') sys.exit() (COM_port, baudrate) = found set_text_attr(FOREGROUND_GREEN | default_bg | FOREGROUND_INTENSITY) print "GNSS/GPS on %s (using baudrate=%d)" % (COM_port, baudrate) print set_text_attr(default_colors) sleep(3) print "Now analyzing timing of NMEA telegrams, to determine final telegram in data block." print "This lasts for 10 seconds, or 100 telegrams - whichever comes first." print sleep(3) s = serial.Serial(COM_port, baudrate, timeout=1) start_time = time() delta_time = 0.0 previous_time = 0.0 previous_telegram = '' scores = {} telegram = '' for k in range(100): data = s.readline() timestamp = time() - start_time delta_time = timestamp - previous_time if data.startswith('$'): print "%1.3f s -- delta=%1.3f s: " % (timestamp, delta_time), set_text_attr(FOREGROUND_YELLOW | default_bg | FOREGROUND_INTENSITY) print "%s" % (data), set_text_attr(default_colors) t = data.split(',') telegram = t[0] if previous_telegram in scores: scores[previous_telegram] += delta_time else: scores[previous_telegram] = delta_time previous_time = timestamp previous_telegram = telegram if (timestamp >= 10.0): break s.close() print print "Accumulated interval of silence after telegram transmitted ---" print "the one with highest score represents end-of-block telegram." max_value = 0.0 block_tail = '' for telegram in scores: if telegram: print " %s: %1.3f s" % (telegram, scores[telegram]) if scores[telegram] > max_value: max_value = scores[telegram] block_tail = telegram set_text_attr(FOREGROUND_GREEN | default_bg | FOREGROUND_INTENSITY) print "Last telegram in block:", block_tail set_text_attr(default_colors) sleep(1) return (COM_port, baudrate, block_tail) # ------------------------------------------------- # Thread to catch keyboard input # Win can't have select on stdin, only on sockets #-------------------------------------------------- def keyboard_watch(): global Running command = raw_input('\nPress to stop program\n\n') # Will just wait for key press here. print "Shutting down / closing:" #sleep(0.3) Running = False #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #++++++ G P S C L A S S #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ class c_GPS: def __init__(self, serial_port, serial_speed, UDP_port, mode, last_in_block, simulation_mode=''): self.type = 'GNSS receiver' try: self.s_UDP = socket.socket( socket.AF_INET, socket.SOCK_DGRAM ) # Get UDP socket except socket.error, msg: set_text_attr(FOREGROUND_RED | default_bg | FOREGROUND_INTENSITY) print "GNSS object: Failed to create UDP socket. Error code: " + str(msg[0]) + " Message: " + msg[1] set_text_attr(default_colors) self.running = False sys.exit() self.s_UDP.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) self.s_UDP.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) self.mode = mode self.simulation_mode = simulation_mode self.host = '' #'' #Bind to all ##"127.0.0.1" self.UDP_port = UDP_port self.TCP_port = UDP_port self.UDP_Telegram = '' self.TCP_Telegram = '' self.IP_address = '' self.host_name = '' self.serial_port = serial_port self.serial_speed = serial_speed self.last_in_block = last_in_block self.id = 'SN:1' self.data = '' self.data_last_24_hours = [] self.processing_queue = Queue.LifoQueue() # Construct Last-In-First-Out queue that will act as pipe between data collecting thread and data processing thread self.TCP_queue = Queue.LifoQueue() # Construct Last-In-First-Out queue that will act as pipe between data processing thread and TCP/IP server thread self.distribution_queue = Queue.LifoQueue() self.TCP_lock = threading.Lock() self.TCP_data = json.dumps({}) ##self.plot_counter = 0 ##self.plot_time = -1 ##self.plot_interval = 20 # In seconds ##self.plot_filename = 'gps_plot.png' self.busy = False self.running = False self.ok = True self.starttime = '' self.timestamp = 0.0 self.XML ='' self.verbose = True self.msg_counter = 0 self.json_txt = '' self.terminal_courtesy_message_interval = 15 # In seconds self.terminal_line_counter = self.terminal_courtesy_message_interval - 4 self.date = '' self.time = '' self.message_prefix = '' self.Latitude_decimal = '' self.Longitude_decimal = '' if self.verbose: print "Created GNSS object" print "UDP network broadcast at port=%d" % self.UDP_port self.start() def start(self): self.running = True if self.mode == 'GPS': try: self.GPS_port = serial.Serial(port=self.serial_port, baudrate=self.serial_speed, timeout=1) except: set_text_attr(FOREGROUND_RED | default_bg | FOREGROUND_INTENSITY) print "Error: Could not open GNSS serial port:", self.serial_port print "Check: Control Panel > System > Hardware > Device Manager > Ports (COM & LPT)" print "Exit program" set_text_attr(default_colors) self.running = False sys.exit() self.GPS_port.close() print "Starting threads:" thread.start_new(self.supervisor_thread,()) sleep(0.1) thread.start_new(self.data_distribution_thread,()) sleep(0.1) thread.start_new(self.process_data_thread,()) sleep(0.1) thread.start_new(self.tcp_server_thread,()) sleep(0.1) print self.mode if self.mode == 'GPS': thread.start_new(self.serial_port_thread,()) elif self.mode == 'SIM': thread.start_new(self.GPS_simulator_thread,()) else: print "ERROR: Invalid system mode: ", self.mode sys.exit() def stop(self): self.running = False ##print self.message_prefix, "says:" print " - %s object" % self.type self.s_UDP.shutdown(1) self.s_UDP.close() print " - UDP socket" def serial_port_thread(self): # -- THREAD: Get data from GPS on serial port data_packet = [] if self.verbose: print " - Started GNSS/GPS serial port data collection on %s" % self.serial_port try: self.GPS_port = serial.Serial(port=self.serial_port, baudrate=self.serial_speed, timeout=1) except: set_text_attr(FOREGROUND_RED | default_bg | FOREGROUND_INTENSITY) print "Error: Could not open GNSS serial port:", self.serial_port print "Check: Control Panel > System > Hardware > Device Manager > Ports (COM & LPT)" print "Exit program" set_text_attr(default_colors) sleep(0.1) self.running = False while self.running: self.data = self.GPS_port.readline() self.data = self.data.strip() #print self.data #------ Sanitize date before sending them to procssing if ((len(self.data) > 0) and (self.data[0] == "$") and ('*' in self.data)): #---- $GPGGA or $GNGGA --------- if (("$GPGGA" in self.data) or ("$GNGGA" in self.data)): data_packet.append(self.data) #---- $GPRMC or $GNRMC --------- if (("$GPRMC" in self.data) or ("$GNRMC" in self.data)): data_packet.append(self.data) #---- $GPGSA or $GNGSA : GPS/GNSS DOP and Active Satellites --------- if (("$GPGSA" in self.data) or ("$GNGSA" in self.data)): data_packet.append(self.data) #---- $GPGSV --------- if "$GPGSV" in self.data: data_packet.append(self.data) #---- $GNGNS : GNSS fix data--------- if "$GNGNS" in self.data: data_packet.append(self.data) #---- $GNGST : GNSS Pseudo Range Error Statistics --------- if "$GNGST" in self.data: data_packet.append(self.data) #---- Compare to last NMEA telegram in block --------- if self.last_in_block in self.data: self.processing_queue.put(data_packet) data_packet = [] self.GPS_port.close() #-- Shutting down print " - GNSS/GPS serial port on %s" % self.serial_port print " - GNSS/GPS data collection thread" #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ # THREAD: GPS simulation #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ def GPS_simulator_thread(self): from math import cos, sin data_packet = [] RADIUS = 10000 #PERIOD = 20*1.666662769e-5 # 50 meter in 5 minutes; total 62832m circle length (r=10000m) PERIOD = 30*1.666662769e-5 # 50 meter in ca 1 minute; total 62832m circle length (r=10000m) SLEEP = 1.0 FALSE_EASTING = 500000 # UTM FALSE_NORTHING = 10000000 # GREENWICH_MERIDIAN = 166021.443179 print "GPS - simulation: Thread started." print " Simulation mode =", self.simulation_mode ################################################### ######### SIM MODE A ####################### ################################################### if self.simulation_mode == 'A': # Sources: # http://gis.stackexchange.com/questions/47/what-tools-in-python-are-available-for-doing-great-circle-distance-line-creatio # http://www.movable-type.co.uk/scripts/latlong.html import math ptlon1 = 12.0 ptlat1 = 81.0 ptlon2 = -4.0 ptlat2 = 82.5 numberofsegments = 303400 onelessthansegments = numberofsegments - 1 fractionalincrement = (1.0/onelessthansegments) ptlon1_radians = math.radians(ptlon1) ptlat1_radians = math.radians(ptlat1) ptlon2_radians = math.radians(ptlon2) ptlat2_radians = math.radians(ptlat2) distance_radians=2*math.asin(math.sqrt(math.pow((math.sin((ptlat1_radians-ptlat2_radians)/2)),2) + math.cos(ptlat1_radians)*math.cos(ptlat2_radians)*math.pow((math.sin((ptlon1_radians-ptlon2_radians)/2)),2))) # 6371.009 represents the mean radius of the earth # shortest path distance distance_km = 6371.009 * distance_radians mylats = [] mylons = [] # write the starting coordinates mylats.append([]) mylons.append([]) mylats[0] = ptlat1 mylons[0] = ptlon1 f = fractionalincrement icounter = 1 while (icounter < onelessthansegments): icountmin1 = icounter - 1 mylats.append([]) mylons.append([]) # f is expressed as a fraction along the route from point 1 to point 2 A=math.sin((1-f)*distance_radians)/math.sin(distance_radians) B=math.sin(f*distance_radians)/math.sin(distance_radians) x = A*math.cos(ptlat1_radians)*math.cos(ptlon1_radians) + B*math.cos(ptlat2_radians)*math.cos(ptlon2_radians) y = A*math.cos(ptlat1_radians)*math.sin(ptlon1_radians) + B*math.cos(ptlat2_radians)*math.sin(ptlon2_radians) z = A*math.sin(ptlat1_radians) + B*math.sin(ptlat2_radians) newlat=math.atan2(z,math.sqrt(math.pow(x,2)+math.pow(y,2))) newlon=math.atan2(y,x) newlat_degrees = math.degrees(newlat) newlon_degrees = math.degrees(newlon) mylats[icounter] = newlat_degrees mylons[icounter] = newlon_degrees icounter += 1 f = f + fractionalincrement # write the ending coordinates mylats.append([]) mylons.append([]) mylats[onelessthansegments] = ptlat2 mylons[onelessthansegments] = ptlon2 # Now, the array mylats[] and mylons[] have the coordinate pairs for intermediate points along the geodesic # My mylat[0],mylat[0] and mylat[num_of_segments-1],mylat[num_of_segments-1] are the geodesic end points k = 0 last = len(mylats)-1 print " Start coordinates:", mylats[0], mylons[0] print " End coordinates:", mylats[last], mylons[last] print while self.running: if k == len(mylats): k = 0 lat, lon = mylats[k], mylons[k] ##print lat, lon k += 1 ########################### DUPLICATE CODE - CLEAN UP LATER ########### #--------- Check: South of Equator? if lat < 0.0: hemisphere_NS = 'S' lat = -lat else: hemisphere_NS = 'N' #-------- West of Greenwich meridian? if lon < 0.0: hemisphere_EW = 'W' lon = -lon else: hemisphere_EW = 'E' #-------- Prepare GPS lat/lon format (like 0530.0000,N for 5.5 deg N) lat_fraction = '0.' + ("%1.8f" % lat).split('.')[1] lon_fraction = '0.' + ("%1.8f" % lon).split('.')[1] lat_min = "%07.4f" % (float(lat_fraction) * 60) lon_min = "%07.4f" % (float(lon_fraction) * 60) lat_deg = "%02d" % (int(("%1.8f" % lat).split('.')[0])) lon_deg = "%03d" % (int(("%1.8f" % lon).split('.')[0])) gps_lat = lat_deg + lat_min gps_lon = lon_deg + lon_min time_hhmmss = strftime("%H%M%S", gmtime()) date_ddmmyy = strftime("%d%m%y", gmtime()) GPGGA = "$GPGGA,%s,%s,%s,%s,%s,1,04,2.9,43.8,M,43.9,M,,*78" % ( time_hhmmss, gps_lat, hemisphere_NS, gps_lon, hemisphere_EW) GPRMC = "$GPRMC,%s,A,%s,%s,%s,%s,0.1,180.0,%s,5.0,E,A*78" % ( time_hhmmss, gps_lat, hemisphere_NS, gps_lon, hemisphere_EW, date_ddmmyy) data_packet.append(GPGGA) data_packet.append(GPRMC) self.processing_queue.put(data_packet) data_packet = [] sleep(1.0) ################################################### ######### SIM MODE B ####################### ################################################### elif self.simulation_mode == 'B': GPGGA = '' GPRMC = '' Time, Day, Month, Year = '','','','' t = 0 while self.running: r = RADIUS * sin(t*PERIOD/5) # Polar curve: A nice rose! x = r * cos(t*PERIOD) y = r * sin(t*PERIOD) #x = RADIUS * cos(PERIOD * t) # These two for a circle #y = RADIUS * sin(PERIOD * t) t += 1 easting = GREENWICH_MERIDIAN + x if y < 0.0: # South of Equator? northing = FALSE_NORTHING + y zone = '31M' else: northing = y zone = '31N' #-------------------------------------------------- # Convert Easting, Northing to Lat/Long (lat, lon) = LatLongUTMconversion.UTMtoLL(23, northing, easting, zone) # Beware: Small numbers given in scientific format, e.g. 1.2345e-05 #--------- Check: South of Equator? if lat < 0.0: hemisphere_NS = 'S' lat = -lat else: hemisphere_NS = 'N' #-------- West of Greenwich meridian? if lon < 0.0: hemisphere_EW = 'W' lon = -lon else: hemisphere_EW = 'E' #-------- Prepare GPS lat/lon format (like 0530.0000,N for 5.5 deg N) lat_fraction = '0.' + ("%1.8f" % lat).split('.')[1] lon_fraction = '0.' + ("%1.8f" % lon).split('.')[1] lat_min = "%07.4f" % (float(lat_fraction) * 60) lon_min = "%07.4f" % (float(lon_fraction) * 60) lat_deg = "%02d" % (int(("%1.8f" % lat).split('.')[0])) lon_deg = "%03d" % (int(("%1.8f" % lon).split('.')[0])) gps_lat = lat_deg + lat_min gps_lon = lon_deg + lon_min time_hhmmss = strftime("%H%M%S", gmtime()) date_ddmmyy = strftime("%d%m%y", gmtime()) GPGGA = "$GPGGA,%s,%s,%s,%s,%s,1,04,2.9,43.8,M,43.9,M,,*78" % ( time_hhmmss, gps_lat, hemisphere_NS, gps_lon, hemisphere_EW) GPRMC = "$GPRMC,%s,A,%s,%s,%s,%s,0.1,180.0,%s,5.0,E,A*78" % ( time_hhmmss, gps_lat, hemisphere_NS, gps_lon, hemisphere_EW, date_ddmmyy) data_packet.append(GPGGA) data_packet.append(GPRMC) self.processing_queue.put(data_packet) data_packet = [] sleep(1.0) else: print "Invalid choice, exit" self.running = False print " - GPS simulation thread" #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ # THREAD: Process data #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ def process_data_thread(self): print " - Data processing" while self.running: if not self.processing_queue.empty(): input = self.processing_queue.get() self.processing_queue.task_done() #--- Reset all fields try: data.clear() except: data = {} data['Time'] = '' data['Date'] = '' data['Latitude'] = '' data['Longitude'] = '' data['NS_Hemisphere'] = '' data['EW_Greenwich'] = '' data['Speed_over_ground'] = '' data['Course_made_good'] = '' data['Fix_Quality'] = '' data['Number_of_Satellites'] = '' data['Hours'] = '' data['Minutes'] = '' data['Seconds'] = '' data['Day'] = '' data['Month'] = '' data['Year'] = '' data['IP_address'] = socket.gethostbyname(socket.gethostname()) data['host_name'] = socket.gethostname() data['Latitude_decimal'] = 0.0 data['Longitude_decimal'] = 0.0 data['Latitude_readable'] = '' data['Longitude_readable'] = '' data['UTM_zone'] = '' data['Easting'] = '' data['Northing'] = '' #--data['GPGSV_data'] = {} #--data['sats_in_use'] = [] data['RMC_status'] = '' data['GSA_opMode'] = '' data['GSA_navMode'] = '' data['GSA_PDOP'] = '' data['GSA_HDOP'] = '' data['GST_stdLat'] = '' data['GST_stdLong'] = '' data['GGA_Fix_Quality'] = '' data['GGA_nsats'] = '' data['GGA_HDOP'] = '' data['GNS_posMode'] = '' data['GNS_nsats'] = '' data['GNS_HDOP'] = '' GNSS = {} GNSS['GPS'] = 0 GNSS['GLONASS'] = 0 GNSS['Galileo'] = 0 GNSS['BeiDou'] = 0 GNSS['SBAS'] = 0 for item in input: L = item.split(',') #---- GPGGA or GNGGA ---- if (("$GPGGA" in item) or ("$GNGGA" in item)): data['GGA_Fix_Quality'] = L[6] data['GGA_nsats'] = L[7] data['GGA_HDOP'] = L[8] #---- GPRMC or GNRMC ---- if (("$GPRMC" in item) or ("$GNRMC" in item)): if L[2] == 'V': data['RMC_status'] = 'invalid' else: data['RMC_status'] = 'ok' data['Latitude'] = L[3] data['Longitude'] = L[5] data['NS_Hemisphere'] = L[4] data['EW_Greenwich'] = L[6] data['Time'] = L[1] data['Date'] = L[9] data['Speed_over_ground'] = L[7] # In knots data['Course_made_good'] = L[8] if '.' in data['Time']: data['Time'] = data['Time'].split('.')[0] # Get integer part of time (some GPS provide fraction of seconds) data['Hours'] = data['Time'][:2] data['Minutes'] = data['Time'][2:4] data['Seconds'] = data['Time'][4:6] data['Day'] = data['Date'][:2] data['Month'] = data['Date'][2:4] data['Year'] = "20" + data['Date'][4:6] # A year 2100 problem is ticking here ... if (data['Latitude'] and data['Longitude'] and data['NS_Hemisphere'] and data['EW_Greenwich']): data['Latitude_decimal'] = float(data['Latitude'][:2]) + (float(data['Latitude'][2:])/60.0) data['Longitude_decimal'] = float(data['Longitude'][:3]) + (float(data['Longitude'][3:])/60.0) la = "%1.5f" % data['Latitude_decimal'] lo = "%1.5f" % data['Longitude_decimal'] la = (float(la) - int(data['Latitude'][:2])) * 60.0 lo = (float(lo) - int(data['Longitude'][:3])) * 60.0 ##data['Latitude_readable'] = "%s° %05.3f' %s" % (data['Latitude'][:2].lstrip('0'), la, data['NS_Hemisphere']) ##data['Longitude_readable'] = "%s° %05.3f' %s" % (data['Longitude'][:3].lstrip('0'), lo, data['EW_Greenwich']) data['Latitude_readable'] = "%s° %05.3f' %s" % (data['Latitude'][:2], la, data['NS_Hemisphere']) data['Longitude_readable'] = "%s° %05.3f' %s" % (data['Longitude'][:3], lo, data['EW_Greenwich']) #East Longitudes are positive, West longitudes are negative. #North latitudes are positive, South latitudes are negative if data['NS_Hemisphere'] == 'S': data['Latitude_decimal'] *= -1.0 if data['EW_Greenwich'] == 'W': data['Longitude_decimal'] *= -1.0 # '23' = WGS 84 ( data['UTM_zone'], data['Easting'], data['Northing'] ) = LatLongUTMconversion.LLtoUTM( 23, data['Latitude_decimal'], data['Longitude_decimal'] ) #e = "%1.1f" % e #n = "%1.1f" % n #print #print z, e, n, self.Number_of_Satellites, self.HDOP #---- GPGSV = ---- ''' if "$GPGSV" in item: # $GPGSV,3,1,11, 25,70,246,44, 12,64,098,34, 14,36,243,44, 29,30,197,41*7B # 4 8 12 16 try: for k in range(4): m = 4*(k+1) if '*' in L[m+3]: L[m+3] = L[m+3].split('*')[0] data['GPGSV_data'][L[m]] = ( (L[m+1], L[m+2], L[m+3]) ) except: pass ''' #---- GNGNS = GNSS fix data if "$GNGNS" in item: data['GNS_posMode'] = L[6] data['GNS_nsats'] = L[7] data['GNS_HDOP'] = L[8] #---- GPGSA or GNGSA = DOP and active satellites ---- if (("$GPGSA" in item) or ("$GNGSA" in item)): data['GSA_opMode'] = L[1] data['GSA_navMode'] = L[2] data['GSA_PDOP'] = L[15] data['GSA_HDOP'] = L[16] for k in range(12): if not L[k+3]: break if 0 < int(L[k+3]) < 33: GNSS['GPS'] += 1 if 64 < int(L[k+3]) < 97: GNSS['GLONASS'] += 1 #---- GNGST = GNSS Pseudo Range Error Statistics ---- if "$GNGST" in item: data['GST_stdLat'] = L[6] data['GST_stdLong'] = L[7] #---- End of processing, now forward package to data distribution thread self.distribution_queue.put(data) #---- Print info to screen if (data['Hours'] and data['Minutes'] and data['Seconds']): print "%s:%s:%s %s %09.6f%s %010.6f%s %02d|%02d %s %sm / %sm" % ( data['Hours'], data['Minutes'], data['Seconds'], data['RMC_status'], data['Latitude_decimal'], data['NS_Hemisphere'], data['Longitude_decimal'], data['EW_Greenwich'], GNSS['GPS'], GNSS['GLONASS'], data['GNS_HDOP'], data['GST_stdLat'], data['GST_stdLong'] ), else: print "No fix - %s" % data['RMC_status'], if self.mode == 'GPS': print else: print " [mode = %s]" % self.mode if (data['Seconds'] == '00'): set_text_attr(FOREGROUND_YELLOW | default_bg | FOREGROUND_INTENSITY) print "------------------------------------------------------------------" print " UTC Fix Lat Long US|RU HDOP StdLat/StdLong" print "------------------------------------------------------------------" set_text_attr(default_colors) if (data['Seconds'] == '30'): set_text_attr(FOREGROUND_YELLOW | default_bg | FOREGROUND_INTENSITY) print "Terminate program by pressing key" set_text_attr(default_colors) sleep(0.1) sleep(0.2) print " - Data processing thread" # Exit program #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ # THREAD: Data distribution #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ def data_distribution_thread(self): print " - Data distribution" self.UDP_external_network_blocked = False while self.running: if not self.distribution_queue.empty(): input = self.distribution_queue.get() self.distribution_queue.task_done() ##Time_Unix_epoch = int(timegm(strptime("%s %s %s %s%s%s " % (input['Day'], input['Month'], input['Year'], input['Hours'], input['Minutes'], input['Seconds']) + "UTC", "%d %m %Y %H%M%S %Z"))) ##print Time_Unix_epoch ##print strftime("%d-%b-%Y, %H:%M:%S [UTC]", gmtime(Time_Unix_epoch)) self.msg_counter += 1 try: TCP.clear() except: TCP = {} TCP['type'] = 'GPS' TCP['id'] = self.id TCP['mode'] = self.mode TCP['counter'] = self.msg_counter TCP['ip'] = input['IP_address'] TCP['host'] = input['host_name'] ##print input ##exit() TCP['RMC_status'] = input['RMC_status'] # 'ok' or 'invalid' TCP['time'] = "%s:%s:%s" % (input['Hours'], input['Minutes'], input['Seconds']) TCP['date'] = "%s.%s.%s" % (input['Day'], input['Month'], input['Year'] ) TCP['lat'] = input['Latitude_readable'] ##input['Latitude'] TCP['lon'] = input['Longitude_readable'] try: Time_Unix_epoch = int(timegm(strptime("%s %s %s %s%s%s " % (input['Day'], input['Month'], input['Year'], input['Hours'], input['Minutes'], input['Seconds']) + "UTC", "%d %m %Y %H%M%S %Z"))) TCP['unix_time'] = "%d" % Time_Unix_epoch TCP['lat_decimal'] = "%1.6f" % input['Latitude_decimal'] TCP['lon_decimal'] = "%1.6f" % input['Longitude_decimal'] TCP['utm_zone'] = input['UTM_zone'] TCP['easting'] = "%1.1f" % input['Easting'] TCP['northing'] = "%1.1f" % input['Northing'] TCP['hemisphere_NS'] = input['NS_Hemisphere'] TCP['hemisphere_EW'] = input['EW_Greenwich'] except: TCP['unix_time'] = '' TCP['lat_decimal'] = '' TCP['lon_decimal'] = '' TCP['utm_zone'] = '' TCP['easting'] = '' TCP['northing'] = '' TCP['hemisphere_NS'] = '' TCP['hemisphere_EW'] = '' TCP['nsats'] = input['GNS_nsats'] TCP['hdop'] = input['GNS_HDOP'] ##TCP['plot_counter'] = '0' ##TCP['plot_filename'] = '' ##TCP['status'] = 'ok' ##TCP['filename'] = '' ##if ( input['Year'] and input['Month'] and input['Day'] and input['Hours'] and input['Minutes'] and input['Seconds'] ): ## TCP['filename'] = '%s_%s_%s_%s%s%s' % (input['Year'], input['Month'], input['Day'], input['Hours'], input['Minutes'], input['Seconds'] ) ##TCP['delta'] = '' try: UDP.clear() except: UDP = {} UDP['type'] = 'GPS' UDP['id'] = self.id UDP['mode'] = self.mode UDP['counter'] = self.msg_counter UDP['status'] = 'ok' UDP['ip'] = input['IP_address'] UDP['host'] = input['host_name'] UDP['RMC_status'] = input['RMC_status'] # 'ok' or 'invalid' UDP['time'] = "%s:%s:%s" % (input['Hours'], input['Minutes'], input['Seconds']) UDP['date'] = "%s.%s.%s" % (input['Day'], input['Month'], input['Year'] ) UDP['lat'] = input['Latitude_readable'] ##input['Latitude'] UDP['lon'] = input['Longitude_readable'] UDP['latitude'] = input['Latitude'] UDP['longitude'] = input['Longitude'] UDP['hemisphere_NS'] = input['NS_Hemisphere'] UDP['hemisphere_EW'] = input['EW_Greenwich'] UDP['time_raw'] = input['Time'] UDP['date_raw'] = input['Date'] UDP['unix_time'] = TCP['unix_time'] try: UDP['lat_decimal'] = "%1.6f" % input['Latitude_decimal'] UDP['lon_decimal'] = "%1.6f" % input['Longitude_decimal'] UDP['utm_zone'] = input['UTM_zone'] UDP['easting'] = "%1.1f" % input['Easting'] UDP['northing'] = "%1.1f" % input['Northing'] except: UDP['lat_decimal'] = '' UDP['lon_decimal'] = '' UDP['utm_zone'] = '' UDP['easting'] = '' UDP['northing'] = '' UDP['GNS_nsats'] = input['GNS_nsats'] UDP['GNS_HDOP'] = input['GNS_HDOP'] UDP['GNS_posMode'] = input['GNS_posMode'] UDP['GSA_opMode'] = input['GSA_opMode'] UDP['GSA_navMode'] = input['GSA_navMode'] UDP['GSA_PDOP'] = input['GSA_PDOP'] UDP['GSA_HDOP'] = input['GSA_HDOP'] UDP['GST_stdLat'] = input['GST_stdLat'] UDP['GST_stdLong'] = input['GST_stdLong'] self.TCP_lock.acquire() self.TCP_data = json.dumps(TCP) self.TCP_lock.release() ##print " Distribution thread: Data available for TCP/IP server" self.UDP_Telegram = json.dumps(UDP) try: # UDP broadcast to own (internal) network interface self.s_UDP.sendto(self.UDP_Telegram, ('127.0.0.1', self.UDP_port)) # Send UDP telegram #self.s_UDP.sendto(self.UDP_Telegram, (self.host, self.UDP_port)) # Send UDP telegram ##print TCP['time'], "UTC: UDP telegram -> " except socket.error, msg: set_text_attr(FOREGROUND_RED | default_bg | FOREGROUND_INTENSITY) print "ERROR: Unable to send internal UDP telegram. Code=" + str(msg[0]) + ": " + msg[1] set_text_attr(default_colors) try: # UDP broadcast to external network interface self.s_UDP.sendto(self.UDP_Telegram, ('', self.UDP_port)) # Send UDP telegram #self.s_UDP.sendto(self.UDP_Telegram, (self.host, self.UDP_port)) # Send UDP telegram ##print TCP['time'], "UTC: UDP telegram -> " if self.UDP_external_network_blocked: # If we got so far, sending was a success. set_text_attr(FOREGROUND_GREEN | default_bg | FOREGROUND_INTENSITY) print "Now sending external UDP telegram" set_text_attr(default_colors) self.UDP_external_network_blocked = False except socket.error, msg: if not self.UDP_external_network_blocked: set_text_attr(FOREGROUND_RED | default_bg | FOREGROUND_INTENSITY) print "WARNING: Unable to send external UDP telegram" print " Code=" + str(msg[0]) + ": " + msg[1] print " Only internal UDP telegram sent" set_text_attr(default_colors) self.UDP_external_network_blocked = True sleep(0.1) sleep(0.1) print " - Data distribution thread" #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ # THREAD: TCP server, with handler that is called for each new clent #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #--- Routine that handles each new TCP socket client. Data is continously sent to client, #--- as JSON-formatted telegram. #--- Try this command in terminal window: "telnet 127.0.0.1 20001", to see raw data. #--- Polling is used (at 3 Hz) to determine arrival of new data; this means there #--- can be a delay of 0.33 seconds until client gets new data (which is acceptable #--- for this application). def handle_client(self, client_socket, address, client_number): set_text_attr(FOREGROUND_BLUE | default_bg | FOREGROUND_INTENSITY) print "TCP client #%d connected" % client_number set_text_attr(default_colors) last_timestamp = '' while self.running: self.TCP_lock.acquire() data = json.loads(self.TCP_data) self.TCP_lock.release() if data['unix_time'] != last_timestamp: try: sent = client_socket.send(json.dumps(data)) last_timestamp = data['unix_time'] if sent == 0: set_text_attr(FOREGROUND_BLUE | default_bg | FOREGROUND_INTENSITY) print "TCP client #%d exits" % client_number set_text_attr(default_colors) break except: set_text_attr(FOREGROUND_BLUE | default_bg | FOREGROUND_INTENSITY) print "TCP client #%d abruptly terminated TCP socket connection" % client_number set_text_attr(default_colors) break sleep(0.33) client_socket.close() #--- This is the container TCP server. It spawns a new handler for each client (max 5 simultaneously) def tcp_server_thread(self): s2 = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s2.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR,1) s2.bind(('', self.TCP_port)) s2.listen(5) print " - TCP server running on port:", self.TCP_port self.TCP_client_number = 1 while self.running: (client_socket, address) = s2.accept() # Blocking wait ... t = threading.Thread(target=self.handle_client, args=(client_socket, address, self.TCP_client_number)) self.TCP_client_number += 1 t.daemon = True t.start() print " - TCP server thread" # NOTE: Since accept() is a blocking wait, this thread will never exit #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ # THREAD: Superviser #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ def supervisor_thread(self): print " - Supervisor" while self.running: sleep(0.1) sleep(0.3) print " - Supervisor thread" #=================================================================== #======== S T A R T O F P R O G R A M #=================================================================== print VERSION sleep(3) Running = True Mode = 'GPS' block_tail = '' # Initial values of for auto detect of (in case we are running in SIM mode) GPS_SERIAL_PORT = '' # --- Last NMEA telegram in block sent from GNSS/GPS receiver GPS_SERIAL_SPEED = 9600 # --- Baudrate Simulation_mode = '' if len(sys.argv) > 1: if (sys.argv[1].upper() == 'SIM'): print print "SYSTEM RUNNING IN SIMULATION MODE" Mode = sys.argv[1].upper() print ''' A = Great circle starting at 81degN/12degE to 82.5degN/-4degW (303.4km), travelling approx 1m/sec B = Rose pattern centered on crossing of Greenwich and Equator, testing all four quadrants Your choice: ''' Simulation_mode = raw_input().upper() sleep(2) else: (GPS_SERIAL_PORT, GPS_SERIAL_SPEED, block_tail) = check_serial_ports() thread.start_new(keyboard_watch,()) # Start thread listening on keyboard ... #--- Create GNSS / GPS / SIMULATOR object ... GPS = c_GPS(serial_port=GPS_SERIAL_PORT, serial_speed=GPS_SERIAL_SPEED, UDP_port=GPS_UDP_PORT, mode=Mode, simulation_mode=Simulation_mode, last_in_block=block_tail) ## def __init__(self, serial_port=serial_port, UDP_port=UDP_port, mode=mode): while Running: sleep(1.0) GPS.stop() sleep(1) set_text_attr(FOREGROUND_GREY | default_bg) sys.exit(0)