From b9fa4e5696a0a8b8d02a0734302302f3b7a1f7bd Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Tue, 4 May 2021 18:50:45 +0200 Subject: [PATCH] Add more sanity checks to paramsd (#20820) * add more sanity checks to paramsd * Update selfdrive/locationd/paramsd.py old-commit-hash: 9fb79024ef34206e127a30a408b7bac970387c08 --- selfdrive/locationd/paramsd.py | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 0da862b6d8..dbc7f359a9 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -38,8 +38,9 @@ class ParamsLearner: yaw_rate = msg.angularVelocityCalibrated.value[2] yaw_rate_std = msg.angularVelocityCalibrated.std[2] yaw_rate_valid = msg.angularVelocityCalibrated.valid - yaw_rate_valid = yaw_rate_valid and (not math.isnan(yaw_rate)) - yaw_rate_valid = yaw_rate_valid and (not math.isnan(yaw_rate_std)) + yaw_rate_valid = yaw_rate_valid and math.isfinite(yaw_rate) + yaw_rate_valid = yaw_rate_valid and math.isfinite(yaw_rate_std) + yaw_rate_valid = yaw_rate_valid and yaw_rate_std < 1e6 if self.active: if msg.inputsOK and msg.posenetOK and yaw_rate_valid: @@ -92,16 +93,18 @@ def main(sm=None, pm=None): cloudlog.info("Parameter learner found parameters for wrong car.") params = 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 params is not None and not params_sane: - cloudlog.info(f"Invalid starting values found {params}") + # 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: + cloudlog.info(f"Invalid starting values found {params}") + params = None + except Exception as e: + cloudlog.info(f"Error reading params {params}: {str(e)}") params = None - except Exception as e: - cloudlog.info(f"Error reading params {params}: {str(e)}") - params = None # TODO: cache the params with the capnp struct if params is None: @@ -129,10 +132,10 @@ def main(sm=None, pm=None): if sm.updated['liveLocationKalman']: x = learner.kf.x - if any(map(math.isnan, x)): + if not all(map(math.isfinite, x)): cloudlog.error("NaN in liveParameters estimate. Resetting to default values") learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0) - continue + x = learner.kf.x msg = messaging.new_message('liveParameters') msg.logMonoTime = sm.logMonoTime['carState']