def debug_gps():
sleep(1)
time = datetime.datetime.now()
gga1 = pynmea2.GGA(
'GN', 'GGA',
(time.strftime("%H%M%S"), '5231.059', 'N', '01324.946', 'E', '1', '04',
'2.6', '69.00', 'M', '-33.9', 'M', '', '0000'))
gga2 = pynmea2.GGA(
'GN', 'GGA',
(time.strftime("%H%M%S"), '5231.058', 'N', '01324.946', 'E', '0', '04',
'2.6', '73.00', 'M', '-33.9', 'M', '', '0000'))
gga3 = pynmea2.GGA(
'GN', 'GGA',
(time.strftime("%H%M%S"), '5231.057', 'N', '01324.946', 'E', '1', '04',
'2.6', '73.00', 'M', '-33.9', 'M', '', '0000'))
rmc1 = pynmea2.RMC(
'GN', 'RMC',
(time.strftime("%H%M%S"), 'A', '5231.056', 'N', '01324.946', 'E', '0',
'26.2', time.strftime("%d%m%y"), 'A'))
rmc2 = pynmea2.RMC(
'GN', 'RMC',
(time.strftime("%H%M%S"), 'A', '5231.060', 'N', '01324.946', 'E', '2',
'324.2', time.strftime("%d%m%y"), 'A'))
nmea = [gga1, rmc1, gga2, gga3, rmc2]
return str(random.choice(nmea))
def gps_str(self, lat, lon, time):
head = "$GPGGA"
lat_dir = "N"
lon_dir = "W"
tail = "A"
last = 0
newlat = self.conv_gps_deg([], lat)
gps_str = []
for i, j, k in zip(newlat, lon, time):
if i < 0:
i = -i
lat_dir = "S"
else:
lat_dir = "N"
newi = "{:07.2F}".format(i)
newj = "{:08.2F}".format(j)
newk = str(k)
msg = pynmea2.GGA("GP", "GGA",
(newk, newi, lat_dir, newj, lon_dir, "1", "08",
"0.9", "500000", "46.9"))
# print str(msg)
gps_str.append(str(msg))
return gps_str
def build_nmea_gpgaa_string(time, lat, lng, alt):
'''
Build NEMA GPGAA message string from given UNIX universal time timestamp,
latitude and longitude in the ISO 6709 string Annex H format and altiitude
in meters above the sea level.
'''
msg = (
'GP',
'GGA',
(
timestamp_to_gpstime(time),
str(abs(lat)),
latitude_to_hemisphere(lat),
str(abs(lng)),
longitude_to_hemisphere(lng),
'1',
'01',
'1.0', # Fix quality, satelites num, HDOP
str(alt),
'M',
'0',
'M', # Height of geoid above WGS84 ellipsoid
'',
'0000')) # Time since DGPS last update and id
return str(pynmea2.GGA(*msg))
def serial_def():
ser = serial.Serial()
ser.port = "/dev/ttyUSB2"
ser.baudrate = 115200
ser.bytesize = serial.EIGHTBITS #number of bits per bytes
ser.parity = serial.PARITY_NONE #set parity check: no parity
ser.stopbits = serial.STOPBITS_ONE #number of stop bits
ser.timeout = 2 #timeout block read
ser.xonxoff = False #disable software flow control
ser.rtscts = False #disable hardware (RTS/CTS) flow control
ser.dsrdtr = False #disable hardware (DSR/DTR) flow control
ser.writeTimeout = 2
ser.open()
if ser.isOpen():
print(ser.name + ' is open...')
ser.write("AT+CGPSINFO=0"+"\r\n")
ser.write("ATE1"+"\r\n")
ser.write("ATI"+"\r\n")
ser.write("AT+CGPS=1,1"+"\r\n")
ser.write("AT+CGPSOUT=8"+"\r\n")
ser.write("AT+CGPSINFO=5"+"\r\n")
time.sleep(1)
out=''
while True:
out += ser.read(1)
out2 = ser.readline()
#print("out1: " + out)
if re.match(r"CGPSINFO:", out2):
print("out2: " + out2)
print(out2.split(","))
out3 = ((out2.split(",")[0].split(": ")[1:] + out2.split(",")[1:-1] + out2.split(",")[-1].split("\r")[0:]))[0:-1]
msg = pynmea2.GGA('GP', 'GSV', out3)
lat = int(str(float(out3[0].strip()[:-1])/100).split(".")[0])
lng = int(str(float(out3[2].strip()[:-1])/100).split(".")[0])
latdec = float("." + str(float(out3[0].strip()[:-1])/100).split(".")[1]) * 60
lngdec = float("." + str(float(out3[2].strip()[:-1])/100).split(".")[1]) * 60
latmin = int(str(latdec).split(".")[0])
lngmin = int(str(lngdec).split(".")[0])
latsec = float("." + str(latdec).split(".")[1]) * 60
lngsec = float("." + str(lngdec).split(".")[1]) * 60
print(pynmea2.parse(str(msg)))
print("Latitude: " + str(lat) + " Degrees " + str(latmin) + " Minutes " + str(latsec) + " Seconds ") #str(msg.lat))
print("Longitude: " + str(lng) + " Degrees " + str(lngmin) + " Minutes " + str(lngsec) + " Seconds ") #str(msg.lon))
print("Alt: " + str(msg.altitude) + " Meters")
#print(pynmea2.parse(out))
ser.close()
def DataConverterAndWriter(lne):
msg = pynmea2.parse(lne)
idx = msg.name_to_idx
msg.data[idx["gps_qual"]] = '1'
msg.data[idx["timestamp"]] = "%s.00" % msg.data[idx["timestamp"]][:6]
msg = pynmea2.GGA('GP', 'GGA', msg.data)
sys.stdout.write(str(msg)+"\n")
sys.stdout.flush()
time.sleep(2)
return
def get_nmea_string(longitude, latitude, date_value, time_value):
msg = pynmea2.GGA(
'GP', 'GGA',
((datetime.datetime.strptime(time_value,
'%H:%M:%S').time().strftime("%H%M%S.00")),
('%03i%02.5f' %
(int(float(math.floor(abs(latitude)))),
(abs(latitude) - float(math.floor(abs(latitude)))) * 60.00)),
('S' if latitude < 0 else 'N'),
('%03i%02.5f' %
(int(float(math.floor(abs(longitude)))),
(abs(longitude) - float(math.floor(abs(longitude)))) * 60.00)),
('W' if longitude < 0 else 'E'), '1', '04', '2.6', '100.00', 'M',
'-33.9', 'M', '', '0000'))
return msg
def decode(self, cl, id, length, payload):
data = []
try:
format = MSGFMT_INV[((cl, id), length)]
data.append(dict(zip(format[1], struct.unpack(format[0],
payload))))
except KeyError:
try:
# Try if this is one of the variable field messages
format = MSGFMT_INV[((cl, id), None)]
fmt_base = format[:3]
fmt_rep = format[3:]
# Check if the length matches
if (length - fmt_base[0]) % fmt_rep[0] != 0:
logging.error(
"Variable length message class 0x%x, id 0x%x has wrong length %i"
% (cl, id, length))
return
data.append(
dict(
zip(fmt_base[2],
struct.unpack(fmt_base[1],
payload[:fmt_base[0]]))))
for i in range(0, (length - fmt_base[0]) // fmt_rep[0]):
offset = fmt_base[0] + fmt_rep[0] * i
data.append(
dict(
zip(
fmt_rep[2],
struct.unpack(
fmt_rep[1],
payload[offset:offset + fmt_rep[0]]))))
except KeyError:
logging.info("Unknown message class 0x%x, id 0x%x, length %i" %
(cl, id, length))
return
logging.debug("Got UBX packet of type %s: %s" % (format[-1], data))
if (not "ITOW" in data[0]):
return
if (self.lastsolution["ITOW"] == None):
self.lastsolution["ITOW"] = data[0]["ITOW"]
if (self.lastsolution["ITOW"] != data[0]["ITOW"]):
# dump output for current ITOW here
if ("NAV-PVT" in self.lastsolution):
currentTime = datetime.strptime(
"%04d-%02d-%02d %02d:%02d:%02d" %
(self.lastsolution["NAV-PVT"]["year"],
self.lastsolution["NAV-PVT"]["month"],
self.lastsolution["NAV-PVT"]["day"],
self.lastsolution["NAV-PVT"]["hour"],
self.lastsolution["NAV-PVT"]["min"],
self.lastsolution["NAV-PVT"]["sec"]), "%Y-%m-%d %H:%M:%S")
currentTime += timedelta(
microseconds=self.lastsolution["NAV-PVT"]["nano"] / 1000)
lon = 1.0 * self.lastsolution["NAV-PVT"]["lon"] / 10**7
lat = 1.0 * self.lastsolution["NAV-PVT"]["lat"] / 10**7
#pp.pprint(self.lastsolution)
if ("NAV-DOP" in self.lastsolution):
hDOP = 1.0 * self.lastsolution["NAV-DOP"]["hDOP"]
logging.debug("Using proper hDOP " + str(hDOP))
else:
hDOP = 1.0 * self.lastsolution["NAV-PVT"]["pDOP"]
logging.debug("Using fake pDOP " + str(hDOP))
hDOP /= 100
if (self.lastsolution["NAV-PVT"]["fixType"] != ""
and self.lastsolution["NAV-PVT"]["fixType"]
in ubxToNMEAGGAFix):
self.outfd.write(
str(
pynmea2.GGA('GN', 'GGA', (
currentTime.strftime("%H%M%S") + ".%02d" %
(currentTime.microsecond / 10000),
"%02d%08.5f" % (divmod(abs(lat) * 60, 60.0)),
('S', 'N')[lat >= 0.0], '%03d%08.5f' %
(divmod(abs(lon) * 60, 60.0)),
('W', 'E')[lon >= 0.0], ubxToNMEAGGAFix[
self.lastsolution["NAV-PVT"]["fixType"]],
"%02d" % self.lastsolution["NAV-PVT"]["numSV"],
"%03.1f" % (hDOP), "%.1f" %
(1.0 * self.lastsolution["NAV-PVT"]["hMSL"] /
1000), 'M', "%.1f" %
(1.0 * self.lastsolution["NAV-PVT"]["height"] /
1000), 'M', '', '0000'))) + "\n")
self.outfd.write(
str(
pynmea2.GGA('GN', 'ZDA', (
currentTime.strftime("%H%M%S") + ".%02d" %
(currentTime.microsecond / 10000),
"%02d" % self.lastsolution["NAV-PVT"]["day"],
"%02d" % self.lastsolution["NAV-PVT"]["month"],
"%04d" % self.lastsolution["NAV-PVT"]["year"],
"", ""))) + "\n")
self.outfd.write(
str(
pynmea2.GGA('GN', 'RMC', (
currentTime.strftime("%H%M%S") + ".%02d" %
(currentTime.microsecond / 10000),
('A', 'V')[ubxToNMEAGGAFix[
self.lastsolution["NAV-PVT"]["fixType"]] ==
"0"], "%02d%08.5f" %
(divmod(abs(lat) * 60, 60.0)),
('S', 'N')[lat >= 0.0], "%03d%08.5f" %
(divmod(abs(lon) * 60, 60.0)),
('W', 'E')[lon >= 0.0], "%05.1f" %
(1.0 * self.lastsolution["NAV-PVT"]["gSpeed"] /
514.444), "%05.1f" %
(1.0 * self.lastsolution["NAV-PVT"]["headMot"]
/ 10**5), "%02d%02d%s" %
(self.lastsolution["NAV-PVT"]["day"],
self.lastsolution["NAV-PVT"]["month"],
str(self.lastsolution["NAV-PVT"]["year"])[-2:]
), "%05.1f" %
abs(self.lastsolution["NAV-PVT"]["magDec"]),
('E', 'W')[self.lastsolution["NAV-PVT"]
["magDec"] >= 0.0]))) + "\n")
if ("NAV-SVINFO" in self.lastsolution):
reportedSVsbyConstellation = {}
# this is preparation for NMEA 4.x reporting
for i in range(1, len(self.lastsolution["NAV-SVINFO"])):
(talkerID, satID) = ubxToNMEASatNum(
args.NMEAversion[0],
self.lastsolution["NAV-SVINFO"][i]["SVID"])
if (talkerID == None): talkerID = "None"
if (not talkerID in reportedSVsbyConstellation):
reportedSVsbyConstellation[talkerID] = {}
if (satID != None):
reportedSVsbyConstellation[talkerID][satID] = i
for con in sorted(reportedSVsbyConstellation):
satsToReport = len(reportedSVsbyConstellation[con])
NMEAcon = (con, '')[con == "None"]
minQI = 4
lockedSats = 0
sats = [
s for s in sorted(reportedSVsbyConstellation[con])
if self.lastsolution["NAV-SVINFO"][
reportedSVsbyConstellation[con][s]]["QI"] >= minQI
]
NMEAGSVreportsPerMsg = 3
if (len(sats) > 0):
satsReports = flatten([[
"%02d" % x,
"%-02d" % self.lastsolution["NAV-SVINFO"][
reportedSVsbyConstellation[con][x]]["Elev"],
"%03d" % self.lastsolution["NAV-SVINFO"][
reportedSVsbyConstellation[con][x]]["Azim"],
"%02d" % self.lastsolution["NAV-SVINFO"][
reportedSVsbyConstellation[con][x]]["CNO"]
] for x in sats])
if ("NAV-PVT" in self.lastsolution):
if (len(sats) !=
self.lastsolution["NAV-PVT"]["numSV"]):
logging.info(
"Inconsistency in number of satellites observed: NAV-PVT reported %d, while NAV-SVINFO indicated %d with minimum QI of %d"
% (self.lastsolution["NAV-PVT"]["numSV"],
len(sats), minQI))
lockedSats = self.lastsolution["NAV-PVT"]["numSV"]
else:
lockedSats = len(sats)
for i in range(0, len(satsReports),
NMEAGSVreportsPerMsg * 4):
self.outfd.write(
str(
pynmea2.GSV(NMEAcon, 'GSV', (
"%d" % len(sats), "%d" %
(i / (NMEAGSVreportsPerMsg * 4) + 1),
"%d" % lockedSats, ",".join(
satsReports[i:i +
NMEAGSVreportsPerMsg *
4])))) + "\n")
# end of dump
self.lastsolution = {}
self.lastsolution["ITOW"] = data[0]["ITOW"]
if (format[-1] == "NAV-SVINFO" or format[-1] == "RXM-SVSI"):
self.lastsolution[format[-1]] = data
#pass
else:
self.lastsolution[format[-1]] = data[0]
speedKmh = speedMeterPerSeconds * 3600 / 1000
time = f"{point.time:%H%M%S.%f}"[:-3]
lat, lon = sd_to_dm(point.latitude, point.longitude)
# Génération des traces NMEA
msg = pynmea2.GGA(
"GP",
"GGA",
(
time,
lat[0],
lat[1],
lon[0],
lon[1],
"1",
"00",
"",
f"{point.elevation:.1f}",
"M",
"0.0",
"M",
"",
"0000",
),
)
nmea_buffer.write(f"{msg}\r\n")
msg = pynmea2.RMC(
"GP",
"RMC",
(
time, # Timestamp