def setup_port(port1, port2, log1, log2, append=False):
if (port1!=None):
dev1 = ublox.UBlox(port1, baudrate=opts.baudrate, timeout=0.1)
#dev1.set_preferred_dynamic_model(opts.dynmodel1)
#dev1.set_logfile(log1, append=append)
#dev1.set_binary()
#dev1.configure_poll_port()
#dev1.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_USB)
#dev1.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5)
#dev1.configure_poll(ublox.CLASS_MON, ublox.MSG_MON_HW)
#dev1.configure_poll(ublox.CLASS_NAV, ublox.MSG_NAV_DGPS)
#dev1.configure_poll(ublox.CLASS_MON, ublox.MSG_MON_VER)
#dev1.configure_port_USB()
#dev1.configure_solution_rate(rate_ms=200)
#dev1.configure_port(port=ublox.PORT_USB, inMask=32, outMask=1)
#dev1.configure_loadsave(clearMask=0, saveMask=7967, loadMask=0, deviceMask=23)
#dev1.configure_poll_port()
#dev1.configure_poll_port(ublox.PORT_USB)
else:
print("Please set a 1st port of ublox device.")
return
if (port2!=None):
#if (False):
dev2 = ublox.UBlox(port2, baudrate=opts.baudrate, timeout=0.1)
#dev2.set_preferred_dynamic_model(opts.dynmodel2)
#dev2.set_logfile(log2, append=append)
#dev2.set_binary()
#dev2.configure_poll_port()
#dev2.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_USB)
#dev2.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5)
#dev2.configure_poll(ublox.CLASS_MON, ublox.MSG_MON_HW)
#dev2.configure_poll(ublox.CLASS_NAV, ublox.MSG_NAV_DGPS)
#dev2.configure_poll(ublox.CLASS_MON, ublox.MSG_MON_VER)
#dev2.configure_solution_rate(rate_ms=200)
#dev2.configure_port(port=ublox.PORT_USB, inMask=32, outMask=1)
#dev2.configure_poll_port()
#dev2.configure_poll_port(ublox.PORT_USB)
return dev1, dev2
else:
print("localization will be done with a device.")
return dev1, None
def start_gnss_logger(BOX_ID=1,
port="/dev/ttyS1",
mcast_object=multicast_sender):
logging.info("Box Id as read from config.yaml file and my IP is: " +
str(BOX_ID))
# Create ublox Log directory and log file based on current time.
UBX_RAW_LOG_DIR = MAIN_LOG_DIR + "logs/ublox/"
if not os.path.exists(os.path.dirname(UBX_RAW_LOG_DIR)):
os.makedirs(os.path.dirname(UBX_RAW_LOG_DIR))
UBX_RAW_LOG_FILE = UBX_RAW_LOG_DIR + CURRENT_DATE_TIME + ".ubx"
logging.info("Logging ublox RAW data to the file: " +
str(UBX_RAW_LOG_FILE))
dev = ublox.UBlox(port)
dev.set_logfile(UBX_RAW_LOG_FILE)
BOX_POS_LOG_DIR = MAIN_LOG_DIR + "logs/boxes/" + str(BOX_ID) + "/"
if not os.path.exists(os.path.dirname(BOX_POS_LOG_DIR)):
os.makedirs(os.path.dirname(BOX_POS_LOG_DIR))
BOX_POS_LOG_FILE = BOX_POS_LOG_DIR + CURRENT_DATE_TIME + ".log"
pos_log_file = open(BOX_POS_LOG_FILE, "wa", buffering=1)
logging.info("Logging Processed data to the file: " +
str(BOX_POS_LOG_FILE))
logging.info("Starting Data acquisition loop from UBLOX. ")
GPS_WEEK = "null"
rtk_status = 0
while True:
msg = dev.receive_message(ignore_eof=True)
if msg is None:
logging.info("No Message.")
break
msg_name = msg.name()
if msg_name == "NAV_TIMEGPS":
msg.unpack()
GPS_WEEK = str(msg.week)
if msg_name == "NAV_STATUS":
msg.unpack()
rtk_status = bin(msg.flags)[8]
if msg_name == "NAV_HPPOSLLH":
msg.unpack()
high_p_lon = (msg.lon + msg.lonHp * 0.01) * 0.0000001
high_p_lat = (msg.lat + msg.latHp * 0.01) * 0.0000001
if hasattr(msg, 'iTOW'):
string_payload = GPS_WEEK+"," + str(msg.iTOW) + "," + str(BOX_ID) + "," + str(1) + "," + \
str(high_p_lon) + "," + str(high_p_lat) + "," + str(1) + "," + \
str(msg.hAcc) + "," + str(rtk_status)
# GPS-week, GPS-TimeOfWeek,BOX-ID,Crane-ID,LONGITUDE, LATITUDE, OFFSET, Horizontal Accuracy (mm), RTK-Status, HASH
msg_to_send = string_payload + "," + str(len(string_payload))
mcast_object.send_data(msg_to_send)
msg_to_write = msg_to_send + "\n"
pos_log_file.write(msg_to_write)
pos_log_file.close()
def init_reader():
port_counter = 0
while True:
try:
dev = ublox.UBlox(ports[port_counter],
baudrate=baudrate,
timeout=timeout,
panda=panda,
grey=grey)
configure_ublox(dev)
return dev
except serial.serialutil.SerialException as e:
print(e)
port_counter = (port_counter + 1) % len(ports)
time.sleep(2)
def main():
signal.signal(signal.SIGINT, handler)
global runstate
runstate = True
parser = argparse.ArgumentParser(
description="GPS 3D fix check using UBX protocol")
parser.add_argument('-i',
'--port',
help='UBX port',
metavar='port',
dest='port',
required=True)
parser.add_argument('-b',
'--baud_rate',
help='baud rate',
metavar='baud',
dest='baud',
required=False,
default=57600,
type=int)
args = parser.parse_args()
port = args.port
baudrate = args.baud
dev = ublox.UBlox(port, baudrate)
while runstate:
# Parse and format UBX binary messages into a list of fields
msg = dev.receive_message(ignore_eof=True)
if msg is None:
continue
msg.unpack()
ubxMsg = str(msg)
ubxMsgFields = ubxMsg.split()
# Return only when 3D fix is acquired
if 'NAV_PVT:' in ubxMsgFields:
ubxMsgItems = re.split(': |, |=', ubxMsg)
if 'fixType=3,' in ubxMsgFields:
return 0
else:
continue
def initialize_gps():
# ubl = navio2.ublox.UBlox("spi:0.0", baudrate=5000000, timeout=2)
ubl = ublox.UBlox("spi:0.0", baudrate=5000000, timeout=2)
ubl.configure_poll_port()
ubl.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_USB)
ubl.configure_port(port=ublox.PORT_SERIAL1, inMask=1, outMask=0)
ubl.configure_port(port=ublox.PORT_USB, inMask=1, outMask=1)
ubl.configure_port(port=ublox.PORT_SERIAL2, inMask=1, outMask=0)
ubl.configure_poll_port()
ubl.configure_poll_port(ublox.PORT_SERIAL1)
ubl.configure_poll_port(ublox.PORT_SERIAL2)
ubl.configure_poll_port(ublox.PORT_USB)
ubl.configure_solution_rate(rate_ms=500)
# this rate can be faster, but there's not much of a performance increase.
ubl.set_preferred_dynamic_model(None)
ubl.set_preferred_usePPP(None)
ubl.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_POSLLH, 1)
ubl.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_VELNED, 1)
ubl.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_STATUS, 1)
ubl.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW, 0)
ubl.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW2, 0)
ubl.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_DOP, 0)
ubl.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_PVT, 0)
ubl.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_SOL, 0)
ubl.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_SVINFO, 0)
ubl.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_VELECEF, 0)
ubl.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_POSECEF, 0)
ubl.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_RAW, 0)
ubl.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_SFRB, 0)
ubl.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_SVSI, 0)
ubl.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_ALM, 0)
ubl.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_EPH, 0)
ubl.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_TIMEGPS, 0)
ubl.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_CLOCK, 0)
ubl.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_DGPS, 0)
for x in range(30):
msg = ubl.receive_message_noerror()
# print(msg)
return ubl
def setup_port(port, log, append=False):
dev = ublox.UBlox(port, baudrate=opts.baudrate, timeout=0.01)
dev.set_logfile(log, append=append)
dev.set_binary()
dev.configure_poll_port()
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_USB)
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5)
dev.configure_poll(ublox.CLASS_MON, ublox.MSG_MON_HW)
dev.configure_poll(ublox.CLASS_NAV, ublox.MSG_NAV_DGPS)
dev.configure_poll(ublox.CLASS_MON, ublox.MSG_MON_VER)
dev.configure_port(port=ublox.PORT_SERIAL1, inMask=0x7, outMask=1)
dev.configure_port(port=ublox.PORT_USB, inMask=0x7, outMask=1)
dev.configure_port(port=ublox.PORT_SERIAL2, inMask=0x7, outMask=1)
dev.configure_poll_port()
dev.configure_poll_port(ublox.PORT_SERIAL1)
dev.configure_poll_port(ublox.PORT_SERIAL2)
dev.configure_poll_port(ublox.PORT_USB)
return dev
if opts.reference:
reference_position = util.ParseLLH(opts.reference)
else:
reference_position = None
# create a figure
f = pyplot.figure(1)
f.clf()
pyplot.axis([-opts.size, opts.size, -opts.size, opts.size])
pyplot.ion()
colours = ['ro', 'bo', 'go', 'yo']
devs = []
for d in args:
devs.append(ublox.UBlox(d))
if opts.seek != 0:
for d in devs:
d.seek_percent(opts.seek)
last_t = time.time()
def get_xy(pos, home):
distance = util.gps_distance(home[0], home[1], pos[0], pos[1])
bearing = util.gps_bearing(home[0], home[1], pos[0], pos[1])
x = distance * math.sin(math.radians(bearing))
y = distance * math.cos(math.radians(bearing))
return (x, y)
parser.add_option("--dynModel",
type='int',
default=None,
help='set dynamic navigation model')
parser.add_option("--usePPP",
action='store_true',
default=False,
help='enable precise point positioning')
parser.add_option("--dots",
action='store_true',
default=False,
help='print a dot on each message')
(opts, args) = parser.parse_args()
dev = ublox.UBlox(opts.port, baudrate=opts.baudrate, timeout=2)
dev.set_logfile(opts.log, append=opts.append)
dev.set_binary()
dev.configure_poll_port()
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_USB)
dev.configure_poll(ublox.CLASS_MON, ublox.MSG_MON_HW)
dev.configure_port(port=ublox.PORT_SERIAL1, inMask=1, outMask=0)
dev.configure_port(port=ublox.PORT_USB, inMask=1, outMask=1)
dev.configure_port(port=ublox.PORT_SERIAL2, inMask=1, outMask=0)
dev.configure_poll_port()
dev.configure_poll_port(ublox.PORT_SERIAL1)
dev.configure_poll_port(ublox.PORT_SERIAL2)
dev.configure_poll_port(ublox.PORT_USB)
dev.configure_solution_rate(rate_ms=1000)
dev.set_preferred_dynamic_model(opts.dynModel)
configs = yaml.load(open('config.yaml', 'r'))
BOX_ID = MY_IP.split(".")[-1][0]
KEY = 'BOX' + str(BOX_ID)
OFFSET = configs[KEY]['OFFSET']
KRANID = configs[KEY]['KRANID']
logging.info("Box Id as read from config.yaml file and my IP is: " +
str(BOX_ID))
# Create ublox Log directory and log file based on current time.
UBX_RAW_LOG_DIR = MAIN_LOG_DIR + "logs/ublox/"
if not os.path.exists(os.path.dirname(UBX_RAW_LOG_DIR)):
os.makedirs(os.path.dirname(UBX_RAW_LOG_DIR))
UBX_RAW_LOG_FILE = UBX_RAW_LOG_DIR + CURRENT_DATE_TIME + ".ubx"
logging.info("Logging ublox RAW data to the file: " + str(UBX_RAW_LOG_FILE))
dev = ublox.UBlox("/dev/ttyS1")
dev.set_logfile(UBX_RAW_LOG_FILE)
BOX_POS_LOG_DIR = MAIN_LOG_DIR + "logs/boxes/" + str(BOX_ID) + "/"
if not os.path.exists(os.path.dirname(BOX_POS_LOG_DIR)):
os.makedirs(os.path.dirname(BOX_POS_LOG_DIR))
BOX_POS_LOG_FILE = BOX_POS_LOG_DIR + CURRENT_DATE_TIME + ".log"
pos_log_file = open(BOX_POS_LOG_FILE, "wa", buffering=1)
logging.info("Logging Processed data to the file: " + str(BOX_POS_LOG_FILE))
logging.info("Starting Data acquisition loop from UBLOX. ")
GPS_WEEK = "null"
rtk_status = 0
current_heading = 999
current_poslength = 999
def main():
ser = serial.Serial(port='/dev/ttyS0',
baudrate=9600,
bytesize=8,
parity='N',
stopbits=1,
timeout=0.01)
# Create data file and write header #
tstr = time.strftime('%Y-%m-%d_%H-%M-%S-%Z.txt')
filename = str('Telemetry Data ') + tstr
ubl = ublox.UBlox("spi:0.0", baudrate=5000000, timeout=2)
ubl.set_preferred_dynamic_model(None)
ubl.set_preferred_usePPP(None)
# Configure the GPS messages
ubl.configure_solution_rate(rate_ms=1000)
ubl.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_POSLLH, 1)
ubl.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_STATUS, 1)
ubl.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_VELNED, 1)
ubl.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_PVT, 1)
message_names = ['NAV_POSLLH', 'NAV_STATUS', 'NAV_VELNED', 'NAV_PVT']
check_msg = []
while True:
msg = ubl.receive_message()
print(msg)
if msg is None:
if opts.reopen:
ubl.close()
ubl = ublox.UBlox("spi:0.0", baudrate=5000000, timeout=2)
continue
print(empty)
break
if msg.name() == "NAV_STATUS":
outstr = str(msg).split(",")
fix_id = int(str_to_num(
outstr[1])) # GPS fix - from UBX-NAV-STATUS function
fix_ok = int(
str_to_num(outstr[3])
) # gpsFixOk (1 = position and velocity valid and within DOP and ACC)
write_to_file(filename, outstr)
check_msg.append(msg.name())
print(outstr)
if msg.name() == "NAV_POSLLH":
outstr = str(msg).split(",")
lon = float(
'%.7f' %
(str_to_num(outstr[1]) * 10**-7)) # GPS longitude (deg)
lat = float('%.7f' %
(str_to_num(outstr[2]) * 10**-7)) # GPS latitude (deg)
h_msl = float(
'%.1f' %
(int(str_to_num(outstr[4]) / 1000))) # GPS height MSL (m)
write_to_file(filename, outstr)
check_msg.append(msg.name())
if msg.name() == "NAV_VELNED":
outstr = str(msg).split(",")
gps_hdg = float(
'%.1f' % (str_to_num(outstr[6]) * 10**-5)) # GPS heading (deg)
gps_gspd = int(str_to_num(outstr[5]) /
100) # GPS ground speed (m/s)
write_to_file(filename, outstr)
check_msg.append(msg.name())
if msg.name() == "NAV_PVT":
outstr = str(msg).split(",")
hour = outstr[4]
min = outstr[5]
sec = outstr[6]
gps_time = '{}{}{}'.format(hour, min, sec)
write_to_file(filename, outstr)
check_msg.append(msg.name())
if all(elem in check_msg for elem in message_names):
data = [gps_time, 'NAVMSG', int(time.time()), lat, lon, h_msl]
write_to_file(filename, data)
send_gps(ser, data)
check_msg = []
#! /usr/bin/env python
from pylab import *
import ublox
dev = ublox.UBlox(sys.argv[1])
raw = []
svi = []
pos = []
while True:
msg = dev.receive_message()
if msg is None:
break
n = msg.name()
if n not in ['RXM_RAW', 'NAV_SVINFO']:
continue
msg.unpack()
if n == 'RXM_RAW':
raw.append(msg.numSV)
elif n == 'NAV_SVINFO':
svi.append(msg.numCh)
# count how many of the active channels are used in the pos solution
#!/usr/bin/env python
import ublox, sys, fnmatch, os
from optparse import OptionParser
parser = OptionParser("ublox_show.py [options] <file>")
parser.add_option("--types", default='*', help="comma separated list of types to show (wildcards allowed)")
parser.add_option("--seek", type='float', default=0, help="seek percentage to start in log")
parser.add_option("-f", "--follow", action='store_true', default=False, help="ignore EOF")
(opts, args) = parser.parse_args()
dev = ublox.UBlox(args[0])
if opts.seek != 0:
dev.seek_percent(opts.seek)
types = opts.types.split(',')
while True:
msg = dev.receive_message(ignore_eof=opts.follow)
if msg is None:
break
if types != ['*']:
matched = False
try:
name = msg.name()
except ublox.UBloxError as e:
continue
for t in types:
t_wrap = t_last
t += t_wrap
t_last = t
for sv, resid in enumerate(a[1:]): # Cut off the leading timestamp
if float(resid) != 0:
sat_res[t - t_first, sv] = float(resid)
t_first = 0
t_last = 0
t_wrap = 0
print("Parsing UBX")
dev = ublox.UBlox(opts.ubx_log)
while True:
"""process the ublox messages, extracting the ones we need for the sat position"""
msg = dev.receive_message()
if msg is None:
break
if msg.name() == 'NAV_SVINFO':
msg.unpack()
t = msg.iTOW * 0.001
if t_first == 0:
t_first = t
if t_last != 0 and t - t_last >= 2:
print("Missed Epoch")
The --show option is to show NAV_SOL messages containing GPS fix data.
'''
import ublox
from optparse import OptionParser
parser = OptionParser("ublox_test.py [options] <file>")
parser.add_option("--show", action='store_true', default=False, help='Show all UBX messages')
(opts, args) = parser.parse_args()
# multiple files/serial ports can be specified
for f in args:
dev = ublox.UBlox(f)
with open('/var/log/gpsFix.log','a') as log:
while True:
# Parse and format UBX binary messages into a list of fields
msg = dev.receive_message()
if msg is None:
break
msg.unpack()
ubxMsg = str(msg)
ubxMsgFields = ubxMsg.split()
# Print the entire NAV_SOL message if user specified option
if opts.show:
if 'NAV_SOL:' in ubxMsgFields:
print ubxMsg
from optparse import OptionParser
parser = OptionParser("position_estimate.py [options] <file>")
parser.add_option("--plot", action='store_true', default=False, help="plot points")
parser.add_option("--reference", help="reference position (lat,lon,alt)", default=None)
(opts, args) = parser.parse_args()
if len(args) != 1:
print("usage: position_estimate.py <file>")
sys.exit(1)
filename = args[0]
dev = ublox.UBlox(filename)
rtcmfile = open('rtcm2.dat', mode='wb')
if opts.plot:
reference = util.ParseLLH(opts.reference).ToECEF()
plotter = dataPlotter.dataPlotter(reference)
def position_estimate(messages, satinfo):
'''process raw messages to calculate position
'''
# get get position the receiver calculated. We use this to check the calculations
pos = positionEstimate.positionEstimate(satinfo)
if pos is None:
(datetime.datetime.utcnow().strftime('%Y%m%d%H%M%S'))),
"w")
log_fd.write("%s: New log\n" %
(datetime.datetime.utcnow().strftime('%Y%m%d%H%M%S')))
log_fd.flush()
if have_display:
# create a figure
f = pyplot.figure(1)
f.clf()
ax1 = f.add_axes([0.05, 0.05, 0.9, 0.9])
else:
tty_fd = open('/dev/tty3', 'w+')
for d in args:
dev = ublox.UBlox(d)
def send_rtcm(msg):
# print(msg)
dev.write(msg)
rtcm_thread = RTCMv3_decode.run_RTCM_converter(opts.ntrip_server,
opts.ntrip_port,
opts.ntrip_user,
opts.ntrip_password,
opts.ntrip_mount,
rtcm_callback=send_rtcm)
st+= "95% Dist {}\n".format(dist95)
st+= "Av Dist {}\n".format(av_dist)
st+= "Bias {}\n".format(dm)
if reference_position is not None:
st+= "Max Imp {}\n".format(max_imp)
st+= "Av Imp {}\n".format(av_imp)
st+= "95% Imp {}\n".format(imp95)
print st
devs = []
for d in args:
devs.append((ublox.UBlox(d),d))
if opts.seek != 0:
for d, name in devs:
d.seek_percent(opts.seek)
last_t = time.time()
# Load all positions
pos = {}
for i in range(len(devs)):
pos[i] = []
for i, (d, name) in enumerate(devs):
while True:
msg = d.receive_message()
# looking for oscilloscope
devices = getDeviceList()
print(devices)
# initiate oscilloscope
osc = UsbTmcDriver(devices[0])
print('$OSC,', osc.getName().decode(), end='')
osc.write('MENU:RUN')
gpsport = '/dev/ttyACM0'
gpsbaudrate = 921600
# Open GPS port
dev = ublox.UBlox(gpsport, baudrate=gpsbaudrate, timeout=0)
while True:
'''
while True:
try:
msg = dev.receive_message()
except:
continue
if (msg is None):
break
'''
time.sleep(1)
osc.write(":SINGLE")
print('#Waiting...')
args = parser.parse_args()
show = args.show
dataDir = args.dataDir
gpsPrefix = args.prefix
gpsSuffix = args.suffix
runNum = args.runNum
port = args.port
baud = args.baud
# create new GPS log directory if one doesn't already exist
if not os.path.exists(dataDir):
os.makedirs(dataDir)
logFile = open("%s/%s%06d" % (dataDir, gpsPrefix, runNum), "w")
dev = ublox.UBlox(port)
while runstate:
# Parse and format UBX binary messages into a list of fields
msg = dev.receive_message(ignore_eof=True)
if msg is None:
continue
msg.unpack()
ubxMsg = str(msg)
ubxMsgFields = ubxMsg.split()
# Print all UBX messages with no logging
if show:
print ubxMsg
# Log position (lat,long,alt) and GPS time
