diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index e6767db473..d7834f7f1f 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -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) diff --git a/selfdrive/locationd/test/test_lagd.py b/selfdrive/locationd/test/test_lagd.py index 8fb829edd8..a3dfce9c29 100644 --- a/selfdrive/locationd/test/test_lagd.py +++ b/selfdrive/locationd/test/test_lagd.py @@ -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) diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index e242aa0878..96734d69d9 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -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().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().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) {