def position_estimate(messages, satinfo):
'''process raw messages to calculate position
'''
rxm_raw = messages['RXM_RAW']
pos = positionEstimate.positionEstimate(satinfo)
if pos is None:
# not enough information for a fix
return
rtcm = RTCMv2.generateRTCM2_Message1(satinfo)
if len(rtcm) != 0:
print("generated type 1")
rtcmfile.write(rtcm)
if not opts.nortcm:
dev2.write(rtcm)
rtcm = RTCMv2.generateRTCM2_Message3(satinfo)
if len(rtcm) != 0:
print("generated type 3")
rtcmfile.write(rtcm)
if not opts.nortcm:
dev2.write(rtcm)
errset = {}
for svid in satinfo.rtcm_bits.error_history:
errset[svid] = satinfo.rtcm_bits.error_history[svid][-1]
save_satlog(rxm_raw.iTOW, errset)
return pos
def position_estimate(messages, satinfo):
'''process raw messages to calculate position
'''
rxm_raw = messages['RXM_RAW']
pos = positionEstimate.positionEstimate(satinfo)
if pos is None:
# not enough information for a fix
return
rtcm = RTCMv2.generateRTCM2_Message1(satinfo)
if len(rtcm) != 0:
print("generated type 1")
rtcmfile.write(rtcm)
if not opts.nortcm:
dev2.write(rtcm)
rtcm = RTCMv2.generateRTCM2_Message3(satinfo)
if len(rtcm) != 0:
print("generated type 3")
rtcmfile.write(rtcm)
if not opts.nortcm:
dev2.write(rtcm)
errset = {}
for svid in satinfo.rtcm_bits.error_history:
errset[svid] = satinfo.rtcm_bits.error_history[svid][-1]
save_satlog(rxm_raw.iTOW, errset)
return pos
def position_estimate(messages, satinfo):
'''process raw messages to calculate position
'''
rxm_raw = messages['RXM_RAW']
pos = positionEstimate.positionEstimate(satinfo)
if pos is None:
# not enough information for a fix
return
rtcm = RTCMv2.generateRTCM2_Message1(satinfo, maxsats=10)
if len(rtcm) != 0:
#print(rtcm)
rtcmfile.write(rtcm)
port.sendto(rtcm[:-2], (opts.udp_addr, opts.udp_port))
rtcm = RTCMv2.generateRTCM2_Message3(satinfo)
if len(rtcm) != 0:
print(rtcm)
rtcmfile.write(rtcm)
port.sendto(rtcm[:-2], (opts.udp_addr, opts.udp_port))
errset = {}
for svid in satinfo.rtcm_bits.error_history:
errset[svid] = satinfo.rtcm_bits.error_history[svid][-1]
save_satlog(rxm_raw.iTOW, errset)
print(satinfo.receiver_position)
return pos
def position_estimate(messages, satinfo):
'''process raw messages to calculate position
'''
rxm_raw = messages['RXM_RAW']
pos = positionEstimate.positionEstimate(satinfo)
if pos is None:
# not enough information for a fix
return
rtcm = RTCMv2.generateRTCM2_Message1(satinfo, maxsats=10)
if len(rtcm) != 0:
#print(rtcm)
rtcmfile.write(rtcm)
port.sendto(rtcm[:-2], (opts.udp_addr, opts.udp_port))
rtcm = RTCMv2.generateRTCM2_Message3(satinfo)
if len(rtcm) != 0:
print(rtcm)
rtcmfile.write(rtcm)
port.sendto(rtcm[:-2], (opts.udp_addr, opts.udp_port))
errset = {}
for svid in satinfo.rtcm_bits.error_history:
errset[svid] = satinfo.rtcm_bits.error_history[svid][-1]
save_satlog(rxm_raw.iTOW, errset)
print(satinfo.receiver_position)
return pos
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 position_estimate(messages, satinfo):
'''process raw messages to calculate position
'''
rxm_raw = messages['RXM_RAW']
pos = positionEstimate.positionEstimate(satinfo)
if pos is None:
# not enough information for a fix
return
rtcm = RTCMv2.generateRTCM2_Message1(satinfo)
if len(rtcm) != 0:
print("generated type 1")
rtcmfile.write(rtcm)
if not opts.nortcm:
dev2.write(rtcm)
rtcm = RTCMv2.generateRTCM2_Message3(satinfo)
if len(rtcm) != 0:
print("generated type 3")
rtcmfile.write(rtcm)
if not opts.nortcm:
dev2.write(rtcm)
return pos
]
# Globals required to build v2 messages
corr_set = {}
statid = 0 #initially only support 1 reference station
eph = {}
prs = {}
week = 0
itow = 0
ref_pos = None
correct_rxclk = True
rtcm = RTCMv2.RTCMBits()
rtcm.type1_send_time = 0
rtcm.type3_send_time = 0
logfile = time.strftime('satlog-%y%m%d-%H%M.txt')
class DynamicEph:
pass
satlog = None
def save_satlog(t, errset):
global satlog
# GPS to use a highly dynamic model
dev1.set_preferred_dynamic_model(opts.dynmodel1)
dev2.set_preferred_dynamic_model(opts.dynmodel2)
if dev3 is not None:
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):
