diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index a051e13c1d..ee700ffc9c 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -111,8 +111,6 @@ class CarController(CarControllerBase): self.jerk_count = 0.0 self.accel_val = 0 - self.accel_raw = 0 - self.accel_frame = 0 def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float: lead_one = self.sm["radarState"].leadOne @@ -302,7 +300,7 @@ class CarController(CarControllerBase): use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value self.make_jerk(CS, accel, actuators) self.make_accel(accel, actuators) - 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), + 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), 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)) @@ -536,17 +534,8 @@ 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.off: - self.accel_raw, self.accel_val = 0, 0 - elif actuators.longControlState == LongCtrlState.starting and self.accel_frame <= 1: - self.accel_frame += 1 - self.accel_raw, self.accel_val = accel, accel + self.accel_val = 0 else: - if actuators.longControlState == LongCtrlState.stopping: - self.accel_raw = 0 - self.accel_val = clip(self.accel_raw, self.accel_last - 0.1, self.accel_last + 0.1) + self.accel_val = clip(accel, self.accel_last - 0.1, self.accel_last + 0.1) self.accel_last = self.accel_val - - if not actuators.longControlState == LongCtrlState.starting or self.accel_frame >= 2: - self.accel_frame = 0 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 88827e61ab..627328c0d9 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -105,6 +105,7 @@ class CarInterface(CarInterfaceBase): ret.startingState = True ret.vEgoStarting = 0.1 ret.startAccel = 2.0 + ret.stopAccel = 0.0 ret.longitudinalActuatorDelay = 0.5 if DBC[ret.carFingerprint]["radar"] is None: diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index a661c33361..a477b48780 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -51,6 +51,8 @@ class LongControl: k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL) self.last_output_accel = 0.0 + self.frame = 0 + def reset(self): self.pid.reset() @@ -74,13 +76,22 @@ class LongControl: self.reset() elif self.long_control_state == LongCtrlState.starting: - output_accel = self.CP.startAccel - self.reset() + if self.frame < 10 and self.CP.carName == "hyundai": + self.frame += 1 + output_accel = self.CP.startAccel + self.reset() + else: # LongCtrlState.pid + error = a_target - CS.aEgo + output_accel = self.pid.update(error, speed=CS.vEgo, + feedforward=a_target) else: # LongCtrlState.pid error = a_target - CS.aEgo output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=a_target) + if not self.long_control_state == LongCtrlState.starting: + self.frame = 0 + self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) return self.last_output_accel