fix all rounding errors

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])