mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-02-27 06:03:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Support quectel gps (#25299)

Browse Source 
* qcom clock gets corrected randomly it seems

* Use quectel gps

* fix small laikad bugs

* Support both

* Support ublox and qcom clock model

* fix laikad test

* fix typo

* Back to original value

* More typos

Co-authored-by: Comma Device <device@comma.ai>
This commit is contained in:
HaraldSchafer
2022-07-28 19:56:59 +02:00
committed by GitHub
parent b2c1098cd8
commit 11da2804f6
5 changed files with 98 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

81
selfdrive/locationd/laikad.py
View File

@@ -19,7 +19,7 @@ from laika.downloader import DownloadFailed
from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem
from laika.gps_time import GPSTime
from laika.helpers import ConstellationId
from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox
from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom
from selfdrive.locationd.laikad_helpers import calc_pos_fix_gauss_newton, get_posfix_sympy_fun
from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind
from selfdrive.locationd.models.gnss_kf import GNSSKalman
@@ -36,7 +36,7 @@ POS_FIX_RESIDUAL_THRESHOLD = 100.0
class Laikad:
def __init__(self, valid_const=("GPS", "GLONASS"), auto_fetch_orbits=True, auto_update=False,
valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV),
save_ephemeris=False):
save_ephemeris=False, use_qcom=False):
"""
valid_const: GNSS constellation which can be used
auto_fetch_orbits: If true fetch orbits from internet when needed
@@ -45,14 +45,14 @@ class Laikad:
save_ephemeris: If true saves and loads nav and orbit ephemeris to cache.
"""
self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, clear_old_ephemeris=True, cache_dir=DOWNLOADS_CACHE_FOLDER)
self.gnss_kf = GNSSKalman(GENERATED_DIR, cython=True)
self.gnss_kf = GNSSKalman(GENERATED_DIR, cython=True, erratic_clock=use_qcom)
self.auto_fetch_orbits = auto_fetch_orbits
self.orbit_fetch_executor: Optional[ProcessPoolExecutor] = None
self.orbit_fetch_future: Optional[Future] = None
self.last_fetch_orbits_t = None
self.got_first_ublox_msg = False
self.got_first_gnss_msg = False
self.last_cached_t = None
self.save_ephemeris = save_ephemeris
self.load_cache()
@@ -61,6 +61,7 @@ class Laikad:
self.last_pos_fix = []
self.last_pos_residual = []
self.last_pos_fix_t = None
self.use_qcom = use_qcom
def load_cache(self):
if not self.save_ephemeris:
@@ -105,17 +106,40 @@ class Laikad:
cloudlog.debug(f"Pos fix failed with median: {residual_median.round()}. All residuals: {np.round(pos_fix_residual)}")
return self.last_pos_fix
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)
def is_good_report(self, gnss_msg):
if gnss_msg.which == 'drMeasurementReport' and self.use_qcom:
constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source)
# TODO support GLONASS
return constellation_id in [ConstellationId.GPS, ConstellationId.SBAS]
elif gnss_msg.which == 'measurementReport' and not self.use_qcom:
return True
else:
return False
def read_report(self, gnss_msg):
if self.use_qcom:
report = gnss_msg.drMeasurementReport
week = report.gpsWeek
tow = report.gpsMilliseconds / 1000.0
new_meas = read_raw_qcom(report)
else:
report = gnss_msg.measurementReport
week = report.gpsWeek
tow = report.rcvTow
new_meas = read_raw_ublox(report)
return week, tow, new_meas
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)
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]
@@ -123,7 +147,7 @@ class Laikad:
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 ublox_mono_time % 10 == 0:
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)
@@ -139,20 +163,21 @@ class Laikad:
dat = messaging.new_message("gnssMeasurements")
measurement_msg = log.LiveLocationKalman.Measurement.new_message
dat.gnssMeasurements = {
"gpsWeek": report.gpsWeek,
"gpsTimeOfWeek": report.rcvTow,
"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": ublox_mono_time,
"ubloxMonoTime": gnss_mono_time,
"correctedMeasurements": meas_msgs
}
return dat
elif ublox_msg.which == 'ephemeris':
ephem = convert_ublox_ephem(ublox_msg.ephemeris)
# 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)
# elif ublox_msg.which == 'ionoData':
#elif gnss_msg.which == 'ionoData':
# todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them.
def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]):
@@ -303,24 +328,30 @@ class EphemerisSourceType(IntEnum):
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(['ubloxGnss', 'clocks'])
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)
laikad = Laikad(save_ephemeris=not replay, auto_fetch_orbits=use_internet, use_qcom=use_qcom)
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 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_ublox_msg and sm.updated['clocks']:
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:

12
selfdrive/locationd/locationd.cc
View File

@@ -443,6 +443,8 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
this->time_check(t);
if (log.isSensorEvents()) {
this->handle_sensors(t, log.getSensorEvents());
} else if (log.isGpsLocation()) {
this->handle_gps(t, log.getGpsLocation());
} else if (log.isGpsLocationExternal()) {
this->handle_gps(t, log.getGpsLocationExternal());
} else if (log.isCarState()) {
@@ -490,11 +492,17 @@ void Localizer::determine_gps_mode(double current_time) {
}
int Localizer::locationd_thread() {
const std::initializer_list<const char *> service_list = {"gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState", "carParams"};
const char* gps_location_socket;
if (util::file_exists("/persist/comma/use-quectel-rawgps")) {
gps_location_socket = "gpsLocation";
} else {
gps_location_socket = "gpsLocationExternal";
}
const std::initializer_list<const char *> service_list = {gps_location_socket, "sensorEvents", "cameraOdometry", "liveCalibration", "carState", "carParams"};
PubMaster pm({"liveLocationKalman"});
// TODO: remove carParams once we're always sending at 100Hz
SubMaster sm(service_list, {}, nullptr, {"gpsLocationExternal", "carParams"});
SubMaster sm(service_list, {}, nullptr, {gps_location_socket, "carParams"});
uint64_t cnt = 0;
bool filterInitialized = false;

15
selfdrive/locationd/models/gnss_kf.py
View File

@@ -39,12 +39,6 @@ class GNSSKalman():
1e14, (100)**2, (0.2)**2,
(10)**2, (1)**2])
# process noise
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
3**2, 3**2, 3**2,
(.1)**2, (0)**2, (0.005)**2,
.1**2, (.01)**2])
maha_test_kinds: List[int] = [] # ObservationKind.PSEUDORANGE_RATE, ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_GLONASS]
@staticmethod
@@ -120,7 +114,14 @@ class GNSSKalman():
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds)
def __init__(self, generated_dir, cython=False):
def __init__(self, generated_dir, cython=False, erratic_clock=False):
# process noise
clock_error_drift = 100.0 if erratic_clock else 0.1
self.Q = np.diag([0.03**2, 0.03**2, 0.03**2,
3**2, 3**2, 3**2,
(clock_error_drift)**2, (0)**2, (0.005)**2,
.1**2, (.01)**2])
self.dim_state = self.x_initial.shape[0]
# init filter

38
selfdrive/locationd/models/loc_kf.py
View File

@@ -91,22 +91,6 @@ class LocKalman():
0.05**2, 0.05**2, 0.05**2,
0.01**2, 0.01**2, 0.01**2])
# process noise
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
0.0**2, 0.0**2, 0.0**2,
0.0**2, 0.0**2, 0.0**2,
0.1**2, 0.1**2, 0.1**2,
(.1)**2, (0.0)**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
(0.02 / 100)**2,
3**2, 3**2, 3**2,
0.001**2,
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2,
(.1)**2, (.01)**2,
0.005**2,
(0.02 / 100)**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2])
# measurements that need to pass mahalanobis distance outlier rejector
maha_test_kinds = [ObservationKind.ORB_FEATURES, ObservationKind.ORB_FEATURES_WIDE] # , ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE]
@@ -345,9 +329,29 @@ class LocKalman():
msckf_params = None
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, msckf_params, maha_test_kinds)
def __init__(self, generated_dir, N=4):
def __init__(self, generated_dir, N=4, erratic_clock=False):
name = f"{self.name}_{N}"
# process noise
clock_error_drift = 100.0 if erratic_clock else 0.1
self.Q = np.diag([0.03**2, 0.03**2, 0.03**2,
0.0**2, 0.0**2, 0.0**2,
0.0**2, 0.0**2, 0.0**2,
0.1**2, 0.1**2, 0.1**2,
(clock_error_drift)**2, (0)**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
(0.02 / 100)**2,
3**2, 3**2, 3**2,
0.001**2,
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2,
(.1)**2, (.01)**2,
0.005**2,
(0.02 / 100)**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2])
self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2),
ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]),
ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]),

6
selfdrive/locationd/test/test_laikad.py
View File

@@ -28,7 +28,7 @@ def get_log(segs=range(0)):
def verify_messages(lr, laikad, return_one_success=False):
good_msgs = []
for m in lr:
msg = laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime, block=True)
msg = laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True)
if msg is not None and len(msg.gnssMeasurements.correctedMeasurements) > 0:
good_msgs.append(msg)
if return_one_success:
@@ -152,7 +152,7 @@ class TestLaikad(unittest.TestCase):
self.assertFalse(all(laikad.kf_valid(m.logMonoTime * 1e-9)))
kf_valid = False
for m in self.logs:
laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime, block=True)
laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True)
kf_valid = all(laikad.kf_valid(m.logMonoTime * 1e-9))
if kf_valid:
break
@@ -201,7 +201,7 @@ class TestLaikad(unittest.TestCase):
laikad = Laikad(auto_update=False)
has_orbits = False
for m in self.logs:
laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime, block=False)
laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=False)
if laikad.orbit_fetch_future is not None:
laikad.orbit_fetch_future.result()
vals = laikad.astro_dog.orbits.values()