diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 1031bde6528c9f5..46eca5038edfbd1 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -12,6 +12,7 @@ MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \ UNSUPPORTED_DSU_CAR, STOP_AND_GO_CAR, Ecu from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS +from openpilot.selfdrive.controls.lib.pid import PIDController from opendbc.can.packer import CANPacker from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_acceleration import get_max_allowed_accel @@ -24,7 +25,7 @@ # The up limit allows the brakes/gas to unwind quickly leaving a stop, # the down limit roughly matches the rate of ACCEL_NET, reducing PCM compensation windup -ACCEL_WINDUP_LIMIT = 0.5 # m/s^2 / frame +ACCEL_WINDUP_LIMIT = 6.0 * DT_CTRL * 3 # m/s^2 / frame ACCEL_WINDDOWN_LIMIT = -4.0 * DT_CTRL * 3 # m/s^2 / frame # LKA limits @@ -62,8 +63,8 @@ def __init__(self, dbc_name, CP, VM): self.distance_button = 0 self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL) - self.net_acceleration_request = FirstOrderFilter(0, 0.15, DT_CTRL * 3) + self.accel_pid = PIDController(2.0, 0.5, 1 / (DT_CTRL * 3)) self.pcm_accel_compensation = FirstOrderFilter(0, 0.5, DT_CTRL * 3) # the PCM's reported acceleration request can sometimes mismatch aEgo, close the loop @@ -73,8 +74,10 @@ def __init__(self, dbc_name, CP, VM): # so we error correct on the filtered PCM acceleration request using the actuator delay. # TODO: move the delay into the interface self.pcm_accel_net = FirstOrderFilter(0, self.CP.longitudinalActuatorDelay, DT_CTRL * 3) + self.net_acceleration_request = FirstOrderFilter(0, 0.15, DT_CTRL * 3) if not any(fw.ecu == Ecu.hybrid for fw in self.CP.carFw): self.pcm_accel_net.update_alpha(self.CP.longitudinalActuatorDelay + 0.2) + self.net_acceleration_request.update_alpha(self.CP.longitudinalActuatorDelay + 0.2) self.packer = CANPacker(dbc_name) self.accel = 0 @@ -253,15 +256,15 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): # let PCM handle stopping for now, error correct on a delayed acceleration request pcm_accel_compensation = 0.0 if not stopping: - pcm_accel_compensation = 2.0 * (new_pcm_accel_net - self.net_acceleration_request.x) - - # prevent compensation windup - if frogpilot_toggles.sport_plus: - pcm_accel_compensation = clip(pcm_accel_compensation, pcm_accel_cmd - get_max_allowed_accel(CS.out.vEgo), - pcm_accel_cmd - self.params.ACCEL_MIN) + # prevent compensation windup + if frogpilot_toggles.sport_plus: + self.accel_pid.neg_limit = pcm_accel_cmd - get_max_allowed_accel(CS.out.vEgo) + else: + self.accel_pid.neg_limit = pcm_accel_cmd - self.params.ACCEL_MAX + self.accel_pid.pos_limit = pcm_accel_cmd - self.params.ACCEL_MIN + pcm_accel_compensation = self.accel_pid.update(new_pcm_accel_net - self.net_acceleration_request.x) else: - pcm_accel_compensation = clip(pcm_accel_compensation, pcm_accel_cmd - self.params.ACCEL_MAX, - pcm_accel_cmd - self.params.ACCEL_MIN) + self.accel_pid.reset() pcm_accel_cmd = pcm_accel_cmd - self.pcm_accel_compensation.update(pcm_accel_compensation) @@ -270,6 +273,7 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles): self.pcm_accel_net_offset.x = 0.0 self.net_acceleration_request.x = 0.0 self.pcm_accel_net.x = CS.pcm_accel_net + self.accel_pid.reset() self.permit_braking = True # Along with rate limiting positive jerk above, this greatly improves gas response time diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index c2853731cf89d39..807c8ff65fe3139 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -62,10 +62,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) \ and not (ret.flags & FrogPilotToyotaFlags.SMART_DSU) - if candidate == CAR.LEXUS_ES_TSS2 and Ecu.hybrid not in found_ecus: - ret.flags |= ToyotaFlags.RAISED_ACCEL_LIMIT.value - - if candidate == CAR.LEXUS_ES_TSS2 and Ecu.hybrid not in found_ecus: + if candidate in (CAR.LEXUS_ES_TSS2, CAR.TOYOTA_COROLLA_TSS2) and Ecu.hybrid not in found_ecus: ret.flags |= ToyotaFlags.RAISED_ACCEL_LIMIT.value if candidate == CAR.TOYOTA_PRIUS: