def handle_device3(msg):
global rx3_pos
'''handle message from uncorrected rover GPS'''
if msg.name() == "NAV_POSECEF":
msg.unpack()
pos = util.PosVector(msg.ecefX*0.01, msg.ecefY*0.01, msg.ecefZ*0.01)
rx3_pos = pos
def calcRTCMPosition(self, satinfo, msgsatid, msgprc, scalefactors):
'''
calculate a position using the raw reference receiver data
and the generated RTCM data. This should be close to the reference
position if we are calculating the RTCM data correctly
'''
msgsatcnt = len(msgsatid)
pranges = {}
for i in range(msgsatcnt):
svid = msgsatid[i]
err = msgprc[i]*0.02
if scalefactors[i] == 1:
err *= 16.0
if not svid in satinfo.prSmoothed:
continue
pranges[svid] = satinfo.prSmoothed[svid] + satinfo.satellite_clock_error[svid]*util.speedOfLight
# pranges[svid] = satinfo.prMeasured[svid] + satinfo.satellite_clock_error[svid]*util.speedOfLight - (satinfo.tropospheric_correction[svid])
# pranges[svid] = satinfo.prMeasured[svid] + satinfo.satellite_clock_error[svid]*util.speedOfLight - (satinfo.ionospheric_correction[svid])
# pranges[svid] = satinfo.prMeasured[svid] + satinfo.satellite_clock_error[svid]*util.speedOfLight - (satinfo.tropospheric_correction[svid] + satinfo.ionospheric_correction[svid])
pranges[svid] += err
#print(svid, prc, err, satinfo.receiver_clock_error*util.speedOfLight, satinfo.satellite_clock_error[svid]*util.speedOfLight, satinfo.tropospheric_correction[svid], satinfo.ionospheric_correction[svid])
lastpos = satinfo.rtcm_position
if lastpos is None:
lastpos = util.PosVector(0,0,0)
if len(pranges) >= 4:
print pranges
print satinfo.prCorrected
satinfo.rtcm_position = positionEstimate.positionLeastSquares_ranges(satinfo, pranges, lastpos, 0)
def positionLeastSquares_ranges(satinfo,
pranges,
lastpos,
last_clock_error,
weights=None):
'''estimate ECEF position of receiver via least squares fit to satellite positions and pseudo-ranges
The weights dictionary is optional. If supplied, it is the weighting from 0 to 1 for each satellite.
A weight of 1 means it has more influence on the solution
'''
import scipy
from scipy import optimize
data = []
for svid in satinfo.satpos:
if svid in pranges:
if weights is not None:
weight = weights[svid]
else:
weight = 1.0
data.append((satinfo.satpos[svid], pranges[svid], weight))
p0 = [lastpos.X, lastpos.Y, lastpos.Z, last_clock_error]
p1, ier = optimize.leastsq(positionErrorFunction, p0[:], args=(data))
if not ier in [1, 2, 3, 4]:
raise RuntimeError("Unable to find solution")
# return position and clock error
return util.PosVector(p1[0], p1[1], p1[2], extra=p1[3])
def handle_device3(msg):
'''handle message from uncorrected rover GPS'''
if msg.name() == "NAV_POSECEF":
msg.unpack()
pos = util.PosVector(msg.ecefX * 0.01, msg.ecefY * 0.01,
msg.ecefZ * 0.01)
satinfo.recv3_position = pos
def regen_v2_type3():
if ref_pos is None:
return
msg = rtcm.RTCMType3_ext(itow, week, util.PosVector(*ref_pos))
if len(msg) > 0:
return msg
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:
# not enough information for a fix
return
if opts.plot:
plotter.plotPosition(pos, 0)
plotter.plotPosition(satinfo.average_position, 1)
import RTCMv2
rtcm = RTCMv2.generateRTCM2_Message1(satinfo)
rtcmfile.write(rtcm)
rtcm = RTCMv2.generateRTCM2_Message3(satinfo)
if len(rtcm) > 0:
rtcmfile.write(rtcm)
if 'NAV_POSECEF' in messages:
posecef = messages['NAV_POSECEF']
ourpos = util.PosVector(posecef.ecefX*0.01, posecef.ecefY*0.01, posecef.ecefZ*0.01)
posdiff = pos.distance(ourpos)
print("posdiff=%f pos=%s avg=%s %s" % (posdiff, pos.ToLLH(), satinfo.average_position.ToLLH(), time.ctime(satinfo.raw.gps_time)))
else:
print("pos=%s" % (pos.ToLLH()))
return pos
def handle_device2(msg):
'''handle message from rover GPS'''
global pos_count
if msg.name() == 'NAV_DGPS':
msg.unpack()
print("DGPS: age=%u numCh=%u pos_count=%u" %
(msg.age, msg.numCh, pos_count))
if msg.name() == "NAV_POSECEF":
msg.unpack()
pos = util.PosVector(msg.ecefX * 0.01, msg.ecefY * 0.01,
msg.ecefZ * 0.01)
satinfo.recv2_position = pos
if satinfo.average_position is None or satinfo.position_estimate is None:
return
print("-----------------")
display_diff("RECV1<->RECV2", satinfo.receiver_position, pos)
display_diff("RECV2<->AVG", satinfo.receiver_position,
satinfo.average_position)
display_diff("AVG<->RECV1", satinfo.average_position,
satinfo.receiver_position)
display_diff("AVG<->RECV2", satinfo.average_position, pos)
if satinfo.reference_position is not None:
display_diff("REF<->AVG", satinfo.reference_position,
satinfo.average_position)
display_diff("POS<->REF", satinfo.position_estimate,
satinfo.reference_position)
if satinfo.rtcm_position is not None:
display_diff("RTCM<->REF", satinfo.rtcm_position,
satinfo.reference_position)
display_diff("RTCM<->RECV2", satinfo.rtcm_position,
satinfo.recv2_position)
display_diff("RECV1<->REF", satinfo.receiver_position,
satinfo.reference_position)
display_diff("RECV2<->REF", satinfo.recv2_position,
satinfo.reference_position)
pos_count += 1
if satinfo.recv3_position is not None:
display_diff("RECV3<->REF", satinfo.recv3_position,
satinfo.reference_position)
errlog.write("%f %f %f %f\n" %
(satinfo.reference_position.distance(
satinfo.recv3_position),
satinfo.reference_position.distance(
satinfo.recv2_position),
satinfo.reference_position.distanceXY(
satinfo.recv3_position),
satinfo.reference_position.distanceXY(
satinfo.recv2_position)))
errlog.flush()
else:
errlog.write("%f %f %f %f\n" %
(satinfo.reference_position.distance(
satinfo.receiver_position),
satinfo.reference_position.distance(
satinfo.recv2_position),
satinfo.reference_position.distanceXY(
satinfo.receiver_position),
satinfo.reference_position.distanceXY(
satinfo.recv2_position)))
errlog.flush()
def satPosition(satinfo, svid, transmitTime):
if svid in satinfo.ephemeris:
eph = satinfo.ephemeris[svid]
if eph.is_valid():
satinfo.satpos[svid] = satPosition_raw(eph, svid, transmitTime)
return
satinfo.satpos[svid] = util.PosVector(0, 0, 0)
def __init__(self):
self.azimuth = {}
self.elevation = {}
self.lastpos = util.PosVector(0,0,0)
self.receiver_clock_error = 0
self.rtcm_bits = None
self.reset()
self.average_position = None
self.position_sum = util.PosVector(0,0,0)
self.position_count = 0
self.ephemeris = {}
self.visible_satellites = []
# the reference position given by the user, if any
self.reference_position = None
# the position reported by the reference receiver
self.receiver_position = None
# the position reported by the corrected rover
self.recv2_position = None
# the position reported by the uncorrected rover
self.recv3_position = None
# the position calculated from the reference receivers raw data
# and the generated RTCM data.
self.rtcm_position = None
# the last position calculated from smoothed pseudo ranges
self.position_estimate = None
#self.ephemeris = util.loadObject('ephemeris.dat')
#if self.ephemeris is None:
# self.ephemeris = {}
self.ephemeris = {}
#self.ionospheric = util.loadObject('ionospheric.dat')
#if self.ionospheric is None:
# self.ionospheric = {}
self.min_elevation = 5.0
self.min_quality = 6
self.smooth = prSmooth.prSmooth()
def clockErrorFunction(p, data):
"""error function for least squares position fit"""
pos = util.PosVector(*data[0])
recv_clockerr = p[0]
ret = []
for d in data[1:]:
satpos, prange, weight = d
dist = pos.distance(satpos)
ret.append((dist - (prange + util.speedOfLight * recv_clockerr)) * weight)
return ret
def p_yt_xt_mu(xt):
''' Return mean of Gaussian PDF for measurement yt given xt. Mean is at the ideal measurement'''
rxpos = util.PosVector(xt[0], xt[1], xt[2])
ideal = [0] * 32
for i in range(32):
if i in satinfo.satpos and i in satinfo.prCorrected:
ideal[i] = satinfo.satpos[i].distance(rxpos) + xt[3] * util.speedOfLight
#print(xt)
#print(numpy.array(ideal) - numpy.array(meas))
return numpy.array(ideal)
def positionErrorFunction(p, data):
'''error function for least squares position fit'''
pos = util.PosVector(p[0], p[1], p[2])
recv_clockerr = p[3]
ret = []
for d in data:
satpos, prange, weight = d
dist = pos.distance(satpos)
ret.append(
(dist - (prange + util.speedOfLight * recv_clockerr)) * weight)
return ret
def handle_device2(msg):
"""handle message from rover GPS"""
if msg.name() == 'NAV_DGPS':
msg.unpack()
print("DGPS: age=%u numCh=%u" % (msg.age, msg.numCh))
if msg.name() == "NAV_POSECEF":
msg.unpack()
rx2_pos = util.PosVector(msg.ecefX * 0.01, msg.ecefY * 0.01,
msg.ecefZ * 0.01)
print("-----------------")
display_diff("RECV1<->RECV2", rx1_pos, rx2_pos)
if dev3 is not None:
display_diff("RECV1<->RECV3", rx1_pos, rx3_pos)
errlog.write(
"%f %f %f %f\n" %
(rx1_pos.distance(rx3_pos), rx1_pos.distance(rx2_pos),
rx1_pos.distanceXY(rx3_pos), rx1_pos.distanceXY(rx2_pos)))
errlog.flush()
def handle_device1(msg):
"""handle message from reference GPS"""
global messages, satinfo, itow, week, rx1_pos, svid_seen
if msg.name() in ['NAV_SVINFO', 'NAV_SOL', 'AID_EPH']:
try:
msg.unpack()
except ublox.UBloxError as e:
print(e)
if msg.name() == 'NAV_SVINFO':
svinfo_to_rtcm(msg)
elif msg.name() == 'NAV_SOL':
itow = msg.iTOW * 0.001
week = msg.week
rx1_pos = util.PosVector(msg.ecefX / 100., msg.ecefY / 100.,
msg.ecefZ / 100.)
elif msg.name() == 'AID_EPH':
eph = ephemeris.EphemerisData(msg)
if eph.valid:
svid_iode[eph.svid] = eph.iode
def decode_1006(pkt):
global ref_pos
staid = pkt.read(12).uint
# Only set reference station location if it's the one used by
# the observations
if staid != statid:
return
itrf = pkt.read(6).uint
pkt.read(4)
ref_x = pkt.read(38).int * 0.0001
pkt.read(2)
ref_y = pkt.read(38).int * 0.0001
pkt.read(2)
ref_z = pkt.read(38).int * 0.0001
anth = pkt.read(16).uint * 0.0001
ref_pos = [ref_x, ref_y, ref_z]
print(ref_pos)
print(util.PosVector(*ref_pos).ToLLH())
dev1.configure_poll(ublox.CLASS_AID, ublox.MSG_AID_EPH,
struct.pack('<B', sv))
svid_seen[sv] = tnow
last_msg1_time = time.time()
last_msg2_time = time.time()
last_msg3_time = time.time()
messages = {}
satinfo = satelliteData.SatelliteData()
if opts.reference is not None:
satinfo.reference_position = util.ParseLLH(opts.reference).ToECEF()
elif opts.ecef_reference is not None:
satinfo.reference_position = util.PosVector(
*opts.ecef_reference.split(','))
else:
satinfo.reference_position = None
satinfo.min_elevation = opts.minelevation
satinfo.min_quality = opts.minquality
def handle_device1(msg):
'''handle message from reference GPS'''
global messages, satinfo
if msg.name() in [
'RXM_RAW', 'NAV_POSECEF', 'RXM_SFRB', 'RXM_RAW', 'AID_EPH',
'NAV_POSECEF'
]:
def add_NAV_POSECEF(self, msg):
"""add a NAV_POSECEF message"""
self.receiver_position = util.PosVector(msg.ecefX*0.01, msg.ecefY*0.01, msg.ecefZ*0.01)
dev3.set_preferred_dynamic_model(opts.dynmodel3)
dev2.set_preferred_dgps_timeout(60)
# enable PPP on the ground side if we can
dev1.set_preferred_usePPP(opts.usePPP)
dev2.set_preferred_usePPP(False)
if dev3 is not None:
dev3.set_preferred_usePPP(False)
rtcmfile = open('rtcm2.dat', mode='wb')
rtcm_gen = RTCMv2.RTCMBits()
itow = 0
week = 0
rx1_pos = util.PosVector(0, 0, 0)
rx2_pos = util.PosVector(0, 0, 0)
rx3_pos = util.PosVector(0, 0, 0)
svid_seen = {}
svid_iode = {}
def svinfo_to_rtcm(svinfo):
resid = {}
for i in range(msg.numCh):
sv = msg.recs[i].svid
tnow = time.time()
if not sv in svid_seen or tnow > svid_seen[sv] + 30:
dev1.configure_poll(ublox.CLASS_AID, ublox.MSG_AID_EPH,
def regen_v2_type1():
if ref_pos is None:
return
errset = {}
pranges = {}
for svid in prs:
if svid not in eph:
#print("Don't have ephemeris for {}, only {}".format(svid, eph.keys()))
continue
toc = eph[svid].toc
tof = prs[svid] / 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 = itow - tof
T = util.correctWeeklyTime(transmitTime - toc)
satpos = satPosition.satPosition_raw(eph[svid], svid, transmitTime)
Trel = satpos.extra
satPosition.correctPosition_raw(satpos, tof)
geo = satpos.distance(util.PosVector(*ref_pos))
dTclck = eph[svid].af0 + eph[svid].af1 * T + eph[
svid].af2 * T * T + Trel - eph[svid].Tgd
# Incoming PR is already corrected for receiver clock bias
prAdjusted = prs[svid] + dTclck * util.speedOfLight
errset[svid] = geo - prAdjusted
pranges[svid] = prAdjusted
save_satlog(itow, errset)
if correct_rxclk:
rxerr = positionEstimate.clockLeastSquares_ranges(
eph, pranges, itow, ref_pos, 0)
if rxerr is None:
return
rxerr *= util.speedOfLight
for svid in errset:
errset[svid] += rxerr
pranges[svid] += rxerr
rxerr = positionEstimate.clockLeastSquares_ranges(
eph, pranges, itow, ref_pos, 0) * util.speedOfLight
print("Residual RX clock error {}".format(rxerr))
iode = {}
for svid in eph:
iode[svid] = eph[svid].iode
msg = rtcm.RTCMType1_ext(errset, itow, week, iode)
if len(msg) > 0:
return msg
if len(rtcm) > 0:
rtcmfile.write(rtcm)
if 'NAV_POSECEF' in messages:
posecef = messages['NAV_POSECEF']
ourpos = util.PosVector(posecef.ecefX*0.01, posecef.ecefY*0.01, posecef.ecefZ*0.01)
posdiff = pos.distance(ourpos)
print("posdiff=%f pos=%s avg=%s %s" % (posdiff, pos.ToLLH(), satinfo.average_position.ToLLH(), time.ctime(satinfo.raw.gps_time)))
else:
print("pos=%s" % (pos.ToLLH()))
return pos
satinfo = satelliteData.SatelliteData()
messages = {}
pos_sum = util.PosVector(0,0,0)
pos_count = 0
while True:
'''process the ublox messages, extracting the ones we need for the position'''
msg = dev.receive_message()
if msg is None:
break
if msg.name() in [ 'RXM_RAW', 'NAV_POSECEF', 'RXM_SFRB', 'RXM_RAW', 'AID_EPH' ]:
try:
msg.unpack()
messages[msg.name()] = msg
satinfo.add_message(msg)
except ublox.UBloxError as e:
print(e)
if msg.name() == 'RXM_RAW':
def satPosition_raw(eph, svid, transmitTime):
"""calculate satellite position
Based upon http://home-2.worldonline.nl/~samsvl/stdalone.pas
This fills in the satpos element of the satinfo object
"""
from math import sqrt, atan, sin, cos
# WGS 84 value of earth's univ. grav. par.
mu = 3.986005E+14
# WGS 84 value of earth's rotation rate
Wedot = 7.2921151467E-5
# relativistic correction term constant
F = -4.442807633E-10
pi = util.gpsPi
try:
Crs = eph.crs
dn = eph.deltaN
M0 = eph.M0
Cuc = eph.cuc
ec = eph.ecc
Cus = eph.cus
A = eph.A
Toe = eph.toe
Cic = eph.cic
W0 = eph.omega0
Cis = eph.cis
i0 = eph.i0
Crc = eph.crc
w = eph.omega
Wdot = eph.omega_dot
idot = eph.idot
except AttributeError:
# The given ephemeride doesn't contain the correct fields
return None
T = transmitTime - Toe
if T > 302400:
T = T - 604800
if T < -302400:
T = T + 604800
n0 = sqrt(mu / (A * A * A))
n = n0 + dn
M = M0 + n * T
E = M
for ii in range(20):
Eold = E
E = M + ec * sin(E)
if abs(E - Eold) < 1.0e-12:
break
else:
print("WARNING: Kepler Eqn didn't converge for sat {} (last step {})".format(svid, E - Eold))
snu = sqrt(1 - ec * ec) * sin(E) / (1 - ec * cos(E))
cnu = (cos(E) - ec) / (1 - ec * cos(E))
if cnu == 0:
nu = pi / 2 * snu / abs(snu)
elif (snu == 0) and (cnu > 0):
nu = 0
elif (snu == 0) and (cnu < 0):
nu = pi
else:
nu = atan(snu / cnu)
if cnu < 0:
nu += pi * snu / abs(snu)
phi = nu + w
du = Cuc * cos(2 * phi) + Cus * sin(2 * phi)
dr = Crc * cos(2 * phi) + Crs * sin(2 * phi)
di = Cic * cos(2 * phi) + Cis * sin(2 * phi)
u = phi + du
r = A * (1 - ec * cos(E)) + dr
i = i0 + idot * T + di
Xdash = r * cos(u)
Ydash = r * sin(u)
Wc = W0 + (Wdot - Wedot) * T - Wedot * Toe
satpos = util.PosVector(
Xdash * cos(Wc) - Ydash * cos(i) * sin(Wc),
Xdash * sin(Wc) + Ydash * cos(i) * cos(Wc),
Ydash * sin(i))
# relativistic correction term
satpos.extra = F * ec * sqrt(A) * sin(E)
return satpos
def satPosition_raw(eph, svid, transmitTime):
'''calculate satellite position
Based upon http://home-2.worldonline.nl/~samsvl/stdalone.pas
This fills in the satpos element of the satinfo object
'''
from math import sqrt, atan, sin, cos, atan2
# WGS 84 value of earth's univ. grav. par.
mu = 3.986005E+14
# WGS 84 value of earth's rotation rate
Wedot = 7.2921151467E-5
# relativistic correction term constant
F = -4.442807633E-10
pi = util.gpsPi
c = 299792458
try:
Crs = eph.crs
dn = eph.deltaN
M0 = eph.M0
Cuc = eph.cuc
ec = eph.ecc
Cus = eph.cus
A = eph.A
Toe = eph.toe
Cic = eph.cic
W0 = eph.omega0
Cis = eph.cis
i0 = eph.i0
Crc = eph.crc
w = eph.omega
Wdot = eph.omega_dot
idot = eph.idot
Tgd = eph.Tgd
Toc = eph.toc
af0 = eph.af0
af1 = eph.af1
af2 = eph.af2
except AttributeError:
# The given ephemeride doesn't contain the correct fields
return None
# Clock correction (without relativity)
t_k = (transmitTime - Toe)
#Navstar:
# Thus, the user who utilizes the L1 P(Y) signal only shall modify the code phase offset
# in accordance with paragraph 20.3.3.3.3.1 with the equation
for ii in range(3):
t_k -= af0 + af1 * t_k + af2 * t_k * t_k
# Week crossover check
#T = t_k - dt_sv
dt_sv = af0 + af1 * t_k + af2 * t_k * t_k
T = t_k - dt_sv
if T > 302400:
T = T - 604800
if T < -302400:
T = T + 604800
n0 = sqrt(mu / (A * A * A))
n = n0 + dn
M = M0 + n * T
E = M
for ii in range(22):
E = E - (E - ec * sin(E) - M) / (1 - ec * cos(E))
# relativistic correction term
#T = T - t_r
nu = atan2(sqrt(1 - ec * ec) * sin(E), cos(E) - ec)
#snu = sqrt(1 - ec*ec) * sin(E) / (1 - ec*cos(E))
#cnu = (cos(E) - ec) / (1 - ec*cos(E))
#if cnu == 0:
# nu = pi/2 * snu / abs(snu)
#elif (snu == 0) and (cnu > 0):
# nu = 0
#elif (snu == 0) and (cnu < 0):
# nu = pi
#else:
# nu = atan(snu/cnu)
# if cnu < 0:
# nu += pi * snu / abs(snu)
phi = nu + w
du = Cuc * cos(2 * phi) + Cus * sin(2 * phi)
dr = Crc * cos(2 * phi) + Crs * sin(2 * phi)
di = Cic * cos(2 * phi) + Cis * sin(2 * phi)
u = phi + du
r = A * (1 - ec * cos(E)) + dr
i = i0 + idot * T + di
Xdash = r * cos(u)
Ydash = r * sin(u)
Wc = W0 + (Wdot - Wedot) * T - Wedot * Toe
satpos = util.PosVector(Xdash * cos(Wc) - Ydash * cos(i) * sin(Wc),
Xdash * sin(Wc) + Ydash * cos(i) * cos(Wc),
Ydash * sin(i))
satpos.extra = dt_sv - 2 * sqrt(mu * A) * ec * sin(E) / (c * c)
return satpos
