From 7560d8db9e44938380b07c428c515d8a3dfd3f10 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 11:11:00 -0400 Subject: [PATCH] do stopping accel in carcontroller --- selfdrive/car/hyundai/carcontroller.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 028bc796d6..a345c53762 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -110,6 +110,7 @@ class CarController(CarControllerBase): self.cb_lower = 0.0 self.jerk_count = 0.0 + self.accel_raw = 0 self.accel_val = 0 def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float: @@ -299,7 +300,7 @@ class CarController(CarControllerBase): jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0 use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value self.make_accel(accel, actuators) - can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2), + can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, self.accel_raw, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2), hud_control, set_speed_in_units, stopping, CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance, self.cb_lower, self.cb_upper)) @@ -533,14 +534,14 @@ class CarController(CarControllerBase): self.cb_lower = clip(0.8 + accel * 0.2, 0, 1.2) def make_accel(self, accel, actuators): + self.accel_raw = accel if actuators.longControlState == LongCtrlState.pid: - rate_up, rate_down = 0.0, 0.0 - self.accel_last = accel + rate_up, rate_down = 0.1, 0.1 else: rate_up = 0.02 * self.jerk_u rate_down = 0.02 * self.jerk_l if actuators.longControlState == LongCtrlState.off: - self.accel_val = 0 + self.accel_raw, self.accel_val = 0, 0 else: - self.accel_val = clip(accel, self.accel_last - rate_down, self.accel_last + rate_up) + self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up) self.accel_last = self.accel_val