From ef26abcf658b9fe13f939c43a31f57bd6ae25232 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Tue, 13 Sep 2022 23:34:51 -0700
Subject: [PATCH] ui: support ego speed hysteresis (#25702)

* toyota: match set speed from dash

* Use unit bit

* Use RSA2

* flip this

* Universal unit signal, set vEgoCluster

* remove this and bump opendbc

* detect if car interface sets cluster fields

* revert

* needs to be cp

* UI_SPEED is actually always in kph?

* forgot to actually convert it

* same in onroad

* try conv factor only for imperial

* Seems like UI_SPEED is not the UI speed at all

* the dash might floor it

* same openpilot behavior

* bump

* ego speed factor is dynamic across speeds, handle Lexus exceptions with
diff msg

* remove test, bump opendbc

* secret formula

* secret formula v2

* 1.03 is sufficient

* try short press

* bump opendbc

* surely this can be cleaned up

surely this can be cleaned up

* use filter

* redo factors

* try UI_SPEED again with a factor

try UI_SPEED again with a factor

* dash applies hysteresis to speed. this matches pretty well, but not exactly

* make hysteresis generic

* clean up

* revert this

* try this

* read from fake params

* should be mostly good for imperial now

* revert

* revert

* remove toyota stuff

* common function for everyone

* rm line

* fix

* an example with Toyota

* use CS.out

* one mph

* 1 kph

* cmt

* remove cmt

* abs it

Co-authored-by: Willem Melching <willem.melching@gmail.com>
---
 selfdrive/car/__init__.py   |  8 ++++++++
 selfdrive/car/interfaces.py | 10 +++++++++-
 2 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py
index f2d198338d..8ff39ceaeb 100644
--- a/selfdrive/car/__init__.py
+++ b/selfdrive/car/__init__.py
@@ -12,6 +12,14 @@ ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
 
 
+def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float:
+  if val > val_steady + hyst_gap:
+    val_steady = val - hyst_gap
+  elif val < val_steady - hyst_gap:
+    val_steady = val + hyst_gap
+  return val_steady
+
+
 def create_button_event(cur_but: int, prev_but: int, buttons_dict: Dict[int, capnp.lib.capnp._EnumModule],
                         unpressed: int = 0) -> capnp.lib.capnp._DynamicStructBuilder:
   if cur_but != unpressed:
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index 1614003e87..bc6c31e12c 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -9,7 +9,7 @@ from common.basedir import BASEDIR
 from common.conversions import Conversions as CV
 from common.kalman.simple_kalman import KF1D
 from common.realtime import DT_CTRL
-from selfdrive.car import create_button_enable_events, gen_empty_fingerprint
+from selfdrive.car import apply_hysteresis, create_button_enable_events, gen_empty_fingerprint
 from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
 from selfdrive.controls.lib.events import Events
 from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -171,6 +171,12 @@ class CarInterfaceBase(ABC):
     else:
       self.v_ego_cluster_seen = True
 
+    # Many cars apply hysteresis to the ego dash speed
+    if self.CS is not None:
+      ret.vEgoCluster = apply_hysteresis(ret.vEgoCluster, self.CS.out.vEgoCluster, self.CS.cluster_speed_hyst_gap)
+      if abs(ret.vEgo) < self.CS.cluster_min_speed:
+        ret.vEgoCluster = 0.0
+
     if ret.cruiseState.speedCluster == 0:
       ret.cruiseState.speedCluster = ret.cruiseState.speed
 
@@ -272,6 +278,8 @@ class CarStateBase(ABC):
     self.right_blinker_cnt = 0
     self.left_blinker_prev = False
     self.right_blinker_prev = False
+    self.cluster_speed_hyst_gap = 0.0
+    self.cluster_min_speed = 0.0  # min speed before dropping to 0
 
     # Q = np.matrix([[0.0, 0.0], [0.0, 100.0]])
     # R = 0.3