diff --git a/common/params.cc b/common/params.cc index 4e00f3f0fc..39abb8ed28 100644 --- a/common/params.cc +++ b/common/params.cc @@ -261,6 +261,7 @@ std::unordered_map keys = { {"HandsOnWheelMonitoring", PERSISTENT | BACKUP}, {"HasAcceptedTermsSP", PERSISTENT}, {"HideVEgoUi", PERSISTENT | BACKUP}, + {"HkgCustomLongTuning", PERSISTENT | BACKUP}, {"HkgSmoothStop", PERSISTENT | BACKUP}, {"HotspotOnBoot", PERSISTENT}, {"HotspotOnBootConfirmed", PERSISTENT}, diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 013b253e0b..b5cd6dad8a 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -1,7 +1,7 @@ from cereal import car import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV -from openpilot.common.numpy_fast import clip +from openpilot.common.numpy_fast import clip, interp from openpilot.common.params import Params from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg @@ -97,6 +97,19 @@ class CarController(CarControllerBase): self.hkg_can_smooth_stop = self.param_s.get_bool("HkgSmoothStop") 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 + self.accel_last_jerk = 0 + self.hkg_custom_long_tuning = self.param_s.get_bool("HkgCustomLongTuning") + def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float: lead_one = self.sm["radarState"].leadOne lead_two = self.sm["radarState"].leadTwo @@ -199,6 +212,9 @@ class CarController(CarControllerBase): if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: can_sends.append(make_tester_present_msg(0x7b1, self.CAN.ECAN, suppress_response=True)) + if self.CP.openpilotLongitudinalControl: + self.make_jerk(CS, accel, actuators) + # CAN-FD platforms if self.CP.carFingerprint in CANFD_CAR: hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2 @@ -227,7 +243,7 @@ class CarController(CarControllerBase): can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame)) if self.frame % 2 == 0: can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CS, CC.enabled and CS.out.cruiseState.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, - set_speed_in_units, hud_control)) + set_speed_in_units, hud_control, self.jerk_u, self.jerk_l)) self.accel_last = accel else: # button presses @@ -277,9 +293,10 @@ 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)) + CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance, self.cb_lower, self.cb_upper)) # 20 Hz LFA MFA message if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value: @@ -453,3 +470,66 @@ 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_jerk + + 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 not self.hkg_custom_long_tuning: + self.jerk_u = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0 + self.jerk_l = 5.0 + elif True: #self.CP.carFingerprint in CANFD_CAR or self.CP.carFingerprint == CAR.HYUNDAI_KONA_EV_2022: + startingJerk = 0.5 + 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 + else: + 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 + else: + self.jerk_u = min(max(0.5, jerk * 2.0), jerk_max) + self.jerk_l = min(max(0.5, -jerk * 2.0), jerkLimit) + 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 + rate_down = 0.1 + 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_val = self.accel_raw + self.accel_last = self.accel_val + self.accel_last_jerk = self.accel_val diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index e54e450787..fdea5c4971 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, } @@ -172,10 +172,10 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, se commands.append(packer.make_can_msg("SCC12", 0, scc12_values)) scc14_values = { - "ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values - "ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values + "ComfortBandUpper": cb_upper, # stock usually is 0 but sometimes uses higher values + "ComfortBandLower": cb_lower, # stock usually is 0 but sometimes uses higher values "JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values - "JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values + "JerkLowerLimit": lower_jerk, # stock usually is 0.5 but sometimes uses higher values "ACCMode": 2 if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage "ObjGap": get_object_gap(lead_distance), # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead } @@ -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/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index 12b84ad0f6..e32e462f1b 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -121,7 +121,7 @@ def create_lfahda_cluster(packer, CAN, enabled, lat_active, lateral_paused, blin return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values) -def create_acc_control(packer, CAN, CS, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control): +def create_acc_control(packer, CAN, CS, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, upper_jerk, lower_jerk): jerk = 5 jn = jerk / 50 if not enabled or gas_override: @@ -137,8 +137,8 @@ def create_acc_control(packer, CAN, CS, enabled, accel_last, accel, stopping, ga "aReqValue": a_val, "aReqRaw": a_raw, "VSetDis": set_speed, - "JerkLowerLimit": jerk if enabled else 1, - "JerkUpperLimit": 3.0, + "JerkLowerLimit": lower_jerk, + "JerkUpperLimit": upper_jerk, "ACC_ObjDist": 1, "ObjValid": 0, diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index c2140d2925..1a50d98b12 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 e3cd4bb654..1ee8a7b013 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -75,6 +75,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 == 0.0: + 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/ui/sunnypilot/qt/offroad/settings/vehicle_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle_settings.cc index 54fe4df7ce..6a78d5f71a 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle_settings.cc @@ -101,13 +101,14 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge // Hyundai/Kia/Genesis addItem(new LabelControlSP(tr("Hyundai/Kia/Genesis"))); - auto hkgSmoothStop = new ParamControlSP( - "HkgSmoothStop", - tr("HKG CAN: Smoother Stopping Performance (Beta)"), - tr("Smoother stopping behind a stopped car or desired stopping event. This is only applicable to HKG CAN platforms using openpilot longitudinal control."), - "../assets/offroad/icon_blank.png"); - hkgSmoothStop->setConfirmation(true, false); - addItem(hkgSmoothStop); + auto hkgCustomLongTuning = new ParamControlSP( + "HkgCustomLongTuning", + tr("HKG: Custom Tuning for New Longitudinal API"), + tr(""), + "../assets/offroad/icon_blank.png" + ); + hkgCustomLongTuning->setConfirmation(true, false); + addItem(hkgCustomLongTuning); // Subaru addItem(new LabelControlSP(tr("Subaru"))); @@ -205,7 +206,7 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge // trigger offroadTransition when going onroad/offroad connect(uiStateSP(), &UIStateSP::offroadTransition, [=](bool offroad) { is_onroad = !offroad; - hkgSmoothStop->setEnabled(offroad); + hkgCustomLongTuning->setEnabled(offroad); toyotaTss2LongTune->setEnabled(offroad); toyotaAbh->setEnabled(offroad); toyotaEnhancedBsm->setEnabled(offroad);