diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 7e30b1e3a7..830f0df6e5 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -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,