diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 5d25ce159d..10f3bde0fd 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -1,5 +1,6 @@ from collections import deque import copy +import math from cereal import car from common.conversions import Conversions as CV @@ -47,7 +48,7 @@ class CarState(CarStateBase): self.frame += 1 self.is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0 - speed_conv = CV.MPH_TO_MS if cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 1 else CV.KPH_TO_MS + speed_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS ret = car.CarState.new_message() @@ -68,19 +69,26 @@ class CarState(CarStateBase): ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.standstill = ret.vEgoRaw < 0.1 - dash_speed = cp.vl["CLU15"]["CF_Clu_VehicleSpeed2"] - self.dash_speed_seen = self.dash_speed_seen or dash_speed > 1e-3 - if self.dash_speed_seen: - ret.vEgoCluster = dash_speed * speed_conv + # dash_speed = cp.vl["CLU15"]["CF_Clu_VehicleSpeed2"] + # self.dash_speed_seen = self.dash_speed_seen or dash_speed > 1e-3 + # if self.dash_speed_seen: + # ret.vEgoCluster = dash_speed * speed_conv - if len(cp.vl_all["CLU15"]["CF_Clu_VehicleSpeed"]): - self.updates += 1 - print(self.updates, self.frame) - if self.frame > 50: + # on some cars, CLU15 can be 12+ Hz and noisy (expected only 4 Hz), while the dash likely only samples at a much lower rate + if self.frame > 25: # 5 Hz self.frame = 0 self.dash_speed_alt = cp.vl["CLU15"]["CF_Clu_VehicleSpeed"] - ret.vEgoCluster = self.dash_speed_alt * CV.KPH_TO_MS + if self.is_metric: + # TODO: do we need to do anything when it's kph? likely not besides the oscillation that's already taken care of + ret.vEgoCluster = self.dash_speed_alt + else: + # dash applies some weird rounding + # TODO: debug comment, will be removed + # for example, 117 kph is 72.7 (73) mph, but 115 is 71.45 mph (which would be rounded to 71 normally, but dash shows 72 mph) + # without this and rounding normally, the C3 will never hit the speeds: 77 mph, 72 mph, 67 mph, 59 mph, 54 mph, 49 mph, ... 26 mph, 18 mph, 13 mph, 8 mph, 3 mph + # 0.6 also worked, but was wrong on a few speeds. 0.62... or kph_to_mph made the most sense and was the most correct + ret.vEgoCluster = math.floor(self.dash_speed_alt * CV.KPH_TO_MPH + CV.KPH_TO_MPH) * CV.MPH_TO_MS self.dat.append([ret.vEgo, ret.vEgoRaw, ret.vEgoCluster, cp.vl["CLU15"]["CF_Clu_VehicleSpeed"], self.dash_speed_seen, self.dash_speed_alt])