beginnings of a gps testing script

busunit/testing/gps/gps-sim.py

@@ -0,0 +1,170 @@
+#!/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 <https://www.gnu.org/licenses/>.
+#
+
+# Script to test GPS positioning by simulating output of a GPS module.
+#
+# Outputs to stdout
+#
+
+import getopt
+import sys
+import time
+import math
+
+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
+
+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
+      #print _p, _pos
+      sys.stdout.write(str(_pos[0]) + " " + str(_pos[1]) + "\n")
+      sys.stdout.flush()
+
+    time.sleep(DS)
+
+  curidx = (curidx + 1)%nidx
+
+  time.sleep(DT)
+  if ITER_COUNT>=0: curiter+=1
+
+

busunit/testing/gps/latlon.csv

@@ -0,0 +1,4 @@
+4227.188000,-7630.277000
+4227.235000,-7630.291000
+4227.209000,-7630.377000
+4227.188000,-7630.277000