diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 1d1d353315..c0afe16974 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -77,6 +77,8 @@ def main(sm=None, pm=None): CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True)) cloudlog.info("paramsd got CarParams") + min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio + params = params_reader.get("LiveParameters") # Check if car model matches @@ -86,6 +88,12 @@ def main(sm=None, pm=None): cloudlog.info("Parameter learner found parameters for wrong car.") params = None + if (params is not None) and not all(( + abs(params['angleOffsetAverage']) < 10.0, + min_sr <= params['steerRatio'] <= max_sr)): + cloudlog.info(f"Invalid starting values found {params}") + params = None + if params is None: params = { 'carFingerprint': CP.carFingerprint, @@ -100,7 +108,6 @@ def main(sm=None, pm=None): params['stiffnessFactor'] = 1.0 learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverage'])) - min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio while True: sm.update()