diff --git a/cereal/log.capnp b/cereal/log.capnp index e7edfd4c36..af0533f69d 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2275,12 +2275,17 @@ struct LiveTorqueParametersData { } struct LiveDelayData { - steerActuatorDelay @0 :Float32; - totalPoints @1 :Int32; - points @2 :List(List(Float32)); - isEstimated @3 :Bool; - correlation @4 :Float32; - validBlocks @5 :Int32; + lateralDelay @0 :Float32; + validBlocks @1 :Int32; + status @2 :Status; + + estimatedLateralDelay @3 :Float32; + points @4 :List(Float32); + + enum Status { + initial @0; + estimated @1; + } } struct LiveMapDataDEPRECATED { @@ -2513,7 +2518,7 @@ struct Event { gnssMeasurements @91 :GnssMeasurements; liveParameters @61 :LiveParametersData; liveTorqueParameters @94 :LiveTorqueParametersData; - liveActuatorDelay @146 : LiveDelayData; + liveDelay @146 : LiveDelayData; cameraOdometry @63 :CameraOdometry; thumbnail @66: Thumbnail; onroadEvents @134: List(OnroadEvent); diff --git a/cereal/services.py b/cereal/services.py index 416418f08d..82fc04bd00 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -36,7 +36,7 @@ _services: dict[str, tuple] = { "errorLogMessage": (True, 0., 1), "liveCalibration": (True, 4., 4), "liveTorqueParameters": (True, 4., 1), - "liveActuatorDelay": (True, 4., 1), + "liveDelay": (True, 4., 1), "androidLog": (True, 0.), "carState": (True, 100., 10), "carControl": (True, 100., 10), diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index c4d6fc9189..aa0acf9bcc 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -12,6 +12,7 @@ from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose BLOCK_SIZE = 100 BLOCK_NUM = 50 +BLOCK_NUM_NEEDED = 5 MOVING_WINDOW_SEC = 300.0 MIN_OKAY_WINDOW_SEC = 30.0 MIN_RECOVERY_BUFFER_SEC = 2.0 @@ -134,12 +135,12 @@ class BlockAverage: valid_block_idx = [i for i in range(self.valid_blocks) if i != self.block_idx] if not valid_block_idx: return None - return np.mean(self.values[valid_block_idx], axis=0) + return float(np.mean(self.values[valid_block_idx], axis=0).item()) class LagEstimator: def __init__(self, CP, dt, - block_count=BLOCK_NUM, block_size=BLOCK_SIZE, + block_count=BLOCK_NUM, min_valid_block_count=BLOCK_NUM_NEEDED, block_size=BLOCK_SIZE, window_sec=MOVING_WINDOW_SEC, okay_window_sec=MIN_OKAY_WINDOW_SEC, min_recovery_buffer_sec=MIN_RECOVERY_BUFFER_SEC, min_vego=MIN_VEGO, min_yr=MIN_ABS_YAW_RATE, min_ncc=MIN_NCC): self.dt = dt @@ -149,6 +150,7 @@ class LagEstimator: self.initial_lag = CP.steerActuatorDelay + 0.2 self.block_size = block_size self.block_count = block_count + self.min_valid_block_count = min_valid_block_count self.min_vego = min_vego self.min_yr = min_yr self.min_ncc = min_ncc @@ -173,18 +175,25 @@ class LagEstimator: window_len = int(self.window_sec / self.dt) self.points = Points(window_len) self.block_avg = BlockAverage(self.block_count, self.block_size, valid_blocks, initial_lag) - self.lag = initial_lag def get_msg(self, valid): - msg = messaging.new_message('liveActuatorDelay') + msg = messaging.new_message('liveDelay') msg.valid = valid - liveDelay = msg.liveActuatorDelay - liveDelay.steerActuatorDelay = self.lag - liveDelay.isEstimated = self.block_avg.valid_blocks > 0 + liveDelay = msg.liveDelay + + estimated_lag = self.block_avg.get() + liveDelay.estimatedLateralDelay = estimated_lag or self.initial_lag + if self.block_avg.valid_blocks >= self.min_valid_block_count and estimated_lag is not None: + liveDelay.status = log.LiveDelayData.Status.estimated + liveDelay.lateralDelay = estimated_lag + else: + liveDelay.status = log.LiveDelayData.Status.initial + liveDelay.lateralDelay = self.initial_lag liveDelay.validBlocks = self.block_avg.valid_blocks - liveDelay.points = self.block_avg.values.tolist() + # TODO only in debug + liveDelay.points = self.block_avg.values.flatten().tolist() return msg @@ -238,8 +247,6 @@ class LagEstimator: return self.block_avg.update(delay) - if (new_lag := self.block_avg.get()) is not None: - self.lag = float(new_lag.item()) def correlation_lags(self, sig_len, dt): return np.arange(0, sig_len) * dt @@ -261,7 +268,7 @@ class LagEstimator: def main(): config_realtime_process([0, 1, 2, 3], 5) - pm = messaging.PubMaster(['liveActuatorDelay', 'alertDebug']) + pm = messaging.PubMaster(['liveDelay', 'alertDebug']) sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carControl', 'carState', 'controlsState'], poll='livePose') params = Params() @@ -272,8 +279,8 @@ def main(): if lag_params: try: with log.Event.from_bytes(lag_params) as msg: - lag_init = msg.liveActuatorDelay.steerActuatorDelay - valid_blocks = msg.liveActuatorDelay.validBlocks + lag_init = msg.liveDelay.estimatedLateralDelay + valid_blocks = msg.liveDelay.validBlocks estimator.reset(lag_init, valid_blocks) except Exception: print("Error reading cached LagParams") @@ -289,15 +296,17 @@ def main(): if sm.frame % 25 == 0: msg = estimator.get_msg(sm.all_checks()) + + # TODO remove alert_msg = messaging.new_message('alertDebug') alert_msg.alertDebug.alertText1 = f"Lag estimate (fixed: {CP.steerActuatorDelay:.2f} s)" - alert_msg.alertDebug.alertText2 = f"{msg.liveActuatorDelay.steerActuatorDelay:.2f} s ({msg.liveActuatorDelay.isEstimated})" - - pm.send('liveActuatorDelay', msg) + alert_msg.alertDebug.alertText2 = f"{msg.liveDelay.lateralDelay:.2f} s ({msg.liveDelay.status == 'estimated'})" pm.send('alertDebug', alert_msg) - if msg.liveActuatorDelay.isEstimated: # TODO maybe to often once estimated - params.put_nonblocking("LiveLag", msg.to_bytes()) + msg_dat = msg.to_bytes() + if msg.liveDelay.status == 'estimated': # TODO maybe to often once estimated + params.put_nonblocking("LiveLag", msg_dat) + pm.send('liveDelay', msg_dat) if __name__ == "__main__": diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index c90ff02ad8..8c3c8ba4d6 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -195,7 +195,7 @@ def main(demo=False): # messaging pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"]) - sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveActuatorDelay"]) + sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"]) publish_state = PublishState() params = Params() @@ -260,7 +260,7 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) - steer_delay = sm["liveActuatorDelay"].steerActuatorDelay + steer_delay = sm["liveDelay"].lateralDelay lateral_control_params = np.array([v_ego, steer_delay], dtype=np.float32) if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 68e5924085..18339e8887 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -572,7 +572,7 @@ CONFIGS = [ ProcessConfig( proc_name="lagd", pubs=["carState", "carControl", "controlsState", "livePose", "liveCalibration"], - subs=["liveActuatorDelay"], + subs=["liveDelay"], ignore=["logMonoTime"], init_callback=get_car_params_callback, should_recv_callback=lagd_rcv_callback,