diff --git a/cereal/log.capnp b/cereal/log.capnp index 9ab51e0b77..102118c3b6 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2291,6 +2291,7 @@ struct LiveDelayData { lateralDelayEstimate @3 :Float32; lateralDelayEstimateStd @5 :Float32; points @4 :List(Float32); + calPerc @6 :Int8; enum Status { unestimated @0; diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index e070c16b1c..e6767db473 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -229,6 +229,8 @@ class LateralLagEstimator: liveDelay.lateralDelayEstimateStd = 0.0 liveDelay.validBlocks = self.block_avg.valid_blocks + liveDelay.calPerc = min(100 * (self.block_avg.valid_blocks * self.block_size + self.block_avg.idx) // + (self.min_valid_block_count * self.block_size), 100) if debug: liveDelay.points = self.block_avg.values.flatten().tolist() diff --git a/selfdrive/locationd/test/test_lagd.py b/selfdrive/locationd/test/test_lagd.py index a7f9c75ab4..8fb829edd8 100644 --- a/selfdrive/locationd/test/test_lagd.py +++ b/selfdrive/locationd/test/test_lagd.py @@ -94,6 +94,7 @@ class TestLagd: assert np.allclose(msg.liveDelay.lateralDelay, estimator.initial_lag) assert np.allclose(msg.liveDelay.lateralDelayEstimate, estimator.initial_lag) assert msg.liveDelay.validBlocks == 0 + assert msg.liveDelay.calPerc == 0 def test_estimator_basics(self, subtests): for lag_frames in range(5): @@ -107,6 +108,7 @@ class TestLagd: assert np.allclose(msg.liveDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01) assert np.allclose(msg.liveDelay.lateralDelayEstimateStd, 0.0, atol=0.01) assert msg.liveDelay.validBlocks == BLOCK_NUM_NEEDED + assert msg.liveDelay.calPerc == 100 def test_disabled_estimator(self): mocked_CP = car.CarParams(steerActuatorDelay=0.8) @@ -119,6 +121,7 @@ class TestLagd: assert np.allclose(msg.liveDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01) assert np.allclose(msg.liveDelay.lateralDelayEstimateStd, 0.0, atol=0.01) assert msg.liveDelay.validBlocks == BLOCK_NUM_NEEDED + assert msg.liveDelay.calPerc == 100 def test_estimator_masking(self): mocked_CP, lag_frames = car.CarParams(steerActuatorDelay=0.8), random.randint(1, 19) @@ -127,6 +130,7 @@ class TestLagd: msg = estimator.get_msg(True) assert np.allclose(msg.liveDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01) assert np.allclose(msg.liveDelay.lateralDelayEstimateStd, 0.0, atol=0.01) + assert msg.liveDelay.calPerc == 100 @pytest.mark.skipif(PC, reason="only on device") @pytest.mark.timeout(60)