diff --git a/common/pid.py b/common/pid.py index e3fa8afdf40..f95e6433691 100644 --- a/common/pid.py +++ b/common/pid.py @@ -43,7 +43,8 @@ def set_limits(self, pos_limit, neg_limit): self.pos_limit = pos_limit self.neg_limit = neg_limit - def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False): + def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False, + compose=None): self.speed = speed self.p = self.k_p * float(error) self.d = self.k_d * error_rate @@ -52,8 +53,17 @@ def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integr if not freeze_integrator: i = self.i + self.k_i * self.i_dt * error - # Don't allow windup if already clipping - test_control = self.p + i + self.d + self.f + # Don't allow windup if already clipping. + # + # `compose` lets a caller that reshapes p/i/d/f before sending them (per-term scales, a + # scheduled output multiplier, an additive trim) tell us what the candidate integral would + # actually become on the wire. Without it we clip against p+i+d+f, which is not the command + # for such a caller, so the integrator is protected against the wrong number in both + # directions. Callers that send p+i+d+f unchanged pass nothing and keep the original path. + if compose is None: + test_control = self.p + i + self.d + self.f + else: + test_control = compose(self.p, i, self.d, self.f) i_upperbound = self.i if test_control > self.pos_limit else self.pos_limit i_lowerbound = self.i if test_control < self.neg_limit else self.neg_limit self.i = np.clip(i, i_lowerbound, i_upperbound) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 72cb474052d..323ee3d7bae 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1,3 +1,4 @@ +import inspect #!/usr/bin/env python3 import math from numbers import Number @@ -23,7 +24,7 @@ get_lateral_active, ) from openpilot.selfdrive.controls.lib.lane_centering import LaneCenteringController -from openpilot.selfdrive.controls.lib.latcontrol import LatControl +from openpilot.selfdrive.controls.lib.latcontrol import LatControl, integrator_wind_blocked from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature @@ -373,6 +374,37 @@ def get_torque_control_params(CP, torque_params, starpilot_toggles, use_live_par return lat_accel_factor, lat_accel_offset, friction +_INTEGRATOR_FIX_PATH = "/data/HondaIntegratorFix" + + +_INTEGRATOR_FIX_CACHE = [0.0, -1] + + +def _read_integrator_fix_threshold_cached(frame: int, default: float = 0.0) -> float: + """Cached wrapper: this sits in the 100 Hz control loop, and the value is a hand-edited knob. + Re-read once a second; 1 s of latency on a tuning file is irrelevant, a syscall per frame is not.""" + if frame - _INTEGRATOR_FIX_CACHE[1] >= 100 or _INTEGRATOR_FIX_CACHE[1] < 0: + _INTEGRATOR_FIX_CACHE[0] = _read_integrator_fix_threshold(default) + _INTEGRATOR_FIX_CACHE[1] = frame + return _INTEGRATOR_FIX_CACHE[0] + + +def _read_integrator_fix_threshold(default: float = 0.0) -> float: + """Relative freeze threshold as a percent. Absent/0/garbage -> 0.0 = legacy absolute behaviour. + + File-gated rather than a Params key so it needs no params_pyx.so rebuild and can be flipped + live (the value is re-read every frame; the controller picks it up within a frame). + """ + try: + with open(_INTEGRATOR_FIX_PATH, "rb") as f: + value = float(f.read().strip()) / 100.0 + except (OSError, ValueError, TypeError): + return default + if value != value or value in (float("inf"), float("-inf")): + return default + return min(max(value, 0.0), 1.0) + + class Controls: def __init__(self) -> None: self.params = Params() @@ -389,6 +421,7 @@ def __init__(self) -> None: self.pm = messaging.PubMaster(['carControl', 'controlsState', 'starpilotLateralState']) self.steer_limited_by_safety = False + self.integrator_wind_blocked = None self.curvature = 0.0 self.desired_curvature = 0.0 self.lc_smooth_release = 0.0 @@ -432,6 +465,11 @@ def __init__(self) -> None: if self.CP.lateralTuning.which() == "torque" and (self.starpilot_toggles.nnff or self.starpilot_toggles.nnff_lite): self.LaC = LatControlNNFF(self.CP, self.CI, DT_CTRL) + # Resolved once, and only after EVERY possible self.LaC assignment above -- including the NNFF + # replacement. Only LatControlPID accepts the integrator-freeze classification; angle, + # curvature, torque and NNFF keep their existing signatures. + self._lac_takes_wind_blocked = "integrator_wind_blocked" in inspect.signature(self.LaC.update).parameters + def update_nrdr_autotune_params(self): self.learn_steer_ratio = self.params.get_bool("NrdrLearnSteerRatio", default=True) self.learn_stiffness = self.params.get_bool("NrdrLearnStiffness", default=True) @@ -783,12 +821,17 @@ def state_control(self): lat_delay = self.sm["liveDelay"].lateralDelay + lat_smooth_seconds actuators.curvature = self.desired_curvature + # Only LatControlPID takes the integrator-freeze classification. The angle, curvature, torque + # and NNFF controllers keep their existing signatures, so passing it unconditionally would be a + # TypeError on every one of those cars -- gate state or not. + lac_extra = (self.integrator_wind_blocked,) if self._lac_takes_wind_blocked else () steer, lateral_output, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.steer_limited_by_safety, self.desired_curvature, curvature_limited, lat_delay, self.calibrated_pose, self.sm['modelV2'], - self.starpilot_toggles) + self.starpilot_toggles, + *lac_extra) actuators.torque = float(steer) if self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: actuators.curvature = float(lateral_output) @@ -880,9 +923,21 @@ def publish(self, CC, lac_log): self.steer_limited_by_safety = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ STEER_ANGLE_SATURATION_THRESHOLD else: + # steer_limited_by_safety keeps its original absolute meaning -- it also feeds saturation + # and "take control" detection, which we are not changing. The integrator gets a separate, + # relative judgement so intentional carcontroller shaping (LPF, override ramp) stops being + # mistaken for the actuator refusing to follow. Threshold comes from + # /data/HondaIntegratorFix: absent or 0 restores the legacy behaviour exactly. self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2 + # None means "fix disabled" -- the lateral controller then takes its entirely legacy path, + # anti-windup composition included. One gate for the whole change, so it is on or off, never + # half-applied. + _thr = _read_integrator_fix_threshold_cached(self.sm.frame) + self.integrator_wind_blocked = integrator_wind_blocked( + float(CC.actuators.torque), float(CO.actuatorsOutput.torque), _thr) if _thr > 0.0 else None else: self.steer_limited_by_safety = False + self.integrator_wind_blocked = None # TODO: both controlsState and carControl valids should be set by # sm.all_checks(), but this creates a circular dependency diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index ed6bafe04f1..875614b7fa4 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -6,6 +6,31 @@ from openpilot.selfdrive.locationd.helpers import Pose +def integrator_wind_blocked(requested: float, applied: float, rel_threshold: float) -> bool: + """Is the actuator genuinely prevented from following us, or is this just intentional shaping? + + controlsd's `abs(requested - applied) > 1e-2` cannot tell the two apart. On a Honda whose + carcontroller low-pass filters and ramps the command on purpose, that absolute test is true on + ~99% of frames and starves the integrator (measured 80% of steady-turn frames frozen). + + A RELATIVE test separates them cleanly. Measured over 67k logged frames on a modified-EPS + Civic Bosch, |requested-applied|/|requested| is: + steady turns (LPF phase lag) : p50 0.12 p75 0.23 p90 0.44 + first 0.3 s of override fade : p50 0.92 p75 0.98 p90 1.00 + At a 0.30 threshold that is 18% of steady-turn frames against 100% of early-fade frames. + + The sign term catches a reversal, where the actuator is moving opposite to the request. + + rel_threshold <= 0 disables the relative test and restores the legacy absolute behaviour. + """ + gap = abs(requested - applied) + if rel_threshold <= 0.0: + return gap > 1e-2 + if requested * applied < 0.0: + return True + return gap > max(1e-2, rel_threshold * abs(requested)) + + class LatControl(ABC): def __init__(self, CP, CI, dt): self.dt = dt diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 8c57c1b729c..3fb133eb40d 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -109,6 +109,41 @@ }) + +_TURN_IN_ASYMMETRY_PATH = "/data/HondaTurnInAsymmetry" +_UNWIND_ASYMMETRY_PATH = "/data/HondaUnwindAsymmetry" + +# The carcontroller zeroes the LKAS request below NrdrMinSteerSpeed. Fixed here rather than reading +# that param so the controller stays independent of a carcontroller knob; it sits inside the +# modified-EPS freeze band, so the worst case is leaking an already-frozen integrator early. +_MIN_STEER_SPEED_CLEAR_MS = 2.0 * 0.44704 +_INTEGRATOR_LEAK_TAU_S = 0.5 + + +def _read_turn_in_asymmetry(default: float = 1.0) -> float: + """Percent of the Clarity-fitted left/right turn-in split, 100 = unchanged, 0 = symmetric.""" + try: + with open(_TURN_IN_ASYMMETRY_PATH, "rb") as f: + value = float(f.read().strip()) / 100.0 + except (OSError, ValueError, TypeError): + return default + if not math.isfinite(value): + return default + return min(max(value, 0.0), 1.0) + + +def _read_unwind_asymmetry(default: float = 1.0) -> float: + """Percent of the Clarity-fitted left/right UNWIND split, 100 = unchanged, 0 = symmetric.""" + try: + with open(_UNWIND_ASYMMETRY_PATH, "rb") as f: + value = float(f.read().strip()) / 100.0 + except (OSError, ValueError, TypeError): + return default + if not math.isfinite(value): + return default + return min(max(value, 0.0), 1.0) + + def get_nrdr_modified_eps_kf(v_ego: float) -> float: return float(np.interp(v_ego, NRDR_MODIFIED_EPS_KF_SPEED_BP, NRDR_MODIFIED_EPS_KF_V)) @@ -256,6 +291,8 @@ def _clarity_eps_pid_output_scale( center_taper_high: float, center_boost_threshold_deg: float, center_boost_min_speed_ms: float, + turn_in_asymmetry: float = 1.0, + unwind_asymmetry: float = 1.0, ) -> float: abs_angle = abs(desired_angle_deg) speed_weight = min(max((v_ego - 4.0) / 10.0, 0.0), 1.0) @@ -275,12 +312,35 @@ def _clarity_eps_pid_output_scale( center_speed_weight = 1.0 center_taper = center_taper_high * center_taper_scale * center_speed_weight - mid_turn_scale = 0.1200 if is_left else 0.0150 - mid_turn_turn_in_scale = -0.5500 if is_left else -0.0524 - mid_turn_unwind_scale = -0.0743 if is_left else -0.0842 - base_scale = 0.0722 if is_left else 0.0972 - turn_in_scale = -0.0799 if is_left else 0.0888 - unwind_scale = 0.1600 if is_left else 0.2000 + # Fitted on a Clarity and strongly asymmetric on TURN-IN: above ~26 mph at a large angle they + # floor a left turn-in at 0.6863 against 1.113-1.149 right, a 1.62x gap. `turn_in_asymmetry` + # blends left toward right; 1.0 is the Clarity behaviour, 0.0 is symmetric. Hold/unwind is + # bit-identical at every setting. + a = min(max(float(turn_in_asymmetry), 0.0), 1.0) + if is_left: + # Blend ONLY the turn-in constants. mid_turn_scale and base_scale are shared with hold/unwind; + # blending those too bought turn-in strength by weakening the left corner exit (~8-15 deg of + # extra held angle on the way out). + mid_turn_scale = 0.1200 + base_scale = 0.0722 + mid_turn_turn_in_scale = -0.5500 * a + -0.0524 * (1.0 - a) + turn_in_scale = -0.0799 * a + 0.0888 * (1.0 - a) + else: + mid_turn_scale = 0.0150 + mid_turn_turn_in_scale = -0.0524 + base_scale = 0.0972 + turn_in_scale = 0.0888 + # The UNWIND constants are asymmetric the same way: left keeps more holding torque on the way out + # (0.1600 vs 0.2000), so left exits over-rotate and right exits under-rotate (measured +5.08/+7.34 + # vs -2.65/-5.36). 1.0 is the Clarity behaviour, 0.0 gives left the right-hand constants. The + # low-speed-unwind branch below bypasses these entirely. + b = min(max(float(unwind_asymmetry), 0.0), 1.0) + if is_left: + mid_turn_unwind_scale = -0.0743 * b + -0.0842 * (1.0 - b) + unwind_scale = 0.1600 * b + 0.2000 * (1.0 - b) + else: + mid_turn_unwind_scale = -0.0842 + unwind_scale = 0.2000 scale = 1.0 + (center_weight * center_taper) scale += speed_weight * mid_turn_weight * mid_turn_scale @@ -357,8 +417,57 @@ def __init__(self, CP, CI, dt): self.unwind_boost_elapsed = 0.0 self.lat_stiction = LatStiction(dt, self.steer_max) self.lat_stiction_enabled = False + self.turn_in_asymmetry = 1.0 + self._compose_state = (1.0, 1.0, 1.0, 1.0, 0.0) + self._stiction_delta = 0.0 + self.unwind_asymmetry = 1.0 self.prev_saturated = False + def _compose_scaled(self, p, i, d, f): + """The scaled PID command: per-term scales then the scheduled output scale. Single source of + truth for that arithmetic -- the real output and the anti-windup candidate both start here.""" + p_scale, i_scale, f_scale, out_scale, _ = self._compose_state + return (p * p_scale + i * i_scale + d + f * f_scale) * out_scale + + def _compose_candidate(self, p, i, d, f): + """What the candidate integral would actually put on the wire: the scaled command plus the + additive stages that follow it. The learner trim is a pure map lookup so it is exact; the + stiction delta is last frame's, because stiction transforms the output and is stateful -- exact + to within one 10 ms frame against its 0.30 s capture tau, and zero when saturated, which is the + regime anti-windup exists for. Including these is deliberate: omitting an additive + same-direction term makes the candidate an UNDERestimate, firing the clamp late and permitting + MORE windup, not less.""" + return self._compose_scaled(p, i, d, f) + self._compose_state[4] + self._stiction_delta + + def reset(self): + """Drop every piece of carried-over state when lateral control is not running. + + controlsd calls this on each frame lateral is inactive (selfdrive/controls/controlsd.py:590) + and update()'s own inactive branch calls it too, so the two paths cannot drift apart. + + The inherited LatControl.reset() only cleared the saturation timer, so self.pid.i survived a + disengagement and was re-injected whole on the first frame after re-engagement -- measured at + +0.19115 across a 0.2 s gap and -0.13616 across a 0.35 s gap, the latter against a positive + proportional term. LatControlCurvature.reset() already did the right thing; this matches it. + + The full PIDController.reset() is used rather than clearing i alone: p, d and f are recomputed + unconditionally at the top of PIDController.update() before anything reads them, so zeroing + them cannot change the first active frame, and using the controller's own API keeps this from + depending on which fields happen to be stale. + """ + super().reset() + self.pid.reset() + self.eps_modified_steering_pressed_filter_s = 0.0 + self.eps_modified_steering_pressed_prev = False + self.center_taper_scale.x = 1.0 + self.unwind_boost_elapsed = 0.0 + self.prev_output_torque = 0.0 + self.prev_saturated = False + self.lat_stiction.reset() + # Stiction is stateful and _compose_candidate consumes last frame's delta; with the stiction + # filter itself reset, the delta it goes with is zero. + self._stiction_delta = 0.0 + def update_honda_lateral_pid_gain_scale(self, starpilot_toggles): if not self.is_honda_pid_lateral: return @@ -374,7 +483,7 @@ def update_honda_lateral_pid_gain_scale(self, starpilot_toggles): self.pid._k_i = [self.base_ki_bp, scale_lateral_pid_gain_values(self.base_ki_v, ki_scale)] def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, - lat_delay, calibrated_pose, model_data, starpilot_toggles): + lat_delay, calibrated_pose, model_data, starpilot_toggles, integrator_wind_blocked=None): self.update_honda_lateral_pid_gain_scale(starpilot_toggles) pid_log = log.ControlsState.LateralPIDState.new_message() @@ -412,14 +521,10 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat if not active: output_torque = 0.0 pid_log.active = False + # The only piece of inactive-frame state that is frame dependent: everything else is + # cleared by reset(), which is also what controlsd calls on every inactive frame. self.prev_angle_steers_des_no_offset = angle_steers_des_no_offset - self.eps_modified_steering_pressed_filter_s = 0.0 - self.eps_modified_steering_pressed_prev = False - self.center_taper_scale.x = 1.0 - self.unwind_boost_elapsed = 0.0 - self.prev_output_torque = 0.0 - self.prev_saturated = False - self.lat_stiction.reset() + self.reset() else: self.frame += 1 @@ -454,6 +559,8 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat ff_unwind_weight *= time_gate ff *= 1.0 + ff_unwind_weight * max(unwind_ff_boost - 1.0, 0.0) + learner_trim = 0.0 + raw_steering_pressed = bool(CS.steeringPressed) steering_pressed = CS.steeringPressed # Civic Bosch used to take a graded detector of its own here. It now shares the generic # modified-EPS one with the Clarity, so override feel is identical across the cars. @@ -467,17 +574,33 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat ) self.eps_modified_steering_pressed_prev = steering_pressed + # This carcontroller reshapes torque every frame on purpose (low-speed zeroing, override ramp, + # LPF), so controlsd's absolute mismatch flag is true on ~99% of frames and starves the + # integrator. That is intentional shaping, not an actuator limit: no active-state torque clamp + # is applied, the output is clipped to +-steer_max below, and `compose` stops the integrator + # pushing the composed command past that rail. Use the relative judgement when it is available; + # None (legacy path) or 0 in the gate file keeps the old absolute rule exactly. + if integrator_wind_blocked is None: + mismatch_freezes_i = steer_limited_by_safety + else: + mismatch_freezes_i = bool(integrator_wind_blocked) freeze_threshold = 2.0 if self.is_eps_modified else 5.0 - freeze_integrator = steer_limited_by_safety or steering_pressed or CS.vEgo < freeze_threshold + # Below the carcontroller's cutoff the wire carries zero whatever we ask for, so an integral + # stored there gets applied in full when the car rolls back above it. Leak rather than freeze + # (preserves it) or hard-clear (bumpy). + if self.is_eps_modified and integrator_wind_blocked is not None and CS.vEgo < _MIN_STEER_SPEED_CLEAR_MS: + self.pid.i *= math.exp(-self.dt / _INTEGRATOR_LEAK_TAU_S) + + # The filtered detector delays a same-direction press up to 0.28 s while the carcontroller is + # already fading torque out; freeze on the raw press to close that window. Rides the same gate + # so the disabled path stays bit-identical. + raw_press_freezes_i = raw_steering_pressed and integrator_wind_blocked is not None + freeze_integrator = (mismatch_freezes_i or raw_press_freezes_i or steering_pressed + or CS.vEgo < freeze_threshold) unwind_detected = phase < UNWIND_FREEZE_PHASE_THRESHOLD and abs_angle_des < UNWIND_FREEZE_ANGLE_NEAR_CENTER if self.is_eps_modified and self.unwind_freeze_enabled and unwind_detected: freeze_integrator = True - output_torque = self.pid.update(error, - feedforward=ff, - speed=CS.vEgo, - freeze_integrator=freeze_integrator) - # The Civic Bosch testing ground applies its own hardcoded center taper below; let it own the # output scale so the two tapers can never compound. civic_bosch_testing_ground = self.is_civic_bosch_modified and civic_bosch_modified_lateral_testing_ground_active() @@ -504,11 +627,16 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat self.unwind_ff_multiplier = _get_param_float(self.params, "HondaUnwindFfMultiplier", 2.0, 1.0, 4.0) self.unwind_boost_cap_s = _get_param_float(self.params, "HondaUnwindBoostSeconds", 1.0, 0.0, 3.0) self.lat_stiction_enabled = _get_param_bool(self.params, "NrdrLatStiction") + # 100 = Clarity split (previous behaviour), 0 = symmetric. A plain file, not a param: the + # key is not in params_keys.h so no params_pyx.so rebuild is needed, and it must live + # outside /data/params/d, which is pruned of unregistered keys on boot. Missing or garbage + # keeps the previous behaviour. + self.turn_in_asymmetry = _read_turn_in_asymmetry() + self.unwind_asymmetry = _read_unwind_asymmetry() p_scale = _lat_pid_scale_banded(CS.vEgo, self.lat_p_scale_low, self.lat_p_scale_standard, self.lat_p_scale_highway) i_scale = _lat_pid_scale_banded(CS.vEgo, self.lat_i_scale_low, self.lat_i_scale_standard, self.lat_i_scale_highway) f_scale = _lat_pid_scale_banded(CS.vEgo, self.lat_f_scale_low, self.lat_f_scale_standard, self.lat_f_scale_highway) - output_torque = self.pid.p * p_scale + self.pid.i * i_scale + self.pid.d + self.pid.f * f_scale lane_change = bool(getattr(CS, "leftBlinker", False) or getattr(CS, "rightBlinker", False)) if lane_change: @@ -516,8 +644,10 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat center_taper_scale = 0.0 else: center_taper_scale = float(self.center_taper_scale.update(1.0)) - if not civic_bosch_testing_ground: - output_torque *= _clarity_eps_pid_output_scale( + if civic_bosch_testing_ground: + eps_output_scale = 1.0 + else: + eps_output_scale = _clarity_eps_pid_output_scale( angle_steers_des_no_offset, phase, float(CS.steeringRateDeg), @@ -526,8 +656,31 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat self.center_taper_high, self.center_boost_threshold, self.center_boost_min_speed * _MPH_TO_MS, + self.turn_in_asymmetry, + self.unwind_asymmetry, ) + # Judge the candidate integral against the torque actually sent: the command is rebuilt with + # per-term scales, a scheduled output scale, a learner trim and a stiction stage, so it + # differs from p+i+d+f by up to ~30%. _compose_output is the single source of truth for both + # the real output and the candidate. The stiction delta is last frame's (it is stateful and + # transforms the output); it is zero when saturated, which is the regime anti-windup exists + # for. Omitting it would make the candidate an underestimate and permit more windup, not less. + learner_trim = float(self.tune_learner.apply(CS.vEgo, angle_steers_des)) + self._compose_state = (p_scale, i_scale, f_scale, eps_output_scale, learner_trim) + + output_torque = self.pid.update(error, + feedforward=ff, + speed=CS.vEgo, + freeze_integrator=freeze_integrator, + compose=self._compose_candidate if integrator_wind_blocked is not None else None) + output_torque = self._compose_scaled(self.pid.p, self.pid.i, self.pid.d, self.pid.f) + else: + output_torque = self.pid.update(error, + feedforward=ff, + speed=CS.vEgo, + freeze_integrator=freeze_integrator) + if self.is_subaru_impreza: raw_output_torque = self.pid.p + self.pid.i + self.pid.d + self.pid.f output_torque = raw_output_torque * get_subaru_impreza_pid_output_scale(error) @@ -552,7 +705,7 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat output_torque = self.prev_output_torque + (output_alpha * (output_torque - self.prev_output_torque)) output_torque = float(max(min(output_torque, self.steer_max), -self.steer_max)) - output_torque += self.tune_learner.apply(CS.vEgo, angle_steers_des) + output_torque += learner_trim if self.is_eps_modified else self.tune_learner.apply(CS.vEgo, angle_steers_des) output_torque = float(max(min(output_torque, self.steer_max), -self.steer_max)) paramsd_ok = bool( @@ -567,18 +720,24 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat if self.lat_stiction_enabled: des_rate_degs = desired_angle_delta / self.dt lane_change_stiction = bool(getattr(CS, "leftBlinker", False) or getattr(CS, "rightBlinker", False)) + _pre_stiction = output_torque output_torque = float(self.lat_stiction.update( active, CS.vEgo, error, des_rate_degs, float(CS.steeringRateDeg), output_torque, steering_pressed, lane_change_stiction, self.prev_saturated)) + self._stiction_delta = output_torque - _pre_stiction else: self.lat_stiction.reset() + self._stiction_delta = 0.0 pid_log.active = True pid_log.p = float(self.pid.p) pid_log.i = float(self.pid.i) pid_log.f = float(self.pid.f) pid_log.output = float(output_torque) - pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) + # Same classification as the integrator: _check_saturation() refuses to accumulate while the + # limiter flag is set, so leaving the ~99%-true mismatch here would keep saturation and + # "take control" detection suppressed on modified-EPS cars even after it stops freezing I. + pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, mismatch_freezes_i, curvature_limited)) self.prev_angle_steers_des_no_offset = angle_steers_des_no_offset self.prev_output_torque = float(output_torque) self.prev_saturated = bool(pid_log.saturated) diff --git a/selfdrive/controls/lib/tests/test_integrator_wind_blocked.py b/selfdrive/controls/lib/tests/test_integrator_wind_blocked.py new file mode 100644 index 00000000000..5e981872cbe --- /dev/null +++ b/selfdrive/controls/lib/tests/test_integrator_wind_blocked.py @@ -0,0 +1,50 @@ +"""The offline analysis tool and the shipped rule must be the same function, forever. + +The integrator-freeze decision is reconstructed offline from logged carControl.actuators.torque and +carOutput.actuatorsOutput.torque, because there is no spare capnp field to log it in. That is only +sound while the reconstruction and `integrator_wind_blocked()` agree exactly. This test pins that +against a fixture of real logged frames from six drives, at every threshold we have used. +""" +import numpy as np + +from openpilot.selfdrive.controls.lib.latcontrol import integrator_wind_blocked + +FIXTURE = "selfdrive/controls/lib/tests/wind_blocked_fixture.npy" +THRESHOLDS = (0.0, 0.15, 0.30, 0.40) + + +def _offline(requested, applied, threshold): + """The vectorised form used by the offline tool. Must match the scalar rule exactly.""" + gap = np.abs(requested - applied) + if threshold <= 0.0: + return gap > 1e-2 + return (requested * applied < 0.0) | (gap > np.maximum(1e-2, threshold * np.abs(requested))) + + +class TestIntegratorWindBlocked: + def test_matches_offline_reconstruction(self): + data = np.load(FIXTURE) + requested, applied = data[:, 0], data[:, 1] + for threshold in THRESHOLDS: + expected = _offline(requested, applied, threshold) + for k in range(len(requested)): + assert bool(expected[k]) == integrator_wind_blocked(float(requested[k]), float(applied[k]), threshold), \ + f"mismatch at frame {k}, threshold {threshold}: cmd={requested[k]} out={applied[k]}" + + def test_zero_threshold_is_legacy_absolute_rule(self): + assert integrator_wind_blocked(0.30, 0.28, 0.0) # 0.02 gap > 1e-2 + assert not integrator_wind_blocked(0.30, 0.295, 0.0) # 0.005 gap + + def test_relative_rule_admits_shaping_and_blocks_fade(self): + # steady turn: LPF phase lag, ~13% of command -> integrator may wind + assert not integrator_wind_blocked(0.30, 0.26, 0.30) + # early override fade: applied is still ramping from zero -> must not wind + assert integrator_wind_blocked(0.30, 0.02, 0.30) + + def test_sign_reversal_always_blocks(self): + assert integrator_wind_blocked(0.30, -0.05, 0.30) + assert integrator_wind_blocked(-0.30, 0.05, 0.30) + + def test_tiny_commands_use_the_absolute_floor(self): + # relative test alone would be hypersensitive near zero; the 1e-2 floor prevents that + assert not integrator_wind_blocked(0.01, 0.005, 0.30) diff --git a/selfdrive/controls/lib/tests/test_latcontrol_pid_reset.py b/selfdrive/controls/lib/tests/test_latcontrol_pid_reset.py new file mode 100644 index 00000000000..62e96cb819e --- /dev/null +++ b/selfdrive/controls/lib/tests/test_latcontrol_pid_reset.py @@ -0,0 +1,179 @@ +"""LatControlPID must not carry integrator state across a disengagement. + +controlsd calls LaC.reset() on every frame lateral is inactive +(selfdrive/controls/controlsd.py:589-591), but the inherited LatControl.reset() +(selfdrive/controls/lib/latcontrol.py:47-48) clears only the saturation timer. LatControlPID's +own inactive branch cleared stiction, the pressed filter, the centre taper, the unwind state and +the previous output, but never the PID controller itself, so self.pid.i survived a disengagement +and was re-injected whole on the first frame after re-engagement. + +The fixtures below are the measured values from two logged drives on the modified-EPS Civic +Bosch; see REENGAGEMENTS. +""" +# LatControlPID pulls in common.transformations.transformations, a compiled extension that only +# exists for the device architecture. Try the real one first -- on a device or in CI this block is a +# no-op -- and only if it will not load, stand in a module of the same shape so this file can be +# collected on a development host. The stubs raise if anything ever calls them: this suite exercises +# the lateral controller, never the geometry, and a silently wrong rotation would be worse than an +# ImportError. Nothing else is stubbed, and this is deliberately NOT a conftest so it cannot reach +# any other suite. +try: + import openpilot.common.transformations.transformations # noqa: F401 +except (ImportError, OSError): + import sys + import types + + def _needs_native_extension(name): + def unavailable(*args, **kwargs): + raise RuntimeError(f"{name} needs the compiled transformations extension; this suite never calls it") + return unavailable + + _stub = types.ModuleType("openpilot.common.transformations.transformations") + for _fn in ("ecef_euler_from_ned_single", "euler2quat_single", "euler2rot_single", + "ned_euler_from_ecef_single", "quat2euler_single", "quat2rot_single", + "rot2euler_single", "rot2quat_single"): + setattr(_stub, _fn, _needs_native_extension(_fn)) + sys.modules["openpilot.common.transformations.transformations"] = _stub + +import math +from types import SimpleNamespace + +import pytest + +from opendbc.car import structs +from opendbc.car.honda.interface import CarInterface +from opendbc.car.honda.values import CAR +from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID + +CarParams = structs.CarParams + +TOGGLES = SimpleNamespace(force_torque_controller=False, nnff=False, nnff_lite=False) + +# a comma in the eps fw version is what marks a modified EPS -- this is the car that was driven +MODIFIED_FW = b'39990-TGG,A120\x00\x00' + +# LatControlPID only reaches into CI for the feedforward function +STUB_CI = SimpleNamespace(get_steer_feedforward_function=lambda: (lambda angle, v_ego: angle)) + +DT = 0.01 + +# Real transitions, read out of the rlogs of two drives on 2026-09-01. Each row is +# (route, t_last_active, i_at_last_active, t_reengage, p_at_reengage). +# The integral logged on the first re-engaged frame equalled i_at_last_active to five decimals +# in both cases; drive 1's is the interesting one, because the carried-over integral is NEGATIVE +# while the proportional term on that first frame is strongly POSITIVE. +REENGAGEMENTS = [ + ("98a5b56c6d", 475.437, +0.19115, 475.638, -1.14554), + ("5900224619", 147.988, -0.13616, 148.347, +0.44807), +] + +# drive 3 98a5b56c6d: lateral went inactive at t=206.090 and did not come back until t=263.890. +LONGEST_LOGGED_INACTIVE_GAP_S = 57.811 + + +class _CS: + steeringAngleDeg = 0.0 + steeringRateDeg = 0.0 + vEgo = 20.0 + steeringPressed = False + steeringTorque = 0.0 + leftBlinker = False + rightBlinker = False + + +class _VM: + sR = 15.38 + + def get_steer_from_curvature(self, curv, v_ego, roll): + return math.radians(curv * 1000.0 * self.sR) + + +_PARAMS = SimpleNamespace(roll=0.0, angleOffsetDeg=0.0) + + +@pytest.fixture +def lat(): + car_fw = [CarParams.CarFw(ecu=CarParams.Ecu.eps, fwVersion=MODIFIED_FW, address=0x18DA30F1, subAddress=0)] + CP = CarInterface.get_params(CAR.HONDA_CIVIC_BOSCH, {0: {}, 1: {}, 2: {}}, car_fw, False, False, False, TOGGLES) + return LatControlPID(CP, STUB_CI, DT) + + +def _inactive_update(controller): + """One frame through update() with lateral inactive, the way controlsd would call it.""" + return controller.update(False, _CS(), _VM(), _PARAMS, False, 0.0, False, 0.0, None, None, TOGGLES) + + +@pytest.mark.parametrize("route, t_last, i_last, t_reengage, p_reengage", REENGAGEMENTS) +def test_reset_clears_a_logged_integral(lat, route, t_last, i_last, t_reengage, p_reengage): + """The controlsd path. This is the one that was broken: controlsd calls reset(), not update().""" + lat.pid.i = i_last + lat.reset() + assert lat.pid.i == 0.0, ( + f"{route}: integral {i_last:+.5f} logged at t={t_last:.3f} survived reset() and would be " + f"re-injected at t={t_reengage:.3f}, where the proportional term was {p_reengage:+.5f}" + ) + + +@pytest.mark.parametrize("route, t_last, i_last, t_reengage, p_reengage", REENGAGEMENTS) +def test_inactive_update_clears_a_logged_integral(lat, route, t_last, i_last, t_reengage, p_reengage): + """The update() path, so the two inactive paths cannot drift apart.""" + lat.pid.i = i_last + _inactive_update(lat) + assert lat.pid.i == 0.0, f"{route}: integral {i_last:+.5f} survived an inactive update() frame" + + +def test_integral_cannot_survive_a_long_inactive_gap(lat): + """Drive 3's 57.8 s gap, run frame by frame through the path controlsd actually takes.""" + lat.pid.i = -0.01897 + for _ in range(int(LONGEST_LOGGED_INACTIVE_GAP_S / DT)): + _inactive_update(lat) + lat.reset() + assert lat.pid.i == 0.0 + + +def test_reset_clears_the_whole_pid_controller(lat): + """p, d and f are recomputed at the top of PIDController.update() before anything reads them, + so clearing them cannot change the first active frame -- but leaving them set would make + pid_log and self.pid.control report last engagement's numbers while lateral is off.""" + lat.pid.p, lat.pid.i, lat.pid.d, lat.pid.f, lat.pid.control = 0.1, 0.2, 0.3, 0.4, 0.5 + lat.reset() + assert (lat.pid.p, lat.pid.i, lat.pid.d, lat.pid.f, lat.pid.control) == (0.0, 0.0, 0.0, 0.0, 0) + + +def test_reset_still_clears_the_saturation_timer(lat): + """The override must not lose what the base class did.""" + lat.sat_time = 1.5 + lat.reset() + assert lat.sat_time == 0.0 + + +def test_reset_clears_the_rest_of_the_inactive_state(lat): + """Everything the inactive branch used to clear inline now has to come out of reset(), or + moving it there would have been a regression.""" + lat.eps_modified_steering_pressed_filter_s = 0.4 + lat.eps_modified_steering_pressed_prev = True + lat.center_taper_scale.x = 0.25 + lat.unwind_boost_elapsed = 0.7 + lat.prev_output_torque = 0.3 + lat.prev_saturated = True + lat._stiction_delta = 0.05 + lat.reset() + assert lat.eps_modified_steering_pressed_filter_s == 0.0 + assert lat.eps_modified_steering_pressed_prev is False + assert lat.center_taper_scale.x == 1.0 + assert lat.unwind_boost_elapsed == 0.0 + assert lat.prev_output_torque == 0.0 + assert lat.prev_saturated is False + assert lat._stiction_delta == 0.0 + + +def test_active_frames_are_untouched(lat): + """The normal path must not be affected: an active frame never calls reset(), and the + integral it builds has to persist from one active frame to the next.""" + torque, _, pid_log = lat.update(True, _CS(), _VM(), _PARAMS, False, 5e-5, False, 0.0, None, None, TOGGLES) + assert pid_log.active + first_i = lat.pid.i + assert first_i != 0.0, "an active frame with a standing error must integrate" + lat.update(True, _CS(), _VM(), _PARAMS, False, 5e-5, False, 0.0, None, None, TOGGLES) + assert lat.pid.i != first_i, "the integral must keep accumulating across active frames" + assert torque != 0.0 diff --git a/selfdrive/controls/lib/tests/wind_blocked_fixture.npy b/selfdrive/controls/lib/tests/wind_blocked_fixture.npy new file mode 100644 index 00000000000..168eb08809f Binary files /dev/null and b/selfdrive/controls/lib/tests/wind_blocked_fixture.npy differ diff --git a/selfdrive/controls/tests/test_lac_dispatch.py b/selfdrive/controls/tests/test_lac_dispatch.py new file mode 100644 index 00000000000..8145c6cb9d0 --- /dev/null +++ b/selfdrive/controls/tests/test_lac_dispatch.py @@ -0,0 +1,56 @@ +"""Controls.__init__ must resolve the lateral-controller dispatch AFTER self.LaC exists, and the +dispatch must call every controller with an argument count it accepts. + +Both defects this guards against were shipped and caught in review, not by testing: + 1. controlsd passed the integrator-freeze flag positionally to every controller, but only + LatControlPID accepts it -> TypeError on the first control frame for angle/curvature/torque/ + NNFF cars, regardless of gate state. + 2. the fix for (1) resolved the dispatch before self.LaC was assigned -> AttributeError in + __init__ on EVERY car, including the one it was meant to help. +A syntax check catches neither. This does. +""" +import inspect +from pathlib import Path + +from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID +from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle +from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque + +FLAG = "integrator_wind_blocked" +BASE_ARGS = 11 # active, CS, VM, params, steer_limited_by_safety, desired_curvature, + # curvature_limited, lat_delay, calibrated_pose, model_data, starpilot_toggles + + +class TestLateralDispatch: + def test_only_pid_takes_the_flag(self): + assert FLAG in inspect.signature(LatControlPID.update).parameters + for cls in (LatControlAngle, LatControlCurvature, LatControlTorque): + assert FLAG not in inspect.signature(cls.update).parameters, \ + f"{cls.__name__} unexpectedly accepts {FLAG}; the dispatch assumes it does not" + + def test_every_controller_accepts_what_the_dispatch_sends(self): + """Mirrors controlsd: BASE_ARGS always, plus one iff the controller declares the flag.""" + for cls in (LatControlPID, LatControlAngle, LatControlCurvature, LatControlTorque): + params = list(inspect.signature(cls.update).parameters)[1:] # drop self + takes = FLAG in params + sent = BASE_ARGS + (1 if takes else 0) + required = [p for p, v in list(inspect.signature(cls.update).parameters.items())[1:] + if v.default is inspect.Parameter.empty] + assert len(required) <= sent <= len(params), \ + f"{cls.__name__}: dispatch sends {sent} args, signature takes {len(required)}..{len(params)}" + + def test_resolution_happens_after_every_lac_assignment(self): + """The guard must not read self.LaC before the controller-selection block has run. + + Read as text rather than importing controlsd: that module pulls compiled device-only + extensions, and this defect is an ordering property of the source, so a textual check is both + sufficient and runnable anywhere. + """ + src = Path(__file__).resolve().parents[2] / "controls" / "controlsd.py" + lines = src.read_text().splitlines() + guard = next(i for i, l in enumerate(lines) if "_lac_takes_wind_blocked =" in l) + last_assignment = max(i for i, l in enumerate(lines) if "self.LaC = " in l) + assert guard > last_assignment, ( + f"dispatch resolved at line {guard + 1}, before the last self.LaC assignment at " + f"line {last_assignment + 1} -> AttributeError in Controls.__init__")