From 17c322032236a09eefc4224417ba0b163591da60 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Tue, 18 May 2021 12:59:42 +0200 Subject: [PATCH] Rate limit paramsd angleOffset output (#20946) * rate limit paramsd angle offset * update ref --- selfdrive/locationd/paramsd.py | 14 ++++++++++++-- selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 119d22fb..e297be3a 100755 --- a/selfdrive/locationd/paramsd.py +++ b/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, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index bf3374fa..aa4c8594 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -cd48ec4946068b94bf86b67af4a76c812480ea1d \ No newline at end of file +80151d0d0e0abb0adf07e7fee01209752827032c \ No newline at end of file