diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 556bf13520..416dbc4530 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -1,4 +1,5 @@ import math +from collections import deque from opendbc.car import carlog, apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \ make_tester_present_msg, rate_limit, structs from opendbc.car.can_definitions import CanData @@ -45,6 +46,9 @@ def __init__(self, dbc_name, CP): self.steer_rate_counter = 0 self.distance_button = 0 + self.accel_deque = deque([], maxlen=100) + self.rl_accel = False + self.pcm_accel_compensation = 0.0 self.permit_braking = 0.0 @@ -148,9 +152,23 @@ def update(self, CC, CS, now_nanos): # For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot and imprecise braking # TODO: sometimes when switching from brake to gas quickly, CLUTCH->ACCEL_NET shows a slow unwind. make it go to 0 immediately if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT and CC.longActive and not CS.out.cruiseState.standstill: + self.accel_deque.append(actuators.accel) + + if len(self.accel_deque) > 50: + if abs(actuators.accel - self.accel_deque[-50]) > 1.0: + self.rl_accel = True + elif abs(actuators.accel - self.accel_deque[-50]) < 0.1: + self.rl_accel = False + + if self.rl_accel: + actuators_accel = rate_limit(actuators.accel, self.accel_deque[-2], -0.5 / 100, 0.5 / 100) + else: + actuators_accel = actuators.accel + # calculate amount of acceleration PCM should apply to reach target, given pitch - accel_due_to_pitch = math.sin(CS.slope_angle) * ACCELERATION_DUE_TO_GRAVITY - net_acceleration_request = actuators.accel + accel_due_to_pitch + # accel_due_to_pitch = math.sin(CS.slope_angle) * ACCELERATION_DUE_TO_GRAVITY + accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY + net_acceleration_request = actuators_accel + accel_due_to_pitch if self.CP.flags & ToyotaFlags.HYBRID: # gas includes regen and engine braking @@ -165,11 +183,11 @@ def update(self, CC, CS, now_nanos): pcm_accel_compensation = 2.0 * (pcm_accel_net - net_acceleration_request) # prevent compensation windup - pcm_accel_compensation = clip(pcm_accel_compensation, actuators.accel - self.params.ACCEL_MAX, - actuators.accel - self.params.ACCEL_MIN) + pcm_accel_compensation = clip(pcm_accel_compensation, actuators_accel - self.params.ACCEL_MAX, + actuators_accel - self.params.ACCEL_MIN) - self.pcm_accel_compensation = rate_limit(pcm_accel_compensation, self.pcm_accel_compensation, -0.01, 0.01) - pcm_accel_cmd = actuators.accel - self.pcm_accel_compensation + self.pcm_accel_compensation = rate_limit(pcm_accel_compensation, self.pcm_accel_compensation, -0.01 / 3, 0.01 / 3) + pcm_accel_cmd = actuators_accel - self.pcm_accel_compensation # Along with rate limiting positive jerk below, this greatly improves gas response time # Consider the net acceleration request that the PCM should be applying (pitch included) @@ -178,6 +196,7 @@ def update(self, CC, CS, now_nanos): elif net_acceleration_request > 0.2: self.permit_braking = False else: + self.accel_deque.clear() self.pcm_accel_compensation = 0.0 pcm_accel_cmd = actuators.accel self.permit_braking = True @@ -215,6 +234,7 @@ def update(self, CC, CS, now_nanos): elif self.CP.openpilotLongitudinalControl: # internal PCM gas command can get stuck unwinding from negative accel so we apply a generous rate limit pcm_accel_cmd = min(pcm_accel_cmd, self.accel + ACCEL_WINDUP_LIMIT) if CC.longActive else 0.0 + # pcm_accel_cmd = rate_limit(pcm_accel_cmd, self.accel, -0.5 / 33, 0.5 / 33) if CC.longActive else 0.0 can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.permit_braking, self.standstill_req, lead, CS.acc_type, fcw_alert, self.distance_button))