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_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 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 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 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 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)
import numpy as np import unittest from laika.gps_time import GPSTime from laika import AstroDog gps_times_list = [[1950, 415621.0], [1895, 455457.0], [1885, 443787.0]] svIds = ['G01', 'G31', 'R08'] gps_times = [GPSTime(*gps_time_list) for gps_time_list in gps_times_list] class TestAstroDog(unittest.TestCase): ''' def test_nav_vs_orbit_now(self): dog_orbit = AstroDog(pull_orbit=True) dog_nav = AstroDog(pull_orbit=False) gps_time = GPSTime.from_datetime(datetime.utcnow()) - SECS_IN_DAY*2 for svId in svIds: sat_info_nav = dog_nav.get_sat_info(svId, gps_time) sat_info_orbit = dog_orbit.get_sat_info(svId, gps_time) np.testing.assert_allclose(sat_info_nav[0], sat_info_orbit[0], rtol=0, atol=5) np.testing.assert_allclose(sat_info_nav[1], sat_info_orbit[1], rtol=0, atol=.1) np.testing.assert_allclose(sat_info_nav[2], sat_info_orbit[2], rtol=0, atol=1e-7) np.testing.assert_allclose(sat_info_nav[3], sat_info_orbit[3], rtol=0, atol=1e-11) ''' def test_nav_vs_orbit__old(self): dog_orbit = AstroDog(pull_orbit=True) dog_nav = AstroDog(pull_orbit=False) for gps_time in gps_times: for svId in svIds:
def get_single_measurement(t): return [ EphemerisType.ULTRA_RAPID_ORBIT, GPSTime(week=2177, tow=172800.0 + t), -22481344.405, -14485178.376, -554329.557, 0.000555129133 ]
def main() -> NoReturn: unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report, True) unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker( gps_measurement_report_sv, True) unpack_glonass_meas, size_glonass_meas = dict_unpacker( glonass_measurement_report, True) unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker( glonass_measurement_report_sv, True) unpack_oemdre_meas, size_oemdre_meas = dict_unpacker( oemdre_measurement_report, True) unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker( oemdre_measurement_report_sv, True) log_types = [ LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT, ] pub_types = ['qcomGnss'] unpack_position, _ = dict_unpacker(position_report) log_types.append(LOG_GNSS_POSITION_REPORT) pub_types.append("gpsLocation") # connect to modem diag = ModemDiag() # NV enable OEMDRE # TODO: it has to reboot for this to take effect DIAG_NV_READ_F = 38 DIAG_NV_WRITE_F = 39 NV_GNSS_OEM_FEATURE_MASK = 7165 opcode, payload = send_recv(diag, DIAG_NV_WRITE_F, pack('<HI', NV_GNSS_OEM_FEATURE_MASK, 1)) opcode, payload = send_recv(diag, DIAG_NV_READ_F, pack('<H', NV_GNSS_OEM_FEATURE_MASK)) def try_setup_logs(diag, log_types): for _ in range(5): try: setup_logs(diag, log_types) break except Exception: pass def disable_logs(sig, frame): os.system( "mmcli -m 0 --location-disable-gps-raw --location-disable-gps-nmea" ) cloudlog.warning("rawgpsd: shutting down") try_setup_logs(diag, []) cloudlog.warning("rawgpsd: logs disabled") sys.exit(0) signal.signal(signal.SIGINT, disable_logs) try_setup_logs(diag, log_types) cloudlog.warning("rawgpsd: setup logs done") # disable DPO power savings for more accuracy os.system("mmcli -m 0 --command='AT+QGPSCFG=\"dpoenable\",0'") os.system( "mmcli -m 0 --location-enable-gps-raw --location-enable-gps-nmea") # enable OEMDRE mode DIAG_SUBSYS_CMD_F = 75 DIAG_SUBSYS_GPS = 13 CGPS_DIAG_PDAPI_CMD = 0x64 CGPS_OEM_CONTROL = 202 GPSDIAG_OEMFEATURE_DRE = 1 GPSDIAG_OEM_DRE_ON = 1 # gpsdiag_OemControlReqType opcode, payload = send_recv( diag, DIAG_SUBSYS_CMD_F, pack( '<BHBBIIII', DIAG_SUBSYS_GPS, # Subsystem Id CGPS_DIAG_PDAPI_CMD, # Subsystem Command Code CGPS_OEM_CONTROL, # CGPS Command Code 0, # Version GPSDIAG_OEMFEATURE_DRE, GPSDIAG_OEM_DRE_ON, 0, 0)) pm = messaging.PubMaster(pub_types) while 1: opcode, payload = diag.recv() assert opcode == DIAG_LOG_F (pending_msgs, log_outer_length), inner_log_packet = unpack_from( '<BH', payload), payload[calcsize('<BH'):] if pending_msgs > 0: cloudlog.debug("have %d pending messages" % pending_msgs) assert log_outer_length == len(inner_log_packet) (log_inner_length, log_type, log_time), log_payload = unpack_from( '<HHQ', inner_log_packet), inner_log_packet[calcsize('<HHQ'):] assert log_inner_length == len(inner_log_packet) if log_type not in log_types: continue if DEBUG: print("%.4f: got log: %x len %d" % (time.time(), log_type, len(log_payload))) if log_type == LOG_GNSS_OEMDRE_MEASUREMENT_REPORT: msg = messaging.new_message('qcomGnss') gnss = msg.qcomGnss gnss.logTs = log_time gnss.init('drMeasurementReport') report = gnss.drMeasurementReport dat = unpack_oemdre_meas(log_payload) for k, v in dat.items(): if k in ["gpsTimeBias", "gpsClockTimeUncertainty"]: k += "Ms" if k == "version": assert v == 2 elif k == "svCount" or k.startswith("cdmaClockInfo["): # TODO: should we save cdmaClockInfo? pass elif k == "systemRtcValid": setattr(report, k, bool(v)) else: setattr(report, k, v) report.init('sv', dat['svCount']) sats = log_payload[size_oemdre_meas:] for i in range(dat['svCount']): sat = unpack_oemdre_meas_sv( sats[size_oemdre_meas_sv * i:size_oemdre_meas_sv * (i + 1)]) sv = report.sv[i] sv.init('measurementStatus') for k, v in sat.items(): if k in ["unkn", "measurementStatus2"]: pass elif k == "multipathEstimateValid": sv.measurementStatus.multipathEstimateIsValid = bool(v) elif k == "directionValid": sv.measurementStatus.directionIsValid = bool(v) elif k == "goodParity": setattr(sv, k, bool(v)) elif k == "measurementStatus": for kk, vv in measurementStatusFields.items(): setattr(sv.measurementStatus, kk, bool(v & (1 << vv))) else: setattr(sv, k, v) pm.send('qcomGnss', msg) elif log_type == LOG_GNSS_POSITION_REPORT: report = unpack_position(log_payload) if report["u_PosSource"] != 2: continue vNED = [ report["q_FltVelEnuMps[1]"], report["q_FltVelEnuMps[0]"], -report["q_FltVelEnuMps[2]"] ] vNEDsigma = [ report["q_FltVelSigmaMps[1]"], report["q_FltVelSigmaMps[0]"], -report["q_FltVelSigmaMps[2]"] ] msg = messaging.new_message('gpsLocation') gps = msg.gpsLocation gps.flags = 1 gps.latitude = report["t_DblFinalPosLatLon[0]"] * 180 / math.pi gps.longitude = report["t_DblFinalPosLatLon[1]"] * 180 / math.pi gps.altitude = report["q_FltFinalPosAlt"] gps.speed = math.sqrt(sum([x**2 for x in vNED])) gps.bearingDeg = report["q_FltHeadingRad"] * 180 / math.pi gps.unixTimestampMillis = GPSTime( report['w_GpsWeekNumber'], 1e-3 * report['q_GpsFixTimeMs']).as_unix_timestamp() * 1e3 gps.source = log.GpsLocationData.SensorSource.qcomdiag gps.vNED = vNED gps.verticalAccuracy = report["q_FltVdop"] gps.bearingAccuracyDeg = report[ "q_FltHeadingUncRad"] * 180 / math.pi gps.speedAccuracy = math.sqrt(sum([x**2 for x in vNEDsigma])) pm.send('gpsLocation', msg) if log_type in [ LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT ]: msg = messaging.new_message('qcomGnss') gnss = msg.qcomGnss gnss.logTs = log_time gnss.init('measurementReport') report = gnss.measurementReport if log_type == LOG_GNSS_GPS_MEASUREMENT_REPORT: dat = unpack_gps_meas(log_payload) sats = log_payload[size_gps_meas:] unpack_meas_sv, size_meas_sv = unpack_gps_meas_sv, size_gps_meas_sv report.source = 0 # gps measurement_status_fields = ( measurementStatusFields.items(), measurementStatusGPSFields.items()) elif log_type == LOG_GNSS_GLONASS_MEASUREMENT_REPORT: dat = unpack_glonass_meas(log_payload) sats = log_payload[size_glonass_meas:] unpack_meas_sv, size_meas_sv = unpack_glonass_meas_sv, size_glonass_meas_sv report.source = 1 # glonass measurement_status_fields = ( measurementStatusFields.items(), measurementStatusGlonassFields.items()) else: assert False for k, v in dat.items(): if k == "version": assert v == 0 elif k == "week": report.gpsWeek = v elif k == "svCount": pass else: setattr(report, k, v) report.init('sv', dat['svCount']) if dat['svCount'] > 0: assert len(sats) // dat['svCount'] == size_meas_sv for i in range(dat['svCount']): sv = report.sv[i] sv.init('measurementStatus') sat = unpack_meas_sv(sats[size_meas_sv * i:size_meas_sv * (i + 1)]) for k, v in sat.items(): if k == "parityErrorCount": sv.gpsParityErrorCount = v elif k == "frequencyIndex": sv.glonassFrequencyIndex = v elif k == "hemmingErrorCount": sv.glonassHemmingErrorCount = v elif k == "measurementStatus": for kk, vv in itertools.chain( *measurement_status_fields): setattr(sv.measurementStatus, kk, bool(v & (1 << vv))) elif k == "miscStatus": for kk, vv in miscStatusFields.items(): setattr(sv.measurementStatus, kk, bool(v & (1 << vv))) elif k == "pad": pass else: setattr(sv, k, v) pm.send('qcomGnss', msg)
from laika.lib.coordinates import ecef2geodetic, geodetic2ecef import numpy as np import seaborn as sns from laika import AstroDog constellations = ['GPS'] dog = AstroDog(valid_const=constellations) from datetime import datetime from laika.gps_time import GPSTime time = GPSTime(1946, 222803) sat_prn = 'G02' sat_pos, sat_vel, sat_clock_err, sat_clock_drift = dog.get_sat_info( sat_prn, time) print("Sattelite's position in ecef(m) : \n", sat_pos, '\n') print("Sattelite's velocity in ecef(m/s) : \n", sat_vel, '\n') print("Sattelite's clock error(s) : \n", sat_clock_err, '\n\n') time = GPSTime(1946, 222804) sat_prn = 'G06' sat_pos, sat_vel, sat_clock_err, sat_clock_drift = dog.get_sat_info( sat_prn, time) print("Sattelite's position in ecef(m) : \n", sat_pos, '\n') print("Sattelite's velocity in ecef(m/s) : \n", sat_vel, '\n') print("Sattelite's clock error(s) : \n", sat_clock_err, '\n\n') time = GPSTime(1946, 222805) sat_prn = 'G12' sat_pos, sat_vel, sat_clock_err, sat_clock_drift = dog.get_sat_info( sat_prn, time)