mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 22:23:56 +08:00
Code Issues Packages Projects Releases Wiki Activity

Revert "lagd: disable in release (#35426)" and "lagd calib: hide on release (#35523)" (#35633)

Browse Source 
* Revert "lagd: disable in release (#35426)"

This reverts commit 6f6adc10a8.

* Revert "lagd calib: hide on release (#35523)"

This reverts commit e345f25ce4.

* Fix conflict
This commit is contained in:
Kacper Rączy
2025-07-07 10:18:31 +02:00
committed by GitHub
parent a8ec08e5bb
commit 44c8cc4cb0
3 changed files with 19 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
selfdrive/locationd/lagd.py
View File

@@ -157,8 +157,7 @@ class LateralLagEstimator:
block_count: int = BLOCK_NUM, min_valid_block_count: int = BLOCK_NUM_NEEDED, block_size: int = BLOCK_SIZE,
window_sec: float = MOVING_WINDOW_SEC, okay_window_sec: float = MIN_OKAY_WINDOW_SEC, min_recovery_buffer_sec: float = MIN_RECOVERY_BUFFER_SEC,
min_vego: float = MIN_VEGO, min_yr: float = MIN_ABS_YAW_RATE, min_ncc: float = MIN_NCC,
max_lat_accel: float = MAX_LAT_ACCEL, max_lat_accel_diff: float = MAX_LAT_ACCEL_DIFF, min_confidence: float = MIN_CONFIDENCE,
enabled: bool = True):
max_lat_accel: float = MAX_LAT_ACCEL, max_lat_accel_diff: float = MAX_LAT_ACCEL_DIFF, min_confidence: float = MIN_CONFIDENCE):
self.dt = dt
self.window_sec = window_sec
self.okay_window_sec = okay_window_sec
@@ -173,7 +172,6 @@ class LateralLagEstimator:
self.min_confidence = min_confidence
self.max_lat_accel = max_lat_accel
self.max_lat_accel_diff = max_lat_accel_diff
self.enabled = enabled
self.t = 0.0
self.lat_active = False
@@ -208,7 +206,7 @@ class LateralLagEstimator:
liveDelay = msg.liveDelay
valid_mean_lag, valid_std, current_mean_lag, current_std = self.block_avg.get()
if self.enabled and self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag) and not np.isnan(valid_std):
if self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag) and not np.isnan(valid_std):
if valid_std > MAX_LAG_STD:
liveDelay.status = log.LiveDelayData.Status.invalid
else:
@@ -371,10 +369,7 @@ def main():
params = Params()
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
# TODO: remove me, lagd is in shadow mode on release
is_release = params.get_bool("IsReleaseBranch")
lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency, enabled=not is_release)
lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency)
if (initial_lag_params := retrieve_initial_lag(params, CP)) is not None:
lag, valid_blocks = initial_lag_params
lag_learner.reset(lag, valid_blocks)

13
selfdrive/locationd/test/test_lagd.py
View File

@@ -110,19 +110,6 @@ class TestLagd:
assert msg.liveDelay.validBlocks == BLOCK_NUM_NEEDED
assert msg.liveDelay.calPerc == 100
def test_disabled_estimator(self):
mocked_CP = car.CarParams(steerActuatorDelay=0.8)
estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0, enabled=False)
lag_frames = 5
process_messages(estimator, lag_frames, int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_NUM_NEEDED * BLOCK_SIZE)
msg = estimator.get_msg(True)
assert msg.liveDelay.status == 'unestimated'
assert np.allclose(msg.liveDelay.lateralDelay, 1.0, atol=0.01)
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)
estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0, min_valid_block_count=1)

37
selfdrive/ui/qt/offroad/settings.cc
View File

@@ -333,26 +333,22 @@ void DevicePanel::updateCalibDescription() {
}
}
const bool is_release = params.getBool("IsReleaseBranch");
if (!is_release) {
int lag_perc = 0;
std::string lag_bytes = params.get("LiveDelay");
if (!lag_bytes.empty()) {
try {
AlignedBuffer aligned_buf;
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(lag_bytes.data(), lag_bytes.size()));
lag_perc = cmsg.getRoot<cereal::Event>().getLiveDelay().getCalPerc();
} catch (kj::Exception) {
qInfo() << "invalid LiveDelay";
}
}
desc += "\n\n";
if (lag_perc < 100) {
desc += tr("Steering lag calibration is %1% complete.").arg(lag_perc);
} else {
desc += tr("Steering lag calibration is complete.");
int lag_perc = 0;
std::string lag_bytes = params.get("LiveDelay");
if (!lag_bytes.empty()) {
try {
AlignedBuffer aligned_buf;
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(lag_bytes.data(), lag_bytes.size()));
lag_perc = cmsg.getRoot<cereal::Event>().getLiveDelay().getCalPerc();
} catch (kj::Exception) {
qInfo() << "invalid LiveDelay";
}
}
if (lag_perc < 100) {
desc += tr("\n\nSteering lag calibration is %1% complete.").arg(lag_perc);
} else {
desc += tr("\n\nSteering lag calibration is complete.");
}
std::string torque_bytes = params.get("LiveTorqueParameters");
if (!torque_bytes.empty()) {
@@ -363,11 +359,10 @@ void DevicePanel::updateCalibDescription() {
// don't add for non-torque cars
if (torque.getUseParams()) {
int torque_perc = torque.getCalPerc();
desc += is_release ? "\n\n" : " ";
if (torque_perc < 100) {
desc += tr("Steering torque response calibration is %1% complete.").arg(torque_perc);
desc += tr(" Steering torque response calibration is %1% complete.").arg(torque_perc);
} else {
desc += tr("Steering torque response calibration is complete.");
desc += tr(" Steering torque response calibration is complete.");
}
}
} catch (kj::Exception) {