Rate limit paramsd angleOffset output (#20946)

* rate limit paramsd angle offset

* update ref

selfdrive/locationd/paramsd.py

@@ -8,11 +8,15 @@ import numpy as np
 import cereal.messaging as messaging
 from cereal import car
 from common.params import Params, put_nonblocking
+from common.realtime import DT_MDL
+from common.numpy_fast import clip
 from selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
 from selfdrive.locationd.models.constants import GENERATED_DIR
 from selfdrive.swaglog import cloudlog
 
 
+MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL  # Max 20 deg/s
+
 class ParamsLearner:
   def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset):
     self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset)
@@ -121,6 +125,9 @@ def main(sm=None, pm=None):
 
   learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg']))
 
+  angle_offset_average = params['angleOffsetAverageDeg']
+  angle_offset = angle_offset_average
+
   while True:
     sm.update()
 
@@ -136,6 +143,9 @@ def main(sm=None, pm=None):
         learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
         x = learner.kf.x
 
+      angle_offset_average = clip(math.degrees(x[States.ANGLE_OFFSET]), angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA)
+      angle_offset = clip(math.degrees(x[States.ANGLE_OFFSET] + x[States.ANGLE_OFFSET_FAST]), angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA)
+
       msg = messaging.new_message('liveParameters')
       msg.logMonoTime = sm.logMonoTime['carState']
 
@@ -143,8 +153,8 @@ def main(sm=None, pm=None):
       msg.liveParameters.sensorValid = True
       msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
       msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
-      msg.liveParameters.angleOffsetAverageDeg = math.degrees(x[States.ANGLE_OFFSET])
-      msg.liveParameters.angleOffsetDeg = msg.liveParameters.angleOffsetAverageDeg + math.degrees(x[States.ANGLE_OFFSET_FAST])
+      msg.liveParameters.angleOffsetAverageDeg = angle_offset_average
+      msg.liveParameters.angleOffsetDeg = angle_offset
       msg.liveParameters.valid = all((
         abs(msg.liveParameters.angleOffsetAverageDeg) < 10.0,
         abs(msg.liveParameters.angleOffsetDeg) < 10.0,

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-cd48ec4946068b94bf86b67af4a76c812480ea1d
+80151d0d0e0abb0adf07e7fee01209752827032c