popufare/busunit/testing/gps/gps-sim.py

#!/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
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