diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index b42f61f1b5..e57d54cdca 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -105,6 +105,17 @@ class CarController(CarControllerBase): self.custom_stock_planner_speed = self.param_s.get_bool("CustomStockLongPlanner") self.lead_distance = 0 + self.jerk = 0.0 + self.jerk_l = 0.0 + self.jerk_u = 0.0 + self.jerkStartLimit = 2.0 + self.cb_upper = 0.0 + 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: lead_one = self.sm["radarState"].leadOne lead_two = self.sm["radarState"].leadTwo @@ -301,7 +312,8 @@ class CarController(CarControllerBase): # TODO: unclear if this is needed jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0 use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value - can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, jerk, int(self.frame / 2), + self.make_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), hud_control, set_speed_in_units, stopping, CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance)) @@ -479,3 +491,72 @@ class CarController(CarControllerBase): cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev) # MPH/KPH based button presses return cruise_button + + # jerk calculations thanks to apilot! + def cal_jerk(self, accel, actuators): + self.accel_raw = accel + if actuators.longControlState == LongCtrlState.off: + accel_diff = 0.0 + elif actuators.longControlState == LongCtrlState.stopping:# or hud_control.softHold > 0: + accel_diff = 0.0 + else: + accel_diff = self.accel_raw - self.accel_last + + accel_diff /= DT_CTRL + self.jerk = self.jerk * 0.9 + accel_diff * 0.1 + return self.jerk + + def make_jerk(self, CS, accel, actuators): + jerk = self.cal_jerk(accel, actuators) + a_error = accel - CS.out.aEgo + jerk = jerk + (a_error * 2.0) + + if self.CP.carFingerprint in CANFD_CAR: + startingJerk = 0.5 #self.jerkStartLimit + jerkLimit = 5.0 + self.jerk_count += DT_CTRL + jerk_max = interp(self.jerk_count, [0, 1.5, 2.5], [startingJerk, startingJerk, jerkLimit]) + if actuators.longControlState == LongCtrlState.off: + self.jerk_u = jerkLimit + self.jerk_l = jerkLimit + self.jerk_count = 0 + elif actuators.longControlState == LongCtrlState.stopping: + self.jerk_u += 0.1 if self.jerk_u < 1.5 else -0.1 + self.jerk_l += 0.1 if self.jerk_l < 1.0 else -0.1 + self.jerk_count = 0 + else: + #self.jerk_u = min(max(2.5, jerk * 2.0), jerk_max) + #self.jerk_l = min(max(2.0, -jerk * 3.0), jerkLimit) + self.jerk_u = min(max(0.5, jerk * 2.0), jerk_max) + self.jerk_l = min(max(1.0, -jerk * 3.0), jerkLimit) + else: + startingJerk = self.jerkStartLimit + jerkLimit = 5.0 + self.jerk_count += DT_CTRL + jerk_max = interp(self.jerk_count, [0, 1.5, 2.5], [startingJerk, startingJerk, jerkLimit]) + self.cb_upper = self.cb_lower = 0 + if actuators.longControlState == LongCtrlState.off: + self.jerk_u = jerkLimit + self.jerk_l = jerkLimit + self.jerk_count = 0 + elif actuators.longControlState == LongCtrlState.stopping: + self.jerk_u += 0.1 if self.jerk_u < 0.5 else -0.1 + self.jerk_l += 0.1 if self.jerk_l < 1.0 else -0.1 + self.jerk_count = 0 + else: + self.jerk_u = self.jerk_u * 0.8 + min(max(0.5, jerk * 2.0), jerk_max) * 0.2 + self.jerk_l = self.jerk_l * 0.8 + min(max(0.5, -jerk * 2.0), jerkLimit) * 0.2 + #self.jerk_l = min(max(1.2, -jerk * 2.0), jerkLimit) ## 1.0으로 하니 덜감속, 1.5로하니 너무감속, 1.2로 한번해보자(231228) + self.cb_upper = clip(0.9 + accel * 0.2, 0, 1.2) + self.cb_lower = clip(0.8 + accel * 0.2, 0, 1.2) + + def make_accel(self, actuators): + long_control = actuators.longControlState + is_ice = not self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV) + rate_up = 0.1 * 1 if is_ice else self.jerk_u + rate_down = 0.1 * 1 if is_ice else self.jerk_l + if long_control == LongCtrlState.off: + self.accel_raw, self.accel_val = 0, 0 + else: + self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up) + self.accel_last = self.accel_val diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index a2f44716ef..4e25772d17 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -130,8 +130,8 @@ def create_lfahda_mfc(packer, enabled, lat_active, lateral_paused, blinking_icon } return packer.make_can_msg("LFAHDA_MFC", 0, values) -def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, - CS, escc, CP, lead_distance): +def create_acc_commands(packer, enabled, accel_raw, accel_val, lower_jerk, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, + CS, escc, CP, lead_distance, cb_lower, cb_upper): commands = [] scc11_values = { @@ -150,8 +150,8 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, se scc12_values = { "ACCMode": 2 if enabled and long_override else 1 if enabled else 0, "StopReq": 1 if stopping else 0, - "aReqRaw": accel, - "aReqValue": accel, # stock ramps up and down respecting jerk limit until it reaches aReqRaw + "aReqRaw": accel_raw, + "aReqValue": accel_val, # stock ramps up and down respecting jerk limit until it reaches aReqRaw "CR_VSM_Alive": idx % 0xF, } @@ -207,7 +207,7 @@ def create_acc_opt(packer, escc, CS, CP): commands = [] scc13_values = { - "SCCDrvModeRValue": 2, + "SCCDrvModeRValue": 3, "SCC_Equip": 1, "Lead_Veh_Dep_Alert_USM": 2, } diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 452392fe85..3d78fd6957 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -104,7 +104,9 @@ class CarInterface(CarInterfaceBase): ret.stoppingControl = True ret.startingState = True ret.vEgoStarting = 0.1 - ret.startAccel = 1.0 + ret.startAccel = 1.8 + ret.stopAccel = 0.0 + ret.stoppingDecelRate = 10 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..da2f0e302b 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -71,6 +71,9 @@ class LongControl: if output_accel > self.CP.stopAccel: output_accel = min(output_accel, 0.0) output_accel -= self.CP.stoppingDecelRate * DT_CTRL + elif output_accel < self.CP.stopAccel: + output_accel = min(output_accel, 0.0) + output_accel += self.CP.stoppingDecelRate * DT_CTRL self.reset() elif self.long_control_state == LongCtrlState.starting: diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index a2df527a55..7296a28b11 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -55,7 +55,7 @@ T_IDXS = np.array(T_IDXS_LST) FCW_IDXS = T_IDXS < 5.0 T_DIFFS = np.diff(T_IDXS, prepend=[0.]) COMFORT_BRAKE = 2.5 -STOP_DISTANCE = 6.0 +STOP_DISTANCE = 8.0 def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard): if personality==custom.LongitudinalPersonalitySP.relaxed: