def test_ephemeris_source_in_msg(self):
data_mock = defaultdict(str)
data_mock['sv_id'] = 1
gpstime = GPSTime.from_datetime(datetime(2022, month=3, day=1))
laikad = Laikad()
laikad.fetch_orbits(gpstime, block=True)
meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['R01'][0])
msg = create_measurement_msg(meas)
self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.glonassIacUltraRapid)
# Verify gps satellite returns same source
meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['R01'][0])
msg = create_measurement_msg(meas)
self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.glonassIacUltraRapid)
# Test nasa source by using older date
gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1))
laikad = Laikad()
laikad.fetch_orbits(gpstime, block=True)
meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['G01'][0])
msg = create_measurement_msg(meas)
self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nasaUltraRapid)
# Test nav source type
ephem = GPSEphemeris(data_mock, gpstime)
meas = get_measurement_mock(gpstime, ephem)
msg = create_measurement_msg(meas)
self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav)
def test_one_outside_range(self):
start_time = GPSTime.from_datetime(datetime(2020, 4, 28))
end_time = GPSTime.from_datetime(datetime(2020, 5, 2))
time = GPSTime.from_datetime(datetime(2021, 5, 1))
holder = TimeRangeHolder()
holder.add(start_time, end_time)
self.assertFalse(time in holder)
def test_syncer(self):
ref_mono_time = 509045.61126174999
ref_gps_time = GPSTime(1989, 425928.390)
syncer = TimeSyncer(ref_mono_time, ref_gps_time)
secs = 23432.643534
delta = syncer.gps2mono(
syncer.mono2gps(ref_mono_time + secs)) - ref_mono_time
np.testing.assert_allclose(secs, delta, rtol=0, atol=1e-3)
# real world test check accurate to 1ms
delta = GPSTime(1989, 425939.390) - syncer.mono2gps(509056.61195720872)
np.testing.assert_allclose(0, delta, rtol=0, atol=1e-3)
def test_extend_range_right(self):
merge_range = (GPSTime.from_datetime(datetime(2020, 5, 7)),
GPSTime.from_datetime(datetime(2020, 5, 9)))
range_ = (GPSTime.from_datetime(datetime(2020, 5, 3)),
GPSTime.from_datetime(datetime(2020, 5, 7)))
time = GPSTime.from_datetime(datetime(2020, 5, 8))
holder = TimeRangeHolder()
holder.add(*range_)
self.assertFalse(time in holder)
holder.add(*merge_range)
self.assertTrue(time in holder)
def test_no_block_satellite_when_get_info_from_not_available_period(self):
'''If you first fetch satellite info from period when navigation data
isn't available and next from period when navigation data are available
then you should get correct result'''
prn = "C03"
constellations = ["GPS", "BEIDOU"]
available_date = GPSTime.from_datetime(datetime(2020, 5, 1, 12, 0))
not_available_date = GPSTime.from_datetime(datetime(2000, 1, 1))
dog = AstroDog(valid_const=constellations)
sat_info = dog.get_sat_info(prn, not_available_date)
self.assertIsNone(sat_info)
sat_info = dog.get_sat_info(prn, available_date)
self.assertIsNotNone(sat_info)
def test_gps_time_dt_conversion(self):
for dt in datetimes:
double_converted_dt = GPSTime.from_datetime(dt).as_datetime()
delta_sec = (dt - double_converted_dt).total_seconds()
np.testing.assert_allclose(0, delta_sec, rtol=0, atol=1e-10)
for gps_time, dt in zip(gps_times, datetimes):
delta_sec = gps_time - GPSTime.from_datetime(dt)
np.testing.assert_allclose(0, delta_sec, rtol=0, atol=1e-10)
for gps_time in gps_times:
double_converted_gps_time = GPSTime.from_datetime(
gps_time.as_datetime())
delta_sec = gps_time - double_converted_gps_time
np.testing.assert_allclose(0, delta_sec, rtol=0, atol=1e-10)
def main(sm=None, pm=None):
use_qcom = os.path.isfile("/persist/comma/use-quectel-rawgps")
if use_qcom:
raw_gnss_socket = "qcomGnss"
else:
raw_gnss_socket = "ubloxGnss"
if sm is None:
sm = messaging.SubMaster([raw_gnss_socket, 'clocks'])
if pm is None:
pm = messaging.PubMaster(['gnssMeasurements'])
replay = "REPLAY" in os.environ
use_internet = "LAIKAD_NO_INTERNET" not in os.environ
laikad = Laikad(save_ephemeris=not replay,
auto_fetch_orbits=use_internet,
use_qcom=use_qcom)
while True:
sm.update()
if sm.updated[raw_gnss_socket]:
gnss_msg = sm[raw_gnss_socket]
msg = laikad.process_gnss_msg(gnss_msg,
sm.logMonoTime[raw_gnss_socket],
block=replay)
if msg is not None:
pm.send('gnssMeasurements', msg)
if not laikad.got_first_gnss_msg and sm.updated['clocks']:
clocks_msg = sm['clocks']
t = GPSTime.from_datetime(
datetime.utcfromtimestamp(clocks_msg.wallTimeNanos * 1E-9))
if laikad.auto_fetch_orbits:
laikad.fetch_orbits(t, block=replay)
def test_gps(self):
available_date = GPSTime.from_datetime(datetime(2020, 5, 1, 12))
dog = AstroDog(valid_const=["GPS"],
valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT)
dog.get_orbit_data(available_date, only_predictions=True)
self.assertGreater(len(dog.orbits.keys()), 0)
self.assertTrue(available_date in dog.orbit_fetched_times)
def test_get_all_sat_info_gps(self):
time = GPSTime.from_datetime(datetime(2020, 5, 1, 12, 0, 0))
all_ephem_types = (EphemerisType.FINAL_ORBIT,
EphemerisType.RAPID_ORBIT,
EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV)
kwargs_list = [
*[{
"valid_const": ["GPS"],
"valid_ephem_types": ephem_type
} for ephem_type in all_ephem_types],
*[{
"valid_const": ["GLONASS"],
"valid_ephem_types": ephem_type
} for ephem_type in all_ephem_types],
*[{
"valid_const": ["BEIDOU"],
"valid_ephem_types": ephem_type
} for ephem_type in EphemerisType.all_orbits()],
*[{
"valid_const": ["GALILEO"],
"valid_ephem_types": ephem_type
} for ephem_type in EphemerisType.all_orbits()],
*[{
"valid_const": ["QZNSS"],
"valid_ephem_types": ephem_type
} for ephem_type in EphemerisType.all_orbits()],
]
for kwargs in kwargs_list:
dog = AstroDog(**kwargs)
infos = dog.get_all_sat_info(time)
self.assertGreater(len(infos), 0,
f"No ephemeris found for {kwargs}")
def test_laika_get_orbits_now(self): laikad = Laikad(auto_update=False) laikad.fetch_orbits(GPSTime.from_datetime(datetime.utcnow()), block=True) prn = "G01" self.assertGreater(len(laikad.astro_dog.orbits[prn]), 0) prn = "R01" self.assertGreater(len(laikad.astro_dog.orbits[prn]), 0) print(min(laikad.astro_dog.orbits[prn], key=lambda e: e.epoch).epoch.as_datetime())
def test_ephemeris_parsing(self):
ublox_ephem = Mock()
ublox_ephem.gpsWeek = 0
ublox_ephem.svId = 1
ublox_ephem.toe = 0
ephemeris = convert_ublox_ephem(ublox_ephem)
# Should roll-over twice with steps of 1024
updated_time = GPSTime(ublox_ephem.gpsWeek + 2048, 0)
self.assertEqual(ephemeris.epoch, updated_time)
# Check only one roll-over when passing extra argument current_time
roll_over_time = GPSTime(1024, 0).as_datetime()
ephemeris = convert_ublox_ephem(ublox_ephem, roll_over_time)
# Should roll-over twice with 1024
updated_time = GPSTime(ublox_ephem.gpsWeek + 1024, 0)
self.assertEqual(updated_time, ephemeris.epoch)
def test_gps_and_glonass_2022(self):
# Test GPS and GLONASS separately from the first date that GLONASS Ultra-Rapid prediction orbits were available
available_date = GPSTime.from_datetime(datetime(2022, 1, 29, 11, 31))
for t in range(0, 24, 3):
check_date = available_date + t * SECS_IN_HR
for const in ["GPS", "GLONASS"]:
dog = AstroDog(
valid_const=const,
valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT)
dog.get_orbit_data(check_date, only_predictions=True)
self.assertGreater(len(dog.orbits.keys()), 0)
self.assertTrue(check_date in dog.orbit_fetched_times)
def get_satellite_delays(dog, date):
dog.get_dcb_data(GPSTime.from_datetime(start_date))
res = {}
# published data is in nanoseconds...
# ours is in pseudorange (meters)
factor = constants.SPEED_OF_LIGHT / 1e9
factor = 0.365
for prn in ['G%02d' % i for i in range(1, 33)]:
if hasattr(dog.dcbs[prn][0], 'C1W_C2W'):
res[prn] = dog.dcbs[prn][0].C1W_C2W * factor
elif hasattr(dog.dcbs[prn][0], 'C1P_C2P'):
res[prn] = dog.dcbs[prn][0].C1P_C2P * factor
return res
def test_create_msg_without_errors(self):
gpstime = GPSTime.from_datetime(datetime.now())
meas = GNSSMeasurement(ConstellationId.GPS, 1, gpstime.week,
gpstime.tow, {
'C1C': 0.,
'D1C': 0.
}, {
'C1C': 0.,
'D1C': 0.
})
msg = create_measurement_msg(meas)
self.assertEqual(msg.constellationId, 'gps')
def test_utc_converter(self):
datetimes_strings = [
'2008-04-27 22:22:06', '2012-05-13 08:52:57',
'2012-09-17 12:50:05', '2016-04-08 12:28:19',
'2017-10-23 06:42:34', '2018-01-18 03:16:27', '2017-07-01 00:00:05'
]
gps_times = [
GPSTime.from_datetime(
datetime.strptime(dt_str, '%Y-%m-%d %H:%M:%S'))
for dt_str in datetimes_strings
]
np.testing.assert_allclose((gps_times[0] - gpst_to_utc(gps_times[0])),
14,
rtol=0,
atol=1e-3)
np.testing.assert_allclose((gps_times[1] - gpst_to_utc(gps_times[1])),
15,
rtol=0,
atol=1e-3)
np.testing.assert_allclose((gps_times[2] - gpst_to_utc(gps_times[2])),
16,
rtol=0,
atol=1e-3)
np.testing.assert_allclose((gps_times[3] - gpst_to_utc(gps_times[3])),
17,
rtol=0,
atol=1e-3)
np.testing.assert_allclose((gps_times[4] - gpst_to_utc(gps_times[4])),
18,
rtol=0,
atol=1e-3)
np.testing.assert_allclose((gps_times[5] - gpst_to_utc(gps_times[5])),
18,
rtol=0,
atol=1e-3)
np.testing.assert_allclose((gps_times[6] - gpst_to_utc(gps_times[6])),
17,
rtol=0,
atol=1e-3)
np.testing.assert_allclose(
(gps_times[5] - utc_to_gpst(gpst_to_utc(gps_times[5]))),
0,
rtol=0,
atol=1e-3)
np.testing.assert_allclose(
(gps_times[6] - utc_to_gpst(gpst_to_utc(gps_times[6]))),
0,
rtol=0,
atol=1e-3)
def test_station_position(self):
print(
'WARNING THIS TAKE CAN TAKE A VERY LONG TIME THE FIRST RUN TO DOWNLOAD'
)
dog = AstroDog()
# Building this cache takes forever just copy it from repo
cache_directory = '/tmp/gnss/cors_coord/'
try:
os.mkdir('/tmp/gnss/')
except OSError:
pass
try:
os.mkdir(cache_directory)
except OSError:
pass
examples_directory = os.path.join(
os.path.dirname(os.path.abspath(__file__)), '../examples')
copyfile(os.path.join(examples_directory, 'cors_station_positions'),
os.path.join(cache_directory, 'cors_station_positions'))
station_name = 'sc01'
time = GPSTime.from_datetime(datetime(2020, 1, 11))
slac_rinex_obs_file = download_cors_station(time, station_name,
dog.cache_dir)
obs_data = RINEXFile(slac_rinex_obs_file)
sc01_exact_position = get_station_position('sc01')
rinex_meas_grouped = raw.read_rinex_obs(obs_data)
rinex_corr_grouped = []
for meas in tqdm(rinex_meas_grouped):
proc = raw.process_measurements(meas, dog=dog)
corr = raw.correct_measurements(meas, sc01_exact_position, dog=dog)
rinex_corr_grouped.append(corr)
# Using laika's WLS solver we can now calculate position
# fixes for every epoch (every 30s) over 24h.
ests = []
for corr in tqdm(rinex_corr_grouped[:]):
fix, _ = raw.calc_pos_fix(corr)
ests.append(fix)
ests = np.array(ests)
mean_fix = np.mean(ests[:, :3], axis=0)
np.testing.assert_allclose(mean_fix,
sc01_exact_position,
rtol=0,
atol=1)
def data_for_station(dog, station_name, date=None):
"""
Get data from a particular station and time.
Station names are CORS names (eg: 'slac')
Dates are datetimes (eg: datetime(2020,1,7))
"""
if date is None:
date = datetime(2020,1,7)
time = GPSTime.from_datetime(date)
rinex_obs_file = download_cors_station(time, station_name, dog.cache_dir)
obs_data = RINEXFile(rinex_obs_file)
station_pos = get_station_position(station_name)
return station_pos, raw.read_rinex_obs(obs_data)
def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False):
if ublox_msg.which == 'measurementReport':
t = ublox_mono_time * 1e-9
report = ublox_msg.measurementReport
if report.gpsWeek > 0:
latest_msg_t = GPSTime(report.gpsWeek, report.rcvTow)
self.fetch_orbits(latest_msg_t + SECS_IN_MIN, block)
new_meas = read_raw_ublox(report)
processed_measurements = process_measurements(new_meas, self.astro_dog)
if self.last_pos_fix_t is None or abs(self.last_pos_fix_t - t) >= 2:
min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in processed_measurements) else 4
pos_fix, pos_fix_residual = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements)
if len(pos_fix) > 0:
self.last_pos_fix = pos_fix[:3]
self.last_pos_residual = pos_fix_residual
self.last_pos_fix_t = t
corrected_measurements = correct_measurements(processed_measurements, self.last_pos_fix, self.astro_dog) if self.last_pos_fix_t is not None else []
self.update_localizer(self.last_pos_fix, t, corrected_measurements)
kf_valid = all(self.kf_valid(t))
ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist()
ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist()
pos_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_POS].diagonal())).tolist()
vel_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_VELOCITY].diagonal())).tolist()
meas_msgs = [create_measurement_msg(m) for m in corrected_measurements]
dat = messaging.new_message("gnssMeasurements")
measurement_msg = log.LiveLocationKalman.Measurement.new_message
dat.gnssMeasurements = {
"gpsWeek": report.gpsWeek,
"gpsTimeOfWeek": report.rcvTow,
"positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=kf_valid),
"velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=kf_valid),
"positionFixECEF": measurement_msg(value=self.last_pos_fix, std=self.last_pos_residual, valid=self.last_pos_fix_t == t),
"ubloxMonoTime": ublox_mono_time,
"correctedMeasurements": meas_msgs
}
return dat
elif ublox_msg.which == 'ephemeris':
ephem = convert_ublox_ephem(ublox_msg.ephemeris)
self.astro_dog.add_navs({ephem.prn: [ephem]})
self.cache_ephemeris(t=ephem.epoch)
def test_cache(self):
laikad = Laikad(auto_update=True, save_ephemeris=True)
def wait_for_cache():
max_time = 2
while Params().get(EPHEMERIS_CACHE) is None:
time.sleep(0.1)
max_time -= 0.1
if max_time == 0:
self.fail("Cache has not been written after 2 seconds")
# Test cache with no ephemeris
laikad.cache_ephemeris(t=GPSTime(0, 0))
wait_for_cache()
Params().delete(EPHEMERIS_CACHE)
laikad.astro_dog.get_navs(self.first_gps_time)
laikad.fetch_orbits(self.first_gps_time, block=True)
# Wait for cache to save
wait_for_cache()
# Check both nav and orbits separate
laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV)
# Verify orbits and nav are loaded from cache
self.dict_has_values(laikad.astro_dog.orbits)
self.dict_has_values(laikad.astro_dog.nav)
# Verify cache is working for only nav by running a segment
msg = verify_messages(self.logs, laikad, return_one_success=True)
self.assertIsNotNone(msg)
with patch('selfdrive.locationd.laikad.get_orbit_data',
return_value=None) as mock_method:
# Verify no orbit downloads even if orbit fetch times is reset since the cache has recently been saved and we don't want to download high frequently
laikad.astro_dog.orbit_fetched_times = TimeRangeHolder()
laikad.fetch_orbits(self.first_gps_time, block=False)
mock_method.assert_not_called()
# Verify cache is working for only orbits by running a segment
laikad = Laikad(auto_update=False,
valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT)
msg = verify_messages(self.logs, laikad, return_one_success=True)
self.assertIsNotNone(msg)
# Verify orbit data is not downloaded
mock_method.assert_not_called()
def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False):
if ublox_msg.which == 'measurementReport':
t = ublox_mono_time * 1e-9
report = ublox_msg.measurementReport
if report.gpsWeek > 0:
self.got_first_ublox_msg = True
latest_msg_t = GPSTime(report.gpsWeek, report.rcvTow)
if self.auto_fetch_orbits:
self.fetch_orbits(latest_msg_t, block)
new_meas = read_raw_ublox(report)
# Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites
new_meas = [m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7]
processed_measurements = process_measurements(new_meas, self.astro_dog)
est_pos = self.get_est_pos(t, processed_measurements)
corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog) if len(est_pos) > 0 else []
self.update_localizer(est_pos, t, corrected_measurements)
kf_valid = all(self.kf_valid(t))
ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist()
ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist()
pos_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_POS].diagonal())).tolist()
vel_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_VELOCITY].diagonal())).tolist()
meas_msgs = [create_measurement_msg(m) for m in corrected_measurements]
dat = messaging.new_message("gnssMeasurements")
measurement_msg = log.LiveLocationKalman.Measurement.new_message
dat.gnssMeasurements = {
"gpsWeek": report.gpsWeek,
"gpsTimeOfWeek": report.rcvTow,
"positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=kf_valid),
"velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=kf_valid),
"positionFixECEF": measurement_msg(value=self.last_pos_fix, std=self.last_pos_residual, valid=self.last_pos_fix_t == t),
"ubloxMonoTime": ublox_mono_time,
"correctedMeasurements": meas_msgs
}
return dat
elif ublox_msg.which == 'ephemeris':
ephem = convert_ublox_ephem(ublox_msg.ephemeris)
self.astro_dog.add_navs({ephem.prn: [ephem]})
self.cache_ephemeris(t=ephem.epoch)
def run_station_position(self, length):
dog = AstroDog()
# Building this cache takes forever just copy it from repo
cache_directory = '/tmp/gnss/cors_coord/'
os.makedirs('/tmp/gnss/', exist_ok=True)
os.makedirs(cache_directory, exist_ok=True)
examples_directory = os.path.join(
os.path.dirname(os.path.abspath(__file__)), '../examples')
copyfile(os.path.join(examples_directory, 'cors_station_positions'),
os.path.join(cache_directory, 'cors_station_positions'))
station_name = 'sc01'
time = GPSTime.from_datetime(datetime(2020, 1, 11))
slac_rinex_obs_file = download_cors_station(time, station_name,
dog.cache_dir)
obs_data = RINEXFile(slac_rinex_obs_file)
sc01_exact_position = get_station_position('sc01')
rinex_meas_grouped = raw.read_rinex_obs(obs_data)
# Select small sample out of ~2800 to reduce computation time
rinex_meas_grouped = rinex_meas_grouped[:length]
rinex_corr_grouped = []
for meas in tqdm(rinex_meas_grouped):
proc = raw.process_measurements(meas, dog=dog)
corr = raw.correct_measurements(proc, sc01_exact_position, dog=dog)
rinex_corr_grouped.append(corr)
# Using laika's WLS solver we can now calculate position
# fixes for every epoch (every 30s) over 24h.
ests = []
for corr in tqdm(rinex_corr_grouped):
ret = raw.calc_pos_fix(corr)
if len(ret) > 0:
fix, _ = ret
ests.append(fix)
ests = np.array(ests)
mean_fix = np.mean(ests[:, :3], axis=0)
np.testing.assert_allclose(mean_fix,
sc01_exact_position,
rtol=0,
atol=1)
def test_fetch_orbits_non_blocking(self):
gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1))
laikad = Laikad()
laikad.fetch_orbits(gpstime, block=False)
laikad.orbit_fetch_future.result(30)
# Get results and save orbits to laikad:
laikad.fetch_orbits(gpstime, block=False)
ephem = laikad.astro_dog.orbits['G01'][0]
self.assertIsNotNone(ephem)
laikad.fetch_orbits(gpstime+2*SECS_IN_DAY, block=False)
laikad.orbit_fetch_future.result(30)
# Get results and save orbits to laikad:
laikad.fetch_orbits(gpstime + 2 * SECS_IN_DAY, block=False)
ephem2 = laikad.astro_dog.orbits['G01'][0]
self.assertIsNotNone(ephem)
self.assertNotEqual(ephem, ephem2)
def test_merge_ranges(self):
first_range = (GPSTime.from_datetime(datetime(2020, 5, 1)),
GPSTime.from_datetime(datetime(2020, 5, 3)))
second_range = (GPSTime.from_datetime(datetime(2020, 5, 7)),
GPSTime.from_datetime(datetime(2020, 5, 9)))
merge_range = (GPSTime.from_datetime(datetime(2020, 5, 2)),
GPSTime.from_datetime(datetime(2020, 5, 8)))
time = GPSTime.from_datetime(datetime(2020, 5, 5))
holder = TimeRangeHolder()
holder.add(*first_range)
holder.add(*second_range)
self.assertFalse(time in holder)
holder.add(*merge_range)
self.assertTrue(time in holder)
def data_for_station(dog, station_name, date):
"""
Get data from a particular station and time. Wraps a number of laika function calls.
Station names are CORS names (eg: 'slac')
Dates are datetimes (eg: datetime(2020,1,7))
"""
time = GPSTime.from_datetime(date)
rinex_obs_file = None
# handlers for specific networks
handlers = {'Korea': download_korean_station}
network = station_network_info.get(station_name, None)
# no special network, so try using whatever
if network is None:
try:
station_pos = get_station_position(station_name,
cache_dir=dog.cache_dir)
rinex_obs_file = download_cors_station(time,
station_name,
cache_dir=dog.cache_dir)
except (KeyError, DownloadError):
pass
if not rinex_obs_file:
# station position not in CORS map, try another thing
if station_name in extra_station_info:
station_pos = numpy.array(extra_station_info[station_name])
rinex_obs_file = download_misc_igs_station(
time, station_name, cache_dir=dog.cache_dir)
else:
raise DownloadError
else:
station_pos = numpy.array(extra_station_info[station_name])
rinex_obs_file = handlers[network](time,
station_name,
cache_dir=dog.cache_dir)
obs_data = RINEXFile(rinex_obs_file, rate=30)
return station_pos, raw.read_rinex_obs(obs_data)
def test_fetch_orbits_with_wrong_clocks(self):
laikad = Laikad()
def check_has_orbits():
self.assertGreater(len(laikad.astro_dog.orbits), 0)
ephem = laikad.astro_dog.orbits['G01'][0]
self.assertIsNotNone(ephem)
real_current_time = GPSTime.from_datetime(datetime(2021, month=3, day=1))
wrong_future_clock_time = real_current_time + SECS_IN_DAY
laikad.fetch_orbits(wrong_future_clock_time, block=True)
check_has_orbits()
self.assertEqual(laikad.last_fetch_orbits_t, wrong_future_clock_time)
# Test fetching orbits with earlier time
assert real_current_time < laikad.last_fetch_orbits_t
laikad.astro_dog.orbits = {}
laikad.fetch_orbits(real_current_time, block=True)
check_has_orbits()
self.assertEqual(laikad.last_fetch_orbits_t, real_current_time)
def main(sm=None, pm=None):
if sm is None:
sm = messaging.SubMaster(['ubloxGnss', 'clocks'])
if pm is None:
pm = messaging.PubMaster(['gnssMeasurements'])
replay = "REPLAY" in os.environ
use_internet = "LAIKAD_NO_INTERNET" not in os.environ
laikad = Laikad(save_ephemeris=not replay, auto_fetch_orbits=use_internet)
while True:
sm.update()
if sm.updated['ubloxGnss']:
ublox_msg = sm['ubloxGnss']
msg = laikad.process_ublox_msg(ublox_msg, sm.logMonoTime['ubloxGnss'], block=replay)
if msg is not None:
pm.send('gnssMeasurements', msg)
if not laikad.got_first_ublox_msg and sm.updated['clocks']:
clocks_msg = sm['clocks']
t = GPSTime.from_datetime(datetime.utcfromtimestamp(clocks_msg.wallTimeNanos * 1E-9))
if laikad.auto_fetch_orbits:
laikad.fetch_orbits(t, block=replay)
m = Basemap(projection='mill', lat_0 = 40, lon_0 = 0, resolution = 'c', ax=ax)
m.shadedrelief(scale = 0.5)
# m.drawcoastlines()
# m.drawcountries()
# m.drawstates()
#m.fillcontinents(color='coral', lake_color='aqua')
m.drawparallels(np.arange(-90., 120., 30.))
m.drawmeridians(np.arange(0., 420., 60.))
if dn:
m.nightshade(dn)
#m.drawmapboundary(fill_color='aqua')
return m
time = GPSTime.from_datetime(datetime(2019,5,31,11,0,0))
dog = AstroDog()
ionex_map = dog.get_ionex(time)
# print(dog.get_ionex(time).get_TEC((5,5), time))
# Setup map:
fig = plt.figure(1, figsize=(18, 12))
plt.clf()
ax = fig.add_subplot(111)
m = setup_map(ax)
lon_bins = np.linspace(-180, 180, 73)
lat_bins = np.linspace(-90, 90, 73)
def deserialize_hook(dct):
if 'ephemeris' in dct:
return Ephemeris.from_json(dct)
if 'week' in dct:
return GPSTime(dct['week'], dct['tow'])
return dct
def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False):
if self.is_good_report(gnss_msg):
week, tow, new_meas = self.read_report(gnss_msg)
t = gnss_mono_time * 1e-9
if week > 0:
self.got_first_gnss_msg = True
latest_msg_t = GPSTime(week, tow)
if self.auto_fetch_orbits:
self.fetch_orbits(latest_msg_t, block)
# Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites
new_meas = [
m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7
]
processed_measurements = process_measurements(
new_meas, self.astro_dog)
est_pos = self.get_est_pos(t, processed_measurements)
corrected_measurements = correct_measurements(
processed_measurements, est_pos,
self.astro_dog) if len(est_pos) > 0 else []
if gnss_mono_time % 10 == 0:
cloudlog.debug(
f"Measurements Incoming/Processed/Corrected: {len(new_meas), len(processed_measurements), len(corrected_measurements)}"
)
self.update_localizer(est_pos, t, corrected_measurements)
kf_valid = all(self.kf_valid(t))
ecef_pos = self.gnss_kf.x[GStates.ECEF_POS]
ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY]
p = self.gnss_kf.P.diagonal()
pos_std = np.sqrt(p[GStates.ECEF_POS])
vel_std = np.sqrt(p[GStates.ECEF_VELOCITY])
meas_msgs = [
create_measurement_msg(m) for m in corrected_measurements
]
dat = messaging.new_message("gnssMeasurements")
measurement_msg = log.LiveLocationKalman.Measurement.new_message
dat.gnssMeasurements = {
"gpsWeek":
week,
"gpsTimeOfWeek":
tow,
"positionECEF":
measurement_msg(value=ecef_pos.tolist(),
std=pos_std.tolist(),
valid=kf_valid),
"velocityECEF":
measurement_msg(value=ecef_vel.tolist(),
std=vel_std.tolist(),
valid=kf_valid),
"positionFixECEF":
measurement_msg(value=self.last_pos_fix,
std=self.last_pos_residual,
valid=self.last_pos_fix_t == t),
"ubloxMonoTime":
gnss_mono_time,
"correctedMeasurements":
meas_msgs
}
return dat
# TODO this only works on GLONASS, qcom needs live ephemeris parsing too
elif gnss_msg.which == 'ephemeris':
ephem = convert_ublox_ephem(gnss_msg.ephemeris)
self.astro_dog.add_navs({ephem.prn: [ephem]})
self.cache_ephemeris(t=ephem.epoch)
def gather_data(start_time, station_vtecs):
'''
Looks for 'coincidences'. A 'coincidence' is a set of observables for various sat/rec pairs that cross
into the ionosphere at approximately the same location (lat,lon).
Returns four items:
final_coicidences (dict) {(lat, lon, i): Observation((lat, lon, i), station, prn, vtec, s_to_v, ...}
measurements (list): [Observation 1, ....]
sats, recvrs (set): sets of satellite and receiver objects included.
'''
# mapping of (lat, lon, time) to [measurement_idx]
coincidences = defaultdict(list)
measurements = []
svs = set()
recvs = set()
for cnt, (station, station_dat) in enumerate(station_vtecs.items()):
print("gathering data for %3d/%d" % (cnt, len(station_vtecs)))
for prn, (locs, dats, slants) in station_dat.items():
if len(locs) == 0:
continue
# convert locs to lat lon in bulk for much better speed
# dirty hack to force numpy to treat the array as 1d
locs.append(None)
# indices with locations set
idxs = numpy.where(
numpy.logical_not(numpy.vectorize(is_)(locs, None)))
locs.pop(-1)
if len(idxs[0]) == 0:
continue
locs_lla = ecef2geodetic(numpy.stack(numpy.array(locs)[idxs]))
prev_coi = None
for i, idx in enumerate(idxs[0]):
cois = set()
lat, lon, _ = locs_lla[i]
coi = (round_to_res(lat, lat_res), round_to_res(lon, lon_res),
(idx // (time_res / 30)) * 30)
if coi == prev_coi:
continue
prev_coi = coi
gpstime = start_time + timedelta(seconds=30 * int(idx))
gpstime = GPSTime.from_datetime(gpstime)
obs = Observation(coi, station, prn, dats[idx], slants[idx],
gpstime)
coincidences[coi].append(obs)
final_coincidences = dict()
# only include coincidences with >= 2 measurements
for coi, obss in coincidences.items():
if (len({obs.station
for obs in obss}) > 1 or len({obs.sat
for obs in obss}) > 1):
final_coincidences[coi] = obss
for obs in obss:
svs.add(obs.sat)
recvs.add(obs.station)
measurements.append(obs)
return final_coincidences, measurements, svs, recvs
