class receiver(gr.hier_block2):
def __init__(self):
gr.hier_block2.__init__(self, "ofdm_tx",
gr.io_signature(1, 1, gr.sizeof_gr_complex),
gr.io_signature(1, 1, gr.sizeof_char))
symbol_settings = para.ofdm_symbol()
self.constellationP = helpers.get_constellation(settings.PAYLOAD_BPS)
self.constellationH = helpers.get_constellation(settings.HEADER_BPS)
detector = digital.ofdm_sync_sc_cfb(symbol_settings.get_fft_length(), symbol_settings.get_cp_length())
delayer = blocks.delay(gr.sizeof_gr_complex, symbol_settings.get_time_length_of_symbol)
oscillator = analog.frequency_modulator_fc(-2.0 / symbol_settings.get_fft_length())
splitter = digital.header_payload_demux(
3,
symbol_settings.get_fft_length(), symbol_settings.get_cp_length(),
settings.LENGTH_TAG_KEY,
"",
True
)
mixer = blocks.multiply_cc()
header_fft = fft.fft_vcc(symbol_settings.get_fft_length(),, True, (), True)
chanest = digital.ofdm_chanest_vcvc(symbol_settings._generate_sync_word_one(),symbol_settings._generate_sync_word_two(), 1)
header_equalizer = digital.ofdm_equalizer_simpledfe(
symbol_settings.get_fft_length(),,
self.constellationH.base(),
symbol_settings.get_carrier_tones(),
symbol_settings.get_pilot_tones(),
symbol_settings._generate_pilot_symbols(),
symbols_skipped=0,
)
self.connect(self,detector)
self.connect(self,delay, (mixer,0), (splitter,0))
self.connect((detector,0), oscillator, (mixer,1))
self.connect((detector,1),(splitter,1))
def Fx_pos(lista, no_weight=no_weight): #x, y, z, bc, bg
rows = []
for meas in measurements:
if signal in meas.observables_final and np.isfinite(meas.observables_final[signal]):# se as posições do satélite ja foram corrigidas, então não precisa corrigir mais
pr = meas.observables_final[signal]
sat_pos = meas.sat_pos_final
theta = 0
elif signal in meas.observables and np.isfinite(meas.observables[signal]) and meas.processed:# se as posições do satelite não foram corrigidas, ele corrige a posição do satelite aqui
pr = meas.observables[signal]
pr += meas.sat_clock_err * constants.SPEED_OF_LIGHT
sat_pos = meas.sat_pos
theta = constants.EARTH_ROTATION_RATE * (pr - lista[4]) / constants.SPEED_OF_LIGHT # theta é o quanto a posição do satelite foi deslocada pela rotação da terra durante a transmissão do sinal = rotacao * (delta T)
else:
if not no_nans:
rows.append(np.nan)
continue
if no_weight:
weight = 1
else:
weight = (1 / meas.observables_std[signal])
if get_constellation(meas.prn) == 'GLONASS':
rows.append(weight * (np.sqrt( # distancia do usuario para o satelite(pag 84 formula 9a) - (pseudorange - bias do clock local) = erro(ver formula na pagina do wikipedia na seção do least squares)
(sat_pos[0] * np.cos(theta) + sat_pos[1] * np.sin(theta) - lista[0])**2 + # sat.x - chute.x
(sat_pos[1] * np.cos(theta) - sat_pos[0] * np.sin(theta) - lista[1])**2 +# sat.y - chute.y
(sat_pos[2] - lista[2])**2) - (pr - lista[3] - lista[4]))) # sat.z - chute.z
elif get_constellation(meas.prn) == 'GPS':
rows.append(weight * (np.sqrt(# equivalente ao de cima, mas com um bias a menos sendo necessario
(sat_pos[0] * np.cos(theta) + sat_pos[1] * np.sin(theta) - lista[0])**2 +
(sat_pos[1] * np.cos(theta) - sat_pos[0] * np.sin(theta) - lista[1])**2 +
(sat_pos[2] - lista[2])**2) - (pr - lista[3])))
return rows
def get_frequency(self, prn, time, signal='C1C'):
if get_constellation(prn) == 'GPS':
if signal[1] == '1':
return constants.GPS_L1
elif signal[1] == '2':
return constants.GPS_L2
else:
raise NotImplementedError('Dont know this frequency')
elif get_constellation(prn) == 'GLONASS':
n = self.get_glonass_channel(prn, time)
if signal[1] == '1':
return constants.GLONASS_L1 + n * constants.GLONASS_L1_DELTA
if signal[1] == '2':
return constants.GLONASS_L2 + n * constants.GLONASS_L2_DELTA
else:
raise NotImplementedError('Dont know this frequency')
def parse_dcbs(file_name, SUPPORTED_CONSTELLATIONS):
with open(file_name, 'r+') as DCB_file:
contents = DCB_file.readlines()
data_started = False
dcbs_dict = {}
for line in contents:
if not data_started:
if line[1:4] == 'DSB':
data_started = True
else:
continue
line_components = line.split()
if len(line_components[2]) < 3:
break
prn = line_components[2]
if get_constellation(prn) not in SUPPORTED_CONSTELLATIONS:
continue
dcb_type = line_components[3] + '_' + line_components[4]
epoch = GPSTime.from_datetime(datetime.strptime(line_components[5], '%Y:%j:%f')) + 12*SECS_IN_HR
if prn not in dcbs_dict:
dcbs_dict[prn] = {}
dcbs_dict[prn][dcb_type] = float(line_components[8])
dcbs_dict[prn]['epoch'] = epoch
dcbs = []
for prn in dcbs_dict:
dcbs.append(DCB(prn, dcbs_dict[prn]))
return dcbs
def Fx_pos(arg1, no_weight=no_weight):
x, y, z, bc, bg = arg1
rows = []
for meas in measurements:
if signal in meas.observables_final and np.isfinite(
meas.observables_final[signal]):
pr = meas.observables_final[signal]
sat_pos = meas.sat_pos_final
theta = 0
elif signal in meas.observables and np.isfinite(
meas.observables[signal]) and meas.processed:
pr = meas.observables[signal]
pr += meas.sat_clock_err * constants.SPEED_OF_LIGHT
sat_pos = meas.sat_pos
theta = constants.EARTH_ROTATION_RATE * (
pr - bc) / constants.SPEED_OF_LIGHT
else:
if not no_nans:
rows.append(np.nan)
continue
if no_weight:
weight = 1
else:
weight = (1 / meas.observables_std[signal])
if get_constellation(meas.prn) == 'GLONASS':
rows.append(weight *
(np.sqrt((sat_pos[0] * np.cos(theta) +
sat_pos[1] * np.sin(theta) - x)**2 +
(sat_pos[1] * np.cos(theta) -
sat_pos[0] * np.sin(theta) - y)**2 +
(sat_pos[2] - z)**2) - (pr - bc - bg)))
elif get_constellation(meas.prn) == 'GPS':
rows.append(weight *
(np.sqrt((sat_pos[0] * np.cos(theta) +
sat_pos[1] * np.sin(theta) - x)**2 +
(sat_pos[1] * np.cos(theta) -
sat_pos[0] * np.sin(theta) - y)**2 +
(sat_pos[2] - z)**2) - (pr - bc)))
return rows
def get_tgd_from_nav(self, prn, time):
if prn in self.bad_sats:
return None
if get_constellation(prn) not in self.valid_const:
return None
eph = self.get_nav(prn, time)
if eph:
return eph.get_tgd()
else:
return None
def get_sat_info(self, prn, time):
if prn in self.bad_sats:
return None
if get_constellation(prn) not in self.valid_const:
return None
if self.pull_orbit:
eph = self.get_orbit(prn, time)
else:
eph = self.get_nav(prn, time)
if eph:
return eph.get_sat_info(time)
else:
return None
def parse_sp3_orbits(file_names, SUPPORTED_CONSTELLATIONS):
ephems = []
data = {}
for file_name in file_names:
f = open(file_name, 'r')
while True:
line = f.readline()[:-1]
if not line:
break
# epoch header
if line[0:2] == '* ':
year = int(line[3:7])
month = int(line[8:10])
day = int(line[11:13])
hour = int(line[14:16])
minute = int(line[17:19])
second = int(float(line[20:31]))
epoch = GPSTime.from_datetime(
datetime(year, month, day, hour, minute, second))
# pos line
elif line[0] == 'P':
prn = line[1:4].replace(' ', '0')
if get_constellation(prn) not in SUPPORTED_CONSTELLATIONS:
continue
if prn not in data:
data[prn] = []
#TODO this is a crappy way to deal with overlapping ultra rapid
if len(data[prn]) < 1 or epoch - data[prn][-1][0] > 0:
parsed = [
epoch, 1e3 * float(line[4:18]),
1e3 * float(line[18:32]), 1e3 * float(line[32:46]),
1e-6 * float(line[46:60])
]
if (np.array(parsed[1:]) != 0).all():
data[prn].append(parsed)
f.close()
deg = 16
deg_t = 1
for prn in data:
# TODO Handle this properly
# Currently don't even bother with satellites that have unhealthy times
if (np.array(data[prn])[:, 4] > .99).any():
continue
for i in xrange(len(data[prn]) - deg):
times, x, y, z, clock = [], [], [], [], []
epoch = data[prn][i + deg / 2][0]
for j in xrange(deg + 1):
times.append(data[prn][i + j][0] - epoch)
x.append(data[prn][i + j][1])
y.append(data[prn][i + j][2])
z.append(data[prn][i + j][3])
clock.append(data[prn][i + j][4])
if (np.diff(times) != 900).any():
continue
poly_data = {}
poly_data['t0'] = epoch
poly_data['x'] = np.polyfit(times, x, deg)
poly_data['y'] = np.polyfit(times, y, deg)
poly_data['z'] = np.polyfit(times, z, deg)
poly_data['clock'] = [
(data[prn][i + deg / 2 + 1][4] - data[prn][i + deg / 2 - 1][4])
/ 1800, data[prn][i + deg / 2][4]
]
poly_data['deg'] = deg
poly_data['deg_t'] = deg_t
ephems.append(
PolyEphemeris(prn,
poly_data,
epoch,
healthy=True,
eph_type=EphemerisType.RAPID_ORBIT))
return ephems
def parse_dgps(station_id,
station_obs_file_path,
dog,
max_distance=100000,
required_constellations=['GPS']):
station_pos = get_station_position(station_id, cache_dir=dog.cache_dir)
obsdata = RINEXFile(station_obs_file_path)
measurements = raw.read_rinex_obs(obsdata)
# if not all constellations in first 100 epochs bail
detected_constellations = set()
for m in sum(measurements[:100], []):
detected_constellations.add(get_constellation(m.prn))
for constellation in required_constellations:
if constellation not in detected_constellations:
return None
proc_measurements = []
for measurement in measurements:
proc_measurements.append(raw.process_measurements(measurement,
dog=dog))
# sample at 30s
if len(proc_measurements) > 2880:
proc_measurements = proc_measurements[::int(
len(proc_measurements) / 2880)]
if len(proc_measurements) != 2880:
return None
station_delays = {}
n = len(proc_measurements)
for signal in ['C1C', 'C2P']:
times = []
station_delays[signal] = {}
for i, proc_measurement in enumerate(proc_measurements):
times.append(proc_measurement[0].recv_time)
Fx_pos = raw.pr_residual(proc_measurement, signal=signal)
residual = -np.array(Fx_pos(list(station_pos) + [0, 0]))
for j, m in enumerate(proc_measurement):
prn = m.prn
if prn not in station_delays[signal]:
station_delays[signal][prn] = np.nan * np.ones(n)
station_delays[signal][prn][i] = residual[j]
assert len(times) == n
# TODO crude way to get dgps station's clock errors,
# could this be biased? Only use GPS for convenience.
model_delays = {}
for prn in station_delays['C1C']:
if get_constellation(prn) == 'GPS':
model_delays[prn] = np.nan * np.zeros(n)
for i in xrange(n):
model_delays[prn][i] = dog.get_delay(prn,
times[i],
station_pos,
no_dgps=True)
station_clock_errs = np.zeros(n)
for i in xrange(n):
station_clock_errs[i] = np.nanmean([
(station_delays['C1C'][prn][i] - model_delays[prn][i])
for prn in model_delays
])
# remove clock errors and smooth out signal
for prn in station_delays['C1C']:
station_delays['C1C'][prn] = mean_filter(station_delays['C1C'][prn] -
station_clock_errs)
for prn in station_delays['C2P']:
station_delays['C2P'][
prn] = station_delays['C2P'][prn] - station_clock_errs
return DGPSDelay(station_id, station_pos, station_delays, times,
max_distance)
def __init__(self):
gr.hier_block2.__init__(self, "ofdm_tx",
gr.io_signature(1, 1, gr.sizeof_char),
gr.io_signature(1, 1, gr.sizeof_gr_complex))
self.constellationP = helpers.get_constellation(settings.PAYLOAD_BPS)
self.constellationH = helpers.get_constellation(settings.HEADER_BPS)
self.sync_words = [SYNC_ONE, SYNC_TWO]
#tag the received bytes
tagger_0 = tagger.blk(
settings.PAYLOAD_BPS, settings.LENGTH_TAG_KEY, 1, 0
) #divides the incoming byte stream into packets and append the meta data
"""
this part will handle the header stream
from the tagged data stream the header is derived, scrambled and modulated
"""
header_gen = ownHeader.generate_header_bb(
settings.LENGTH_TAG_KEY) #geneating the header
scramblerH = digital.digital.additive_scrambler_bb( #srambling the header bytes
0x8a,
settings.SCRAMBLED_SEED,
7,
0,
bits_per_byte=8,
reset_tag_key=settings.LENGTH_TAG_KEY)
"""
Modulating the data: first split the bytes into the size of the symbols (unpackerH) then transform the smaller groups of bits into the complex symbols
"""
unpackerH = blocks.repack_bits_bb(8, settings.HEADER_BPS,
settings.LENGTH_TAG_KEY)
modulatorH = digital.chunks_to_symbols_bc(self.constellationH.points())
"""
This part wil handle the data stream
first a error detection code is appended to stream, then the data is scrambled, and modulated to complex symbols
"""
crc = digital.crc32_bb(False,
settings.LENGTH_TAG_KEY) #error detection
scrambler = digital.digital.additive_scrambler_bb( #scrambling
0x8a,
settings.SCRAMBLED_SEED,
7,
0,
bits_per_byte=8,
reset_tag_key=settings.LENGTH_TAG_KEY)
"""
modulation: split into a group of bits with convenient size, transform the smaller groups into the complex symbols
"""
unpacker = blocks.repack_bits_bb(8, settings.PAYLOAD_BPS,
settings.LENGTH_TAG_KEY)
modulator = digital.chunks_to_symbols_bc(self.constellationP.points())
"""
This part will handle the full ofdm frame, both data, header and preamble
first the payload and the header stream is combined, then the complex symbols are allocated the carrier tones and the pilot symbols are append and the preambles is inserted
Thereafter, the FFT is calculated, at last the cyclic prefixes are inserted
"""
header_payload_mux = blocks.tagged_stream_mux(
itemsize=gr.sizeof_gr_complex * 1,
lengthtagname=settings.LENGTH_TAG_KEY,
tag_preserve_head_pos=1)
#self.connect(modulator, blocks.tag_debug(gr.sizeof_gr_complex, "tagsmod"))
#gerating ofdm signals
allocator = digital.ofdm_carrier_allocator_cvc(
symbol_settings.get_fft_length(),
occupied_carriers=symbol_settings.get_carrier_tones(),
pilot_carriers=symbol_settings.get_pilot_tones(),
pilot_symbols=SYM_PILOT,
sync_words=self.sync_words,
len_tag_key=settings.LENGTH_TAG_KEY)
#self.connect(allocator, blocks.tag_debug(gr.sizeof_gr_complex*symbol_settings.get_fft_length(), "tagsalocator"))
fft_ex = fft.fft_vcc(symbol_settings.get_fft_length(), False, (), True)
prefixer = digital.ofdm_cyclic_prefixer(
symbol_settings.get_fft_length(),
symbol_settings.get_cp_length() + symbol_settings.get_fft_length(),
0, settings.LENGTH_TAG_KEY)
print("All blocks initialized correctly")
self.connect(self, tagger_0)
self.connect(tagger_0, header_gen, scramblerH, unpackerH, modulatorH,
(header_payload_mux, 0))
self.connect(tagger_0, crc, scrambler, unpacker, modulator,
(header_payload_mux, 1))
self.connect(header_payload_mux, allocator, fft_ex, prefixer, self)
#self.connect(prefixer, blocks.file_sink(gr.sizeof_gr_complex,"tx.dat"))
print("All blocks properly connected")
def __init__(self):
gr.hier_block2.__init__(self, "ofdm_tx",
gr.io_signature(1, 1, gr.sizeof_gr_complex),
gr.io_signature(1, 1, gr.sizeof_char))
self.constellationP = helpers.get_constellation(settings.PAYLOAD_BPS)
self.constellationH = helpers.get_constellation(settings.HEADER_BPS)
detector = digital.ofdm_sync_sc_cfb(symbol_settings.get_fft_length(),
symbol_settings.get_cp_length(),
True)
self.connect((detector, 0),
blocks.file_sink(gr.sizeof_float, "offset.dat"))
delayer = blocks.delay(gr.sizeof_gr_complex,
symbol_settings.get_time_length_of_symbol() + 5)
oscillator = analog.frequency_modulator_fc(
-2.0 / symbol_settings.get_fft_length())
splitter = digital.header_payload_demux(
3,
symbol_settings.get_fft_length(),
symbol_settings.get_cp_length(),
settings.LENGTH_TAG_KEY,
"",
True,
)
mixer = blocks.multiply_cc()
self.connect(
mixer, blocks.file_sink(gr.sizeof_gr_complex, "mixer_output.dat"))
header_fft = fft.fft_vcc(symbol_settings.get_fft_length(), True, (),
True)
chanest = digital.ofdm_chanest_vcvc(SYNC_ONE, SYNC_TWO, 1)
#self.connect((chanest, 1),blocks.file_sink(gr.sizeof_gr_complex * symbol_settings.get_fft_length(), 'channel-estimate.dat'))
header_equalizer = digital.ofdm_equalizer_simpledfe(
symbol_settings.get_fft_length(),
self.constellationH.base(),
symbol_settings.get_carrier_tones(),
symbol_settings.get_pilot_tones(),
SYM_PILOT,
symbols_skipped=0,
)
header_eq = digital.ofdm_frame_equalizer_vcvc(
header_equalizer.base(),
symbol_settings.get_cp_length(),
settings.LENGTH_TAG_KEY,
True,
1 # Header is 1 symbol long
)
header_serializer = digital.ofdm_serializer_vcc(
symbol_settings.get_fft_length(),
symbol_settings.get_carrier_tones(), settings.LENGTH_TAG_KEY)
header_demod = digital.constellation_decoder_cb(
self.constellationH.base())
header_repack = blocks.repack_bits_bb(settings.HEADER_BPS, 8,
settings.LENGTH_TAG_KEY, True)
scramblerH = digital.digital.additive_scrambler_bb(
0x8a,
settings.SCRAMBLED_SEED,
7,
0,
bits_per_byte=8,
reset_tag_key=settings.LENGTH_HEADER_KEY)
self.connect(scramblerH, blocks.file_sink(gr.sizeof_char,
"header.dat"))
parser = ownHeader.parse_header_bb(settings.LENGTH_HEADER_KEY,
settings.LENGTH_TAG_KEY, 3, 1, 0)
framer_0 = framer.blk(6, settings.LENGTH_HEADER_KEY)
sender = ownHeader.send_to_multiplexer_b(settings.LENGTH_HEADER_KEY)
payload_fft = fft.fft_vcc(symbol_settings.get_fft_length(), True, (),
True)
payload_equalizer = digital.ofdm_equalizer_simpledfe(
symbol_settings.get_fft_length(),
self.constellationP.base(),
symbol_settings.get_carrier_tones(),
symbol_settings.get_pilot_tones(),
SYM_PILOT,
symbols_skipped=1, # (that was already in the header)
alpha=0.1)
#self.connect(mixer, blocks.tag_debug(gr.sizeof_gr_complex, "header"))
#self.connect(payload_fft, blocks.tag_debug(gr.sizeof_gr_complex*64, "payload"))
payload_eq = digital.ofdm_frame_equalizer_vcvc(
payload_equalizer.base(), symbol_settings.get_cp_length(),
settings.LENGTH_TAG_KEY)
payload_serializer = digital.ofdm_serializer_vcc(
symbol_settings.get_fft_length(),
symbol_settings.get_carrier_tones(),
settings.LENGTH_TAG_KEY,
settings.LENGTH_PACKET_KEY,
1 # Skip 1 symbol (that was already in the header)
)
payload_demod = digital.constellation_decoder_cb(
self.constellationP.base())
payload_descrambler = digital.additive_scrambler_bb(
0x8a,
settings.SCRAMBLED_SEED,
7,
0, # Don't reset after fixed length
bits_per_byte=8, # This is after packing
reset_tag_key=settings.LENGTH_PACKET_KEY)
payload_pack = blocks.repack_bits_bb(settings.PAYLOAD_BPS, 8,
settings.LENGTH_PACKET_KEY, True)
crc = digital.crc32_bb(True, settings.LENGTH_PACKET_KEY)
gate = blocks.tag_gate(gr.sizeof_gr_complex, False)
"""
detecting the the preamble
"""
self.connect(self, detector)
self.connect(self, delayer, (mixer, 0))
self.connect(gate, (splitter, 0))
self.connect(mixer, gate)
#self.connect(delayer, (splitter,0))
self.connect((detector, 0), oscillator, (mixer, 1))
self.connect((detector, 1), (splitter, 1))
#header handling stream
"""
parse the header data
"""
self.connect((splitter, 0), header_fft, chanest, header_eq,
header_serializer, header_demod, header_repack, framer_0,
scramblerH, parser, sender)
self.msg_connect(sender, "header", splitter,
"header_data") #feedback to the demux
#data handler stream
"""
retrieve the data
"""
self.connect((splitter, 1), payload_fft, payload_eq,
payload_serializer, payload_demod, payload_pack,
payload_descrambler, crc, self)
time = GPSTime.from_datetime(datetime(2018, 1, 7))
# example data contains an array of raw GNSS observables
# that were recorded during a minute of highway driving of
# a car, this array format can be used to create Laika's
# GNSSMeasurent object which can be processed with astrodog
# to then be analysed or used for position estimated.
with open('raw_gnss_ublox/value', 'rb') as f:
f.seek(0)
example_data = np.load(f)
measurements = [raw.normal_meas_from_array(m_arr) for m_arr in example_data]
# lets limit this to GPS sattelite for the sake of simplicity
measurements = [
m for m in measurements if helpers.get_constellation(m.prn) == 'GPS'
]
print('Exemplo de raw data de um satelite\n' + 'C1C = pseudorange\n' +
'D1C = doppler\n' + 'L1C = carrier phase\n' + 'S1C = signal strength\n',
measurements[0].observables) #observables ta no formato rinex
# we organize the measurements by epoch and by sattelite for easy plotting
measurements_by_epoch = raw.group_measurements_by_epoch(
measurements
) # uma epoch é um instante onde foram obtidas todas as medições dos satelites disponiveis
measurements_by_sattelite = raw.group_measurements_by_sat(measurements)
pos_solutions = []
corrected_measurements_by_epoch = []
for meas_epoch in measurements_by_epoch[::10]: # vai processar 10 instantes
meas.observables[signal]) and meas.processed:
pr = meas.observables[signal]
pr += meas.sat_clock_err * constants.SPEED_OF_LIGHT
sat_pos = meas.sat_pos
theta = constants.EARTH_ROTATION_RATE * (
pr - bc) / constants.SPEED_OF_LIGHT
else:
if not no_nans:
rows.append(np.nan)
continue
if no_weight:
weight = 1
else:
weight = (1 / meas.observables_std[signal])
if get_constellation(meas.prn) == 'GLONASS':
rows.append(weight *
(np.sqrt((sat_pos[0] * np.cos(theta) +
sat_pos[1] * np.sin(theta) - x)**2 +
(sat_pos[1] * np.cos(theta) -
sat_pos[0] * np.sin(theta) - y)**2 +
(sat_pos[2] - z)**2) - (pr - bc - bg)))
elif get_constellation(meas.prn) == 'GPS':
rows.append(weight *
(np.sqrt((sat_pos[0] * np.cos(theta) +
sat_pos[1] * np.sin(theta) - x)**2 +
(sat_pos[1] * np.cos(theta) -
sat_pos[0] * np.sin(theta) - y)**2 +
(sat_pos[2] - z)**2) - (pr - bc)))
return rows
