def __init__(self, gps_week, time_of_week):
# time of week in seconds, including fractions of a second
self.time_of_week = time_of_week
self.gps_week = gps_week
self.gps_time = util.gpsTimeToTime(gps_week, time_of_week)
self.prMeasured = {}
self.cpMeasured = {}
self.quality = {}
self.lli = {}
def __init__(self, gps_week, time_of_week):
# time of week in seconds, including fractions of a second
self.time_of_week = time_of_week
self.gps_week = gps_week
self.gps_time = util.gpsTimeToTime(gps_week, time_of_week)
self.prMeasured = {}
self.cpMeasured = {}
self.quality = {}
self.lli = {}
self.cno = {}
def RTCMType3_step(self, throttle=True):
gpssec = util.gpsTimeToTime(self.gps_week, self.time_of_week)
if gpssec < self.last_type3_time + self.type3_send_time and throttle:
return ''
self.last_type3_time = gpssec
self.reset()
rtcmzcount = self.modZCount()
self.addbits(8, 0x66) # header id
self.addbits(6, 3) # msg type 1
self.addbits(10, 2) # station id
# 1st word should be sent here
self.addbits(13, rtcmzcount) # z-count
self.addbits(3, self.rtcmseq) # seq no.
self.rtcmseq = (self.rtcmseq + 1) % 8
self.addbits(5, 4) # word length
self.addbits(3, 0) # health bits
pos = self.pos
X = int(pos.X * 100.0)
Y = int(pos.Y * 100.0)
Z = int(pos.Z * 100.0)
self.addbits(8, (X>>24)&0xFF)
self.addbits(8, (X>>16)&0xFF)
self.addbits(8, (X>> 8)&0xFF)
self.addbits(8, (X>> 0)&0xFF)
self.addbits(8, (Y>>24)&0xFF)
self.addbits(8, (Y>>16)&0xFF)
self.addbits(8, (Y>> 8)&0xFF)
self.addbits(8, (Y>> 0)&0xFF)
self.addbits(8, (Z>>24)&0xFF)
self.addbits(8, (Z>>16)&0xFF)
self.addbits(8, (Z>> 8)&0xFF)
self.addbits(8, (Z>> 0)&0xFF)
while self.rtcbits != 0:
self.addbits(8, 0xAA) # pad unused bits with 0xAA
return self.buf + "\n\r"
def RTCMType3_step(self, throttle=True):
gpssec = util.gpsTimeToTime(self.gps_week, self.time_of_week)
if gpssec < self.last_type3_time + self.type3_send_time and throttle:
return ''
self.last_type3_time = gpssec
self.reset()
rtcmzcount = self.modZCount()
self.addbits(8, 0x66) # header id
self.addbits(6, 3) # msg type 1
self.addbits(10, 2) # station id
# 1st word should be sent here
self.addbits(13, rtcmzcount) # z-count
self.addbits(3, self.rtcmseq) # seq no.
self.rtcmseq = (self.rtcmseq + 1) % 8
self.addbits(5, 4) # word length
self.addbits(3, 0) # health bits
pos = self.pos
X = int(pos.X * 100.0)
Y = int(pos.Y * 100.0)
Z = int(pos.Z * 100.0)
self.addbits(8, (X>>24)&0xFF)
self.addbits(8, (X>>16)&0xFF)
self.addbits(8, (X>> 8)&0xFF)
self.addbits(8, (X>> 0)&0xFF)
self.addbits(8, (Y>>24)&0xFF)
self.addbits(8, (Y>>16)&0xFF)
self.addbits(8, (Y>> 8)&0xFF)
self.addbits(8, (Y>> 0)&0xFF)
self.addbits(8, (Z>>24)&0xFF)
self.addbits(8, (Z>>16)&0xFF)
self.addbits(8, (Z>> 8)&0xFF)
self.addbits(8, (Z>> 0)&0xFF)
while self.rtcbits != 0:
self.addbits(8, 0xAA) # pad unused bits with 0xAA
return self.buf + "\n\r"
if (msg is None):
continue
#pass#break
else:
print('#', msg)
if ((msg.name() == 'TIM_TM2')): #&(msg.flags == 241)):
#print('Got TM2 message')
try:
msg.unpack()
timestring = '$HIT,'
timestring += str(msg.count)
timestring += ','
timestring += str(datetime.datetime.utcnow())
timestring += ','
filename = util.gpsTimeToTime(
msg.wnR, 1.0e-3 * msg.towMsR + 1.0e-9 * msg.towSubMsR)
timestring += str(filename)
timestring += ','
timestring += str(
datetime.datetime.utcfromtimestamp(filename))
print(timestring)
sys.stdout.flush()
''' Micsig
time.sleep(2)
osc.write("MENU:STOP")
print('#stop')
sys.stdout.flush()
time.sleep(1)
print('#capture')
sys.stdout.flush()
osc.write(':STORage:CAPTure')
def RTCMType1_step(self, throttle=True):
gpssec = util.gpsTimeToTime(self.gps_week, self.time_of_week)
if gpssec < self.last_type1_time + self.type1_send_time and throttle:
return ''
self.last_type1_time = gpssec
self.reset()
tow = self.time_of_week
deltat = tow - self.last_time_of_week
errors = {}
rates = {}
for svid in self.error_history:
#errors[svid] = sum(self.error_history[svid])/float(len(self.error_history[svid]))
l = len(self.error_history[svid])
# Extract the median half of the error array and take the average over that. This
# will reject outliers that we see pretty often, though most of those outliers only
# last for 1 or 2 samples at a time so we might want to broaden this window.
trim = sorted(self.error_history[svid])[l // 4: 3 * l // 4 + 1]
errors[svid] = sum(trim) / float(len(trim))
if svid in self.last_errors and deltat > 0:
rates[svid] = (errors[svid] - self.last_errors[svid]) / deltat
else:
rates[svid] = 0
msgsatid = []
msgprc = []
msgprrc = []
msgiode = []
msgudre = []
scalefactors = []
for svid in self.error_history:
if not svid in self.iode or not svid in errors:
continue
prc = int(round(errors[svid]/0.02))
prrc = int(round(rates[svid]/0.002))
sf = 0
while prc > 32767 or prc < -32768:
sf += 1
prc = (prc + 8) // 16
if sf > 1:
# skip satellites if we can't represent the error in the
# number of bits allowed
continue
if sf == 1:
prrc = (prrc + 8) // 16
prrc = min(prrc, 127)
prrc = max(prrc, -128)
msgsatid.append(svid)
msgprc.append(prc)
msgprrc.append(prrc)
msgiode.append(self.iode[svid])
scalefactors.append(sf)
msgsatcnt = len(msgsatid)
if msgsatcnt == 0:
return ''
# clear the history
self.last_errors = errors.copy()
if self.history_length == 0:
self.error_history = {}
else:
for svid in self.error_history:
while len(self.error_history[svid]) > self.history_length:
self.error_history[svid].pop(0)
self.last_time_of_week = tow
rtcmzcount = self.modZCount()
# first part of header
self.addbits(8, 0x66) # header id
self.addbits(6, 1) # msg type 1
self.addbits(10, self.stationID)
# second part of header
self.addbits(13, rtcmzcount) # z-count
self.addbits(3, self.rtcmseq) # seq no.
self.rtcmseq = (self.rtcmseq + 1) % 8
# now compute the word length of the message
# each word contains 24 bits of data, plus 6 bits of parity
bitlength = msgsatcnt * 40
wordlength = bitlength // 24
if (bitlength % 24) != 0:
wordlength += 1
self.addbits(5, wordlength)
# health bits - mark as healthy
self.addbits(3, 0)
for i in range(msgsatcnt):
self.addbits(1, scalefactors[i])
self.addbits(2, 0) # UDRE
self.addbits(5, msgsatid[i]) # sat id no
# we split the prc into two 8-bit bytes, because an RTCM word
# boundary can occur here
self.addbits(8, msgprc[i] >> 8) # prc hob
self.addbits(8, msgprc[i] & 0xff) # prc lob
self.addbits(8, msgprrc[i]) # prcc
self.addbits(8, msgiode[i]) # IODE
while self.rtcbits != 0:
self.addbits(8, 0xAA) # pad unused bits with 0xAA
print("MSG: bitlength=%u wordlength=%u len=%u" % (bitlength, wordlength, len(self.buf)))
return self.buf + "\r\n"
def positionEstimate(satinfo):
'''process raw messages to calculate position
'''
raw = satinfo.raw
satinfo.reset()
for svid in raw.prMeasured:
if not satinfo.valid(svid):
# we don't have ephemeris data for this space vehicle
#print("not valid")
continue
# get the ephemeris and pseudo-range for this space vehicle
ephemeris = satinfo.ephemeris[svid]
prMes = raw.prMeasured[svid]
prSmooth = satinfo.smooth.prSmoothed[svid]
# calculate the time of flight for this pseudo range
tof = prSmooth / util.speedOfLight
# assume the time_of_week is the exact receiver time of week that the message arrived.
# subtract the time of flight to get the satellite transmit time
transmitTime = raw.time_of_week - tof
timesec = util.gpsTimeToTime(raw.gps_week, raw.time_of_week)
# calculate the satellite position at the transmitTime
satPosition.satPosition(satinfo, svid, transmitTime)
Trel = satinfo.satpos[svid].extra
# correct for earths rotation in the time it took the messages to get to the receiver
satPosition.correctPosition(satinfo, svid, tof)
# calculate satellite azimuth and elevation
satPosition.calculateAzimuthElevation(satinfo, svid, satinfo.lastpos)
# calculate the satellite clock correction
sat_clock_error = rangeCorrection.sv_clock_correction(satinfo, svid, transmitTime, Trel)
# calculate the satellite group delay
sat_group_delay = -satinfo.ephemeris[svid].Tgd
# calculate the ionospheric range correction
ion_corr = rangeCorrection.ionospheric_correction(satinfo, svid, transmitTime, satinfo.lastpos)
# calculate the tropospheric range correction
tropo_corr = rangeCorrection.tropospheric_correction_sass(satinfo, svid, satinfo.lastpos)
# get total range correction
total_range_correction = ion_corr + tropo_corr
# correct the pseudo-range for the clock and atmospheric errors
prCorrected = prSmooth + (sat_clock_error + sat_group_delay)*util.speedOfLight - total_range_correction
# save the values in the satinfo object
satinfo.prMeasured[svid] = prMes
satinfo.prSmoothed[svid] = prSmooth
satinfo.prCorrected[svid] = prCorrected
satinfo.ionospheric_correction[svid] = ion_corr
satinfo.tropospheric_correction[svid] = tropo_corr
satinfo.satellite_clock_error[svid] = sat_clock_error
satinfo.satellite_group_delay[svid] = sat_group_delay
# if we got at least 4 satellites then calculate a position
if len(satinfo.satpos) < 4:
return None
posestimate = positionLeastSquares(satinfo)
satinfo.position_sum += posestimate
satinfo.position_count += 1
satinfo.average_position = satinfo.position_sum / satinfo.position_count
satinfo.position_estimate = posestimate
for svid in satinfo.prCorrected:
if satinfo.reference_position is not None:
satinfo.geometricRange[svid] = satinfo.reference_position.distance(satinfo.satpos[svid])
elif satinfo.receiver_position is not None:
satinfo.geometricRange[svid] = satinfo.receiver_position.distance(satinfo.satpos[svid])
else:
satinfo.geometricRange[svid] = satinfo.average_position.distance(satinfo.satpos[svid])
return posestimate
def calculatePrCorrections(satinfo):
raw = satinfo.raw
satinfo.reset()
errset = {}
for svid in raw.prMeasured:
if not satinfo.valid(svid):
# we don't have ephemeris data for this space vehicle
#print("not valid")
continue
# get the ephemeris and pseudo-range for this space vehicle
ephemeris = satinfo.ephemeris[svid]
prMes = raw.prMeasured[svid]
prSmooth = satinfo.smooth.prSmoothed[svid]
# calculate the time of flight for this pseudo range
tof = prSmooth / util.speedOfLight
# assume the time_of_week is the exact receiver time of week that the message arrived.
# subtract the time of flight to get the satellite transmit time
transmitTime = raw.time_of_week - tof
timesec = util.gpsTimeToTime(raw.gps_week, raw.time_of_week)
# calculate the satellite position at the transmitTime
satPosition.satPosition(satinfo, svid, transmitTime)
Trel = satinfo.satpos[svid].extra
# correct for earths rotation in the time it took the messages to get to the receiver
satPosition.correctPosition(satinfo, svid, tof)
# calculate satellite azimuth and elevation
satPosition.calculateAzimuthElevation(satinfo, svid, satinfo.lastpos)
# calculate the satellite clock correction
sat_clock_error = rangeCorrection.sv_clock_correction(
satinfo, svid, transmitTime, Trel)
# calculate the satellite group delay
sat_group_delay = -satinfo.ephemeris[svid].Tgd
# calculate the ionospheric range correction
ion_corr = rangeCorrection.ionospheric_correction(
satinfo, svid, transmitTime, satinfo.lastpos)
# calculate the tropospheric range correction
tropo_corr = rangeCorrection.tropospheric_correction_sass(
satinfo, svid, satinfo.lastpos)
# get total range correction
total_range_correction = ion_corr + tropo_corr
errset[svid] = -total_range_correction
# correct the pseudo-range for the clock and atmospheric errors
prCorrected = prSmooth + (
sat_clock_error +
sat_group_delay) * util.speedOfLight - total_range_correction
# save the values in the satinfo object
satinfo.prMeasured[svid] = prMes
satinfo.prSmoothed[svid] = prSmooth
satinfo.prCorrected[svid] = prCorrected
satinfo.ionospheric_correction[svid] = ion_corr
satinfo.tropospheric_correction[svid] = tropo_corr
satinfo.satellite_clock_error[svid] = sat_clock_error
satinfo.satellite_group_delay[svid] = sat_group_delay
save_satlog(raw.time_of_week, errset)
# Read GPS messages, if any
while True:
msg = dev.receive_message()
if (msg is None):
pass#break
else:
if msg.name() == 'TIM_TM2':
#print('Got TM2 message')
try:
msg.unpack()
timestring = '$HIT,'
timestring += str(msg.count)
timestring += ','
timestring += str(datetime.datetime.utcnow())
timestring += ','
timestring += str(datetime.datetime.utcfromtimestamp(util.gpsTimeToTime(msg.wnR, 1.0e-3*msg.towMsR)))
print(timestring)
sys.stdout.flush()
osc.write("MENU:STOP")
time.sleep(1)
osc.write(':STORage:CAPTure')
time.sleep(2)
#osc.write(':STORage:SAVECH1,UDISK')
#time.sleep(5)
osc.write("MENU:RUN")
#time.sleep(.1s)
except ublox.UBloxError as e:
print(e)
def RTCMType1_step(self, throttle=True):
gpssec = util.gpsTimeToTime(self.gps_week, self.time_of_week)
if gpssec < self.last_type1_time + self.type1_send_time and throttle:
return ''
self.last_type1_time = gpssec
self.reset()
tow = self.time_of_week
deltat = tow - self.last_time_of_week
errors = {}
rates = {}
for svid in self.error_history:
errors[svid] = sum(self.error_history[svid])/float(len(self.error_history[svid]))
if svid in self.last_errors and deltat > 0:
rates[svid] = (errors[svid] - self.last_errors[svid]) / deltat
else:
rates[svid] = 0
msgsatid = []
msgprc = []
msgprrc = []
msgiode = []
msgudre = []
scalefactors = []
for svid in self.error_history:
if not svid in self.iode:
continue
prc = int(round(errors[svid]/0.02))
prrc = int(round(rates[svid]/0.002))
sf = 0
while prc > 32767 or prc < -32768:
sf += 1
prc = (prc + 8) // 16
if sf > 1:
# skip satellites if we can't represent the error in the
# number of bits allowed
continue
if sf == 1:
prrc = (prrc + 8) // 16
prrc = min(prrc, 127)
prrc = max(prrc, -128)
msgsatid.append(svid)
msgprc.append(prc)
msgprrc.append(prrc)
msgiode.append(self.iode[svid])
scalefactors.append(sf)
msgsatcnt = len(msgsatid)
if msgsatcnt == 0:
return ''
# clear the history
self.last_errors = errors.copy()
if self.history_length == 0:
self.error_history = {}
else:
for svid in self.error_history:
while len(self.error_history[svid]) > self.history_length:
self.error_history[svid].pop(0)
self.last_time_of_week = tow
rtcmzcount = self.modZCount()
# first part of header
self.addbits(8, 0x66) # header id
self.addbits(6, 1) # msg type 1
self.addbits(10, self.stationID)
# second part of header
self.addbits(13, rtcmzcount) # z-count
self.addbits(3, self.rtcmseq) # seq no.
self.rtcmseq = (self.rtcmseq + 1) % 8
# now compute the word length of the message
# each word contains 24 bits of data, plus 6 bits of parity
bitlength = msgsatcnt * 40
wordlength = bitlength // 24
if (bitlength % 24) != 0:
wordlength += 1
self.addbits(5, wordlength)
# health bits - mark as healthy
self.addbits(3, 0)
for i in range(msgsatcnt):
self.addbits(1, scalefactors[i])
self.addbits(2, 0) # UDRE
self.addbits(5, msgsatid[i]) # sat id no
# we split the prc into two 8-bit bytes, because an RTCM word
# boundary can occur here
self.addbits(8, msgprc[i] >> 8) # prc hob
self.addbits(8, msgprc[i] & 0xff) # prc lob
self.addbits(8, msgprrc[i]) # prcc
self.addbits(8, msgiode[i]) # IODE
while self.rtcbits != 0:
self.addbits(8, 0xAA) # pad unused bits with 0xAA
#print("MSG: bitlength=%u wordlength=%u len=%u" % (bitlength, wordlength, len(self.buf)))
return self.buf + "\r\n"
sys.stdout.write('.')
sys.stdout.flush()
if msg.name() == 'TIM_TM2':
try:
msg.unpack()
if (count_initialized == 0):
count_start = msg.count - 1
last_count = msg.count
first_towMsR = msg.towMsR
count_initialized = 1
#print('First counter value: ' + str(count_start))
if (msg.count < count_start):
count_base += 65535
#timestring = datetime.datetime.strftime("%d %b %Y %H:%M:%S.%f", datetime.datetime.utcfromtimestamp(util.gpsTimeToTime(msg.wnR, 1.0e-3*msg.towMsR)))
tm2_time = datetime.datetime.utcfromtimestamp(
util.gpsTimeToTime(msg.wnR, 1.0e-3 * msg.towMsR))
#txt = "Counter = %5d Zero = %5d Diff = %5d %s\n" % (msg.count-count_start, count_start, msg.count-last_count, timestring)
tm2_delta = tm2_time - pcnow
txt = "TM2 %26s PC %26s delta(TM2-PC) %9.6f" % (
tm2_time, str(pcnow), float(tm2_delta.total_seconds()))
print txt
last_count = msg.count
except ublox.UBloxError as e:
print(e)
if msg.name() == 'NAV_TIMEUTC':
try:
#print(str(msg))
msg.unpack()
#print "GPS %04d-%02d-%02d %02d:%02d:%02d.%09d" % (msg.year, msg.month, msg.day, msg.hour, msg.min, msg.sec, msg.nano)
gpstime = datetime.datetime(
msg.year, msg.month, msg.day, msg.hour, msg.min,
else:
if (msg.count > last_count):
timestring = '$HIT,'
# Edges since first count
timestring += str(msg.count - first_count)
timestring += ','
# Edges in this run
timestring += str(msg.count - last_count)
timestring += ','
# Computer time
timestring += str(datetime.datetime.utcnow())
timestring += ','
# This provides the rising edge time to microsecond precision
timestring += str(
util.gpsTimeToTime(
msg.wnR,
1.0e-3 * msg.towMsR + 1.0e-9 * msg.towSubMsR))
timestring += ','
timestring += str(
datetime.datetime.utcfromtimestamp(
util.gpsTimeToTime(
msg.wnR,
1.0e-3 * msg.towMsR + 1.0e-9 * msg.towSubMsR)))
#timestring += str(datetime.datetime.utcfromtimestamp(util.gpsTimeToTime(msg.wnR, 1.0e6*(msg.towMsR % 1000 + msg.SubMsR)))
timestring += ','
# Tow sub millisecond fraction, in nanoseconds
timestring += str(msg.towMsR)
timestring += ','
# Tow sub millisecond fraction, in nanoseconds
timestring += str(msg.towSubMsR)
print(timestring)
def calculatePrCorrections(satinfo):
raw = satinfo.raw
satinfo.reset()
errset={}
for svid in raw.prMeasured:
if not satinfo.valid(svid):
# we don't have ephemeris data for this space vehicle
#print("not valid")
continue
# get the ephemeris and pseudo-range for this space vehicle
ephemeris = satinfo.ephemeris[svid]
prMes = raw.prMeasured[svid]
prSmooth = satinfo.smooth.prSmoothed[svid]
# calculate the time of flight for this pseudo range
tof = prSmooth / util.speedOfLight
# assume the time_of_week is the exact receiver time of week that the message arrived.
# subtract the time of flight to get the satellite transmit time
transmitTime = raw.time_of_week - tof
timesec = util.gpsTimeToTime(raw.gps_week, raw.time_of_week)
# calculate the satellite position at the transmitTime
satPosition.satPosition(satinfo, svid, transmitTime)
Trel = satinfo.satpos[svid].extra
# correct for earths rotation in the time it took the messages to get to the receiver
satPosition.correctPosition(satinfo, svid, tof)
# calculate satellite azimuth and elevation
satPosition.calculateAzimuthElevation(satinfo, svid, satinfo.lastpos)
# calculate the satellite clock correction
sat_clock_error = rangeCorrection.sv_clock_correction(satinfo, svid, transmitTime, Trel)
# calculate the satellite group delay
sat_group_delay = -satinfo.ephemeris[svid].Tgd
# calculate the ionospheric range correction
ion_corr = rangeCorrection.ionospheric_correction(satinfo, svid, transmitTime, satinfo.lastpos)
# calculate the tropospheric range correction
tropo_corr = rangeCorrection.tropospheric_correction_sass(satinfo, svid, satinfo.lastpos)
# get total range correction
total_range_correction = ion_corr + tropo_corr
errset[svid]=-total_range_correction
# correct the pseudo-range for the clock and atmospheric errors
prCorrected = prSmooth + (sat_clock_error + sat_group_delay)*util.speedOfLight - total_range_correction
# save the values in the satinfo object
satinfo.prMeasured[svid] = prMes
satinfo.prSmoothed[svid] = prSmooth
satinfo.prCorrected[svid] = prCorrected
satinfo.ionospheric_correction[svid] = ion_corr
satinfo.tropospheric_correction[svid] = tropo_corr
satinfo.satellite_clock_error[svid] = sat_clock_error
satinfo.satellite_group_delay[svid] = sat_group_delay
save_satlog(raw.time_of_week, errset)
