[paramsd] Add a hysteresis band for valid checks where applicable (#28546)

* add a hysteresis band for the slow offset valid check

* add hysteresis to total_ofset and roll valid cases

* bugfix: roll is in radians, offsets are in degs

* remove defaults

* remove kwarg

* remove slow offset check while loading params
old-commit-hash: dfbdcbad73

selfdrive/locationd/paramsd.py

@@ -16,8 +16,11 @@ from system.swaglog import cloudlog
 MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL  # Max 20 deg/s
 ROLL_MAX_DELTA = math.radians(20.0) * DT_MDL  # 20deg in 1 second is well within curvature limits
 ROLL_MIN, ROLL_MAX = math.radians(-10), math.radians(10)
+ROLL_LOWERED_MAX = math.radians(8)
 ROLL_STD_MAX = math.radians(1.5)
 LATERAL_ACC_SENSOR_THRESHOLD = 4.0
+OFFSET_MAX = 10.0
+OFFSET_LOWERED_MAX = 8.0
 
 
 class ParamsLearner:
@@ -102,6 +105,14 @@ class ParamsLearner:
       self.kf.filter.reset_rewind()
 
 
+def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: float, lowered_threshold: float):
+  if current_valid:
+    current_valid = abs(val) < threshold
+  else:
+    current_valid = abs(val) < lowered_threshold
+  return current_valid
+
+
 def main(sm=None, pm=None):
   config_realtime_process([0, 1, 2, 3], 5)
 
@@ -130,10 +141,8 @@ def main(sm=None, pm=None):
   # Check if starting values are sane
   if params is not None:
     try:
-      angle_offset_sane = abs(params.get('angleOffsetAverageDeg')) < 10.0
       steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr
-      params_sane = angle_offset_sane and steer_ratio_sane
-      if not params_sane:
+      if not steer_ratio_sane:
         cloudlog.info(f"Invalid starting values found {params}")
         params = None
     except Exception as e:
@@ -157,6 +166,9 @@ def main(sm=None, pm=None):
   angle_offset_average = params['angleOffsetAverageDeg']
   angle_offset = angle_offset_average
   roll = 0.0
+  avg_offset_valid = True
+  total_offset_valid = True
+  roll_valid = True
 
   while True:
     sm.update()
@@ -180,6 +192,9 @@ def main(sm=None, pm=None):
       roll_std = float(P[States.ROAD_ROLL])
       # Account for the opposite signs of the yaw rates
       sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE] + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
+      avg_offset_valid = check_valid_with_hysteresis(avg_offset_valid, angle_offset_average, OFFSET_MAX, OFFSET_LOWERED_MAX)
+      total_offset_valid = check_valid_with_hysteresis(total_offset_valid, angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
+      roll_valid = check_valid_with_hysteresis(roll_valid, roll, ROLL_MAX, ROLL_LOWERED_MAX)
 
       msg = messaging.new_message('liveParameters')
 
@@ -192,9 +207,9 @@ def main(sm=None, pm=None):
       liveParameters.angleOffsetAverageDeg = angle_offset_average
       liveParameters.angleOffsetDeg = angle_offset
       liveParameters.valid = all((
-        abs(liveParameters.angleOffsetAverageDeg) < 10.0,
-        abs(liveParameters.angleOffsetDeg) < 10.0,
-        abs(liveParameters.roll) < ROLL_MAX,
+        avg_offset_valid,
+        total_offset_valid,
+        roll_valid,
         roll_std < ROLL_STD_MAX,
         0.2 <= liveParameters.stiffnessFactor <= 5.0,
         min_sr <= liveParameters.steerRatio <= max_sr,