mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 20:03:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
* 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
This commit is contained in:
Vivek Aithal
2023-06-15 20:17:06 -07:00
committed by GitHub
parent bb50193453
commit 09073a7286
1 changed files with 21 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
selfdrive/locationd/paramsd.py
View File

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