diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 26b4465794..446b2bdde9 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -69,14 +69,16 @@ class CarState(CarStateBase): if self.accurate_dash_speed_seen: self.vEgoCluster = accurate_dash_speed * speed_conv else: - self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2.5 # seems to be best at 0.111111 m/s + self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 0.5 # seems to be best at 0.111111 m/s self.cluster_min_speed = CV.KPH_TO_MS # TODO: verify this is always metric ret.vEgoCluster = cp.vl["CLU15"]["CF_Clu_VehicleSpeed"] * CV.KPH_TO_MS - # # i've seen vEgoRaw get to 1-2 before cluster updates, so pick 6 mph to be safe - # if ret.vEgoRaw > 3 and len(cp.vl_all["CLU15"]["CF_Clu_VehicleSpeed"]): - # assert cp.vl["CLU15"]["CF_Clu_VehicleSpeed"] > 0, (ret.vEgoRaw, cp.vl["CLU15"]["CF_Clu_VehicleSpeed2"], cp.vl["CLU15"]["CF_Clu_VehicleSpeed"] * speed_conv) + # i've seen vEgoRaw get to 1-2 before cluster updates, so pick 6 mph to be safe + if ret.vEgoRaw > 3 and len(cp.vl_all["CLU15"]["CF_Clu_VehicleSpeed"]): + assert abs(cp.vl["CLU15"]["CF_Clu_VehicleSpeed"] * CV.KPH_TO_MS - ret.vEgoRaw) < 2.5 + + assert cp.vl["CLU15"]["CF_Clu_VehicleSpeed"] > 0, (ret.vEgoRaw, cp.vl["CLU15"]["CF_Clu_VehicleSpeed2"], cp.vl["CLU15"]["CF_Clu_VehicleSpeed"] * speed_conv) ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"] ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"]