mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-05 13:32:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

Laikad: always return msg (#27599)

Browse Source 
* always return msg

* always return msg

* Not all qcom constellations are understood

* call error error
old-commit-hash: 5c383927859a20feccdccaf0102f8d0924d28134
This commit is contained in:
Harald Schäfer
2023-03-16 13:29:02 -07:00
committed by GitHub
parent 30ddef446a
commit d9cfe0221e
1 changed files with 16 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/laikad.py
+16 -22
View File
@@ -125,8 +125,15 @@ class Laikad:
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]
# TODO: Understand and use remaining unknown constellations
try:
good_constellation = constellation_id in [ConstellationId.GPS, ConstellationId.SBAS]
except NotImplementedError:
good_constellation = False
# gpsWeek 65535 is received rarely from quectel, this cannot be
# passed to GnssMeasurements's gpsWeek (Int16)
good_week = not getattr(gnss_msg, gnss_msg.which()).gpsWeek > np.iinfo(np.int16).max
return good_constellation and good_week
elif gnss_msg.which() == 'measurementReport' and not self.use_qcom:
return True
else:
@@ -196,15 +203,15 @@ class Laikad:
return position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, processed_measurements
def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False):
out_msg = messaging.new_message("gnssMeasurements")
if self.is_ephemeris(gnss_msg):
self.read_ephemeris(gnss_msg)
return None
return out_msg
elif self.is_good_report(gnss_msg):
week, tow, new_meas = self.read_report(gnss_msg)
self.gps_week = week
if len(new_meas) == 0:
return None
return out_msg
t = gnss_mono_time * 1e-9
if week > 0:
@@ -215,17 +222,16 @@ class Laikad:
output = self.process_report(new_meas, t)
if output is None:
return None
return out_msg
position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, _ = output
self.update_localizer(position_estimate, t, corrected_measurements)
meas_msgs = [create_measurement_msg(m) for m in corrected_measurements]
msg = messaging.new_message("gnssMeasurements")
measurement_msg = log.LiveLocationKalman.Measurement.new_message
P_diag = self.gnss_kf.P.diagonal()
kf_valid = all(self.kf_valid(t))
msg.gnssMeasurements = {
out_msg.gnssMeasurements = {
"gpsWeek": week,
"gpsTimeOfWeek": tow,
"kalmanPositionECEF": measurement_msg(value=self.gnss_kf.x[GStates.ECEF_POS].tolist(),
@@ -240,7 +246,7 @@ class Laikad:
"measTime": gnss_mono_time,
"correctedMeasurements": meas_msgs
}
return msg
return out_msg
#elif gnss_msg.which() == 'ionoData':
# TODO: add this, Needed to better correct messages offline. First fix ublox_msg.cc to sent them.
@@ -377,15 +383,6 @@ class EphemerisSourceType(IntEnum):
def process_msg(laikad, gnss_msg, mono_time, block=False):
# TODO: Understand and use remaining unknown constellations
if gnss_msg.which() == "drMeasurementReport":
if getattr(gnss_msg, gnss_msg.which()).source not in ['glonass', 'gps', 'beidou', 'sbas']:
return None
if getattr(gnss_msg, gnss_msg.which()).gpsWeek > np.iinfo(np.int16).max:
# gpsWeek 65535 is received rarely from quectel, this cannot be
# passed to GnssMeasurements's gpsWeek (Int16)
return None
return laikad.process_gnss_msg(gnss_msg, mono_time, block=block)
@@ -424,10 +421,7 @@ def main(sm=None, pm=None, qc=None):
if sm.updated[raw_gnss_socket]:
gnss_msg = sm[raw_gnss_socket]
msg = process_msg(laikad, gnss_msg, sm.logMonoTime[raw_gnss_socket], replay)
if msg is None:
# TODO: beautify this, locationd needs a valid message
msg = messaging.new_message("gnssMeasurements")
msg = laikad.process_gnss_msg(gnss_msg, sm.logMonoTime[raw_gnss_socket], replay)
pm.send('gnssMeasurements', msg)
if not laikad.got_first_gnss_msg and sm.updated['clocks']: