mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-28 01:52:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add lagd calibration percentage (#35511)

Browse Source 
* lagd: publish calibration percentage

* Refine lagd calibration progress

* stash

* cleanup (match calibrationd calculation logic)

* no no no

* nor

* two lines
This commit is contained in:
Shane Smiskol
2025-06-09 22:10:47 -07:00
committed by GitHub
parent 1c11e28448
commit 75b6ec68c6
3 changed files with 7 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/log.capnp
+1
View File
@@ -2291,6 +2291,7 @@ struct LiveDelayData {
lateralDelayEstimate @3 :Float32;
lateralDelayEstimateStd @5 :Float32;
points @4 :List(Float32);
calPerc @6 :Int8;
enum Status {
unestimated @0;
selfdrive/locationd/lagd.py
+2
View File
@@ -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()
selfdrive/locationd/test/test_lagd.py
+4
View File
@@ -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)