From f73b041d43a1ece437df38e26b1dd30759a79c9f Mon Sep 17 00:00:00 2001
From: Willem Melching <willem.melching@gmail.com>
Date: Sat, 17 Sep 2022 02:33:38 +0200
Subject: [PATCH] Hyundai: match ego speed on dash (#25235)

* hyundai: match speed on dash

* still needs conversion to m/s

* always use CF_Clu_VehicleSpeed2

* clean up, like honda

* experiment

* to the source

* works pretty well on optima (matches exactly on Sonata)

* could be 0.5

* clean up test

* revert test_moedls

revert test_moedls

* woops

* woops.

* .

* fix hyst

* only CF_Clu_VehicleSpeed

* omgomgomg

* add all this mess because it always takes a while

* set vEgoCluster

* fix all rounding errors

* stash

* clean up

* clean up

* fix metric conversion

* only calculate when updated

* try to filter (didn't look great from plots)

* Revert "try to filter (didn't look great from plots)"

This reverts commit 7e9876c237341d07163985b0718fd9c553372e72.

* clean up

* update refs

Co-authored-by: Shane Smiskol <shane@smiskol.com>
---
 selfdrive/car/hyundai/carstate.py        | 26 ++++++++++++++++++++----
 selfdrive/test/process_replay/ref_commit |  2 +-
 2 files changed, 23 insertions(+), 5 deletions(-)

diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
index eab6e73f1f..61da04d04b 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
@@ -9,6 +10,7 @@ from selfdrive.car.hyundai.values import HyundaiFlags, DBC, FEATURES, CAMERA_SCC
 from selfdrive.car.interfaces import CarStateBase
 
 PREV_BUTTON_SAMPLES = 8
+CLUSTER_SAMPLE_RATE = 20  # frames
 
 
 class CarState(CarStateBase):
@@ -32,6 +34,10 @@ class CarState(CarStateBase):
     self.park_brake = False
     self.buttons_counter = 0
 
+    # On some cars, CLU15->CF_Clu_VehicleSpeed can oscillate faster than the dash updates. Sample at 5 Hz
+    self.cluster_speed = 0
+    self.cluster_speed_counter = CLUSTER_SAMPLE_RATE
+
     self.params = CarControllerParams(CP)
 
   def update(self, cp, cp_cam):
@@ -39,8 +45,9 @@ class CarState(CarStateBase):
       return self.update_canfd(cp, cp_cam)
 
     ret = car.CarState.new_message()
-
     cp_cruise = cp_cam if self.CP.carFingerprint in CAMERA_SCC_CAR else cp
+    is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0
+    speed_conv = CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS
 
     ret.doorOpen = any([cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"],
                         cp.vl["CGW2"]["CF_Gway_RLDrSw"], cp.vl["CGW2"]["CF_Gway_RRDrSw"]])
@@ -55,9 +62,19 @@ class CarState(CarStateBase):
     )
     ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
-
     ret.standstill = ret.vEgoRaw < 0.1
 
+    self.cluster_speed_counter += 1
+    if self.cluster_speed_counter > CLUSTER_SAMPLE_RATE:
+      self.cluster_speed = cp.vl["CLU15"]["CF_Clu_VehicleSpeed"]
+      self.cluster_speed_counter = 0
+
+      # mimic how dash converts to imperial
+      if not is_metric:
+        self.cluster_speed = math.floor(self.cluster_speed * CV.KPH_TO_MPH + CV.KPH_TO_MPH)
+
+    ret.vEgoCluster = self.cluster_speed * speed_conv
+
     ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"]
     ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"]
     ret.yawRate = cp.vl["ESP12"]["YAW_RATE"]
@@ -78,7 +95,6 @@ class CarState(CarStateBase):
       ret.cruiseState.available = cp_cruise.vl["SCC11"]["MainMode_ACC"] == 1
       ret.cruiseState.enabled = cp_cruise.vl["SCC12"]["ACCMode"] != 0
       ret.cruiseState.standstill = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 4.
-      speed_conv = CV.MPH_TO_MS if cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] else CV.KPH_TO_MS
       ret.cruiseState.speed = cp_cruise.vl["SCC11"]["VSetDis"] * speed_conv
 
     # TODO: Find brake pressure
@@ -227,6 +243,8 @@ class CarState(CarStateBase):
       ("CF_Clu_AmpInfo", "CLU11"),
       ("CF_Clu_AliveCnt1", "CLU11"),
 
+      ("CF_Clu_VehicleSpeed", "CLU15"),
+
       ("ACCEnable", "TCS13"),
       ("ACC_REQ", "TCS13"),
       ("DriverBraking", "TCS13"),
@@ -251,6 +269,7 @@ class CarState(CarStateBase):
       ("TCS13", 50),
       ("TCS15", 10),
       ("CLU11", 50),
+      ("CLU15", 5),
       ("ESP12", 100),
       ("CGW1", 10),
       ("CGW2", 5),
@@ -309,7 +328,6 @@ class CarState(CarStateBase):
 
     if CP.carFingerprint in FEATURES["use_cluster_gears"]:
       signals.append(("CF_Clu_Gear", "CLU15"))
-      checks.append(("CLU15", 5))
     elif CP.carFingerprint in FEATURES["use_tcu_gears"]:
       signals.append(("CUR_GR", "TCU12"))
       checks.append(("TCU12", 100))
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index b1a2785ba3..bca5d0c3eb 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-a4aa1f37c6d966151d3b43a0b51fffbcfa0187b1
\ No newline at end of file
+a9a25795f5d8202f7f4c137f80ae030e790ff974
\ No newline at end of file