diff --git a/cereal/log.capnp b/cereal/log.capnp index 1209f3fd95..8aa95ad6e6 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2238,6 +2238,11 @@ struct LiveParametersData { roll @14 :Float32; debugFilterState @16 :FilterState; + angleOffsetValid @17 :Bool = true; + angleOffsetAverageValid @18 :Bool = true; + steerRatioValid @19 :Bool = true; + stiffnessFactorValid @20 :Bool = true; + yawRateDEPRECATED @7 :Float32; filterStateDEPRECATED @15 :LiveLocationKalman.Measurement; @@ -2564,7 +2569,7 @@ struct Event { # DO change the name of the field and struct # DON'T change the ID (e.g. @107) - # DON'T change which struct it points to + # DON'T change which struct it points to customReserved0 @107 :Custom.CustomReserved0; customReserved1 @108 :Custom.CustomReserved1; customReserved2 @109 :Custom.CustomReserved2; diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index da2f3f20bb..24bf6619cf 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -224,17 +224,21 @@ def main(): liveParameters.posenetValid = True liveParameters.sensorValid = sensors_valid liveParameters.steerRatio = float(x[States.STEER_RATIO].item()) + liveParameters.steerRatioValid = min_sr <= liveParameters.steerRatio <= max_sr liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item()) + liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0 liveParameters.roll = float(roll) liveParameters.angleOffsetAverageDeg = float(angle_offset_average) + liveParameters.angleOffsetAverageValid = bool(avg_offset_valid) liveParameters.angleOffsetDeg = float(angle_offset) + liveParameters.angleOffsetValid = bool(total_offset_valid) liveParameters.valid = all(( - avg_offset_valid, - total_offset_valid, + liveParameters.angleOffsetAverageValid, + liveParameters.angleOffsetValid , roll_valid, roll_std < ROLL_STD_MAX, - 0.2 <= liveParameters.stiffnessFactor <= 5.0, - min_sr <= liveParameters.steerRatio <= max_sr, + liveParameters.stiffnessFactorValid, + liveParameters.steerRatioValid, )) liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item()) liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item()) diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 06c87f849c..d89418e177 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -292,6 +292,24 @@ def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging return NormalPermanentAlert("Calibration Invalid", angles) +def paramsd_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + if not sm['liveParameters'].angleOffsetValid: + angle_offset_deg = sm['liveParameters'].angleOffsetDeg + title = "Steering misalignment detected" + text = f"Angle offset too high (Offset: {angle_offset_deg:.1f}°)" + elif not sm['liveParameters'].steerRatioValid: + steer_ratio = sm['liveParameters'].steerRatio + title = "Steer ratio mismatch" + text = f"Steering rack geometry may be off (Ratio: {steer_ratio:.1f})" + elif not sm['liveParameters'].stiffnessFactorValid: + stiffness_factor = sm['liveParameters'].stiffnessFactor + title = "Abnormal tire stiffness" + text = f"Check tires, pressure, or alignment (Factor: {stiffness_factor:.1f})" + else: + return NoEntryAlert("paramsd Temporary Error") + + return NoEntryAlert(alert_text_1=title, alert_text_2=text) + def overheat_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: cpu = max(sm['deviceState'].cpuTempC, default=0.) gpu = max(sm['deviceState'].gpuTempC, default=0.) @@ -604,7 +622,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { # This alert is thrown when any of these values exceed a sanity check. This can be caused by # bad alignment or bad sensor data. If this happens consistently consider creating an issue on GitHub EventName.paramsdTemporaryError: { - ET.NO_ENTRY: NoEntryAlert("paramsd Temporary Error"), + ET.NO_ENTRY: paramsd_invalid_alert, ET.SOFT_DISABLE: soft_disable_alert("paramsd Temporary Error"), }, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 2838c67f93..3c63f00300 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -d1538daa94478c50008cee81716fa7408df6606e \ No newline at end of file +d343af55506b14a4011bfe43b2a49c7e3c387d75 \ No newline at end of file