From bc1aad6f6a66fbeb490997ee7f8a282feb5f6dfd Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 12 Jul 2024 21:36:28 +0000 Subject: [PATCH 01/89] git-crypt: add collaborators --- .git-crypt/.gitattributes | 4 ++++ .../106E1EB7856273777A0C8BB4EF8EA444C1E7B69C.gpg | Bin 0 -> 725 bytes .../E08F802F45946A6A3E16AD1B1609CD53B15C2462.gpg | Bin 0 -> 735 bytes 3 files changed, 4 insertions(+) create mode 100644 .git-crypt/.gitattributes create mode 100644 .git-crypt/keys/default/0/106E1EB7856273777A0C8BB4EF8EA444C1E7B69C.gpg create mode 100644 .git-crypt/keys/default/0/E08F802F45946A6A3E16AD1B1609CD53B15C2462.gpg diff --git a/.git-crypt/.gitattributes b/.git-crypt/.gitattributes new file mode 100644 index 0000000000..665b10e8f0 --- /dev/null +++ b/.git-crypt/.gitattributes @@ -0,0 +1,4 @@ +# Do not edit this file. To specify the files to encrypt, create your own +# .gitattributes file in the directory where your files are. +* !filter !diff +*.gpg binary diff --git a/.git-crypt/keys/default/0/106E1EB7856273777A0C8BB4EF8EA444C1E7B69C.gpg b/.git-crypt/keys/default/0/106E1EB7856273777A0C8BB4EF8EA444C1E7B69C.gpg new file mode 100644 index 0000000000000000000000000000000000000000..97b89be16eef468dea1688448132cd49bc1a9205 GIT binary patch literal 725 zcmV;`0xJE50t^Eiti!r3*)-+>5CEM;BTRKyWmz_3gO6PMXMbPN8+OdA6CadusYg9^ zWEg3Hz?fKrSv+uXx{Tssm0G=WQZdR4NKEO3c+2^|&X>&>$KG$#mLSy6=!V~-Blgi< z4rbWQe~-g;X9ORi=kxVsUp%TuJzT8$KfwLD=!;G22C0wERyD)W8*Cqx$nwrUQ}{`SO70zzskdift(^gjR0vt;&(*}}_sKQrIoG1GHILGca1FFrY23uBQ6ca*) zN2ZBxDgh+>4NJ=^wpG%=0|AIJdW$G(>Cj!#x{XK+oplLEf2E9Ui6cd(9*v zH2SrDh492MO=i<_&_X5mlVw}px}?7bLxm!ItMLI`Vl-3GhjO*>3*S^}x#3+R_LqTs z0@tS3baUe2=5JPH*zgRlaWq~6WGGaggBmy;;TaC7@~U7}3DrU33Z9lF@?;bC*_7)rtkjg8j6|?1VHGyK^gn?l6xnAgEx#Dza?w=UzyMmxvM+Qe+SWlz+UQ?Q5m53(MN8h>Y>D@RZ-j5M)P(Kq>$`lN{RFr{12*0F2 znlSADOAt%tK)Tu_aX3x5P=wumS{_Dv-_i20k{~-J6SYQl>~UN0JXnmf;WYQ+ zl=rpmLzyLjcWaVY!<;pOvWuLA(=M<#_tbnAXLS)ki|#8FaJ)whiG&coR=Z_ikWR>? zFBgrx0Tgi?(Q}^(EIoM>nkS(~sCSo~1hPsEAML=nFNbsqHIK9R^QNoeBBF7c`X>Pv z2tZj&5Q(v;8Tl^wn9uy`m|qfw_BRewH3nw`$fLi)1Zji~Pel2nzBJymh@fGl!s)#%;n5V(c*u;yCs?@dDe?5QF{V6*~;eqdI%ym|q}v^cIxfgJah zqPTravGaxq?yS&u?A~3E+J@@>Am<`ioGZhA`B?4FL%P5Ju*TZc+T%>mQZjHZ3PF!AeqNfQ7bGbH`6kQ_jEj zxk`Gih-)5$|KoWu-$MlpAwwb|lTjPXCnRW-KEjg+vFbb8bf`&zYCkC94P{ekg-Euy zRaURgwANGWf>8S~<}tx-0%z3@hQAg0*mp#WlH>^*hKrdC1X+)1(5^$P0q!#8j`fGP zk8f_lbc5xFGx{2PXsOrdN6YR9AJA=?S=57 RnAcT|%MwwGhU?B@M#$H9W Date: Mon, 8 Jul 2024 19:10:26 +0200 Subject: [PATCH 02/89] Processor definition check for __APPLE__ has a typo on replay (#32930) (cherry picked from commit 3c74ad145e0145429d782219ac255a26aa2f6135) --- tools/replay/util.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/replay/util.cc b/tools/replay/util.cc index a08b3b3d5e..aaf73749d5 100644 --- a/tools/replay/util.cc +++ b/tools/replay/util.cc @@ -323,7 +323,7 @@ void precise_nano_sleep(int64_t nanoseconds, std::atomic &should_exit) { req.tv_sec = nanoseconds / 1000000000; req.tv_nsec = nanoseconds % 1000000000; while (!should_exit) { -#ifdef __APPLE_ +#ifdef __APPLE__ int ret = nanosleep(&req, &rem); if (ret == 0 || errno != EINTR) break; From 62cc0fa0d23d01575d6615c60681e4cb77457a20 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 16 Jul 2024 02:59:02 +0000 Subject: [PATCH 03/89] Scons: Build sunnypilot elements with added GPG keys --- .gitlab-ci.yml | 2 ++ SConstruct | 47 ++++++++++++++++++++++++++++++ selfdrive/ui/SConscript | 6 ++++ selfdrive/ui/sunnypilot/SConscript | 10 +++++++ 4 files changed, 65 insertions(+) create mode 100644 selfdrive/ui/sunnypilot/SConscript diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 21958355c5..de50202454 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -129,6 +129,8 @@ build: --exclude='**/selfdrive/ui/**/*.h' --exclude='**/selfdrive/ui/qt/offroad/sunnypilot/' --exclude='**/.git/' + --exclude='**/SConstruct' + --exclude='**/SConscript' --delete-excluded --chown=comma:comma ${BUILD_DIR}/ ${OUTPUT_DIR}/ diff --git a/SConstruct b/SConstruct index da70e4e587..13da37c7a8 100644 --- a/SConstruct +++ b/SConstruct @@ -7,6 +7,8 @@ import numpy as np import SCons.Errors +from openpilot.common.basedir import BASEDIR + SCons.Warnings.warningAsException(True) # pending upstream fix - https://github.com/SCons/scons/issues/4461 @@ -16,6 +18,45 @@ TICI = os.path.isfile('/TICI') AGNOS = TICI UBUNTU_FOCAL = int(subprocess.check_output('[ -f /etc/os-release ] && . /etc/os-release && [ "$ID" = "ubuntu" ] && [ "$VERSION_ID" = "20.04" ] && echo 1 || echo 0', shell=True, encoding='utf-8').rstrip()) Export('UBUNTU_FOCAL') +_DEBUG = False + +def is_internal_developer(debug=False): + def collect_required_gpg_key_ids(keys_dir): + try: + key_ids = [f.split('.')[0] for f in os.listdir(keys_dir) if f.endswith(".gpg")] + if debug: + print(f"SP: Required GPG key IDs: {key_ids}") + return key_ids + except OSError as e: + if debug: + print(f"SP: Failed to read GPG key IDs from {keys_dir}. Error: {e}") + return [] + + def is_key_available(required_gpg_key_ids): + for key_id in required_gpg_key_ids: + try: + result = subprocess.check_output(['gpg', '--list-keys', key_id], stderr=subprocess.STDOUT) + if key_id in result.decode(): + if debug: + print(f"SP: GPG key {key_id} is available.") + return True + except subprocess.CalledProcessError as e: + if debug: + print(f"SP: Failed to list GPG key {key_id}. Error:", e.output.decode().strip()) + return False + + keys_dir = os.path.join(BASEDIR, ".git-crypt/keys/default/0") + required_gpg_key_ids = collect_required_gpg_key_ids(keys_dir) + + sunnypilot = is_key_available(required_gpg_key_ids) + + if sunnypilot: + print("SP: Confirmed sunnypilot internal developer.") + print("SP: Loading sunnypilot elements ...") + elif debug: + print("SP: None of the required GPG keys are available.") + + return sunnypilot Decider('MD5-timestamp') @@ -72,6 +113,12 @@ AddOption('--minimal', default=os.path.exists(File('#.lfsconfig').abspath), # minimal by default on release branch (where there's no LFS) help='the minimum build to run openpilot. no tests, tools, etc.') +AddOption('--sunnypilot', + action='store_true', + dest='sunnypilot', + default=is_internal_developer(_DEBUG), # check if the current user is a sunnypilot developer + help='build sunnypilot elements and other sunnypilot-specific items that are meant for internal development') + ## Architecture name breakdown (arch) ## - larch64: linux tici aarch64 ## - aarch64: linux pc aarch64 diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index e1233b5cbc..f4f5a13919 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -17,6 +17,12 @@ if arch == "Darwin": # FIXME: remove this once we're on 5.15 (24.04) qt_env['CXXFLAGS'] += ["-Wno-deprecated-declarations"] +sp_widgets_src = [] +sp_qt_src = [] +if GetOption('sunnypilot'): + SConscript(['sunnypilot/SConscript']) + Import('sp_widgets_src', 'sp_qt_src') + qt_util = qt_env.Library("qt_util", ["#selfdrive/ui/qt/api.cc", "#selfdrive/ui/qt/util.cc"], LIBS=base_libs) widgets_src = ["ui.cc", "qt/widgets/input.cc", "qt/widgets/wifi.cc", "qt/widgets/ssh_keys.cc", "qt/widgets/toggle.cc", "qt/widgets/controls.cc", diff --git a/selfdrive/ui/sunnypilot/SConscript b/selfdrive/ui/sunnypilot/SConscript new file mode 100644 index 0000000000..4431fc3ea0 --- /dev/null +++ b/selfdrive/ui/sunnypilot/SConscript @@ -0,0 +1,10 @@ +widgets_src = [] + +network_src = [] + +qt_src = [] + +sp_widgets_src = widgets_src + network_src +sp_qt_src = qt_src + +Export('sp_widgets_src', 'sp_qt_src') From a9bcc782e0cd8432125817ff36258e83385f8b91 Mon Sep 17 00:00:00 2001 From: Jaosn Wen Date: Tue, 16 Jul 2024 00:07:29 -0400 Subject: [PATCH 04/89] Scons: Set `SUNNYPILOT` to `CPPDEFINES` --- selfdrive/ui/SConscript | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index f4f5a13919..99ec3c161d 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -43,7 +43,7 @@ widgets_src += ["qt/offroad/sunnypilot/display_settings.cc", "qt/offroad/sunnypi widgets_src += ["qt/network/sunnylink/sunnylink_client.cc", "qt/network/sunnylink/services/base_device_service.cc", "qt/network/sunnylink/services/role_service.cc", "qt/network/sunnylink/services/user_service.cc"] -qt_env['CPPDEFINES'] = [] +qt_env['CPPDEFINES'] = ["SUNNYPILOT"] if GetOption('sunnypilot') else [] if maps: base_libs += ['QMapLibre'] widgets_src += ["qt/maps/map_helpers.cc", "qt/maps/map_settings.cc", "qt/maps/map.cc", "qt/maps/map_panel.cc", From 0ca98a8379ec1a687529ce87117c2d9a9da1436f Mon Sep 17 00:00:00 2001 From: Jaosn Wen Date: Tue, 16 Jul 2024 00:20:45 -0400 Subject: [PATCH 05/89] Scons: Split sunnypilot/Sconscript --- selfdrive/ui/SConscript | 20 +++---------------- selfdrive/ui/sunnypilot/SConscript | 32 +++++++++++++++++++++++++++--- 2 files changed, 32 insertions(+), 20 deletions(-) diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 99ec3c161d..3d49b3df14 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -1,6 +1,6 @@ import os import json -Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', 'transformations', 'UBUNTU_FOCAL') +Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', 'transformations') base_libs = [common, messaging, visionipc, transformations, 'm', 'OpenCL', 'ssl', 'crypto', 'pthread'] + qt_env["LIBS"] @@ -28,20 +28,7 @@ widgets_src = ["ui.cc", "qt/widgets/input.cc", "qt/widgets/wifi.cc", "qt/widgets/ssh_keys.cc", "qt/widgets/toggle.cc", "qt/widgets/controls.cc", "qt/widgets/offroad_alerts.cc", "qt/widgets/prime.cc", "qt/widgets/keyboard.cc", "qt/widgets/scrollview.cc", "qt/widgets/cameraview.cc", "#third_party/qrcode/QrCode.cc", - "qt/request_repeater.cc", "qt/qt_window.cc", "qt/network/networking.cc", "qt/network/wifi_manager.cc"] - -widgets_src += ["qt/offroad/sunnypilot/display_settings.cc", "qt/offroad/sunnypilot/sunnypilot_settings.cc", - "qt/offroad/sunnypilot/vehicle_settings.cc", "qt/offroad/sunnypilot/visuals_settings.cc", - "qt/offroad/sunnypilot/trips_settings.cc", "qt/offroad/sunnypilot/mads_settings.cc", - "qt/offroad/sunnypilot/lane_change_settings.cc", "qt/offroad/sunnypilot/speed_limit_control_settings.cc", - "qt/offroad/sunnypilot/monitoring_settings.cc", "qt/offroad/sunnypilot/osm_settings.cc", - "qt/offroad/sunnypilot/custom_offsets_settings.cc", "qt/widgets/sunnypilot/drive_stats.cc", - "qt/offroad/sunnypilot/software_settings_sp.cc", "qt/offroad/sunnypilot/models_fetcher.cc", - "qt/offroad/sunnypilot/speed_limit_warning_settings.cc", "qt/offroad/sunnypilot/speed_limit_policy_settings.cc", - "qt/offroad/sunnypilot/sunnylink_settings.cc"] - -widgets_src += ["qt/network/sunnylink/sunnylink_client.cc", "qt/network/sunnylink/services/base_device_service.cc", - "qt/network/sunnylink/services/role_service.cc", "qt/network/sunnylink/services/user_service.cc"] + "qt/request_repeater.cc", "qt/qt_window.cc", "qt/network/networking.cc", "qt/network/wifi_manager.cc"] + sp_widgets_src qt_env['CPPDEFINES'] = ["SUNNYPILOT"] if GetOption('sunnypilot') else [] if maps: @@ -59,8 +46,7 @@ qt_src = ["main.cc", "qt/sidebar.cc", "qt/body.cc", "qt/offroad/software_settings.cc", "qt/offroad/onboarding.cc", "qt/offroad/driverview.cc", "qt/offroad/experimental_mode.cc", "qt/onroad/onroad_home.cc", "qt/onroad/annotated_camera.cc", - "qt/onroad/buttons.cc", "qt/onroad/alerts.cc", - "qt/onroad_settings.cc", "qt/onroad_settings_panel.cc"] + "qt/onroad/buttons.cc", "qt/onroad/alerts.cc"] + sp_qt_src # build translation files with open(File("translations/languages.json").abspath) as f: diff --git a/selfdrive/ui/sunnypilot/SConscript b/selfdrive/ui/sunnypilot/SConscript index 4431fc3ea0..9290fcea58 100644 --- a/selfdrive/ui/sunnypilot/SConscript +++ b/selfdrive/ui/sunnypilot/SConscript @@ -1,8 +1,34 @@ -widgets_src = [] +widgets_src = [ + "qt/offroad/sunnypilot/custom_offsets_settings.cc", + "qt/offroad/sunnypilot/display_settings.cc", + "qt/offroad/sunnypilot/lane_change_settings.cc", + "qt/offroad/sunnypilot/mads_settings.cc", + "qt/offroad/sunnypilot/models_fetcher.cc", + "qt/offroad/sunnypilot/monitoring_settings.cc", + "qt/offroad/sunnypilot/osm_settings.cc", + "qt/offroad/sunnypilot/software_settings_sp.cc", + "qt/offroad/sunnypilot/speed_limit_control_settings.cc", + "qt/offroad/sunnypilot/speed_limit_policy_settings.cc", + "qt/offroad/sunnypilot/speed_limit_warning_settings.cc", + "qt/offroad/sunnypilot/sunnypilot_settings.cc", + "qt/offroad/sunnypilot/sunnylink_settings.cc", + "qt/offroad/sunnypilot/trips_settings.cc", + "qt/offroad/sunnypilot/vehicle_settings.cc", + "qt/offroad/sunnypilot/visuals_settings.cc", + "qt/widgets/sunnypilot/drive_stats.cc" +] -network_src = [] +network_src = [ + "qt/network/sunnylink/services/base_device_service.cc", + "qt/network/sunnylink/services/role_service.cc", + "qt/network/sunnylink/services/user_service.cc", + "qt/network/sunnylink/sunnylink_client.cc" +] -qt_src = [] +qt_src = [ + "qt/onroad_settings.cc", + "qt/onroad_settings_panel.cc" +] sp_widgets_src = widgets_src + network_src sp_qt_src = qt_src From 4821f1bbc81293ecc64f67e35dc0827502de8c5f Mon Sep 17 00:00:00 2001 From: Jaosn Wen Date: Tue, 16 Jul 2024 20:54:08 -0400 Subject: [PATCH 06/89] HKG Longitudinal: Jerk limits --- selfdrive/car/hyundai/carcontroller.py | 65 +++++++++++++++++++++++++- selfdrive/car/hyundai/hyundaican.py | 2 +- 2 files changed, 64 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index f640f6529a..bc29caa28d 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 openpilot.common.realtime import DT_CTRL from opendbc.can.packer import CANPacker @@ -102,6 +102,11 @@ 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 + def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float: lead_one = self.sm["radarState"].leadOne lead_two = self.sm["radarState"].leadTwo @@ -286,7 +291,7 @@ 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), + can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, 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)) @@ -462,3 +467,59 @@ 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 + + def cal_jerk(self, accel, actuators): + 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 = accel - 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, hud_control): + 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 or hud_control.softHold > 0: + 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 or hud_control.softHold > 0: + 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) diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index a2f44716ef..83ceb38659 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -130,7 +130,7 @@ 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, +def create_acc_commands(packer, enabled, accel, lower_jerk, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, CS, escc, CP, lead_distance): commands = [] From dac5e529b6b737567fb4e5e4113c042b2faf77fe Mon Sep 17 00:00:00 2001 From: Jaosn Wen Date: Tue, 16 Jul 2024 20:55:25 -0400 Subject: [PATCH 07/89] add lower --- selfdrive/car/hyundai/hyundaican.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index 83ceb38659..b28a94e055 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -175,7 +175,7 @@ def create_acc_commands(packer, enabled, accel, lower_jerk, upper_jerk, idx, hud "ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values "ComfortBandLower": 0.0, # 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 } From 91dadaafd1071575ed5cf084a917d0bcafbcd666 Mon Sep 17 00:00:00 2001 From: Jaosn Wen Date: Tue, 16 Jul 2024 21:11:18 -0400 Subject: [PATCH 08/89] more, and comfort band --- selfdrive/car/hyundai/carcontroller.py | 13 +++++++++---- selfdrive/car/hyundai/hyundaican.py | 6 +++--- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index bc29caa28d..74b9bf1811 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -106,6 +106,9 @@ class CarController(CarControllerBase): 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 def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float: lead_one = self.sm["radarState"].leadOne @@ -279,6 +282,8 @@ class CarController(CarControllerBase): self.last_button_frame = self.frame elif self.frame % 2 == 0: can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25) + else: + self.make_jerk(CS, accel, actuators) # Parse lead distance from radarState and display the corresponding distance in the car's cluster if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0: @@ -293,7 +298,7 @@ class CarController(CarControllerBase): 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, 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: @@ -480,7 +485,7 @@ class CarController(CarControllerBase): self.jerk = self.jerk * 0.9 + accel_diff * 0.1 return self.jerk - def make_jerk(self, CS, accel, actuators, hud_control): + 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) @@ -494,7 +499,7 @@ class CarController(CarControllerBase): self.jerk_u = jerkLimit self.jerk_l = jerkLimit self.jerk_count = 0 - elif actuators.longControlState == LongCtrlState.stopping or hud_control.softHold > 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 @@ -513,7 +518,7 @@ class CarController(CarControllerBase): self.jerk_u = jerkLimit self.jerk_l = jerkLimit self.jerk_count = 0 - elif actuators.longControlState == LongCtrlState.stopping or hud_control.softHold > 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 diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index b28a94e055..f3776c46ef 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -131,7 +131,7 @@ 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, lower_jerk, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, - CS, escc, CP, lead_distance): + CS, escc, CP, lead_distance, cb_lower, cb_upper): commands = [] scc11_values = { @@ -172,8 +172,8 @@ def create_acc_commands(packer, enabled, accel, lower_jerk, upper_jerk, idx, hud 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": 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 From faf52492b67dd995b1643c49de602d82929c1e5d Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 17 Jul 2024 03:26:13 +0000 Subject: [PATCH 09/89] ci: git-crypt: Decrypt prior to building --- ...91D8EDBE6BD70DFB803D59292470ADBDD426DEB5.gpg | Bin 0 -> 725 bytes .gitlab-ci.yml | 16 +++++++++++----- 2 files changed, 11 insertions(+), 5 deletions(-) create mode 100644 .git-crypt/keys/default/0/91D8EDBE6BD70DFB803D59292470ADBDD426DEB5.gpg diff --git a/.git-crypt/keys/default/0/91D8EDBE6BD70DFB803D59292470ADBDD426DEB5.gpg b/.git-crypt/keys/default/0/91D8EDBE6BD70DFB803D59292470ADBDD426DEB5.gpg new file mode 100644 index 0000000000000000000000000000000000000000..3da4a7ec9c844f8cb3701d07cb6de3987d8bf3ea GIT binary patch literal 725 zcmV;`0xJE50t^G?W8JRowQkM<5CFerQXytR3w@K7wZ4B*%60DzaJXzr!*q3W+4=9m z17js$=kThC5ai4wEikaIm_X;~NdGfwLAqk*VtrXi3h7v9fhF$}jmr$w}(@|UW zCDSs`cxVBK0OX@n!pU+=3s>=PP<`7I*TgwzlW}bCJCxAdJ=4{%NGkuX2|Y$;ev}{L z5K_5bU1AU%jBP?KVo4zu5kLdZ>i=eEjD7&CVB(9&Se)dt^i%Wd@R;T2ZC?d*n7`AV zWZI}=|MbRfHX7&T2=1TyZJAdaN5iQH?*Z3{`z4N5Y6=`NNKGrYI!f*$v1R`6NzE=$ zQaUW|sFgpJ$pX0W)o~=x)WCHO?{R4=qJRk7)Z}YSJT&nYoyuc;14=$octKO1;nyrH zh?nA@kC?#@x0f28qYC*srv67oHzn+DI^+(3ps`LSlfzcJ@he*!Hn*PEW{;&fCcx4B zqTYwap!a3@OfA*S>CD|^9VJ2)+7+0f&_EVya;dcq=`$JeyO{H!VN6!XkO@SR1MjPX z9LQDJ3%q65Zt!N@N3znu0|6YI82ww*3PRzyAIJeTfu$`dI#tndf?i0VwJCCjU#Zeg z4-(gUMpGm7v8X7c9iP9JD(my5N%*Q2yv0j`6r9vf!p4S`EmHT5gs5-@0>f-CT&9}f z`AuXtw#?1kk{ghL1gqZYO&3}31ZbTi23N5N1fdQh2YSxD{`dF-fEt2;y1)6ri0-mj zn!xr}jV&Qmo;v${hx}{D(h9&5Ra}+i#}apkZbKLQI=iu;U@^hnFk;ij8&U+b Date: Thu, 18 Jul 2024 09:08:52 +0200 Subject: [PATCH 10/89] Back to using reference on pipeline --- .gitlab-ci.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 1775e4ab7a..aceeff95bd 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -231,13 +231,13 @@ publish to public github prebuilt: needs: ["build"] variables: DISCORD_HOOK: "${DISCORD_MANUAL_BUILD_WEBHOOK_URL}" # Default hook if not overriden by children - before_script: ¬ify_discord_before_script + before_script: - 'apk add curl jq envsubst' script: - echo using [${TEMPLATE}] - cat release/ci/${TEMPLATE} | envsubst | tee payload.json - 'curl -X POST -H "Content-Type: application/json" -d "$(cat payload.json)" ${DISCORD_HOOK} | jq .' - rules: ¬ify_discord_rules + rules: - if: $NEW_BRANCH when: on_success - when: never @@ -248,7 +248,7 @@ notify pending action: variables: TEMPLATE: "discord_template_notify_dev_private.json" before_script: - - *notify_discord_before_script + - !reference [".notify_discord", "before_script"] - export AVATAR_URL=$(curl -s -X GET "https://gitlab.com/api/v4/avatar?email=${GITLAB_USER_EMAIL}" | jq -r '.avatar_url') notify new dev build: @@ -261,4 +261,4 @@ notify new dev build: - if: $NEW_BRANCH == "dev-c3" variables: DISCORD_HOOK: "${DISCORD_NEW_BUILD_WEBHOOK_URL}" # Overriding hook because we know we are dev-c3 - - *notify_discord_rules + - !reference [".notify_discord", "rules"] From 0f2e56a203cdefea5fbec3609edd923c5e120330 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sat, 20 Jul 2024 12:00:52 +0200 Subject: [PATCH 11/89] [sunnypilot] Forcing SConstruct to assume always sunnypilot developer true until we actually encrypt things --- SConstruct | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SConstruct b/SConstruct index 13da37c7a8..50e60c3b95 100644 --- a/SConstruct +++ b/SConstruct @@ -116,7 +116,7 @@ AddOption('--minimal', AddOption('--sunnypilot', action='store_true', dest='sunnypilot', - default=is_internal_developer(_DEBUG), # check if the current user is a sunnypilot developer + default=is_internal_developer(_DEBUG) or True, # check if the current user is a sunnypilot developer. TODO: Remove the 'or True' part once the GPG keys are available. help='build sunnypilot elements and other sunnypilot-specific items that are meant for internal development') ## Architecture name breakdown (arch) From 8b6e4039963c3fb62de34989eb40ee6f54d439f4 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 20 Jul 2024 13:06:34 +0000 Subject: [PATCH 12/89] Driving Model Selector v5: Bug fixes --- selfdrive/modeld/custom_model_metadata.py | 4 ++-- selfdrive/modeld/fill_model_msg.py | 6 ++++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/selfdrive/modeld/custom_model_metadata.py b/selfdrive/modeld/custom_model_metadata.py index 360f7046ad..0469bd189c 100644 --- a/selfdrive/modeld/custom_model_metadata.py +++ b/selfdrive/modeld/custom_model_metadata.py @@ -42,14 +42,14 @@ class CustomModelMetadata: self.params: Params = params self.generation: ModelGeneration = self.read_model_generation_param() - self.capabilities: int = self.get_model_capabilities() + self.capabilities: ModelCapabilities = self.get_model_capabilities() self.valid: bool = self.params.get_bool("CustomDrivingModel") and not SIMULATION and \ self.capabilities != ModelCapabilities.Default def read_model_generation_param(self) -> ModelGeneration: return int(self.params.get('DrivingModelGeneration') or ModelGeneration.default) - def get_model_capabilities(self) -> int: + def get_model_capabilities(self) -> ModelCapabilities: """Returns the model capabilities for a given generation.""" if self.generation == ModelGeneration.five: return ModelCapabilities.DesiredCurvatureV2 diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 39a161ed89..8d2bcc0ef4 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -68,7 +68,9 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D driving_model_data.frameDropPerc = frame_drop_perc action = driving_model_data.action - action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) + model_use_lateral_planner = custom_model_valid and custom_model_capabilities & ModelCapabilities.LateralPlannerSolution + if not model_use_lateral_planner: + action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) modelV2 = extended_msg.modelV2 modelV2.frameId = vipc_frame_id @@ -100,7 +102,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D fill_xyz_poly(poly_path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T) # lateral planning - if custom_model_valid and custom_model_capabilities & ModelCapabilities.LateralPlannerSolution: + if model_use_lateral_planner: solution = modelV2.lateralPlannerSolutionDEPRECATED solution.x, solution.y, solution.yaw, solution.yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)] solution.xStd, solution.yStd, solution.yawStd, solution.yawRateStd = [net_output_data['lat_planner_solution_stds'][0,:,i].tolist() for i in range(4)] From d923f2fa5989cc78389e406159a092a82682f0a6 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 21 Jul 2024 16:54:19 -0400 Subject: [PATCH 13/89] only in stopping state at 0.5 (same as stock), and track accel_last --- 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 74b9bf1811..686d9ec7db 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -299,6 +299,7 @@ class CarController(CarControllerBase): can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, 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)) + self.accel_last = accel # 20 Hz LFA MFA message if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value: @@ -473,10 +474,10 @@ 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 - def cal_jerk(self, accel, actuators): + def cal_jerk(self, CS, accel, actuators): if actuators.longControlState == LongCtrlState.off: accel_diff = 0.0 - elif actuators.longControlState == LongCtrlState.stopping:# or hud_control.softHold > 0: + elif actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < self.CP.vEgoStopping:# or hud_control.softHold > 0: accel_diff = 0.0 else: accel_diff = accel - self.accel_last @@ -486,7 +487,7 @@ class CarController(CarControllerBase): return self.jerk def make_jerk(self, CS, accel, actuators): - jerk = self.cal_jerk(accel, actuators) + jerk = self.cal_jerk(CS, accel, actuators) a_error = accel - CS.out.aEgo jerk = jerk + (a_error * 2.0) @@ -499,7 +500,7 @@ class CarController(CarControllerBase): self.jerk_u = jerkLimit self.jerk_l = jerkLimit self.jerk_count = 0 - elif actuators.longControlState == LongCtrlState.stopping: + elif actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < self.CP.vEgoStopping: 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 @@ -518,7 +519,7 @@ class CarController(CarControllerBase): self.jerk_u = jerkLimit self.jerk_l = jerkLimit self.jerk_count = 0 - elif actuators.longControlState == LongCtrlState.stopping: + elif actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < self.CP.vEgoStopping: 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 From 5f2c104ae5ce38894664c45f867ad630935d9b1e Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 21 Jul 2024 17:40:06 -0400 Subject: [PATCH 14/89] try this out (pls don't die) --- selfdrive/car/hyundai/carcontroller.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 686d9ec7db..c9cf066f2f 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -110,6 +110,9 @@ class CarController(CarControllerBase): self.cb_lower = 0.0 self.jerk_count = 0.0 + self.accel_val = 0 + self.accel_raw = 0 + def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float: lead_one = self.sm["radarState"].leadOne lead_two = self.sm["radarState"].leadTwo @@ -284,6 +287,7 @@ class CarController(CarControllerBase): can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25) else: self.make_jerk(CS, accel, actuators) + self.make_accel(CS, accel, stopping) # Parse lead distance from radarState and display the corresponding distance in the car's cluster if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0: @@ -296,10 +300,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, self.jerk_l, self.jerk_u, int(self.frame / 2), + stopping = stopping and CS.out.vEgo < self.CP.vEgoStopping + 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)) - self.accel_last = accel # 20 Hz LFA MFA message if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value: @@ -529,3 +533,10 @@ class CarController(CarControllerBase): #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, CS, accel, stopping): + self.accel_raw = accel + if stopping and CS.out.vEgo < self.CP.vEgoStopping: + self.accel_raw = 0 + self.accel_val = clip(self.accel_raw, self.accel_last - self.jerk_l, self.accel_last + self.jerk_l) + self.accel_last = self.accel_val From b32b216a6b411fa8ee224df2e398a6a166b33097 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 21 Jul 2024 17:41:16 -0400 Subject: [PATCH 15/89] try this out (pls don't die) --- selfdrive/car/hyundai/hyundaican.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index f3776c46ef..58b05d708a 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -130,7 +130,7 @@ 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, lower_jerk, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, +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 = [] @@ -150,8 +150,8 @@ def create_acc_commands(packer, enabled, accel, lower_jerk, upper_jerk, idx, hud 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, } From c5af23a01067e378c603d89b4907db4227387ad4 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 21 Jul 2024 17:44:55 -0400 Subject: [PATCH 16/89] clear values if not engaged --- selfdrive/car/hyundai/carcontroller.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index c9cf066f2f..eaadfa1800 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -287,7 +287,7 @@ class CarController(CarControllerBase): can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25) else: self.make_jerk(CS, accel, actuators) - self.make_accel(CS, accel, stopping) + self.make_accel(CS, accel, actuators) # Parse lead distance from radarState and display the corresponding distance in the car's cluster if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0: @@ -534,9 +534,12 @@ class CarController(CarControllerBase): 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, CS, accel, stopping): + def make_accel(self, CS, accel, actuators): self.accel_raw = accel - if stopping and CS.out.vEgo < self.CP.vEgoStopping: - self.accel_raw = 0 - self.accel_val = clip(self.accel_raw, self.accel_last - self.jerk_l, self.accel_last + self.jerk_l) + if actuators.longControlState == LongCtrlState.off: + self.accel_raw, self.accel_val = 0, 0 + else: + if actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < self.CP.vEgoStopping: + self.accel_raw = 0 + self.accel_val = clip(self.accel_raw, self.accel_last - self.jerk_l, self.accel_last + self.jerk_l) self.accel_last = self.accel_val From 56cf00230717bda8e26878ad5c3222c1657030b5 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 21 Jul 2024 21:43:53 -0400 Subject: [PATCH 17/89] change in 0.1 like stock --- selfdrive/car/hyundai/carcontroller.py | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index eaadfa1800..f393e1935f 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -300,7 +300,6 @@ 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 - stopping = stopping and CS.out.vEgo < self.CP.vEgoStopping 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)) @@ -478,10 +477,10 @@ 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 - def cal_jerk(self, CS, accel, actuators): + def cal_jerk(self, accel, actuators): if actuators.longControlState == LongCtrlState.off: accel_diff = 0.0 - elif actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < self.CP.vEgoStopping:# or hud_control.softHold > 0: + elif actuators.longControlState == LongCtrlState.stopping:# or hud_control.softHold > 0: accel_diff = 0.0 else: accel_diff = accel - self.accel_last @@ -491,7 +490,7 @@ class CarController(CarControllerBase): return self.jerk def make_jerk(self, CS, accel, actuators): - jerk = self.cal_jerk(CS, accel, actuators) + jerk = self.cal_jerk(accel, actuators) a_error = accel - CS.out.aEgo jerk = jerk + (a_error * 2.0) @@ -504,7 +503,7 @@ class CarController(CarControllerBase): self.jerk_u = jerkLimit self.jerk_l = jerkLimit self.jerk_count = 0 - elif actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < self.CP.vEgoStopping: + 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 @@ -523,7 +522,7 @@ class CarController(CarControllerBase): self.jerk_u = jerkLimit self.jerk_l = jerkLimit self.jerk_count = 0 - elif actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < self.CP.vEgoStopping: + 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 @@ -534,12 +533,12 @@ class CarController(CarControllerBase): 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, CS, accel, actuators): + def make_accel(self, accel, actuators): self.accel_raw = accel if actuators.longControlState == LongCtrlState.off: self.accel_raw, self.accel_val = 0, 0 else: - if actuators.longControlState == LongCtrlState.stopping and CS.out.vEgo < self.CP.vEgoStopping: + if actuators.longControlState == LongCtrlState.stopping: self.accel_raw = 0 - self.accel_val = clip(self.accel_raw, self.accel_last - self.jerk_l, self.accel_last + self.jerk_l) + self.accel_val = clip(self.accel_raw, self.accel_last - 0.1, self.accel_last + 0.1) self.accel_last = self.accel_val From 807873e6a8cb237cad7cea8e1b091ef2ed536e69 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 22 Jul 2024 10:12:03 -0400 Subject: [PATCH 18/89] fix --- selfdrive/car/hyundai/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index f393e1935f..b43079a714 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -287,7 +287,7 @@ class CarController(CarControllerBase): can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25) else: self.make_jerk(CS, accel, actuators) - self.make_accel(CS, accel, actuators) + self.make_accel(accel, actuators) # Parse lead distance from radarState and display the corresponding distance in the car's cluster if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0: From d25d83288582337380d37b577c828e0b1ae2bb5f Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 22 Jul 2024 11:29:42 -0400 Subject: [PATCH 19/89] Should run these at 50 Hz --- selfdrive/car/hyundai/carcontroller.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index b43079a714..4d2a2902fa 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -287,7 +287,6 @@ class CarController(CarControllerBase): can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25) else: self.make_jerk(CS, accel, actuators) - self.make_accel(accel, actuators) # Parse lead distance from radarState and display the corresponding distance in the car's cluster if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0: @@ -300,6 +299,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 + 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), 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)) From 0c95fe140869d4646e66c414005a034f859aa9d7 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 22 Jul 2024 11:46:01 -0400 Subject: [PATCH 20/89] send 2.0 startAccel to both aReqRaw/aReqValue for 0.1 second (2 frames), then pid --- selfdrive/car/hyundai/carcontroller.py | 7 +++++++ selfdrive/car/hyundai/interface.py | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 4d2a2902fa..a051e13c1d 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -112,6 +112,7 @@ class CarController(CarControllerBase): 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 @@ -538,8 +539,14 @@ class CarController(CarControllerBase): 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 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_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 e801b47771..88827e61ab 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -104,7 +104,7 @@ class CarInterface(CarInterfaceBase): ret.stoppingControl = True ret.startingState = True ret.vEgoStarting = 0.1 - ret.startAccel = 1.0 + ret.startAccel = 2.0 ret.longitudinalActuatorDelay = 0.5 if DBC[ret.carFingerprint]["radar"] is None: From ab72cf0457f6898b6654e44a8cbdc40a82fc8a85 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 22 Jul 2024 16:14:45 -0400 Subject: [PATCH 21/89] handle this in long control --- selfdrive/car/hyundai/carcontroller.py | 17 +++-------------- selfdrive/car/hyundai/interface.py | 1 + selfdrive/controls/lib/longcontrol.py | 15 +++++++++++++-- 3 files changed, 17 insertions(+), 16 deletions(-) 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 From 5e3cf24afee785e7576e1fcaa3256da5962b613c Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 22 Jul 2024 16:25:51 -0400 Subject: [PATCH 22/89] update rate of change properly --- selfdrive/car/hyundai/carcontroller.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index ee700ffc9c..f41a10c841 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -534,8 +534,10 @@ class CarController(CarControllerBase): self.cb_lower = clip(0.8 + accel * 0.2, 0, 1.2) def make_accel(self, accel, actuators): + rate_up = 0.02 * self.jerk_u + rate_down = 0.02 * self.jerk_l if actuators.longControlState == LongCtrlState.off: self.accel_val = 0 else: - self.accel_val = clip(accel, self.accel_last - 0.1, self.accel_last + 0.1) + self.accel_val = clip(accel, self.accel_last - rate_down, self.accel_last + rate_up) self.accel_last = self.accel_val From 8191c951c7149cb55afcaccaba6b9f7d50aaf270 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 22 Jul 2024 16:33:25 -0400 Subject: [PATCH 23/89] oops it's doing it twice --- selfdrive/car/hyundai/carcontroller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index f41a10c841..16994e76ea 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -284,8 +284,8 @@ class CarController(CarControllerBase): self.last_button_frame = self.frame elif self.frame % 2 == 0: can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25) - else: - self.make_jerk(CS, accel, actuators) + #else: + # self.make_jerk(CS, accel, actuators) # Parse lead distance from radarState and display the corresponding distance in the car's cluster if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0: From e5b19ddc1762a01c48ac4fad0f013d62323fda89 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 22 Jul 2024 16:46:27 -0400 Subject: [PATCH 24/89] only different rate of change if not in pid --- selfdrive/car/hyundai/carcontroller.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 16994e76ea..8b252ad58e 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -534,8 +534,11 @@ class CarController(CarControllerBase): self.cb_lower = clip(0.8 + accel * 0.2, 0, 1.2) def make_accel(self, accel, actuators): - rate_up = 0.02 * self.jerk_u - rate_down = 0.02 * self.jerk_l + if actuators.longControlState == LongCtrlState.pid: + rate_up, rate_down = 0, 0 + else: + rate_up = 0.02 * self.jerk_u + rate_down = 0.02 * self.jerk_l if actuators.longControlState == LongCtrlState.off: self.accel_val = 0 else: From be300df85c858388cc6b5e8b7a33185142299b33 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 22 Jul 2024 16:53:37 -0400 Subject: [PATCH 25/89] should only go up to 3 lol --- selfdrive/car/hyundai/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 8b252ad58e..1a9f8458e5 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -514,7 +514,7 @@ class CarController(CarControllerBase): self.jerk_l = min(max(1.0, -jerk * 3.0), jerkLimit) else: startingJerk = self.jerkStartLimit - jerkLimit = 5.0 + jerkLimit = 3.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 From e64d209f54314f0307e64f27e3f5333d3e002a95 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 22 Jul 2024 16:54:21 -0400 Subject: [PATCH 26/89] set speed stuck --- selfdrive/car/hyundai/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 1a9f8458e5..1374b60e5a 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -535,7 +535,7 @@ class CarController(CarControllerBase): def make_accel(self, accel, actuators): if actuators.longControlState == LongCtrlState.pid: - rate_up, rate_down = 0, 0 + rate_up, rate_down = 0.1, 0.1 else: rate_up = 0.02 * self.jerk_u rate_down = 0.02 * self.jerk_l From 8affb2fcaddb0771a7060f6155457cfc5ee5e463 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 22 Jul 2024 16:56:48 -0400 Subject: [PATCH 27/89] make sure accel_last changes with raw if in pid --- selfdrive/car/hyundai/carcontroller.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 1374b60e5a..c51e385d2c 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -535,7 +535,8 @@ class CarController(CarControllerBase): def make_accel(self, accel, actuators): if actuators.longControlState == LongCtrlState.pid: - rate_up, rate_down = 0.1, 0.1 + rate_up, rate_down = 0.0, 0.0 + self.accel_last = accel else: rate_up = 0.02 * self.jerk_u rate_down = 0.02 * self.jerk_l From 0d499b0857c65b790989dff5fbb0d157130d02b0 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 22 Jul 2024 21:49:03 -0400 Subject: [PATCH 28/89] doesn't work well --- selfdrive/controls/lib/longcontrol.py | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index a477b48780..a661c33361 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -51,8 +51,6 @@ 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() @@ -76,22 +74,13 @@ class LongControl: self.reset() elif self.long_control_state == LongCtrlState.starting: - 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) + 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) - 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 From 58004f4fc65f6999f201a4d2c501e4c23bee072f Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 22 Jul 2024 21:50:31 -0400 Subject: [PATCH 29/89] fix pulsing --- selfdrive/car/hyundai/carcontroller.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index c51e385d2c..53aa9b6111 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -284,8 +284,8 @@ class CarController(CarControllerBase): self.last_button_frame = self.frame elif self.frame % 2 == 0: can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25) - #else: - # self.make_jerk(CS, accel, actuators) + else: + self.make_jerk(CS, accel, actuators) # Parse lead distance from radarState and display the corresponding distance in the car's cluster if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0: @@ -298,7 +298,6 @@ 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 - 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, accel, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2), hud_control, set_speed_in_units, stopping, From f64193924044f741e5779bc47b10c2a5b8206d0f Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 22 Jul 2024 23:19:58 -0400 Subject: [PATCH 30/89] trigger stop earlier --- selfdrive/car/hyundai/interface.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 627328c0d9..87408a738b 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -103,6 +103,7 @@ class CarInterface(CarInterfaceBase): ret.stoppingControl = True ret.startingState = True + ret.vEgoStopping = 1.0 ret.vEgoStarting = 0.1 ret.startAccel = 2.0 ret.stopAccel = 0.0 From 7f06773c26f1a072efad53a061ffb798510d0ae4 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 22 Jul 2024 23:22:33 -0400 Subject: [PATCH 31/89] higher jerk limits --- selfdrive/car/hyundai/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 53aa9b6111..028bc796d6 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -513,7 +513,7 @@ class CarController(CarControllerBase): self.jerk_l = min(max(1.0, -jerk * 3.0), jerkLimit) else: startingJerk = self.jerkStartLimit - jerkLimit = 3.0 + 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 From 7560d8db9e44938380b07c428c515d8a3dfd3f10 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 11:11:00 -0400 Subject: [PATCH 32/89] 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 From 8ec69008ce2158536406cb78007e32e165881d14 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 11:11:12 -0400 Subject: [PATCH 33/89] lower starAccel --- selfdrive/car/hyundai/interface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 87408a738b..9e618c3f6e 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -105,7 +105,7 @@ class CarInterface(CarInterfaceBase): ret.startingState = True ret.vEgoStopping = 1.0 ret.vEgoStarting = 0.1 - ret.startAccel = 2.0 + ret.startAccel = 1.0 ret.stopAccel = 0.0 ret.longitudinalActuatorDelay = 0.5 From c02b3fca53b315ff88789db62862e98b538ce4a7 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 11:32:42 -0400 Subject: [PATCH 34/89] fast resume: disengage acc enabled for 1 frame, then re-engage --- selfdrive/car/hyundai/carcontroller.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index a345c53762..52c1544ab1 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -113,6 +113,9 @@ class CarController(CarControllerBase): self.accel_raw = 0 self.accel_val = 0 + self.acc_enabled = False + self.prev_stop_req = False + def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float: lead_one = self.sm["radarState"].leadOne lead_two = self.sm["radarState"].leadTwo @@ -300,7 +303,8 @@ 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, self.accel_raw, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2), + self.fast_resume(actuators, CC.enabled and CS.out.cruiseState.enabled, stopping) + can_sends.extend(hyundaican.create_acc_commands(self.packer, self.acc_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)) @@ -545,3 +549,11 @@ class CarController(CarControllerBase): else: self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up) self.accel_last = self.accel_val + + def fast_resume(self, actuators, enabled, stopping): + if actuators.longControlState == LongCtrlState.starting and self.acc_enabled and self.prev_stop_req: + self.acc_enabled = False + else: + self.acc_enabled = enabled + + self.prev_stop_req = stopping From b20ce279086b0404fce1d821ce1af35e4976bd09 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 11:40:40 -0400 Subject: [PATCH 35/89] matches stock with 0 m/s^2 at stopping --- selfdrive/car/hyundai/carcontroller.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 52c1544ab1..f3e7fa4c3f 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -538,15 +538,18 @@ class CarController(CarControllerBase): self.cb_lower = clip(0.8 + accel * 0.2, 0, 1.2) def make_accel(self, accel, actuators): + long_control = actuators.longControlState self.accel_raw = accel - if actuators.longControlState == LongCtrlState.pid: + if long_control == LongCtrlState.pid: 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: + if long_control == LongCtrlState.off: self.accel_raw, self.accel_val = 0, 0 else: + if long_control == LongCtrlState.stopping: + self.accel_raw = 0 self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up) self.accel_last = self.accel_val From 68149b71c494c15a341d81810c9fbd4c414e9f43 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 11:41:35 -0400 Subject: [PATCH 36/89] gracefully when stopping --- selfdrive/car/hyundai/carcontroller.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index f3e7fa4c3f..56c9bae40e 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -540,11 +540,11 @@ class CarController(CarControllerBase): def make_accel(self, accel, actuators): long_control = actuators.longControlState self.accel_raw = accel - if long_control == LongCtrlState.pid: - rate_up, rate_down = 0.1, 0.1 - else: + if long_control == LongCtrlState.stopping: rate_up = 0.02 * self.jerk_u rate_down = 0.02 * self.jerk_l + else: + rate_up, rate_down = 0.1, 0.1 if long_control == LongCtrlState.off: self.accel_raw, self.accel_val = 0, 0 else: From 63754678d137a6f65aeb5400155b9bdeac6053b3 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 17:01:42 -0400 Subject: [PATCH 37/89] 0.1 rate of change all around --- selfdrive/car/hyundai/carcontroller.py | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 56c9bae40e..0be27a3523 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -540,11 +540,7 @@ class CarController(CarControllerBase): def make_accel(self, accel, actuators): long_control = actuators.longControlState self.accel_raw = accel - if long_control == LongCtrlState.stopping: - rate_up = 0.02 * self.jerk_u - rate_down = 0.02 * self.jerk_l - else: - rate_up, rate_down = 0.1, 0.1 + rate_up, rate_down = 0.1, 0.1 if long_control == LongCtrlState.off: self.accel_raw, self.accel_val = 0, 0 else: From 2507ec7abf6a03b1b2154200f29f0b47e3bc6658 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 17:12:18 -0400 Subject: [PATCH 38/89] same vEgoStopping, higher startAccel --- selfdrive/car/hyundai/interface.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 9e618c3f6e..8b800be0e5 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -103,9 +103,9 @@ class CarInterface(CarInterfaceBase): ret.stoppingControl = True ret.startingState = True - ret.vEgoStopping = 1.0 + ret.vEgoStopping = 0.5 ret.vEgoStarting = 0.1 - ret.startAccel = 1.0 + ret.startAccel = 1.8 ret.stopAccel = 0.0 ret.longitudinalActuatorDelay = 0.5 From 63a61783607d33f75d2999e5e2885c5cba3a848c Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 17:14:09 -0400 Subject: [PATCH 39/89] faster stops --- selfdrive/car/hyundai/interface.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 8b800be0e5..ca96df01b5 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -107,6 +107,7 @@ class CarInterface(CarInterfaceBase): ret.vEgoStarting = 0.1 ret.startAccel = 1.8 ret.stopAccel = 0.0 + ret.stoppingDecelRate = 2.0 # reach standstill quickly after entering stopping state ret.longitudinalActuatorDelay = 0.5 if DBC[ret.carFingerprint]["radar"] is None: From 6ab66834f02b9314095e130f576204aadea1e166 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 17:54:47 -0400 Subject: [PATCH 40/89] adapt to 0 stopAccel --- selfdrive/controls/lib/longcontrol.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index a661c33361..12aac4487c 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -68,9 +68,12 @@ class LongControl: elif self.long_control_state == LongCtrlState.stopping: output_accel = self.last_output_accel - if output_accel > self.CP.stopAccel: - output_accel = min(output_accel, 0.0) - output_accel -= self.CP.stoppingDecelRate * DT_CTRL + if output_accel < self.CP.stopAccel: + output_accel = max(output_accel, 0.0) + output_accel += self.CP.stoppingDecelRate * DT_CTRL + #if 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: From 067bd2b8fd0734f1f79fa55045f4eadf11ff5273 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 17:55:49 -0400 Subject: [PATCH 41/89] less if/else --- selfdrive/car/hyundai/carcontroller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 0be27a3523..4d70930f21 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -550,9 +550,9 @@ class CarController(CarControllerBase): self.accel_last = self.accel_val def fast_resume(self, actuators, enabled, stopping): + self.acc_enabled = enabled + if actuators.longControlState == LongCtrlState.starting and self.acc_enabled and self.prev_stop_req: self.acc_enabled = False - else: - self.acc_enabled = enabled self.prev_stop_req = stopping From b95933d6527163667db9c54ad0c90a0a9fa0519a Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 17:56:22 -0400 Subject: [PATCH 42/89] disable fast stop attempt for now --- selfdrive/car/hyundai/carcontroller.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 4d70930f21..4ff26ebdf7 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -551,6 +551,7 @@ class CarController(CarControllerBase): def fast_resume(self, actuators, enabled, stopping): self.acc_enabled = enabled + return if actuators.longControlState == LongCtrlState.starting and self.acc_enabled and self.prev_stop_req: self.acc_enabled = False From 63b86a80a6f1e631f11471727c72ab694dad92fe Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 18:18:35 -0400 Subject: [PATCH 43/89] handle stopaccel in long control --- selfdrive/car/hyundai/carcontroller.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 4ff26ebdf7..5565c1c4b2 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -544,8 +544,6 @@ class CarController(CarControllerBase): if long_control == LongCtrlState.off: self.accel_raw, self.accel_val = 0, 0 else: - if long_control == LongCtrlState.stopping: - self.accel_raw = 0 self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up) self.accel_last = self.accel_val From fd48800940f1279b7c23a30dfa20a05c6cd1b374 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 18:31:33 -0400 Subject: [PATCH 44/89] try disengaging for 250 ms --- selfdrive/car/hyundai/carcontroller.py | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 5565c1c4b2..7810e5e895 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -114,6 +114,8 @@ class CarController(CarControllerBase): self.accel_val = 0 self.acc_enabled = False + self.accel_start = False + self.accel_frame = 0 self.prev_stop_req = False def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float: @@ -290,6 +292,7 @@ class CarController(CarControllerBase): can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25) else: self.make_jerk(CS, accel, actuators) + self.fast_resume(actuators, CC.enabled and CS.out.cruiseState.enabled, stopping) # Parse lead distance from radarState and display the corresponding distance in the car's cluster if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0: @@ -303,7 +306,6 @@ 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) - self.fast_resume(actuators, CC.enabled and CS.out.cruiseState.enabled, stopping) can_sends.extend(hyundaican.create_acc_commands(self.packer, self.acc_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)) @@ -549,9 +551,19 @@ class CarController(CarControllerBase): def fast_resume(self, actuators, enabled, stopping): self.acc_enabled = enabled - return - if actuators.longControlState == LongCtrlState.starting and self.acc_enabled and self.prev_stop_req: + if actuators.longControlState == LongCtrlState.off: + return + + if actuators.longControlState == LongCtrlState.starting and self.prev_stop_req: + self.accel_frame = 0 + self.accel_start = True + + if self.accel_start and self.accel_frame < 25: + self.accel_frame += 1 self.acc_enabled = False + else: + self.accel_frame = 0 + self.accel_start = False self.prev_stop_req = stopping From 80c1aa0f0c1f59c95284bdc414d9a5ee4ae2b467 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 19:04:39 -0400 Subject: [PATCH 45/89] only do rate of change in pid mode --- selfdrive/car/hyundai/carcontroller.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 7810e5e895..34cd3c31e9 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -545,8 +545,10 @@ class CarController(CarControllerBase): rate_up, rate_down = 0.1, 0.1 if long_control == LongCtrlState.off: self.accel_raw, self.accel_val = 0, 0 - else: + elif long_control == LongCtrlState.pid: self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up) + else: + self.accel_val = accel self.accel_last = self.accel_val def fast_resume(self, actuators, enabled, stopping): From 85f726298cd13608010c85dffa90e6b947484f95 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 21:49:16 -0400 Subject: [PATCH 46/89] simplify --- selfdrive/car/hyundai/carcontroller.py | 31 ++------------------------ selfdrive/car/hyundai/interface.py | 2 -- 2 files changed, 2 insertions(+), 31 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 34cd3c31e9..9f3734bba1 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -113,11 +113,6 @@ class CarController(CarControllerBase): self.accel_raw = 0 self.accel_val = 0 - self.acc_enabled = False - self.accel_start = False - self.accel_frame = 0 - self.prev_stop_req = False - def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float: lead_one = self.sm["radarState"].leadOne lead_two = self.sm["radarState"].leadTwo @@ -292,7 +287,6 @@ class CarController(CarControllerBase): can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25) else: self.make_jerk(CS, accel, actuators) - self.fast_resume(actuators, CC.enabled and CS.out.cruiseState.enabled, stopping) # Parse lead distance from radarState and display the corresponding distance in the car's cluster if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0: @@ -306,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, self.acc_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, 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)) @@ -545,27 +539,6 @@ class CarController(CarControllerBase): rate_up, rate_down = 0.1, 0.1 if long_control == LongCtrlState.off: self.accel_raw, self.accel_val = 0, 0 - elif long_control == LongCtrlState.pid: + else: self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up) - else: - self.accel_val = accel self.accel_last = self.accel_val - - def fast_resume(self, actuators, enabled, stopping): - self.acc_enabled = enabled - - if actuators.longControlState == LongCtrlState.off: - return - - if actuators.longControlState == LongCtrlState.starting and self.prev_stop_req: - self.accel_frame = 0 - self.accel_start = True - - if self.accel_start and self.accel_frame < 25: - self.accel_frame += 1 - self.acc_enabled = False - else: - self.accel_frame = 0 - self.accel_start = False - - self.prev_stop_req = stopping diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index ca96df01b5..33b6da95fd 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -103,10 +103,8 @@ class CarInterface(CarInterfaceBase): ret.stoppingControl = True ret.startingState = True - ret.vEgoStopping = 0.5 ret.vEgoStarting = 0.1 ret.startAccel = 1.8 - ret.stopAccel = 0.0 ret.stoppingDecelRate = 2.0 # reach standstill quickly after entering stopping state ret.longitudinalActuatorDelay = 0.5 From 597dadd45b6b2514b971161eaa10c51c7f54250a Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 21:50:50 -0400 Subject: [PATCH 47/89] revert --- selfdrive/controls/lib/longcontrol.py | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 12aac4487c..a661c33361 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -68,12 +68,9 @@ class LongControl: elif self.long_control_state == LongCtrlState.stopping: output_accel = self.last_output_accel - if output_accel < self.CP.stopAccel: - output_accel = max(output_accel, 0.0) - output_accel += self.CP.stoppingDecelRate * DT_CTRL - #if output_accel > self.CP.stopAccel: - # output_accel = min(output_accel, 0.0) - # output_accel -= self.CP.stoppingDecelRate * DT_CTRL + if 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: From a3e60f92cf1bb966a5757d25177f36c5e7e26ef7 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 21:53:35 -0400 Subject: [PATCH 48/89] only send 0 when fully stopped --- selfdrive/car/hyundai/carcontroller.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 9f3734bba1..120be3d1ef 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -299,7 +299,7 @@ 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 - self.make_accel(accel, actuators) + self.make_accel(CS, 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, self.cb_lower, self.cb_upper)) @@ -533,12 +533,14 @@ class CarController(CarControllerBase): 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, accel, actuators): + def make_accel(self, CS, accel, actuators): long_control = actuators.longControlState self.accel_raw = accel rate_up, rate_down = 0.1, 0.1 if long_control == LongCtrlState.off: self.accel_raw, self.accel_val = 0, 0 else: + if long_control == LongCtrlState.stopping and CS.out.standstill: + self.accel_raw = 0 self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up) self.accel_last = self.accel_val From 3f25d6f5308ec9cbe5b287b65f2929520302e872 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 23:28:49 -0400 Subject: [PATCH 49/89] disable starting state --- selfdrive/car/hyundai/interface.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 33b6da95fd..a80b3199b0 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -102,9 +102,9 @@ class CarInterface(CarInterfaceBase): ret.pcmCruise = not ret.openpilotLongitudinalControl ret.stoppingControl = True - ret.startingState = True + ret.startingState = False ret.vEgoStarting = 0.1 - ret.startAccel = 1.8 + ret.startAccel = 1.0 ret.stoppingDecelRate = 2.0 # reach standstill quickly after entering stopping state ret.longitudinalActuatorDelay = 0.5 From c288a7792ce8ddfa47ac98fa4ec3f6708606fea3 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 23 Jul 2024 23:30:01 -0400 Subject: [PATCH 50/89] super fast take off!!! --- .../lib/longitudinal_mpc_lib/long_mpc.py | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index a2df527a55..0762e204ee 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -105,6 +105,17 @@ def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP. def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) +def get_stopped_equivalence_factor_krkeegen(v_lead, v_ego): + # KRKeegan this offset rapidly decreases the following distance when the lead pulls + # away, resulting in an early demand for acceleration. + v_diff_offset = 0 + if np.all(v_lead - v_ego > 0): + v_diff_offset = ((v_lead - v_ego) * 1.) + v_diff_offset = np.clip(v_diff_offset, 0, STOP_DISTANCE / 2) + v_diff_offset = np.maximum(v_diff_offset * ((10 - v_ego)/10), 0) + distance = (v_lead**2) / (2 * COMFORT_BRAKE) + v_diff_offset + return distance + def get_safe_obstacle_distance(v_ego, t_follow): return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE @@ -302,11 +313,11 @@ class LongitudinalMpc: def set_weights(self, prev_accel_constraint=True, personality=custom.LongitudinalPersonalitySP.standard): jerk_factor = get_jerk_factor(personality) if self.mode == 'acc': - a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 + a_change_cost = A_CHANGE_COST if prev_accel_constraint else 40 cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] elif self.mode == 'blended': - a_change_cost = 40.0 if prev_accel_constraint else 0 + a_change_cost = 40.0 if prev_accel_constraint else 40 cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0] else: @@ -369,8 +380,8 @@ class LongitudinalMpc: # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance # and then treat that as a stopped car/obstacle at this new distance. - lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) - lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) + lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor_krkeegen(lead_xv_0[:,1], v_ego) + lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor_krkeegen(lead_xv_1[:,1], v_ego) cruise_target_e2ex = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0] e2e_xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1]) From 3683e9eb0aa6c338b71918806c7639fcba174a3f Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Wed, 24 Jul 2024 10:14:13 +0200 Subject: [PATCH 51/89] git-crypt blacklist .git-crypt folder on the release_files.py --- release/release_files.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/release/release_files.py b/release/release_files.py index 31d1518d74..4ae0aad1ed 100755 --- a/release/release_files.py +++ b/release/release_files.py @@ -88,6 +88,8 @@ sunnypilot_blacklist = [ "codecov.yml", "conftest.py", "poetry.lock", + ".git-crypt/", + ".venv", ] # Merge the blacklists From 598e9b48db51c3d6cafe0a85059644917e8fbcf6 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Wed, 24 Jul 2024 16:46:21 +0000 Subject: [PATCH 52/89] New branch from github --- .gitlab-ci.yml | 5 +++-- newfile.sh | 1 + 2 files changed, 4 insertions(+), 2 deletions(-) create mode 100644 newfile.sh diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index aceeff95bd..55aa3eb25f 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -10,6 +10,7 @@ variables: GIT_CONFIG_USER_NAME: "Gitlab Pipeline" PUBLIC_REPO_URL: "https://github.com/sunnyhaibin/sunnypilot" +#test stages: - build - sanity @@ -98,7 +99,7 @@ build: - sed -i '/from .board.jungle import PandaJungle, PandaJungleDFU/s/^/#/' panda/__init__.py # comment panda jungle when prebuilt - scons -j$(nproc) cache_dir=${CI_DIR}/scons_cache --minimal - touch ${BUILD_DIR}/prebuilt - - sudo rm -rf ${OUTPUT_DIR} + - sudo rm -rf ${OUTPUT_DIR} - mkdir -p ${OUTPUT_DIR} # We first include the paths we want to keep, even if we later will be excluding the other things on those paths - rsync -avm @@ -141,7 +142,7 @@ build: --chown=comma:comma ${BUILD_DIR}/ ${OUTPUT_DIR}/ after_script: - # cleanup build dir after doing work + # cleanup build dir after doing work - find $BUILD_DIR/ -mindepth 1 -delete artifacts: paths: diff --git a/newfile.sh b/newfile.sh new file mode 100644 index 0000000000..2a34cca4fc --- /dev/null +++ b/newfile.sh @@ -0,0 +1 @@ +# this is a new file \ No newline at end of file From ff5e9840bcebfd1bb5b314df7a6751bb7cdabe3f Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Wed, 24 Jul 2024 18:52:40 +0200 Subject: [PATCH 53/89] cleaning up --- .gitlab-ci.yml | 1 - newfile.sh | 1 - 2 files changed, 2 deletions(-) delete mode 100644 newfile.sh diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 55aa3eb25f..1eb5fab2ed 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -10,7 +10,6 @@ variables: GIT_CONFIG_USER_NAME: "Gitlab Pipeline" PUBLIC_REPO_URL: "https://github.com/sunnyhaibin/sunnypilot" -#test stages: - build - sanity diff --git a/newfile.sh b/newfile.sh deleted file mode 100644 index 2a34cca4fc..0000000000 --- a/newfile.sh +++ /dev/null @@ -1 +0,0 @@ -# this is a new file \ No newline at end of file From 1ec52f17073a731bbf9511b6fe8b4848b4cb2a10 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Wed, 24 Jul 2024 18:58:36 +0200 Subject: [PATCH 54/89] makin a nice commit from github --- .gitlab-ci.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 1eb5fab2ed..5075a31a0f 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -10,6 +10,8 @@ variables: GIT_CONFIG_USER_NAME: "Gitlab Pipeline" PUBLIC_REPO_URL: "https://github.com/sunnyhaibin/sunnypilot" +# test from github + stages: - build - sanity From b8a2efcb525436336b131b554052c5321cdc8fc4 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Wed, 24 Jul 2024 17:07:07 +0000 Subject: [PATCH 55/89] delting comment --- .gitlab-ci.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 5075a31a0f..da0a0d4bc9 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -10,7 +10,6 @@ variables: GIT_CONFIG_USER_NAME: "Gitlab Pipeline" PUBLIC_REPO_URL: "https://github.com/sunnyhaibin/sunnypilot" -# test from github stages: - build From 2cc53b6e6972c6a86aef32bdc6004f4179538713 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 24 Jul 2024 16:54:27 -0400 Subject: [PATCH 56/89] Revert "disable starting state" This reverts commit 3f25d6f5308ec9cbe5b287b65f2929520302e872. --- selfdrive/car/hyundai/interface.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index a80b3199b0..33b6da95fd 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -102,9 +102,9 @@ class CarInterface(CarInterfaceBase): ret.pcmCruise = not ret.openpilotLongitudinalControl ret.stoppingControl = True - ret.startingState = False + ret.startingState = True ret.vEgoStarting = 0.1 - ret.startAccel = 1.0 + ret.startAccel = 1.8 ret.stoppingDecelRate = 2.0 # reach standstill quickly after entering stopping state ret.longitudinalActuatorDelay = 0.5 From e0414b452833e5cc2e6a844e6e233b9caa3ec996 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 24 Jul 2024 16:57:42 -0400 Subject: [PATCH 57/89] 0.1 interval to stopping --- selfdrive/car/hyundai/interface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 33b6da95fd..cb722d875d 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -105,7 +105,7 @@ class CarInterface(CarInterfaceBase): ret.startingState = True ret.vEgoStarting = 0.1 ret.startAccel = 1.8 - ret.stoppingDecelRate = 2.0 # reach standstill quickly after entering stopping state + ret.stoppingDecelRate = 10.0 # reach standstill quickly after entering stopping state ret.longitudinalActuatorDelay = 0.5 if DBC[ret.carFingerprint]["radar"] is None: From 1b084d7eb9dff29f26420361731c0d455e9de42d Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 24 Jul 2024 21:22:40 -0400 Subject: [PATCH 58/89] revert long mpc --- .../lib/longitudinal_mpc_lib/long_mpc.py | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 0762e204ee..a2df527a55 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -105,17 +105,6 @@ def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP. def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) -def get_stopped_equivalence_factor_krkeegen(v_lead, v_ego): - # KRKeegan this offset rapidly decreases the following distance when the lead pulls - # away, resulting in an early demand for acceleration. - v_diff_offset = 0 - if np.all(v_lead - v_ego > 0): - v_diff_offset = ((v_lead - v_ego) * 1.) - v_diff_offset = np.clip(v_diff_offset, 0, STOP_DISTANCE / 2) - v_diff_offset = np.maximum(v_diff_offset * ((10 - v_ego)/10), 0) - distance = (v_lead**2) / (2 * COMFORT_BRAKE) + v_diff_offset - return distance - def get_safe_obstacle_distance(v_ego, t_follow): return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE @@ -313,11 +302,11 @@ class LongitudinalMpc: def set_weights(self, prev_accel_constraint=True, personality=custom.LongitudinalPersonalitySP.standard): jerk_factor = get_jerk_factor(personality) if self.mode == 'acc': - a_change_cost = A_CHANGE_COST if prev_accel_constraint else 40 + a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] elif self.mode == 'blended': - a_change_cost = 40.0 if prev_accel_constraint else 40 + a_change_cost = 40.0 if prev_accel_constraint else 0 cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0] else: @@ -380,8 +369,8 @@ class LongitudinalMpc: # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance # and then treat that as a stopped car/obstacle at this new distance. - lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor_krkeegen(lead_xv_0[:,1], v_ego) - lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor_krkeegen(lead_xv_1[:,1], v_ego) + lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) + lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) cruise_target_e2ex = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0] e2e_xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1]) From bf31b2ab33c51d9afa205851cfcfe4084adb00a0 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 24 Jul 2024 21:22:58 -0400 Subject: [PATCH 59/89] further stop distance --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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: From 97f0178d5e8c56834c9f9ddca63f9aaa97639a7e Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 25 Jul 2024 17:16:57 +0000 Subject: [PATCH 60/89] ui: Lead car chevron: Time to Lead Car --- CHANGELOGS.md | 5 +++++ .../ui/qt/offroad/sunnypilot/visuals_settings.cc | 4 ++-- selfdrive/ui/qt/onroad/annotated_camera.cc | 14 +++++++++++--- 3 files changed, 18 insertions(+), 5 deletions(-) diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 06a2961de7..2fc99ab8d0 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -40,6 +40,11 @@ sunnypilot - 0.9.8.0 (2024-xx-xx) * Auto Unlock by Shift to P: All doors are automatically unlocked when shifting the shift lever to P * FIXED: Driving Personality: * Maniac mode now correctly enforced when selected +* UI Updates + * Display Metrics Below Chevron + * NEW❗: Time to Lead Car + * Displays the time to reach the position previously occupied by the lead car + * NEW❗: Display Distance, Speed, and Time to Lead Car simultaneously * Kia Ceed Plug-in Hybrid Non-SCC 2022 support thanks to TerminatorNL! sunnypilot - 0.9.7.1 (2024-06-13) diff --git a/selfdrive/ui/qt/offroad/sunnypilot/visuals_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot/visuals_settings.cc index 46bd84b6d9..742725e8ab 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/visuals_settings.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot/visuals_settings.cc @@ -82,12 +82,12 @@ VisualsPanel::VisualsPanel(QWidget *parent) : ListWidget(parent) { dev_ui_settings->showDescription(); // Visuals: Display Metrics above Chevron - std::vector chevron_info_settings_texts{tr("Off"), tr("Distance"), tr("Speed"), tr("Distance\nSpeed")}; + std::vector chevron_info_settings_texts{tr("Off"), tr("Distance"), tr("Speed"), tr("Time"), tr("All")}; chevron_info_settings = new ButtonParamControl( "ChevronInfo", tr("Display Metrics Below Chevron"), tr("Display useful metrics below the chevron that tracks the lead car (only applicable to cars with openpilot longitudinal control)."), "../assets/offroad/icon_blank.png", chevron_info_settings_texts, - 340 + 300 ); chevron_info_settings->showDescription(); diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 009ff76138..bb19798e92 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -1525,20 +1525,28 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState painter.drawPolygon(chevron, std::size(chevron)); if (num == 0) { // Display metrics to the 0th lead car - int chevron_types = 2; + const int chevron_types = 3; + const int chevron_all = chevron_types + 1; // All metrics QStringList chevron_text[chevron_types]; int position; float val; - if (chevron_data == 1 || chevron_data == 3) { + if (chevron_data == 1 || chevron_data == chevron_all) { position = 0; val = std::max(0.0f, d_rel); chevron_text[position].append(QString::number(val,'f', 0) + " " + "m"); } - if (chevron_data == 2 || chevron_data == 3) { + if (chevron_data == 2 || chevron_data == chevron_all) { position = (chevron_data == 2) ? 0 : 1; val = std::max(0.0f, (v_rel + v_ego) * (is_metric ? static_cast(MS_TO_KPH) : static_cast(MS_TO_MPH))); chevron_text[position].append(QString::number(val,'f', 0) + " " + (is_metric ? "km/h" : "mph")); } + if (chevron_data == 3 || chevron_data == chevron_all) { + position = (chevron_data == 3) ? 0 : 2; + val = (d_rel > 0 && v_ego > 0) ? std::max(0.0f, d_rel / v_ego) : 0.0f; + + QString ttc_str = (val > 0 && val < 200) ? QString::number(val, 'f', 1) + "s" : "---"; + chevron_text[position].append(ttc_str); + } float str_w = 200; // Width of the text box, might need adjustment float str_h = 50; // Height of the text box, adjust as necessary From c7f2f159502f77906f2975d83f06646f8bae71ba Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 25 Jul 2024 17:23:33 +0000 Subject: [PATCH 61/89] MADS: Use modern button events parsing --- selfdrive/car/__init__.py | 26 ++++++++++++--- selfdrive/car/chrysler/carcontroller.py | 12 ++++--- selfdrive/car/chrysler/carstate.py | 21 ++++++------ selfdrive/car/chrysler/interface.py | 35 ++++++-------------- selfdrive/car/chrysler/values.py | 14 ++++---- selfdrive/car/ford/carstate.py | 24 +++++++------- selfdrive/car/ford/interface.py | 36 ++++++--------------- selfdrive/car/ford/values.py | 21 ++++++------ selfdrive/car/gm/interface.py | 26 ++++++--------- selfdrive/car/honda/interface.py | 11 ++++--- selfdrive/car/hyundai/interface.py | 23 +++++-------- selfdrive/car/interfaces.py | 8 ++++- selfdrive/car/mazda/carcontroller.py | 9 +++--- selfdrive/car/mazda/carstate.py | 21 ++++++------ selfdrive/car/mazda/interface.py | 38 +++++++--------------- selfdrive/car/mazda/values.py | 18 ++++++----- selfdrive/car/nissan/interface.py | 27 ++++------------ selfdrive/car/subaru/interface.py | 27 ++++------------ selfdrive/car/toyota/interface.py | 28 ++++------------ selfdrive/car/volkswagen/carstate.py | 26 ++------------- selfdrive/car/volkswagen/interface.py | 43 ++++++------------------- selfdrive/car/volkswagen/values.py | 10 ------ 22 files changed, 195 insertions(+), 309 deletions(-) diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 6aebd2bd90..207c90678d 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -43,10 +43,28 @@ def create_button_events(cur_btn: int, prev_btn: int, buttons_dict: dict[int, ca return events -def create_mads_event(mads_event_lock: bool) -> capnp.lib.capnp._DynamicStructBuilder: - be = car.CarState.ButtonEvent(pressed=mads_event_lock) - be.type = ButtonType.altButton1 - return be +class ButtonEvents: + def __init__(self) -> None: + self.is_mads: bool = False + + @staticmethod + def create_cancel_event(long_enabled: bool, prev_long_enabled: bool) -> list[capnp.lib.capnp._DynamicStructBuilder]: + events: list[capnp.lib.capnp._DynamicStructBuilder] = [] + + if not long_enabled and prev_long_enabled: + events.append(car.CarState.ButtonEvent(pressed=True, + type=ButtonType.cancel)) + return events + + def create_mads_event(self, mads_enabled: bool, prev_mads_enabled: bool) -> list[capnp.lib.capnp._DynamicStructBuilder]: + events: list[capnp.lib.capnp._DynamicStructBuilder] = [] + + mads_changed = prev_mads_enabled != mads_enabled + if (mads_changed and not self.is_mads) or (not mads_changed and self.is_mads): + events.append(car.CarState.ButtonEvent(pressed=mads_changed, type=ButtonType.altButton1)) + self.is_mads = not self.is_mads + + return events def gen_empty_fingerprint(): diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index f9277af62e..54bed40b51 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -1,3 +1,5 @@ +from cereal import car + import cereal.messaging as messaging from common.conversions import Conversions as CV from opendbc.can.packer import CANPacker @@ -9,7 +11,7 @@ from openpilot.selfdrive.car.chrysler.values import RAM_CARS, RAM_DT, CarControl from openpilot.selfdrive.car.interfaces import CarControllerBase, FORWARD_GEARS from openpilot.selfdrive.controls.lib.drive_helpers import FCA_V_CRUISE_MIN -BUTTONS_STATES = ["accelCruise", "decelCruise", "cancel", "resumeCruise"] +ButtonType = car.CarState.ButtonEvent.Type class CarController(CarControllerBase): @@ -104,7 +106,7 @@ class CarController(CarControllerBase): self.last_button_frame = CS.button_counter if ram_cars: - if CS.buttonStates["cancel"]: + if any(b.type == ButtonType.cancel for b in CS.out.buttonEvents): can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter, das_bus, self.CP, cancel=True)) else: can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter, das_bus, self.CP, @@ -189,8 +191,10 @@ class CarController(CarControllerBase): # multikyd methods, sunnyhaibin logic def get_cruise_buttons_status(self, CS): - if not CS.out.cruiseState.enabled or any(CS.buttonStates[button_state] for button_state in BUTTONS_STATES): - self.timer = 40 + if not CS.out.cruiseState.enabled: + for be in CS.out.buttonEvents: + if be.type in (ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.resumeCruise) and be.pressed: + self.timer = 40 elif self.timer: self.timer -= 1 else: diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index d29be9226e..a58681175e 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -3,7 +3,7 @@ from openpilot.common.conversions import Conversions as CV from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine from openpilot.selfdrive.car.interfaces import CarStateBase -from openpilot.selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD, RAM_CARS, BUTTON_STATES +from openpilot.selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD, RAM_CARS, BUTTONS class CarState(CarStateBase): @@ -29,8 +29,7 @@ class CarState(CarStateBase): self.lkas_heartbit = None self.lkas_disabled = False - self.buttonStates = BUTTON_STATES.copy() - self.buttonStatesPrev = BUTTON_STATES.copy() + self.button_states = {button.event_type: False for button in BUTTONS} def update(self, cp, cp_cam): @@ -41,7 +40,6 @@ class CarState(CarStateBase): self.prev_mads_enabled = self.mads_enabled self.prev_lkas_enabled = self.lkas_enabled - self.buttonStatesPrev = self.buttonStates.copy() # lock info ret.doorOpen = any([cp.vl["BCM_1"]["DOOR_OPEN_FL"], @@ -76,6 +74,16 @@ class CarState(CarStateBase): unit=1, ) + # Buttons + for button in BUTTONS: + state = (cp.vl[button.can_addr][button.can_msg] in button.values) + if self.button_states[button.event_type] != state: + event = car.CarState.ButtonEvent.new_message() + event.type = button.event_type + event.pressed = state + self.button_events.append(event) + self.button_states[button.event_type] = state + # button presses ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = self.update_blinker_from_stalk(200, cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1, cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2) @@ -116,11 +124,6 @@ class CarState(CarStateBase): ret.leftBlindspot = cp.vl["BSM_1"]["LEFT_STATUS"] == 1 ret.rightBlindspot = cp.vl["BSM_1"]["RIGHT_STATUS"] == 1 - self.buttonStates["accelCruise"] = bool(cp.vl["CRUISE_BUTTONS"]["ACC_Accel"]) - self.buttonStates["decelCruise"] = bool(cp.vl["CRUISE_BUTTONS"]["ACC_Decel"]) - self.buttonStates["cancel"] = bool(cp.vl["CRUISE_BUTTONS"]["ACC_Cancel"]) - self.buttonStates["resumeCruise"] = bool(cp.vl["CRUISE_BUTTONS"]["ACC_Resume"]) - self.lkas_car_model = cp_cam.vl["DAS_6"]["CAR_MODEL"] self.button_counter = cp.vl["CRUISE_BUTTONS"]["COUNTER"] self.cruise_buttons = cp.vl["CRUISE_BUTTONS"] diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index d09fa7ac56..d4911375e7 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 from cereal import car from panda import Panda -from openpilot.selfdrive.car import create_button_events, get_safety_config, create_mads_event -from openpilot.selfdrive.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags, ChryslerFlagsSP, BUTTON_STATES +from openpilot.selfdrive.car import create_button_events, get_safety_config +from openpilot.selfdrive.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags, ChryslerFlagsSP from openpilot.selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type @@ -13,7 +13,6 @@ GearShifter = car.CarState.GearShifter class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) - self.buttonStatesPrev = BUTTON_STATES.copy() @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): @@ -94,19 +93,12 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam) self.sp_update_params() - buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) - - for button in self.CS.buttonStates: - if self.CS.buttonStates[button] != self.buttonStatesPrev[button]: - be = car.CarState.ButtonEvent.new_message() - be.type = button - be.pressed = self.CS.buttonStates[button] - buttonEvents.append(be) + self.CS.button_events.extend(create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})) self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS) self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled, - buttonEvents, c.vCruise, + self.CS.button_events, c.vCruise, enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.resumeCruise) if not self.CP.pcmCruiseSpeed else (ButtonType.accelCruise, ButtonType.decelCruise), resume_button=(ButtonType.resumeCruise,) if not self.CP.pcmCruiseSpeed else @@ -125,7 +117,7 @@ class CarInterface(CarInterfaceBase): self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS) if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed: - if any(b.type == ButtonType.cancel for b in buttonEvents): + if any(b.type == ButtonType.cancel for b in self.CS.button_events): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) if self.get_sp_pedal_disengage(ret): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) @@ -143,17 +135,10 @@ class CarInterface(CarInterfaceBase): ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button)) - # MADS BUTTON - if self.CS.out.madsEnabled != self.CS.madsEnabled: - if self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = False - else: - if not self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = True - - ret.buttonEvents = buttonEvents + ret.buttonEvents = [ + *self.CS.button_events, + *self.button_events.create_mads_event(self.CS.madsEnabled, self.CS.out.madsEnabled) # MADS BUTTON + ] # events events = self.create_common_events(ret, c, extra_gears=[car.CarState.GearShifter.low], pcm_enable=False) @@ -180,6 +165,4 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() - self.buttonStatesPrev = self.CS.buttonStates.copy() - return ret diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 85835d453a..c681040023 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -1,3 +1,4 @@ +from collections import namedtuple from enum import IntFlag from dataclasses import dataclass, field @@ -8,6 +9,7 @@ from openpilot.selfdrive.car.docs_definitions import CarHarness, CarDocs, CarPar from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = car.CarParams.Ecu +Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) class ChryslerFlags(IntFlag): @@ -113,12 +115,12 @@ class CarControllerParams: self.STEER_MAX = 261 # higher than this faults the EPS -BUTTON_STATES = { - "accelCruise": False, - "decelCruise": False, - "cancel": False, - "resumeCruise": False, -} +BUTTONS = [ + Button(car.CarState.ButtonEvent.Type.accelCruise, "CRUISE_BUTTONS", "ACC_Accel", [1]), + Button(car.CarState.ButtonEvent.Type.decelCruise, "CRUISE_BUTTONS", "ACC_Decel", [1]), + Button(car.CarState.ButtonEvent.Type.cancel, "CRUISE_BUTTONS", "ACC_Cancel", [1]), + Button(car.CarState.ButtonEvent.Type.resumeCruise, "CRUISE_BUTTONS", "ACC_Resume", [1]), +] STEER_THRESHOLD = 120 diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 21e2f12def..e52454f81d 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -3,7 +3,7 @@ from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car.ford.fordcan import CanBus -from openpilot.selfdrive.car.ford.values import DBC, CarControllerParams, FordFlags, BUTTON_STATES +from openpilot.selfdrive.car.ford.values import DBC, CarControllerParams, FordFlags, BUTTONS from openpilot.selfdrive.car.interfaces import CarStateBase GearShifter = car.CarState.GearShifter @@ -24,15 +24,14 @@ class CarState(CarStateBase): self.lkas_enabled = None self.prev_lkas_enabled = None - self.buttonStates = BUTTON_STATES.copy() - self.buttonStatesPrev = BUTTON_STATES.copy() + + self.button_states = {button.event_type: False for button in BUTTONS} def update(self, cp, cp_cam): ret = car.CarState.new_message() self.prev_mads_enabled = self.mads_enabled self.prev_lkas_enabled = self.lkas_enabled - self.buttonStatesPrev = self.buttonStates.copy() # Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement # The vehicle usually recovers out of this state within a minute of normal driving @@ -87,6 +86,16 @@ class CarState(CarStateBase): else: ret.gearShifter = GearShifter.drive + # Buttons + for button in BUTTONS: + state = (cp.vl[button.can_addr][button.can_msg] in button.values) + if self.button_states[button.event_type] != state: + event = car.CarState.ButtonEvent.new_message() + event.type = button.event_type + event.pressed = state + self.button_events.append(event) + self.button_states[button.event_type] = state + # safety ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"]) ret.stockAeb = bool(cp_cam.vl["ACCDATA_2"]["CmbbBrkDecel_B_Rq"]) @@ -112,13 +121,6 @@ class CarState(CarStateBase): self.lkas_enabled = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"]) - self.buttonStates["accelCruise"] = bool(cp.vl["Steering_Data_FD1"]["CcAslButtnSetIncPress"]) - self.buttonStates["decelCruise"] = bool(cp.vl["Steering_Data_FD1"]["CcAslButtnSetDecPress"]) - self.buttonStates["cancel"] = bool(cp.vl["Steering_Data_FD1"]["CcAslButtnCnclPress"]) - self.buttonStates["setCruise"] = bool(cp.vl["Steering_Data_FD1"]["CcAslButtnSetPress"]) - self.buttonStates["resumeCruise"] = bool(cp.vl["Steering_Data_FD1"]["CcAsllButtnResPress"]) - self.buttonStates["gapAdjustCruise"] = bool(cp.vl["Steering_Data_FD1"]["AccButtnGapTogglePress"]) - # Stock steering buttons so that we can passthru blinkers etc. self.buttons_stock_values = cp.vl["Steering_Data_FD1"] # Stock values from IPMA so that we can retain some stock functionality diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 0c3449dc14..18dd7b4fa3 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -1,9 +1,9 @@ from cereal import car from panda import Panda from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car import create_button_events, get_safety_config, create_mads_event +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.ford.fordcan import CanBus -from openpilot.selfdrive.car.ford.values import Ecu, FordFlags, BUTTON_STATES +from openpilot.selfdrive.car.ford.values import Ecu, FordFlags from openpilot.selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type @@ -15,8 +15,6 @@ class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) - self.buttonStatesPrev = BUTTON_STATES.copy() - @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "ford" @@ -76,19 +74,12 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam) self.sp_update_params() - buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) - - for button in self.CS.buttonStates: - if self.CS.buttonStates[button] != self.buttonStatesPrev[button]: - be = car.CarState.ButtonEvent.new_message() - be.type = button - be.pressed = self.CS.buttonStates[button] - buttonEvents.append(be) + self.CS.button_events.extend(create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})) self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS) self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled, - buttonEvents, c.vCruise) + self.CS.button_events, c.vCruise) if ret.cruiseState.available: if self.enable_mads: @@ -101,7 +92,7 @@ class CarInterface(CarInterfaceBase): self.CS.madsEnabled = False if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0): - if any(b.type == ButtonType.cancel for b in buttonEvents): + if any(b.type == ButtonType.cancel for b in self.CS.button_events): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) if self.get_sp_pedal_disengage(ret): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) @@ -114,16 +105,10 @@ class CarInterface(CarInterfaceBase): ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button)) - if self.CS.out.madsEnabled != self.CS.madsEnabled: - if self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = False - else: - if not self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = True - - ret.buttonEvents = buttonEvents + ret.buttonEvents = [ + *self.CS.button_events, + *self.button_events.create_mads_event(self.CS.madsEnabled, self.CS.out.madsEnabled) # MADS BUTTON + ] events = self.create_common_events(ret, c, extra_gears=[GearShifter.manumatic], pcm_enable=False) @@ -134,7 +119,4 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() - # update previous car states - self.buttonStatesPrev = self.CS.buttonStates.copy() - return ret diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index edfee3ea1b..04bd592e22 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -1,5 +1,6 @@ import copy import re +from collections import namedtuple from dataclasses import dataclass, field, replace from enum import Enum, IntFlag @@ -11,6 +12,7 @@ from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, Ca from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, LiveFwVersions, OfflineFwVersions, Request, StdQueries, p16 Ecu = car.CarParams.Ecu +Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) class CarControllerParams: @@ -46,16 +48,6 @@ class FordFlags(IntFlag): CANFD = 1 -BUTTON_STATES = { - "accelCruise": False, - "decelCruise": False, - "cancel": False, - "setCruise": False, - "resumeCruise": False, - "gapAdjustCruise": False -} - - class RADAR: DELPHI_ESR = 'ford_fusion_2018_adas' DELPHI_MRR = 'FORD_CADS' @@ -154,6 +146,15 @@ class CAR(Platforms): ) +BUTTONS = [ + Button(car.CarState.ButtonEvent.Type.accelCruise, "Steering_Data_FD1", "CcAslButtnSetIncPress", [1]), + Button(car.CarState.ButtonEvent.Type.decelCruise, "Steering_Data_FD1", "CcAslButtnSetDecPress", [1]), + Button(car.CarState.ButtonEvent.Type.cancel, "Steering_Data_FD1", "CcAslButtnCnclPress", [1]), + Button(car.CarState.ButtonEvent.Type.setCruise, "Steering_Data_FD1", "CcAslButtnSetPress", [1]), + Button(car.CarState.ButtonEvent.Type.resumeCruise, "Steering_Data_FD1", "CcAsllButtnResPress", [1]), +] + + # FW response contains a combined software and part number # A-Z except no I, O or W # e.g. NZ6A-14C204-AAA diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 22a8d619a6..0e428bc13b 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -6,7 +6,7 @@ from panda import Panda from openpilot.common.basedir import BASEDIR from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car import create_button_events, get_safety_config, create_mads_event +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LatControlInputs, NanoFFModel @@ -206,12 +206,11 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback) self.sp_update_params() - buttonEvents = [] distance_button = 0 # Don't add event if transitioning from INIT, unless it's to an actual button if self.CS.cruise_buttons != CruiseButtons.UNPRESS or self.CS.prev_cruise_buttons != CruiseButtons.INIT: - buttonEvents = [ + self.CS.button_events = [ *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, unpressed_btn=CruiseButtons.UNPRESS), *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, @@ -222,11 +221,11 @@ class CarInterface(CarInterfaceBase): self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS) if not self.CP.pcmCruise: - if any(b.type == ButtonType.accelCruise and b.pressed for b in buttonEvents): + if any(b.type == ButtonType.accelCruise and b.pressed for b in self.CS.button_events): self.CS.accEnabled = True self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled, - buttonEvents, c.vCruise) + self.CS.button_events, c.vCruise) if ret.cruiseState.available: if self.enable_mads: @@ -239,7 +238,7 @@ class CarInterface(CarInterfaceBase): self.CS.madsEnabled = False if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0): - if any(b.type == ButtonType.cancel for b in buttonEvents): + if any(b.type == ButtonType.cancel for b in self.CS.button_events): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) if self.get_sp_pedal_disengage(ret): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) @@ -252,17 +251,10 @@ class CarInterface(CarInterfaceBase): ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(distance_button)) - # MADS BUTTON - if self.CS.out.madsEnabled != self.CS.madsEnabled: - if self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = False - else: - if not self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = True - - ret.buttonEvents = buttonEvents + ret.buttonEvents = [ + *self.CS.button_events, + *self.button_events.create_mads_event(self.CS.madsEnabled, self.CS.out.madsEnabled) # MADS BUTTON + ] # The ECM allows enabling on falling edge of set, but only rising edge of resume events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low, diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 457b095e6a..72ab68647f 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -268,7 +268,7 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) self.sp_update_params() - buttonEvents = [ + self.CS.button_events = [ *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT), *create_button_events(self.CS.cruise_setting, self.CS.prev_cruise_setting, SETTINGS_BUTTONS_DICT), ] @@ -276,7 +276,7 @@ class CarInterface(CarInterfaceBase): self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS) self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled, - buttonEvents, c.vCruise) + self.CS.button_events, c.vCruise) if ret.cruiseState.available: if self.enable_mads: @@ -289,7 +289,7 @@ class CarInterface(CarInterfaceBase): self.CS.madsEnabled = False if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed: - if any(b.type == ButtonType.cancel for b in buttonEvents): + if any(b.type == ButtonType.cancel for b in self.CS.button_events): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) if self.get_sp_pedal_disengage(ret): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) @@ -304,7 +304,10 @@ class CarInterface(CarInterfaceBase): min_enable_speed_pcm=(self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed), gap_button=(self.CS.cruise_setting == 3)) - ret.buttonEvents = buttonEvents + ret.buttonEvents = [ + *self.CS.button_events, + *self.button_events.create_mads_event(self.CS.madsEnabled, self.CS.out.madsEnabled) # MADS BUTTON + ] # events events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low], pcm_enable=False) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index e801b47771..dbe0b62895 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -7,7 +7,7 @@ from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, HyundaiFlagsSP, CANFD_UNSUPPORTED_LONGITUDINAL_CAR, NON_SCC_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, \ UNSUPPORTED_LONGITUDINAL_CAR, Buttons from openpilot.selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR -from openpilot.selfdrive.car import create_button_events, get_safety_config, create_mads_event +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.car.disable_ecu import disable_ecu @@ -205,10 +205,10 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam) self.sp_update_params() - buttonEvents = create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT) + self.CS.button_events = create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT) self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled, - buttonEvents, c.vCruise) + self.CS.button_events, c.vCruise) self.CS.mads_enabled = False if not self.mads_main_toggle else self.CS.mads_enabled @@ -231,7 +231,7 @@ class CarInterface(CarInterfaceBase): if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed: if not self.CP.pcmCruise: - if any(b.type == ButtonType.cancel for b in buttonEvents): + if any(b.type == ButtonType.cancel for b in self.CS.button_events): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) if not self.CP.pcmCruiseSpeed: if not ret.cruiseState.enabled: @@ -242,17 +242,10 @@ class CarInterface(CarInterfaceBase): ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=(self.CS.cruise_buttons[-1] == 3)) - # MADS BUTTON - if self.CS.out.madsEnabled != self.CS.madsEnabled: - if self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = False - else: - if not self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = True - - ret.buttonEvents = buttonEvents + ret.buttonEvents = [ + *self.CS.button_events, + *self.button_events.create_mads_event(self.CS.madsEnabled, self.CS.out.madsEnabled) # MADS BUTTON + ] # On some newer model years, the CANCEL button acts as a pause/resume button based on the PCM state # To avoid re-engaging when openpilot cancels, check user engagement intention via buttons diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 5fe64832c8..b8ca13ec86 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -1,3 +1,4 @@ +import capnp import json import os import numpy as np @@ -17,7 +18,7 @@ from openpilot.common.simple_kalman import KF1D, get_kalman_gain from openpilot.common.numpy_fast import clip from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL -from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG +from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG, ButtonEvents from openpilot.selfdrive.car.values import PLATFORMS from openpilot.selfdrive.controls.lib.desire_helper import get_min_lateral_speed from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, V_CRUISE_UNSET, get_friction @@ -253,6 +254,7 @@ class CarInterfaceBase(ABC): self.last_mads_init = 0. self.madsEnabledInit = False self.madsEnabledInitPrev = False + self.button_events = ButtonEvents() self.lat_torque_nn_model = None eps_firmware = str(next((fw.fwVersion for fw in CP.carFw if fw.ecu == "eps"), "")) @@ -425,6 +427,8 @@ class CarInterfaceBase(ABC): if cp is not None: cp.update_strings(can_strings) + self.CS.button_events = [] + # get CarState ret = self._update(c) @@ -780,6 +784,8 @@ class CarStateBase(ABC): self.prev_mads_enabled = False self.control_initialized = False + self.button_events: list[capnp.lib.capnp._DynamicStructBuilder] = [] + Q = [[0.0, 0.0], [0.0, 100.0]] R = 0.3 A = [[1.0, DT_CTRL], [0.0, 1.0]] diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 9832cebe46..368198e1c8 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -11,8 +11,7 @@ from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons from openpilot.selfdrive.controls.lib.drive_helpers import MAZDA_V_CRUISE_MIN VisualAlert = car.CarControl.HUDControl.VisualAlert - -BUTTONS_STATES = ["accelCruise", "decelCruise", "cancel", "resumeCruise"] +ButtonType = car.CarState.ButtonEvent.Type class CarController(CarControllerBase): @@ -141,8 +140,10 @@ class CarController(CarControllerBase): # multikyd methods, sunnyhaibin logic def get_cruise_buttons_status(self, CS): if not CS.out.cruiseState.enabled: - if any(CS.buttonStates[button_state] for button_state in BUTTONS_STATES): - self.timer = 40 + for be in CS.out.buttonEvents: + if be.type in (ButtonType.accelCruise, ButtonType.resumeCruise, + ButtonType.decelCruise, ButtonType.setCruise) and be.pressed: + self.timer = 40 elif self.timer: self.timer -= 1 else: diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index dea768c03a..e5808238f8 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -3,7 +3,7 @@ from openpilot.common.conversions import Conversions as CV from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from openpilot.selfdrive.car.interfaces import CarStateBase -from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags, BUTTON_STATES +from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags, BUTTONS class CarState(CarStateBase): def __init__(self, CP): @@ -24,8 +24,7 @@ class CarState(CarStateBase): self.lkas_enabled = False self.prev_lkas_enabled = False - self.buttonStates = BUTTON_STATES.copy() - self.buttonStatesPrev = BUTTON_STATES.copy() + self.button_states = {button.event_type: False for button in BUTTONS} def update(self, cp, cp_cam): @@ -36,7 +35,6 @@ class CarState(CarStateBase): self.prev_mads_enabled = self.mads_enabled self.prev_lkas_enabled = self.lkas_enabled - self.buttonStatesPrev = self.buttonStates.copy() ret.wheelSpeeds = self.get_wheel_speeds( cp.vl["WHEEL_SPEEDS"]["FL"], @@ -56,6 +54,16 @@ class CarState(CarStateBase): can_gear = int(cp.vl["GEAR"]["GEAR"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + # Buttons + for button in BUTTONS: + state = (cp.vl[button.can_addr][button.can_msg] in button.values) + if self.button_states[button.event_type] != state: + event = car.CarState.ButtonEvent.new_message() + event.type = button.event_type + event.pressed = state + self.button_events.append(event) + self.button_states[button.event_type] = state + ret.genericToggle = bool(cp.vl["BLINK_INFO"]["HIGH_BEAMS"]) ret.leftBlindspot = cp.vl["BSM"]["LEFT_BS_STATUS"] != 0 ret.rightBlindspot = cp.vl["BSM"]["RIGHT_BS_STATUS"] != 0 @@ -114,11 +122,6 @@ class CarState(CarStateBase): self.acc_active_last = ret.cruiseState.enabled - self.buttonStates["accelCruise"] = bool(cp.vl["CRZ_BTNS"]["SET_P"]) - self.buttonStates["decelCruise"] = bool(cp.vl["CRZ_BTNS"]["SET_M"]) - self.buttonStates["cancel"] = bool(cp.vl["CRZ_BTNS"]["CAN_OFF"]) - self.buttonStates["resumeCruise"] = bool(cp.vl["CRZ_BTNS"]["RES"]) - self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"] # camera signals diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index bb9a3f93a0..9447803ae6 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 from cereal import car from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS, BUTTON_STATES -from openpilot.selfdrive.car import create_button_events, get_safety_config, create_mads_event +from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type @@ -12,7 +12,6 @@ GearShifter = car.CarState.GearShifter class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) - self.buttonStatesPrev = BUTTON_STATES.copy() @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): @@ -41,19 +40,15 @@ class CarInterface(CarInterfaceBase): self.sp_update_params() # TODO: add button types for inc and dec - buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) - - for button in self.CS.buttonStates: - if self.CS.buttonStates[button] != self.buttonStatesPrev[button]: - be = car.CarState.ButtonEvent.new_message() - be.type = button - be.pressed = self.CS.buttonStates[button] - buttonEvents.append(be) + self.CS.button_events = [ + *self.CS.button_events, + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + ] self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS) self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled, - buttonEvents, c.vCruise) + self.CS.button_events, c.vCruise) if ret.cruiseState.available: if self.enable_mads: @@ -66,7 +61,7 @@ class CarInterface(CarInterfaceBase): self.CS.madsEnabled = False if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed: - if any(b.type == ButtonType.cancel for b in buttonEvents): + if any(b.type == ButtonType.cancel for b in self.CS.button_events): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) if self.get_sp_pedal_disengage(ret): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) @@ -79,17 +74,10 @@ class CarInterface(CarInterfaceBase): ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button)) - # MADS BUTTON - if self.CS.out.madsEnabled != self.CS.madsEnabled: - if self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = False - else: - if not self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = True - - ret.buttonEvents = buttonEvents + ret.buttonEvents = [ + *self.CS.button_events, + *self.button_events.create_mads_event(self.CS.madsEnabled, self.CS.out.madsEnabled) # MADS BUTTON + ] # events events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake], @@ -108,6 +96,4 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() - self.buttonStatesPrev = self.CS.buttonStates.copy() - return ret diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index 2aac951dbb..2889cbb7b2 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -1,3 +1,4 @@ +from collections import namedtuple from dataclasses import dataclass, field from enum import IntFlag @@ -8,6 +9,7 @@ from openpilot.selfdrive.car.docs_definitions import CarHarness, CarDocs, CarPar from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu +Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) # Steer torque limits @@ -26,14 +28,6 @@ class CarControllerParams: pass -BUTTON_STATES = { - "accelCruise": False, - "decelCruise": False, - "cancel": False, - "resumeCruise": False, -} - - @dataclass class MazdaCarDocs(CarDocs): package: str = "All" @@ -98,6 +92,14 @@ class Buttons: CANCEL = 4 +BUTTONS = [ + Button(car.CarState.ButtonEvent.Type.accelCruise, "CRZ_BTNS", "SET_P", [1]), + Button(car.CarState.ButtonEvent.Type.decelCruise, "CRZ_BTNS", "SET_M", [1]), + Button(car.CarState.ButtonEvent.Type.cancel, "CRZ_BTNS", "CAN_OFF", [1]), + Button(car.CarState.ButtonEvent.Type.resumeCruise, "CRZ_BTNS", "RES", [1]), +] + + FW_QUERY_CONFIG = FwQueryConfig( requests=[ # TODO: check data to ensure ABS does not skip ISO-TP frames on bus 0 diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 3d744a41a0..4a9e5a9125 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -1,6 +1,6 @@ from cereal import car from panda import Panda -from openpilot.selfdrive.car import create_button_events, get_safety_config, create_mads_event +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.car.nissan.values import CAR @@ -34,7 +34,7 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam) self.sp_update_params() - buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + self.CS.button_events = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS) @@ -53,24 +53,11 @@ class CarInterface(CarInterfaceBase): ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button)) - # CANCEL - if self.CS.out.cruiseState.enabled and not ret.cruiseState.enabled: - be = car.CarState.ButtonEvent.new_message() - be.pressed = True - be.type = ButtonType.cancel - buttonEvents.append(be) - - # MADS BUTTON - if self.CS.out.madsEnabled != self.CS.madsEnabled: - if self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = False - else: - if not self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = True - - ret.buttonEvents = buttonEvents + ret.buttonEvents = [ + *self.CS.button_events, + *self.button_events.create_cancel_event(ret.cruiseState.enabled, self.CS.out.cruiseState.enabled), + *self.button_events.create_mads_event(self.CS.madsEnabled, self.CS.out.madsEnabled) # MADS BUTTON + ] events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake], pcm_enable=False) diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 744c017a96..7eefeb66eb 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -1,6 +1,6 @@ from cereal import car from panda import Panda -from openpilot.selfdrive.car import get_safety_config, create_mads_event +from openpilot.selfdrive.car import get_safety_config from openpilot.selfdrive.car.disable_ecu import disable_ecu from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.car.subaru.values import CAR, GLOBAL_ES_ADDR, SubaruFlags, SubaruFlagsSP @@ -118,8 +118,6 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) self.sp_update_params() - buttonEvents = [] - self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS) if ret.cruiseState.available: @@ -145,24 +143,11 @@ class CarInterface(CarInterfaceBase): ret, self.CS = self.get_sp_common_state(ret, self.CS) - # CANCEL - if self.CS.out.cruiseState.enabled and not ret.cruiseState.enabled: - be = car.CarState.ButtonEvent.new_message() - be.pressed = True - be.type = ButtonType.cancel - buttonEvents.append(be) - - # MADS BUTTON - if self.CS.out.madsEnabled != self.CS.madsEnabled: - if self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = False - else: - if not self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = True - - ret.buttonEvents = buttonEvents + ret.buttonEvents = [ + *self.CS.button_events, + *self.button_events.create_cancel_event(ret.cruiseState.enabled, self.CS.out.cruiseState.enabled), + *self.button_events.create_mads_event(self.CS.madsEnabled, self.CS.out.madsEnabled) # MADS BUTTON + ] events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low], pcm_enable=False) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 451265f9cd..d8b2516aba 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -5,7 +5,7 @@ from panda import Panda from panda.python import uds from openpilot.selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, ToyotaFlagsSP, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \ MIN_ACC_SPEED, EPS_SCALE, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR -from openpilot.selfdrive.car import create_button_events, get_safety_config, create_mads_event +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.disable_ecu import disable_ecu from openpilot.selfdrive.car.interfaces import CarInterfaceBase @@ -212,11 +212,10 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam) self.sp_update_params() - buttonEvents = [] distance_button = 0 if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER): - buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + self.CS.button_events = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) distance_button = self.CS.distance_button self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS) @@ -245,24 +244,11 @@ class CarInterface(CarInterfaceBase): ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(distance_button)) - # CANCEL - if self.CS.out.cruiseState.enabled and not ret.cruiseState.enabled: - be = car.CarState.ButtonEvent.new_message() - be.pressed = True - be.type = ButtonType.cancel - buttonEvents.append(be) - - # MADS BUTTON - if self.CS.out.madsEnabled != self.CS.madsEnabled: - if self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = False - else: - if not self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = True - - ret.buttonEvents = buttonEvents + ret.buttonEvents = [ + *self.CS.button_events, + *self.button_events.create_cancel_event(ret.cruiseState.enabled, self.CS.out.cruiseState.enabled), + *self.button_events.create_mads_event(self.CS.madsEnabled, self.CS.out.madsEnabled) # MADS BUTTON + ] # events events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake], diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 6ec27b8be0..1dc4c0e430 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -4,7 +4,7 @@ from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from openpilot.selfdrive.car.volkswagen.values import DBC, CANBUS, NetworkLocation, TransmissionType, GearShifter, \ - CarControllerParams, VolkswagenFlags, BUTTON_STATES, VolkswagenFlagsSP + CarControllerParams, VolkswagenFlags, VolkswagenFlagsSP class CarState(CarStateBase): @@ -17,8 +17,6 @@ class CarState(CarStateBase): self.esp_hold_confirmation = False self.upscale_lead_car_signal = False self.eps_stock_values = False - self.buttonStates = BUTTON_STATES.copy() - self.buttonStatesPrev = BUTTON_STATES.copy() def create_button_events(self, pt_cp, buttons): button_events = [] @@ -41,7 +39,6 @@ class CarState(CarStateBase): ret = car.CarState.new_message() self.prev_mads_enabled = self.mads_enabled - self.buttonStatesPrev = self.buttonStates.copy() # Update vehicle speed and acceleration from ABS wheel speeds. ret.wheelSpeeds = self.get_wheel_speeds( @@ -150,18 +147,10 @@ class CarState(CarStateBase): if ret.cruiseState.speed > 90: ret.cruiseState.speed = 0 - # Update control button states for turn signals and ACC controls. - self.buttonStates["accelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Hoch"]) - self.buttonStates["decelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Runter"]) - self.buttonStates["cancel"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Abbrechen"]) - self.buttonStates["setCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Setzen"]) - self.buttonStates["resumeCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Wiederaufnahme"]) - self.buttonStates["gapAdjustCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Verstellung_Zeitluecke"]) - # Update button states for turn signals and ACC controls, capture all ACC button state/config for passthrough ret.leftBlinker = ret.leftBlinkerOn = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Left"]) ret.rightBlinker = ret.rightBlinkerOn = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Right"]) - ret.buttonEvents = self.create_button_events(pt_cp, self.CCP.BUTTONS) + self.button_events = self.create_button_events(pt_cp, self.CCP.BUTTONS) self.gra_stock_values = pt_cp.vl["GRA_ACC_01"] # Additional safety checks performed in CarInterface. @@ -177,7 +166,6 @@ class CarState(CarStateBase): ret = car.CarState.new_message() self.prev_mads_enabled = self.mads_enabled - self.buttonStatesPrev = self.buttonStates.copy() # Update vehicle speed and acceleration from ABS wheel speeds. ret.wheelSpeeds = self.get_wheel_speeds( @@ -265,18 +253,10 @@ class CarState(CarStateBase): if ret.cruiseState.speed > 70: # 255 kph in m/s == no current setpoint ret.cruiseState.speed = 0 - # Update control button states for turn signals and ACC controls. - self.buttonStates["accelCruise"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Up_kurz"]) - self.buttonStates["decelCruise"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Down_kurz"]) - self.buttonStates["cancel"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Abbrechen"]) - self.buttonStates["setCruise"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Neu_Setzen"]) - self.buttonStates["resumeCruise"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Recall"]) - self.buttonStates["gapAdjustCruise"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Zeitluecke"]) - # Update button states for turn signals and ACC controls, capture all ACC button state/config for passthrough ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = self.update_blinker_from_stalk(300, pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_li"], pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_re"]) - ret.buttonEvents = self.create_button_events(pt_cp, self.CCP.BUTTONS) + self.button_events = self.create_button_events(pt_cp, self.CCP.BUTTONS) self.gra_stock_values = pt_cp.vl["GRA_Neu"] # Additional safety checks performed in CarInterface. diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 31dea25cbb..d965cf2e8b 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -1,10 +1,10 @@ from cereal import car from panda import Panda from openpilot.common.params import Params -from openpilot.selfdrive.car import get_safety_config, create_mads_event +from openpilot.selfdrive.car import get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.car.volkswagen.values import CAR, CANBUS, CarControllerParams, NetworkLocation, TransmissionType, GearShifter, VolkswagenFlags, \ - BUTTON_STATES, VolkswagenFlagsSP + VolkswagenFlagsSP ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName @@ -21,8 +21,6 @@ class CarInterface(CarInterfaceBase): self.ext_bus = CANBUS.cam self.cp_ext = self.cp_cam - self.buttonStatesPrev = BUTTON_STATES.copy() - @staticmethod def _get_params(ret, candidate: CAR, fingerprint, car_fw, experimental_long, docs): ret.carName = "volkswagen" @@ -117,21 +115,10 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType) self.sp_update_params() - buttonEvents = [] - - # Check for and process state-change events (button press or release) from - # the turn stalk switch or ACC steering wheel/control stalk buttons. - for button in self.CS.buttonStates: - if self.CS.buttonStates[button] != self.buttonStatesPrev[button]: - be = car.CarState.ButtonEvent.new_message() - be.type = button - be.pressed = self.CS.buttonStates[button] - buttonEvents.append(be) - self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS) self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled, - buttonEvents, c.vCruise, + self.CS.button_events, c.vCruise, enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise)) if ret.cruiseState.available: @@ -144,7 +131,7 @@ class CarInterface(CarInterfaceBase): self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS) if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed: - if any(b.type == ButtonType.cancel for b in buttonEvents): + if any(b.type == ButtonType.cancel for b in self.CS.button_events): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) if self.get_sp_pedal_disengage(ret): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) @@ -155,19 +142,13 @@ class CarInterface(CarInterfaceBase): self.CS.accEnabled = False self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled - ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.buttonStates["gapAdjustCruise"])) + ret, self.CS = self.get_sp_common_state(ret, self.CS, + gap_button=any(b.type == ButtonType.gapAdjustCruise and b.pressed for b in self.CS.button_events)) - # MADS BUTTON - if self.CS.out.madsEnabled != self.CS.madsEnabled: - if self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = False - else: - if not self.mads_event_lock: - buttonEvents.append(create_mads_event(self.mads_event_lock)) - self.mads_event_lock = True - - ret.buttonEvents = buttonEvents + ret.buttonEvents = [ + *self.CS.button_events, + *self.button_events.create_mads_event(self.CS.madsEnabled, self.CS.out.madsEnabled) # MADS BUTTON + ] events = self.create_common_events(ret, c, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic], pcm_enable=False, @@ -199,8 +180,4 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() - # update previous car states - self.buttonStatesPrev = self.CS.buttonStates.copy() - return ret - diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 2996db602f..80b7ca0b0b 100644 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -147,16 +147,6 @@ class VolkswagenFlagsSP(IntFlag): SP_CC_ONLY_NO_RADAR = 2 -BUTTON_STATES = { - "accelCruise": False, - "decelCruise": False, - "cancel": False, - "setCruise": False, - "resumeCruise": False, - "gapAdjustCruise": False -} - - @dataclass class VolkswagenMQBPlatformConfig(PlatformConfig): dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('vw_mqb_2010', None)) From a2eb4545da7228885ff09f593cb5d3d93be3adde Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 25 Jul 2024 13:54:38 -0400 Subject: [PATCH 62/89] a lot less delay --- selfdrive/car/hyundai/interface.py | 1 - 1 file changed, 1 deletion(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index cb722d875d..b1226440bb 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -106,7 +106,6 @@ class CarInterface(CarInterfaceBase): ret.vEgoStarting = 0.1 ret.startAccel = 1.8 ret.stoppingDecelRate = 10.0 # reach standstill quickly after entering stopping state - ret.longitudinalActuatorDelay = 0.5 if DBC[ret.carFingerprint]["radar"] is None: if ret.spFlags & (HyundaiFlagsSP.SP_ENHANCED_SCC | HyundaiFlagsSP.SP_CAMERA_SCC_LEAD): From 2038d9b469401ea7a2708aa5b6994595affc5baa Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 25 Jul 2024 15:18:34 -0400 Subject: [PATCH 63/89] send 0 at stop req --- selfdrive/car/hyundai/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 120be3d1ef..70fb05f0d8 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -540,7 +540,7 @@ class CarController(CarControllerBase): if long_control == LongCtrlState.off: self.accel_raw, self.accel_val = 0, 0 else: - if long_control == LongCtrlState.stopping and CS.out.standstill: + if long_control == LongCtrlState.stopping: self.accel_raw = 0 self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up) self.accel_last = self.accel_val From 7eee21f293589802d32712fb135813c0d90f41ea Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 25 Jul 2024 16:12:09 -0400 Subject: [PATCH 64/89] add back delay --- selfdrive/car/hyundai/interface.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index b1226440bb..07a500eb41 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 = 1.8 + ret.longitudinalActuatorDelay = 0.5 ret.stoppingDecelRate = 10.0 # reach standstill quickly after entering stopping state if DBC[ret.carFingerprint]["radar"] is None: From d4096d62343e0ba92cab4ec50c9a0e4d4974bc5b Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 25 Jul 2024 16:26:30 -0400 Subject: [PATCH 65/89] not really needed --- selfdrive/car/hyundai/interface.py | 1 - 1 file changed, 1 deletion(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 07a500eb41..0a4076ebae 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -106,7 +106,6 @@ class CarInterface(CarInterfaceBase): ret.vEgoStarting = 0.1 ret.startAccel = 1.8 ret.longitudinalActuatorDelay = 0.5 - ret.stoppingDecelRate = 10.0 # reach standstill quickly after entering stopping state if DBC[ret.carFingerprint]["radar"] is None: if ret.spFlags & (HyundaiFlagsSP.SP_ENHANCED_SCC | HyundaiFlagsSP.SP_CAMERA_SCC_LEAD): From c943126d26866ee4fb0fb1ae7066d784f419e3b2 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 25 Jul 2024 16:32:52 -0400 Subject: [PATCH 66/89] send 0 to car when stopping --- selfdrive/car/hyundai/carcontroller.py | 10 ++++------ selfdrive/controls/lib/longcontrol.py | 4 +++- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 70fb05f0d8..d1d0c40ac5 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -299,7 +299,7 @@ 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 - self.make_accel(CS, accel, actuators) + 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, self.cb_lower, self.cb_upper)) @@ -478,12 +478,13 @@ class CarController(CarControllerBase): return cruise_button 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 = accel - self.accel_last + accel_diff = self.accel_raw - self.accel_last accel_diff /= DT_CTRL self.jerk = self.jerk * 0.9 + accel_diff * 0.1 @@ -533,14 +534,11 @@ class CarController(CarControllerBase): 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, CS, accel, actuators): + def make_accel(self, actuators): long_control = actuators.longControlState - self.accel_raw = accel rate_up, rate_down = 0.1, 0.1 if long_control == LongCtrlState.off: self.accel_raw, self.accel_val = 0, 0 else: - if long_control == LongCtrlState.stopping: - self.accel_raw = 0 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/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index a661c33361..085320db0a 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -68,7 +68,9 @@ class LongControl: elif self.long_control_state == LongCtrlState.stopping: output_accel = self.last_output_accel - if output_accel > self.CP.stopAccel: + if self.CP.stoppingControl: + output_accel = 0.0 + elif output_accel > self.CP.stopAccel: output_accel = min(output_accel, 0.0) output_accel -= self.CP.stoppingDecelRate * DT_CTRL self.reset() From a33d4e51c7b7587b971026d5b48c7b3b230e680b Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 25 Jul 2024 16:44:54 -0400 Subject: [PATCH 67/89] use jerk limit to change accel to car --- selfdrive/car/hyundai/carcontroller.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index d1d0c40ac5..44c7135fcc 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -536,7 +536,8 @@ class CarController(CarControllerBase): def make_accel(self, actuators): long_control = actuators.longControlState - rate_up, rate_down = 0.1, 0.1 + rate_up = 0.1 * self.jerk_u + rate_down = 0.1 * self.jerk_l if long_control == LongCtrlState.off: self.accel_raw, self.accel_val = 0, 0 else: From 7b8a03b5f272603891893dd7623621b927ebb81d Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Fri, 26 Jul 2024 09:24:27 +0000 Subject: [PATCH 68/89] Add new parameters for enabling GitLab runner and Sunnylink uploader --- .gitlab-ci.yml | 4 ++-- common/params.cc | 2 ++ system/manager/manager.py | 2 ++ system/manager/process_config.py | 25 ++++++++++++------------- system/manager/sunnylink.py | 5 +++++ 5 files changed, 23 insertions(+), 15 deletions(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index da0a0d4bc9..a2c27da614 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -30,13 +30,13 @@ default: - 'git config --global user.name "${GIT_CONFIG_USER_NAME}"' -workflow: # If running on any branch other than main, use the `aws-datacontracts-dev` account; otherwise use `aws-datacontracts` +workflow: # If running on any branch other than main. rules: # We are an MR, but it's a draft, we won't proceed with anything. - if: '$CI_MERGE_REQUEST_TITLE =~ /^wip:/i || $CI_MERGE_REQUEST_TITLE =~ /^draft:/i' when: never # We are a merge request - - if: $CI_MERGE_REQUEST_IID #|| $CI_COMMIT_REF_NAME == "gitlab-pipelines" # TBD once merged + - if: $CI_MERGE_REQUEST_IID variables: EXTRA_VERSION_IDENTIFIER: "-${CI_PIPELINE_IID}" NEW_BRANCH: ${CI_COMMIT_REF_NAME}-prebuilt diff --git a/common/params.cc b/common/params.cc index 054c5d2355..86f18a0a5e 100644 --- a/common/params.cc +++ b/common/params.cc @@ -336,6 +336,8 @@ std::unordered_map keys = { {"SunnylinkCache_Users", PERSISTENT}, {"SunnylinkCache_Roles", PERSISTENT}, + {"EnableGitlabRunner", PERSISTENT | BACKUP}, + {"EnableSunnylinkUploader", PERSISTENT | BACKUP}, // PFEIFER - MAPD {{ {"MapdVersion", PERSISTENT}, diff --git a/system/manager/manager.py b/system/manager/manager.py index 388198736c..032fc0a667 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -117,6 +117,8 @@ def manager_init() -> None: ("CustomDrivingModel", "0"), ("DrivingModelGeneration", "4"), ("LastSunnylinkPingTime", "0"), + ("EnableGitlabRunner", "0"), + ("EnableSunnylinkUploader", "0"), ] if not PC: default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8'))) diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 8818d3b851..a718ea021b 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -6,7 +6,7 @@ from openpilot.system.hardware import PC, TICI from openpilot.selfdrive.modeld.custom_model_metadata import CustomModelMetadata, ModelCapabilities from openpilot.system.manager.process import PythonProcess, NativeProcess, DaemonProcess from openpilot.system.mapd_manager import MAPD_PATH, COMMON_DIR -from openpilot.system.manager.sunnylink import sunnylink_need_register, sunnylink_ready +from openpilot.system.manager.sunnylink import sunnylink_need_register, sunnylink_ready, use_sunnylink_uploader WEBCAM = os.getenv("USE_WEBCAM") is not None @@ -44,6 +44,10 @@ def only_onroad(started: bool, params, CP: car.CarParams) -> bool: def only_offroad(started, params, CP: car.CarParams) -> bool: return not started +def use_gitlab_runner(started, params, CP: car.CarParams) -> bool: + return (not PC and params.get_bool("EnableGitlabRunner") and only_offroad(started, params, CP) + and os.path.exists("./gitlab_runner.sh")) + def model_use_nav(started, params, CP: car.CarParams) -> bool: custom_model_metadata = CustomModelMetadata(params=params, init_only=True) return started and custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.NoO @@ -56,6 +60,10 @@ def sunnylink_need_register_shim(started, params, CP: car.CarParams) -> bool: """Shim for sunnylink_need_register to match the process manager signature.""" return sunnylink_need_register(params) +def use_sunnylink_uploader_shim(started, params, CP: car.CarParams) -> bool: + """Shim for use_sunnylink_uploader to match the process manager signature.""" + return use_sunnylink_uploader(params) and os.path.exists("../loggerd/sunnylink_uploader.py") + procs = [ DaemonProcess("manage_athenad", "system.athena.manage_athenad", "AthenadPid"), @@ -112,21 +120,12 @@ procs = [ PythonProcess("webrtcd", "system.webrtc.webrtcd", notcar), PythonProcess("webjoystick", "tools.bodyteleop.web", notcar), + # Sunnypilot devs + NativeProcess("gitlab_runner_start", "system/manager", ["./gitlab_runner.sh", "start"], use_gitlab_runner, sigkill=False), # Sunnylink <3 DaemonProcess("manage_sunnylinkd", "system.athena.manage_sunnylinkd", "SunnylinkdPid"), PythonProcess("sunnylink_registration", "system.manager.sunnylink", sunnylink_need_register_shim), + PythonProcess("sunnylink_uploader", "system.loggerd.sunnylink_uploader", use_sunnylink_uploader_shim), ] -if os.path.exists("../loggerd/sunnylink_uploader.py"): - procs += [ - PythonProcess("sunnylink_uploader", "system.loggerd.sunnylink_uploader", sunnylink_ready_shim), - ] - -if os.path.exists("./gitlab_runner.sh") and not PC: - # Only devs! - procs += [ - NativeProcess("gitlab_runner_start", "system/manager", ["./gitlab_runner.sh", "start"], only_offroad, sigkill=False), - NativeProcess("gitlab_runner_stop", "system/manager", ["./gitlab_runner.sh", "stop"], only_onroad, sigkill=False) - ] - managed_processes = {p.name: p for p in procs} diff --git a/system/manager/sunnylink.py b/system/manager/sunnylink.py index 84161fb817..10a5f95a3d 100755 --- a/system/manager/sunnylink.py +++ b/system/manager/sunnylink.py @@ -27,6 +27,11 @@ def sunnylink_ready(params=Params()) -> bool: return is_sunnylink_enabled and is_registered +def use_sunnylink_uploader(params) -> bool: + """Check if the device is ready to use Sunnylink and the uploader is enabled.""" + return sunnylink_ready(params) and params.get_bool("EnableSunnylinkUploader") + + def sunnylink_need_register(params=Params()) -> bool: """Check if the device needs to be registered with Sunnylink.""" is_sunnylink_enabled, is_registered = get_sunnylink_status(params) From 72a0142bf97d07895254817e2c26291c66218df3 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 26 Jul 2024 09:52:30 -0400 Subject: [PATCH 69/89] do this at the car level --- selfdrive/car/hyundai/interface.py | 2 ++ selfdrive/controls/lib/longcontrol.py | 4 +--- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 0a4076ebae..5b284abdcb 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -105,6 +105,8 @@ class CarInterface(CarInterfaceBase): ret.startingState = True ret.vEgoStarting = 0.1 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 085320db0a..a661c33361 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -68,9 +68,7 @@ class LongControl: elif self.long_control_state == LongCtrlState.stopping: output_accel = self.last_output_accel - if self.CP.stoppingControl: - output_accel = 0.0 - elif output_accel > self.CP.stopAccel: + if output_accel > self.CP.stopAccel: output_accel = min(output_accel, 0.0) output_accel -= self.CP.stoppingDecelRate * DT_CTRL self.reset() From 981d150a3b75b0395a5cd55e3e7a13fb93e345d4 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 26 Jul 2024 10:13:06 -0400 Subject: [PATCH 70/89] different rate of change for ICE, HEV, PHEV, EV --- selfdrive/car/hyundai/carcontroller.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 44c7135fcc..bd84c773d7 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -536,8 +536,9 @@ class CarController(CarControllerBase): def make_accel(self, actuators): long_control = actuators.longControlState - rate_up = 0.1 * self.jerk_u - rate_down = 0.1 * self.jerk_l + 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: From 796e6ae92416f1c856ccdaf177f7985a43cfcd18 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 26 Jul 2024 23:59:40 -0400 Subject: [PATCH 71/89] SCC "Fast" mode --- selfdrive/car/hyundai/hyundaican.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index 58b05d708a..200e1a9682 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -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, } From 348cd735f2ab0f0effbdc0b892c974535a054323 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 27 Jul 2024 00:25:37 -0400 Subject: [PATCH 72/89] handle stopAccel = 0 --- selfdrive/controls/lib/longcontrol.py | 3 +++ 1 file changed, 3 insertions(+) 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: From 5ba3e90676c3cf6d989aec8a9ec61192b3c9a538 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 27 Jul 2024 02:03:15 -0400 Subject: [PATCH 73/89] comment --- selfdrive/car/hyundai/carcontroller.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index bd84c773d7..858bc0757c 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -477,6 +477,7 @@ 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: From b4c98bc72f9c55b701dd568a470df4f62706c2bf Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 27 Jul 2024 14:37:02 -0400 Subject: [PATCH 74/89] add kona ev 2022 to can-fd --- selfdrive/car/hyundai/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 858bc0757c..a93b6a14c0 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -496,7 +496,7 @@ class CarController(CarControllerBase): a_error = accel - CS.out.aEgo jerk = jerk + (a_error * 2.0) - if self.CP.carFingerprint in CANFD_CAR: + if self.CP.carFingerprint in CANFD_CAR or self.CP.carFingerprint == CAR.HYUNDAI_KONA_EV_2022: startingJerk = 0.5 #self.jerkStartLimit jerkLimit = 5.0 self.jerk_count += DT_CTRL From 04872944955f8cc506cc8e6c7412c9ce94a42b33 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 28 Jul 2024 14:19:52 -0400 Subject: [PATCH 75/89] try this to make car stop a bit further --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 2 +- selfdrive/controls/lib/longitudinal_planner.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 7296a28b11..a2df527a55 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 = 8.0 +STOP_DISTANCE = 6.0 def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard): if personality==custom.LongitudinalPersonalitySP.relaxed: diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 62ed68a77e..48874282e8 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -83,7 +83,7 @@ class LongitudinalPlanner: self.dt = dt self.a_desired = init_a - self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) + self.v_desired_filter = FirstOrderFilter(init_v, 0.5, self.dt) self.v_model_error = 0.0 self.v_desired_trajectory = np.zeros(CONTROL_N) From f9be24b154489f4f5e21070285893955a065dba6 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 28 Jul 2024 22:32:44 -0400 Subject: [PATCH 76/89] Revert "try this to make car stop a bit further" This reverts commit 04872944955f8cc506cc8e6c7412c9ce94a42b33. --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 2 +- selfdrive/controls/lib/longitudinal_planner.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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: diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 48874282e8..62ed68a77e 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -83,7 +83,7 @@ class LongitudinalPlanner: self.dt = dt self.a_desired = init_a - self.v_desired_filter = FirstOrderFilter(init_v, 0.5, self.dt) + self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) self.v_model_error = 0.0 self.v_desired_trajectory = np.zeros(CONTROL_N) From cb76fb797fa58e0a14017b49b1b4565ce899be18 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 28 Jul 2024 22:44:01 -0400 Subject: [PATCH 77/89] dynamic stopping distance --- .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 7296a28b11..20838608be 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 = 8.0 +STOP_DISTANCE = 6.0 def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard): if personality==custom.LongitudinalPersonalitySP.relaxed: @@ -102,11 +102,17 @@ def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP. return np.interp(v_ego, x_vel, y_dist) +def get_stop_distance(v_ego): + v_ego = np.asarray(v_ego) + stop_distance = np.where(v_ego < 1.5, 8.0, STOP_DISTANCE) + return stop_distance + def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) def get_safe_obstacle_distance(v_ego, t_follow): - return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE + stop_distance = get_stop_distance(v_ego) + return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + stop_distance def desired_follow_distance(v_ego, v_lead, t_follow=None): if t_follow is None: From 4a2b96ae4ecd37dd99395586d28b0bff59ebab4f Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 29 Jul 2024 07:18:21 -0400 Subject: [PATCH 78/89] Revert "dynamic stopping distance" This reverts commit cb76fb797fa58e0a14017b49b1b4565ce899be18. --- .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 20838608be..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: @@ -102,17 +102,11 @@ def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP. return np.interp(v_ego, x_vel, y_dist) -def get_stop_distance(v_ego): - v_ego = np.asarray(v_ego) - stop_distance = np.where(v_ego < 1.5, 8.0, STOP_DISTANCE) - return stop_distance - def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) def get_safe_obstacle_distance(v_ego, t_follow): - stop_distance = get_stop_distance(v_ego) - return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + stop_distance + return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE def desired_follow_distance(v_ego, v_lead, t_follow=None): if t_follow is None: From 1dd9ed10421fb90f9aa2c6243afd5f1bf0c752f2 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 31 Jul 2024 00:00:09 -0400 Subject: [PATCH 79/89] more aggressive jerk --- selfdrive/car/hyundai/carcontroller.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index a93b6a14c0..7c7dfbb1c9 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -112,6 +112,7 @@ class CarController(CarControllerBase): self.accel_raw = 0 self.accel_val = 0 + self.accel_last_jerk = 0 def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float: lead_one = self.sm["radarState"].leadOne @@ -485,7 +486,7 @@ class CarController(CarControllerBase): 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 = self.accel_raw - self.accel_last_jerk accel_diff /= DT_CTRL self.jerk = self.jerk * 0.9 + accel_diff * 0.1 From 74481be4ff5a411aa07c1052d27fb87beab7e337 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 31 Jul 2024 00:02:04 -0400 Subject: [PATCH 80/89] same rate up/down --- selfdrive/car/hyundai/carcontroller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 7c7dfbb1c9..9653b5b537 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -539,8 +539,8 @@ class CarController(CarControllerBase): 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 + rate_up = 0.1 + rate_down = 0.1 if long_control == LongCtrlState.off: self.accel_raw, self.accel_val = 0, 0 else: From d472f129e61b030571a278de153c72e7b06b5d52 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 31 Jul 2024 00:03:04 -0400 Subject: [PATCH 81/89] stock stop distance again --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 7296a28b11..a2df527a55 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 = 8.0 +STOP_DISTANCE = 6.0 def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard): if personality==custom.LongitudinalPersonalitySP.relaxed: From 381b51423000cbe57600d263d0e3f836553a2887 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 31 Jul 2024 19:31:56 -0400 Subject: [PATCH 82/89] new tuning! --- selfdrive/car/hyundai/carcontroller.py | 20 +++++--------------- 1 file changed, 5 insertions(+), 15 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 9653b5b537..7ad944b12f 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -498,7 +498,7 @@ class CarController(CarControllerBase): jerk = jerk + (a_error * 2.0) if self.CP.carFingerprint in CANFD_CAR or self.CP.carFingerprint == CAR.HYUNDAI_KONA_EV_2022: - startingJerk = 0.5 #self.jerkStartLimit + 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]) @@ -506,13 +506,7 @@ class CarController(CarControllerBase): 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: @@ -525,14 +519,9 @@ class CarController(CarControllerBase): 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.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) @@ -544,5 +533,6 @@ class CarController(CarControllerBase): 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 = 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 From fcca72af9fd2c255d5f4c124dbf7a93b307d4c61 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 1 Aug 2024 06:13:34 -0400 Subject: [PATCH 83/89] add new toggle, remove old toggle --- common/params.cc | 1 + selfdrive/car/hyundai/carcontroller.py | 4 ++++ .../ui/qt/offroad/sunnypilot/vehicle_settings.cc | 14 +++++++------- 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/common/params.cc b/common/params.cc index 86f18a0a5e..db085e567d 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 7ad944b12f..2d69e847f7 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -113,6 +113,7 @@ class CarController(CarControllerBase): 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 @@ -497,6 +498,9 @@ class CarController(CarControllerBase): a_error = accel - CS.out.aEgo jerk = jerk + (a_error * 2.0) + if self.hkg_custom_long_tuning: + self.jerk_u = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0 + self.jerk_l = 5.0 if self.CP.carFingerprint in CANFD_CAR or self.CP.carFingerprint == CAR.HYUNDAI_KONA_EV_2022: startingJerk = 0.5 jerkLimit = 5.0 diff --git a/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc index ef0954c1ed..c40c12dee7 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc @@ -72,14 +72,14 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge // Hyundai/Kia/Genesis addItem(new LabelControl(tr("Hyundai/Kia/Genesis"))); - auto hkgSmoothStop = new ParamControl( - "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."), + auto hkgCustomLongTuning = new ParamControl( + "HkgCustomLongTuning", + tr("HKG CAN: Custom Tuning for New Longitudinal API"), + tr(""), "../assets/offroad/icon_blank.png" ); - hkgSmoothStop->setConfirmation(true, false); - addItem(hkgSmoothStop); + hkgCustomLongTuning->setConfirmation(true, false); + addItem(hkgCustomLongTuning); // Subaru addItem(new LabelControl(tr("Subaru"))); @@ -186,7 +186,7 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge // trigger offroadTransition when going onroad/offroad connect(uiState(), &UIState::offroadTransition, [=](bool offroad) { is_onroad = !offroad; - hkgSmoothStop->setEnabled(offroad); + hkgCustomLongTuning->setEnabled(offroad); toyotaTss2LongTune->setEnabled(offroad); toyotaAbh->setEnabled(offroad); toyotaEnhancedBsm->setEnabled(offroad); From 0c78b6b9b8650ebfc398e901916ba3ecaeb4f3b0 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 1 Aug 2024 06:15:26 -0400 Subject: [PATCH 84/89] maybe this is why it has been iffy --- selfdrive/car/hyundai/carcontroller.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 2d69e847f7..4b7c4ac52b 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -540,3 +540,4 @@ class CarController(CarControllerBase): #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 From f79ed8f06b04e751df0a60f2653716f3e846d875 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 1 Aug 2024 06:28:20 -0400 Subject: [PATCH 85/89] enable for CAN-FD --- selfdrive/car/hyundai/carcontroller.py | 7 ++++--- selfdrive/car/hyundai/hyundaicanfd.py | 6 +++--- selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc | 2 +- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 4b7c4ac52b..b41ff020a0 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -221,6 +221,9 @@ class CarController(CarControllerBase): if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", self.CAN.ECAN]) + 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 @@ -249,7 +252,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, 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 @@ -287,8 +290,6 @@ class CarController(CarControllerBase): self.last_button_frame = self.frame elif self.frame % 2 == 0: can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25) - else: - self.make_jerk(CS, accel, actuators) # Parse lead distance from radarState and display the corresponding distance in the car's cluster if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0: diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index 8225166bfa..cdb2d40c3c 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, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control): +def create_acc_control(packer, CAN, 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, enabled, accel_last, accel, stopping, gas_ov "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/ui/qt/offroad/sunnypilot/vehicle_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc index c40c12dee7..267e3260e2 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc @@ -74,7 +74,7 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge addItem(new LabelControl(tr("Hyundai/Kia/Genesis"))); auto hkgCustomLongTuning = new ParamControl( "HkgCustomLongTuning", - tr("HKG CAN: Custom Tuning for New Longitudinal API"), + tr("HKG: Custom Tuning for New Longitudinal API"), tr(""), "../assets/offroad/icon_blank.png" ); From 9ad059f05ef472d49deef3ece76aa091afd50a0c Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 1 Aug 2024 06:41:26 -0400 Subject: [PATCH 86/89] wrong logic --- selfdrive/car/hyundai/carcontroller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index b41ff020a0..0733ff0fc7 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -499,10 +499,10 @@ class CarController(CarControllerBase): a_error = accel - CS.out.aEgo jerk = jerk + (a_error * 2.0) - if self.hkg_custom_long_tuning: + 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 - if self.CP.carFingerprint in CANFD_CAR or self.CP.carFingerprint == CAR.HYUNDAI_KONA_EV_2022: + elif 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 From 48371eb08974b22a4336aa58625eddc9b1783f69 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 3 Aug 2024 15:58:49 -0400 Subject: [PATCH 87/89] use CAN-FD tune for every car --- selfdrive/car/hyundai/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 0733ff0fc7..50fdc2dbf7 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -502,7 +502,7 @@ class CarController(CarControllerBase): 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 self.CP.carFingerprint in CANFD_CAR or self.CP.carFingerprint == CAR.HYUNDAI_KONA_EV_2022: + 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 From b61f75a8fbf7558b0fe03f98f0d4c85cacb78669 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 4 Aug 2024 09:10:43 -0400 Subject: [PATCH 88/89] remove from sync --- .git-crypt/.gitattributes | 4 ---- .../106E1EB7856273777A0C8BB4EF8EA444C1E7B69C.gpg | Bin 725 -> 0 bytes .../91D8EDBE6BD70DFB803D59292470ADBDD426DEB5.gpg | Bin 725 -> 0 bytes .../E08F802F45946A6A3E16AD1B1609CD53B15C2462.gpg | Bin 735 -> 0 bytes release/release_files.py | 2 -- selfdrive/car/interfaces.py | 2 -- system/manager/process_config.py | 1 - 7 files changed, 9 deletions(-) delete mode 100644 .git-crypt/.gitattributes delete mode 100644 .git-crypt/keys/default/0/106E1EB7856273777A0C8BB4EF8EA444C1E7B69C.gpg delete mode 100644 .git-crypt/keys/default/0/91D8EDBE6BD70DFB803D59292470ADBDD426DEB5.gpg delete mode 100644 .git-crypt/keys/default/0/E08F802F45946A6A3E16AD1B1609CD53B15C2462.gpg diff --git a/.git-crypt/.gitattributes b/.git-crypt/.gitattributes deleted file mode 100644 index 665b10e8f0..0000000000 --- a/.git-crypt/.gitattributes +++ /dev/null @@ -1,4 +0,0 @@ -# Do not edit this file. To specify the files to encrypt, create your own -# .gitattributes file in the directory where your files are. -* !filter !diff -*.gpg binary diff --git a/.git-crypt/keys/default/0/106E1EB7856273777A0C8BB4EF8EA444C1E7B69C.gpg b/.git-crypt/keys/default/0/106E1EB7856273777A0C8BB4EF8EA444C1E7B69C.gpg deleted file mode 100644 index 97b89be16eef468dea1688448132cd49bc1a9205..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 725 zcmV;`0xJE50t^Eiti!r3*)-+>5CEM;BTRKyWmz_3gO6PMXMbPN8+OdA6CadusYg9^ zWEg3Hz?fKrSv+uXx{Tssm0G=WQZdR4NKEO3c+2^|&X>&>$KG$#mLSy6=!V~-Blgi< z4rbWQe~-g;X9ORi=kxVsUp%TuJzT8$KfwLD=!;G22C0wERyD)W8*Cqx$nwrUQ}{`SO70zzskdift(^gjR0vt;&(*}}_sKQrIoG1GHILGca1FFrY23uBQ6ca*) zN2ZBxDgh+>4NJ=^wpG%=0|AIJdW$G(>Cj!#x{XK+oplLEf2E9Ui6cd(9*v zH2SrDh492MO=i<_&_X5mlVw}px}?7bLxm!ItMLI`Vl-3GhjO*>3*S^}x#3+R_LqTs z0@tS3baUe2=5JPH*zgRlaWq~6WGGaggBmy;;TaC7@~U7}3DrU33Z9lF@?;bC*_7)rtkjg8j6|?1VHGyK^v9fhF$}jmr$w}(@|UW zCDSs`cxVBK0OX@n!pU+=3s>=PP<`7I*TgwzlW}bCJCxAdJ=4{%NGkuX2|Y$;ev}{L z5K_5bU1AU%jBP?KVo4zu5kLdZ>i=eEjD7&CVB(9&Se)dt^i%Wd@R;T2ZC?d*n7`AV zWZI}=|MbRfHX7&T2=1TyZJAdaN5iQH?*Z3{`z4N5Y6=`NNKGrYI!f*$v1R`6NzE=$ zQaUW|sFgpJ$pX0W)o~=x)WCHO?{R4=qJRk7)Z}YSJT&nYoyuc;14=$octKO1;nyrH zh?nA@kC?#@x0f28qYC*srv67oHzn+DI^+(3ps`LSlfzcJ@he*!Hn*PEW{;&fCcx4B zqTYwap!a3@OfA*S>CD|^9VJ2)+7+0f&_EVya;dcq=`$JeyO{H!VN6!XkO@SR1MjPX z9LQDJ3%q65Zt!N@N3znu0|6YI82ww*3PRzyAIJeTfu$`dI#tndf?i0VwJCCjU#Zeg z4-(gUMpGm7v8X7c9iP9JD(my5N%*Q2yv0j`6r9vf!p4S`EmHT5gs5-@0>f-CT&9}f z`AuXtw#?1kk{ghL1gqZYO&3}31ZbTi23N5N1fdQh2YSxD{`dF-fEt2;y1)6ri0-mj zn!xr}jV&Qmo;v${hx}{D(h9&5Ra}+i#}apkZbKLQI=iu;U@^hnFk;ij8&U+bgn?l6xnAgEx#Dza?w=UzyMmxvM+Qe+SWlz+UQ?Q5m53(MN8h>Y>D@RZ-j5M)P(Kq>$`lN{RFr{12*0F2 znlSADOAt%tK)Tu_aX3x5P=wumS{_Dv-_i20k{~-J6SYQl>~UN0JXnmf;WYQ+ zl=rpmLzyLjcWaVY!<;pOvWuLA(=M<#_tbnAXLS)ki|#8FaJ)whiG&coR=Z_ikWR>? zFBgrx0Tgi?(Q}^(EIoM>nkS(~sCSo~1hPsEAML=nFNbsqHIK9R^QNoeBBF7c`X>Pv z2tZj&5Q(v;8Tl^wn9uy`m|qfw_BRewH3nw`$fLi)1Zji~Pel2nzBJymh@fGl!s)#%;n5V(c*u;yCs?@dDe?5QF{V6*~;eqdI%ym|q}v^cIxfgJah zqPTravGaxq?yS&u?A~3E+J@@>Am<`ioGZhA`B?4FL%P5Ju*TZc+T%>mQZjHZ3PF!AeqNfQ7bGbH`6kQ_jEj zxk`Gih-)5$|KoWu-$MlpAwwb|lTjPXCnRW-KEjg+vFbb8bf`&zYCkC94P{ekg-Euy zRaURgwANGWf>8S~<}tx-0%z3@hQAg0*mp#WlH>^*hKrdC1X+)1(5^$P0q!#8j`fGP zk8f_lbc5xFGx{2PXsOrdN6YR9AJA=?S=57 RnAcT|%MwwGhU?B@M#$H9W Date: Sun, 4 Aug 2024 09:11:02 -0400 Subject: [PATCH 89/89] only for hkg --- selfdrive/controls/lib/longcontrol.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index d0727be8bf..1ee8a7b013 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -75,7 +75,7 @@ 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: + elif output_accel < self.CP.stopAccel == 0.0: output_accel = min(output_accel, 0.0) output_accel += self.CP.stoppingDecelRate * DT_CTRL self.reset()