Use the delay it in the model

pull/34531/head
Kacper Rączy 3 months ago
parent 53e0bdc4c1
commit a1a69b2934
  1. 6
      selfdrive/modeld/modeld.py

@ -184,7 +184,7 @@ def main(demo=False):
# messaging # messaging
pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"]) pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl"]) sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveActuatorDelay"])
publish_state = PublishState() publish_state = PublishState()
params = Params() params = Params()
@ -209,9 +209,6 @@ def main(demo=False):
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
cloudlog.info("modeld got CarParams: %s", CP.brand) cloudlog.info("modeld got CarParams: %s", CP.brand)
# TODO this needs more thought, use .2s extra for now to estimate other delays
steer_delay = CP.steerActuatorDelay + .2
DH = DesireHelper() DH = DesireHelper()
while True: while True:
@ -252,6 +249,7 @@ def main(demo=False):
is_rhd = sm["driverMonitoringState"].isRHD is_rhd = sm["driverMonitoringState"].isRHD
frame_id = sm["roadCameraState"].frameId frame_id = sm["roadCameraState"].frameId
v_ego = max(sm["carState"].vEgo, 0.) v_ego = max(sm["carState"].vEgo, 0.)
steer_delay = sm["liveActuatorDelay"].steerActuatorDelay
lateral_control_params = np.array([v_ego, steer_delay], dtype=np.float32) lateral_control_params = np.array([v_ego, steer_delay], dtype=np.float32)
if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)

Loading…
Cancel
Save