#!/usr/bin/python # # Copyright (c) 2019 Clementine Computing LLC. # # This file is part of PopuFare. # # PopuFare is free software: you can redistribute it and/or modify # it under the terms of the GNU Affero General Public License as published by # the Free Software Foundation, either version 3 of the License, or # (at your option) any later version. # # PopuFare is distributed in the hope that it will be useful, # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU Affero General Public License for more details. # # You should have received a copy of the GNU Affero General Public License # along with PopuFare. If not, see . # # Script to test GPS positioning by simulating output of a GPS module. # # Outputs to stdout # import getopt import sys import time import math import datetime DT = 0.5 DS = 2 DTUPDATE = 2.0 ITER_COUNT = -1 LOCK_DELAY = 0 DSTEP = 16 argv = sys.argv def show_help(ofp): ofp.write("\nusage: gps-sim.py [-dt dt] [-ds ds] [-p lat0,lon0] [-p lat1,lon1]\n") ofp.write("\n") ofp.write(" -dt dt time between waypoints (default " + str(DT) + "seconds)\n") ofp.write(" -ds ds time to stop at waypoint (default" + str(DS) + "seconds)\n") ofp.write(" -p lat,lon[,dt[,ds]] latitudie, longitude of waypoint with optional dt, ds (can specify multiple times)\n") ofp.write(" -f fn file containing lat,lon positions\n") ofp.write(" -t t time between print updates (default " + str(DTUPDATE) + " seconds)\n") ofp.write(" -n n number of iterations (<0 for infinite, default " + str(ITER_COUNT) + ")\n") ofp.write(" --lock-delay d time to delay before inital gps lock (default " + str(LOCK_DELAY) + ")\n") ofp.write("\n") def parse_latlon(s): v = s.strip("\n").split(",") if len(v) < 2: return [] if len(v) > 4: return [] lat = float(v[0]) lon = float(v[1]) dt = DT ds = DS if len(v) > 2: dt = float(v[2]) if len(v) > 3: ds = float(v[3]) return [lat, lon, dt, ds] def load_latlon_csv(fn): latlon = [] with open(fn) as fp: for line in fp: v = parse_latlon(line) latlon.append(v) return latlon def nmea_checksum(msg): cksum = 0 for idx in range(len(msg)): c = ord(msg[idx]) cksum = cksum ^ (c & 0xff) return ("%02X" % cksum) #tstmsg = [ "$GPGSV,3,1,12,17,43,264,38,08,12,066,38,28,65,320,35,11,57,059,32*73", # "$GPGSV,3,2,12,07,23,180,32,19,21,254,32,01,59,075,31,13,08,275,25*7C", # "$GPGSV,3,3,12,22,18,106,25,30,47,210,15,18,38,053,,04,,,*44", # "$GPGSV,4,4,13,04,,,*7F", # "$GPGGA,040341.1,4226.692702,N,07630.419493,W,1,07,1.0,117.5,M,0,M,,*6D", # "$GPVTG,NaN,T,,M,0.0,N,0.0,K,A*42", # "$GPRMC,040341.1,A,4226.692702,N,07630.419493,W,0.0,0.0,050500,,,A*75", # "$GPGSA,A,3,01,07,08,11,17,19,28,,,,,,1.5,1.0,1.2*3E", # "$GPGSV,3,1,12,08,12,066,38,17,43,264,38,28,65,320,35,11,57,059,33*72", # "$GPGSV,3,2,12,07,23,180,32,01,59,075,32,19,21,254,32,22,18,106,26*78", # "$GPGSV,3,3,12,13,08,275,25,30,47,210,17,18,38,053,,04,,,*42", # "$GPGSV,4,4,13,04,,,*7F", # "$GPGGA,040344.1,4226.692722,N,07630.419464,W,1,07,1.0,117.3,M,0,M,,*64", # "$GPVTG,NaN,T,,M,0.0,N,0.0,K,A*42", # "$GPRMC,040344.1,A,4226.692722,N,07630.419464,W,0.0,0.0,050500,,,A*7A", # "$GPGSA,A,3,01,07,08,11,17,19,28,,,,,,1.5,1.0,1.2*3E" ] # #for msg in tstmsg: # s = msg[1:-3] # print s, "(", msg, ")" # print " >>", nmea_checksum(s) # #sys.exit(0) def print_gps_nmea_messages(ofp, lat, lon, utc_str = ""): if len(utc_str) == 0: n = datetime.datetime.utcnow() utc_str = n.strftime("%H") + n.strftime("%M") + n.strftime("%S") + "." + n.strftime("%f")[0:1] ofp.write("$GPGSV,4,1,13,08,12,066,38,28,65,320,36,17,43,264,36,01,59,075,32*79\n") ofp.write("$$GPGSV,4,2,13,11,57,059,32,07,23,180,31,19,21,254,29,22,18,106,29*79\n") ofp.write("$GPGSV,4,3,13,13,08,275,23,03,09,127,20,30,47,210,20,18,38,053,*7E\n") ofp.write("$GPGSV,4,4,13,04,,,*7F\n") latdir = 'N' if lat < 0.0: latdir = 'S' londir = 'E' if lon < 0.0: londir = 'W' msg = "GPGGA," + utc_str + "," + str(abs(lat)) + "," + latdir + "," + str(abs(lon)) + "," + londir + ",1,08,0.9,116.9,M,0,M,," ofp.write("$" + msg + "*" + nmea_checksum(msg) + "\n") ofp.write("$GPVTG,NaN,T,,M,0.0,N,0.0,K,A*42\n") msg = "GPRMC," + utc_str + ",A," + str(abs(lat)) + "," + latdir + "," + str(abs(lon)) + "," + londir + ",0.0,0.0,050500,,,A" ofp.write("$" + msg + "*" + nmea_checksum(msg) + "\n") ofp.write("$GPGSA,A,3,01,07,08,11,17,19,22,28,,,,,1.5,0.9,1.1*35\n") latlon = [] noarg = True ifn = "" argvv = [] av = [] for p in range(len(argv[1:])): if (p%2) == 0: av = [argv[p+1]] else: av.append(argv[p+1]) argvv.append(av) av = [] for argval in argvv: arg = argval[0] val = argval[1] if arg in ("-h", "--help"): show_help(sys.stdout) sys.exit(0) noarg = False elif arg in ("-dt"): DT = int(val) noarg = False elif arg in ("-ds"): DS = int(val) noarg = False elif arg in ("-p"): x = parse_latlon(val) latlon.append(x) noarg = False elif arg in ("-f"): ifn = val noarg = False elif arg in ("-n"): ITER_COUNT = int(val) elif arg in ("-t"): DTUPDATE = float(val) elif arg in ("--lock-delay"): LOCK_DELAY = float(val) if len(ifn)>0: latlon = load_latlon_csv(ifn) if len(latlon)==0: sys.stderr.write("Provide input file or lat/lon points\n") show_help(sys.stderr) sys.exit(-1) if noarg: show_help(sys.stderr) sys.exit(1) print "# dt,ds,t:", DT, DS, DTUPDATE print "# latlon:", latlon print "# iter_count:", ITER_COUNT print "# ifn:", ifn print "# lock_delay:", LOCK_DELAY curiter = 0 if ITER_COUNT<0: curiter = ITER_COUNT-1 lastupdate_sec = 0.0 curidx = 0 nidx = len(latlon) while curiter < ITER_COUNT: curpos = latlon[curidx] nxtpos = latlon[ (curidx+1)%nidx ] dlat = curpos[1] - curpos[0] dlon = curpos[1] - curpos[0] dpos = math.sqrt( (dlat*dlat) + (dlon*dlon) ) for _step in range(DSTEP): _p = float(_step) / float(DSTEP) _pos = [ curpos[0] + _p*(nxtpos[0] - curpos[0]), curpos[1] + _p*(nxtpos[1] - curpos[1]) ] cursec = float(time.time()) if (cursec - lastupdate_sec) > DTUPDATE: lastupdate_sec = cursec #sys.stdout.write(str(_pos[0]) + " " + str(_pos[1]) + "\n") print_gps_nmea_messages(sys.stdout, _pos[0], _pos[1]) sys.stdout.flush() time.sleep(DS) curidx = (curidx + 1)%nidx time.sleep(DT) if ITER_COUNT>=0: curiter+=1