retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Rate limit paramsd angleOffset output (#20946) 

* rate limit paramsd angle offset

* update ref
albatross
Willem Melching 2021-05-18 12:59:42 +02:00 committed by GitHub
parent 4215f63769
commit 17c3220322
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 13 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
selfdrive/locationd/paramsd.py
View File

@ -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,

2
selfdrive/test/process_replay/ref_commit
View File

@ -1 +1 @@
cd48ec4946068b94bf86b67af4a76c812480ea1d
80151d0d0e0abb0adf07e7fee01209752827032c