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