diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 3dcd736991..6e7e22c4ba 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -33,7 +33,7 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA # LTA control can be more delayed and winds up more often - ret.steerActuatorDelay = 0.18 + ret.steerActuatorDelay = 0.35 ret.steerLimitTimer = 0.8 else: CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4f3b6a9128..4a8963b252 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -614,10 +614,16 @@ class Controls: actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) # Steering PID loop and lateral MPC - self.desired_curvature, self.desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo, - lat_plan.psis, - lat_plan.curvatures, - lat_plan.curvatureRates) + desired_curvature_lower, desired_curvature_rate_lower = get_lag_adjusted_curvature(0.18, CS.vEgo, + lat_plan.psis, + lat_plan.curvatures, + lat_plan.curvatureRates) + desired_curvature_upper, desired_curvature_rate_upper = get_lag_adjusted_curvature(0.35, CS.vEgo, + lat_plan.psis, + lat_plan.curvatures, + lat_plan.curvatureRates) + self.desired_curvature = desired_curvature_lower if abs(desired_curvature_lower) < abs(desired_curvature_upper) else desired_curvature_upper + self.desired_curvature_rate = desired_curvature_rate_lower if abs(desired_curvature_rate_lower) < abs(desired_curvature_rate_upper) else desired_curvature_rate_upper actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.last_actuators, self.steer_limited, self.desired_curvature, self.desired_curvature_rate, self.sm['liveLocationKalman']) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 00916ddf78..9141c48481 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -163,7 +163,7 @@ def rate_limit(new_value, last_value, dw_step, up_step): return clip(new_value, last_value + dw_step, last_value + up_step) -def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): +def get_lag_adjusted_curvature(steer_actuator_delay, v_ego, psis, curvatures, curvature_rates): if len(psis) != CONTROL_N: psis = [0.0]*CONTROL_N curvatures = [0.0]*CONTROL_N @@ -171,7 +171,7 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): v_ego = max(MIN_SPEED, v_ego) # TODO this needs more thought, use .2s extra for now to estimate other delays - delay = CP.steerActuatorDelay + .2 + delay = steer_actuator_delay + .2 # MPC can plan to turn the wheel and turn back before t_delay. This means # in high delay cases some corrections never even get commanded. So just use