new msgs default to invalid (#30587)

* new msgs default to invalid

* fix lm

* set more valid

* update rest

* update refs

* fix logMessage

* more valids

* cleanup

* fix llk test

* pigeond is also valid

* more valids
old-commit-hash: a1d36961cf
chrysler-long2
Adeeb Shihadeh 1 year ago committed by GitHub
parent b761d0efb7
commit 1cc4f1a634
  1. 2
      cereal
  2. 4
      selfdrive/controls/controlsd.py
  3. 1
      selfdrive/controls/radard.py
  4. 24
      selfdrive/locationd/calibrationd.py
  5. 1
      selfdrive/locationd/test/test_locationd.py
  6. 2
      selfdrive/manager/manager.py
  7. 2
      selfdrive/modeld/dmonitoringmodeld.py
  8. 2
      selfdrive/monitoring/dmonitoringd.py
  9. 4
      selfdrive/navd/navd.py
  10. 2
      selfdrive/test/process_replay/migration.py
  11. 2
      selfdrive/test/process_replay/ref_commit
  12. 2
      selfdrive/thermald/thermald.py
  13. 2
      system/loggerd/uploader.py
  14. 6
      system/logmessaged.py
  15. 2
      system/micd.py
  16. 8
      system/qcomgpsd/qcomgpsd.py
  17. 2
      system/sensord/pigeond.py

@ -1 +1 @@
Subproject commit 60d1ee0490a33830168b30a1e92f8d4e43cfbb15
Subproject commit 033dad2b370dac411479de19381f6e0253e51e01

@ -80,7 +80,7 @@ class Controls:
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
'testJoystick'] + self.camera_packets + self.sensor_packets,
ignore_alive=ignore, ignore_avg_freq=['radarState', 'testJoystick'])
ignore_alive=ignore, ignore_avg_freq=['radarState', 'testJoystick'], ignore_valid=['testJoystick', ])
if CI is None:
# wait for one pandaState and one CAN packet
@ -824,6 +824,7 @@ class Controls:
# onroadEvents - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev):
ce_send = messaging.new_message('onroadEvents', len(self.events))
ce_send.valid = True
ce_send.onroadEvents = car_events
self.pm.send('onroadEvents', ce_send)
self.events_prev = self.events.names.copy()
@ -831,6 +832,7 @@ class Controls:
# carParams - logged every 50 seconds (> 1 per segment)
if (self.sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message('carParams')
cp_send.valid = True
cp_send.carParams = self.CP
self.pm.send('carParams', cp_send)

@ -270,6 +270,7 @@ class RadarD:
# publish tracks for UI debugging (keep last)
tracks_msg = messaging.new_message('liveTracks', len(self.tracks))
tracks_msg.valid = self.radar_state_valid
for index, tid in enumerate(sorted(self.tracks.keys())):
tracks_msg.liveTracks[index] = {
"trackId": tid,

@ -164,7 +164,7 @@ class Calibrator:
write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5)
if self.param_put and write_this_cycle:
put_nonblocking("CalibrationParams", self.get_msg().to_bytes())
put_nonblocking("CalibrationParams", self.get_msg(True).to_bytes())
def handle_v_ego(self, v_ego: float) -> None:
self.v_ego = v_ego
@ -227,12 +227,13 @@ class Calibrator:
return new_rpy
def get_msg(self) -> capnp.lib.capnp._DynamicStructBuilder:
def get_msg(self, valid: bool) -> capnp.lib.capnp._DynamicStructBuilder:
smooth_rpy = self.get_smooth_rpy()
msg = messaging.new_message('liveCalibration')
liveCalibration = msg.liveCalibration
msg.valid = valid
liveCalibration = msg.liveCalibration
liveCalibration.validBlocks = self.valid_blocks
liveCalibration.calStatus = self.cal_status
liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)
@ -250,19 +251,16 @@ class Calibrator:
return msg
def send_data(self, pm: messaging.PubMaster) -> None:
pm.send('liveCalibration', self.get_msg())
def send_data(self, pm: messaging.PubMaster, valid: bool) -> None:
pm.send('liveCalibration', self.get_msg(valid))
def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[messaging.PubMaster] = None) -> NoReturn:
def main() -> NoReturn:
gc.disable()
set_realtime_priority(1)
if sm is None:
sm = messaging.SubMaster(['cameraOdometry', 'carState', 'carParams'], poll=['cameraOdometry'])
if pm is None:
pm = messaging.PubMaster(['liveCalibration'])
sm = messaging.SubMaster(['cameraOdometry', 'carState', 'carParams'], poll=['cameraOdometry'])
calibrator = Calibrator(param_put=True)
@ -286,11 +284,7 @@ def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[m
# 4Hz driven by cameraOdometry
if sm.frame % 5 == 0:
calibrator.send_data(pm)
def main(sm: Optional[messaging.SubMaster] = None, pm: Optional[messaging.PubMaster] = None) -> NoReturn:
calibrationd_thread(sm, pm)
calibrator.send_data(pm, sm.all_checks())
if __name__ == "__main__":

@ -60,6 +60,7 @@ class TestLocationdProc(unittest.TestCase):
msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
msg.logMonoTime = t
msg.valid = True
return msg
def test_params_gps(self):

@ -171,7 +171,7 @@ def manager_thread() -> None:
cloudlog.debug(running)
# send managerState
msg = messaging.new_message('managerState')
msg = messaging.new_message('managerState', valid=True)
msg.managerState.processes = [p.get_process_state_msg() for p in managed_processes.values()]
pm.send('managerState', msg)

@ -101,7 +101,7 @@ def fill_driver_state(msg, ds_result: DriverStateResult):
def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts: int, execution_time: float, dsp_execution_time: float):
model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(DMonitoringModelResult)).contents
msg = messaging.new_message('driverStateV2')
msg = messaging.new_message('driverStateV2', valid=True)
ds = msg.driverStateV2
ds.frameId = frame_id
ds.modelExecutionTime = execution_time

@ -54,7 +54,7 @@ def dmonitoringd_thread():
driver_status.update_events(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill)
# build driverMonitoringState packet
dat = messaging.new_message('driverMonitoringState')
dat = messaging.new_message('driverMonitoringState', valid=sm.all_checks())
dat.driverMonitoringState = {
"events": events.to_msg(),
"faceDetected": driver_status.face_detected,

@ -196,7 +196,7 @@ class RouteEngine:
self.send_route()
def send_instruction(self):
msg = messaging.new_message('navInstruction')
msg = messaging.new_message('navInstruction', valid=True)
if self.step_idx is None:
msg.valid = False
@ -302,7 +302,7 @@ class RouteEngine:
for path in self.route_geometry:
coords += [c.as_dict() for c in path]
msg = messaging.new_message('navRoute')
msg = messaging.new_message('navRoute', valid=True)
msg.navRoute.coordinates = coords
self.pm.send('navRoute', msg)

@ -85,6 +85,7 @@ def migrate_peripheralState(lr):
continue
new_msg = messaging.new_message("peripheralState")
new_msg.valid = msg.valid
new_msg.logMonoTime = msg.logMonoTime
all_msg.append(new_msg.as_reader())
@ -149,6 +150,7 @@ def migrate_carParams(lr, old_logtime=False):
for msg in lr:
if msg.which() == 'carParams':
CP = messaging.new_message('carParams')
CP.valid = True
CP.carParams = msg.carParams.as_builder()
for car_fw in CP.carParams.carFw:
car_fw.brand = CP.carParams.carName

@ -1 +1 @@
a42195085f2b92df02d13d60a8cee80354a84c7a
921222d49db204071f0a7006fc895690e1045b5d

@ -81,7 +81,7 @@ def read_tz(x):
def read_thermal(thermal_config):
dat = messaging.new_message('deviceState')
dat = messaging.new_message('deviceState', valid=True)
dat.deviceState.cpuTempC = [read_tz(z) / thermal_config.cpu[1] for z in thermal_config.cpu[0]]
dat.deviceState.gpuTempC = [read_tz(z) / thermal_config.gpu[1] for z in thermal_config.gpu[0]]
dat.deviceState.memoryTempC = read_tz(thermal_config.mem[0]) / thermal_config.mem[1]

@ -228,7 +228,7 @@ class Uploader:
return success
def get_msg(self):
msg = messaging.new_message("uploaderState")
msg = messaging.new_message("uploaderState", valid=True)
us = msg.uploaderState
us.immediateQueueSize = int(self.immediate_size / 1e6)
us.immediateQueueCount = self.immediate_count

@ -35,13 +35,11 @@ def main() -> NoReturn:
continue
# then we publish them
msg = messaging.new_message()
msg.logMessage = record
msg = messaging.new_message(None, valid=True, logMessage=record)
log_message_sock.send(msg.to_bytes())
if level >= 40: # logging.ERROR
msg = messaging.new_message()
msg.errorLogMessage = record
msg = messaging.new_message(None, valid=True, errorLogMessage=record)
error_log_message_sock.send(msg.to_bytes())
finally:
sock.close()

@ -53,7 +53,7 @@ class Mic:
self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False)
def update(self):
msg = messaging.new_message('microphone')
msg = messaging.new_message('microphone', valid=True)
msg.microphone.soundPressure = float(self.sound_pressure)
msg.microphone.soundPressureWeighted = float(self.sound_pressure_weighted)

@ -321,7 +321,7 @@ def main() -> NoReturn:
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')
msg = messaging.new_message('qcomGnss', valid=True)
gnss = msg.qcomGnss
gnss.logTs = log_time
@ -370,7 +370,7 @@ def main() -> NoReturn:
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')
msg = messaging.new_message('gpsLocation', valid=True)
gps = msg.gpsLocation
gps.latitude = report["t_DblFinalPosLatLon[0]"] * 180/math.pi
gps.longitude = report["t_DblFinalPosLatLon[1]"] * 180/math.pi
@ -396,7 +396,7 @@ def main() -> NoReturn:
pm.send('gpsLocation', msg)
elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT:
msg = messaging.new_message('qcomGnss')
msg = messaging.new_message('qcomGnss', valid=True)
dat = unpack_svpoly(log_payload)
dat = relist(dat)
gnss = msg.qcomGnss
@ -433,7 +433,7 @@ def main() -> NoReturn:
pm.send('qcomGnss', msg)
elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]:
msg = messaging.new_message('qcomGnss')
msg = messaging.new_message('qcomGnss', valid=True)
gnss = msg.qcomGnss
gnss.logTs = log_time

@ -291,7 +291,7 @@ def run_receiving(pigeon: TTYPigeon, pm: messaging.PubMaster, duration: int = 0)
continue
# send out to socket
msg = messaging.new_message('ubloxRaw', len(dat))
msg = messaging.new_message('ubloxRaw', len(dat), valid=True)
msg.ubloxRaw = dat[:]
pm.send('ubloxRaw', msg)
else:

Loading…
Cancel
Save