From 7853d218a388f5d5ba6077177b17358316ba6956 Mon Sep 17 00:00:00 2001 From: Your Name Date: Tue, 6 Feb 2024 01:08:15 -0600 Subject: [PATCH] wip --- common/params.cc | 3 + notes | 1 + panda/board/safety/safety_hyundai.h | 3 +- panda/board/safety/safety_hyundai_canfd.h | 3 +- selfdrive/car/hyundai/carcontroller.py | 66 ++++++++++++++++-- selfdrive/car/hyundai/hyundaicanfd.py | 3 +- selfdrive/car/hyundai/interface.py | 6 ++ selfdrive/controls/controlsd.py | 4 +- selfdrive/controls/lib/drive_helpers.py | 11 ++- .../frog_theme/images/button_settings.png | Bin 2779 -> 1846 bytes .../frogpilot/functions/frogpilot_planner.py | 10 ++- selfdrive/frogpilot/ui/control_settings.cc | 2 +- 12 files changed, 96 insertions(+), 16 deletions(-) diff --git a/common/params.cc b/common/params.cc index f2b3b64..82f7fb1 100644 --- a/common/params.cc +++ b/common/params.cc @@ -244,6 +244,9 @@ std::unordered_map keys = { {"Compass", PERSISTENT}, {"ConditionalExperimental", PERSISTENT}, {"CurrentRandomEvent", PERSISTENT}, + {"CSLCAvailable", PERSISTENT}, + {"CSLCEnabled", PERSISTENT}, + {"CSLCSpeed", PERSISTENT}, {"CurveSensitivity", PERSISTENT}, {"CustomColors", PERSISTENT}, {"CustomIcons", PERSISTENT}, diff --git a/notes b/notes index 730711a..4e6faaf 100644 --- a/notes +++ b/notes @@ -6,6 +6,7 @@ Goals: 3 - chirp when speed limit changes 4 - Use LFA button to set speed of cruise to desired speed - 0-36 speed limit +3, 36-59 + 5, >= 60 +7 5 - On experimental mode engage, adjust cruise speed to experimental model speed + - Verify pause lateral under 30 is enabled Wishlist: - When car reports seeing lanes, turn lane keep over to car, resume when car can't see lanes or turn signal is activated - Get hyundai speed limit reader merged into frogpilot diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h index cc2dc41..5d3b29e 100644 --- a/panda/board/safety/safety_hyundai.h +++ b/panda/board/safety/safety_hyundai.h @@ -273,7 +273,8 @@ static bool hyundai_tx_hook(CANPacket_t *to_send) { bool allowed_resume = (button == 1) && controls_allowed; bool allowed_cancel = (button == 4) && cruise_engaged_prev; - if (!(allowed_resume || allowed_cancel)) { + bool allowed_cslc = (button == 1 || button == 2) && controls_allowed; + if (!(allowed_resume || allowed_cancel || allowed_cslc)) { tx = false; } } diff --git a/panda/board/safety/safety_hyundai_canfd.h b/panda/board/safety/safety_hyundai_canfd.h index daa72d2..e246c88 100644 --- a/panda/board/safety/safety_hyundai_canfd.h +++ b/panda/board/safety/safety_hyundai_canfd.h @@ -247,8 +247,9 @@ static bool hyundai_canfd_tx_hook(CANPacket_t *to_send) { int button = GET_BYTE(to_send, 2) & 0x7U; bool is_cancel = (button == HYUNDAI_BTN_CANCEL); bool is_resume = (button == HYUNDAI_BTN_RESUME); + bool is_cslc = (button == HYUNDAI_BTN_RESUME || button == HYUNDAI_BTN_SET); - bool allowed = (is_cancel && cruise_engaged_prev) || (is_resume && controls_allowed); + bool allowed = (is_cancel && cruise_engaged_prev) || (is_resume && controls_allowed) || (is_cslc && controls_allowed); if (!allowed) { tx = false; } diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 9aa1eb7..38e3a14 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -6,11 +6,15 @@ from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus -from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR +from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR, LEGACY_SAFETY_MODE_CAR +from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX +from openpilot.common.params import Params VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState +params_memory = Params("/dev/shm/params") + # EPS faults if you apply torque while the steering angle is above 90 degrees for more than 1 second # All slightly below EPS thresholds to avoid fault MAX_ANGLE = 85 @@ -60,6 +64,10 @@ class CarController: actuators = CC.actuators hud_control = CC.hudControl + hud_v_cruise = hud_control.setSpeed + if hud_v_cruise > 70: + hud_v_cruise = 0 + # steering torque new_steer = int(round(actuators.steer * self.params.STEER_MAX)) apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) @@ -146,6 +154,25 @@ class CarController: if not self.CP.openpilotLongitudinalControl: can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True)) + # CSLC + if frogpilot_variables.CSLC and frogpilot_variables.CSLCA and CC.enabled and not CS.out.gasPressed: #and CS.cruise_buttons == Buttons.NONE: + cslcSetSpeed = get_set_speed(self, hud_v_cruise) + self.cruise_button = get_cslc_button(self, cslcSetSpeed, CS) + if self.cruise_button != Buttons.NONE: + if self.CP.carFingerprint in LEGACY_SAFETY_MODE_CAR: + send_freq = 1 + # send resume at a max freq of 10Hz + if (self.frame - self.last_button_frame) * DT_CTRL > 0.1 * send_freq: + # send 25 messages at a time to increases the likelihood of cruise buttons being accepted + can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25) + if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15 * send_freq: + self.last_button_frame = self.frame + elif self.frame % 2 == 0: + if self.CP.carFingerprint in CANFD_CAR: + can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, self.cruise_button)) + else: + can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25) + if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl: # TODO: unclear if this is needed jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0 @@ -179,12 +206,14 @@ class CarController: if CS.custom_speed_down: CS.custom_speed_down = False - if self.CP.openpilotLongitudinalControl: - CC.cruiseControl.resume = True - self.CP.openpilotLongitudinalControl = False - else: - CC.cruiseControl.cancel = True - self.CP.openpilotLongitudinalControl = True + # Test me. + can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, 1, Buttons.RES_ACCEL)) + # if self.CP.openpilotLongitudinalControl: + # CC.cruiseControl.resume = True + # self.CP.openpilotLongitudinalControl = False + # else: + # CC.cruiseControl.cancel = True + # self.CP.openpilotLongitudinalControl = True if use_clu11: if CC.cruiseControl.cancel: @@ -221,3 +250,26 @@ class CarController: # can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.SET_DECEL, self.CP.carFingerprint)] * 25) return can_sends + +def get_set_speed(self, hud_v_cruise): + v_cruise_kph = min(hud_v_cruise * CV.MS_TO_KPH, V_CRUISE_MAX) + v_cruise = int(round(v_cruise_kph * CV.KPH_TO_MPH)) + + v_cruise_slc: int = 0 + v_cruise_slc = params_memory.get_int("CSLCSpeed") + + if v_cruise_slc > 0: + v_cruise = v_cruise_slc + return v_cruise + +def get_cslc_button(self, cslcSetSpeed, CS): + cruiseBtn = Buttons.NONE + speedSetPoint = int(round(CS.out.cruiseState.speed * CV.MS_TO_MPH)) + + if cslcSetSpeed < speedSetPoint and speedSetPoint > 25: + cruiseBtn = Buttons.SET_DECEL + elif cslcSetSpeed > speedSetPoint: + cruiseBtn = Buttons.RES_ACCEL + else: + cruiseBtn = Buttons.NONE + return cruiseBtn diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index a368c87..639870a 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -116,7 +116,8 @@ def create_acc_cancel(packer, CP, CAN, cruise_info_copy): def create_lfahda_cluster(packer, CAN, enabled): values = { "HDA_ICON": 1 if enabled else 0, - "LFA_ICON": 2 if enabled else 0, + "LFA_ICON": 0 if enabled else 0, + # "LFA_ICON": 2 if enabled else 0, } return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 6aa2cc2..0a85483 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -9,6 +9,9 @@ from openpilot.selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR 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 +from openpilot.common.params import Params + +params_memory = Params("/dev/shm/params") Ecu = car.CarParams.Ecu SafetyModel = car.CarParams.SafetyModel @@ -35,6 +38,7 @@ class CarInterface(CarInterfaceBase): def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "hyundai" ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None + params_memory.put_bool("CSLCAvailable", True) # These cars have been put into dashcam only due to both a lack of users and test coverage. # These cars likely still work fine. Once a user confirms each car works and a test route is @@ -50,6 +54,7 @@ class CarInterface(CarInterfaceBase): if hda2: if 0x110 in fingerprint[CAN.CAM]: ret.flags |= HyundaiFlags.CANFD_HDA2_ALT_STEERING.value + params_memory.put_bool("CSLCAvailable", False) else: # non-HDA2 if 0x1cf not in fingerprint[CAN.ECAN]: @@ -315,6 +320,7 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC if ret.openpilotLongitudinalControl: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG + params_memory.put_bool("CSLCAvailable", False) if candidate in HYBRID_CAR: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_HYBRID_GAS elif candidate in EV_CAR: diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index c9eac9b..985c986 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -604,7 +604,7 @@ class Controls: else: self.state = State.enabled self.current_alert_types.append(ET.ENABLE) - self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.conditional_experimental_mode) + self.v_cruise_helper.initialize_v_cruise(CS, self.frogpilot_variables, self.experimental_mode, self.conditional_experimental_mode) # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES @@ -1002,6 +1002,8 @@ class Controls: self.average_desired_curvature = self.params.get_bool("AverageCurvature") self.conditional_experimental_mode = self.params.get_bool("ConditionalExperimental") + self.frogpilot_variables.CSLC = self.params.get_bool("CSLCEnabled") + self.frogpilot_variables.CSLCA = self.params.get_bool("CSLCAvailable") custom_theme = self.params.get_bool("CustomTheme") custom_sounds = self.params.get_int("CustomSounds") if custom_theme else 0 diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index d6186be..99ca671 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -56,7 +56,7 @@ class VCruiseHelper: self.v_cruise_kph_last = self.v_cruise_kph if CS.cruiseState.available: - if not self.CP.pcmCruise: + if not self.CP.pcmCruise or frogpilot_variables.CSLC: # if stock cruise is completely disabled, then we can use our own set speed logic self._update_v_cruise_non_pcm(CS, enabled, is_metric, reverse_cruise_increase, set_speed_offset) self.v_cruise_cluster_kph = self.v_cruise_kph @@ -134,9 +134,14 @@ class VCruiseHelper: self.button_timers[b.type.raw] = 1 if b.pressed else 0 self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} - def initialize_v_cruise(self, CS, experimental_mode: bool, conditional_experimental_mode) -> None: + def initialize_v_cruise(self, CS, frogpilot_variables, experimental_mode: bool, conditional_experimental_mode) -> None: # initializing is handled by the PCM - if self.CP.pcmCruise: + if self.CP.pcmCruise and not frogpilot_variables.CSLC: + return + + if frogpilot_variables.CSLC: + self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH + self.v_cruise_cluster_kph = self.v_cruise_kph return initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode and not conditional_experimental_mode else V_CRUISE_INITIAL diff --git a/selfdrive/frogpilot/assets/custom_themes/frog_theme/images/button_settings.png b/selfdrive/frogpilot/assets/custom_themes/frog_theme/images/button_settings.png index 55927599758a9b4744392bdeafe60b526cd4fa9c..549e2bedb87062904bea1a8a7e8be3138d0a25f1 100644 GIT binary patch delta 1819 zcmV+$2juwM6}AqLFne(cBLv_MX)on;I z_8f|xmd;Ygs&PnfkJm4snYVX8%=?zcqpxuy@bZQEaQD-EI(Zx@|9(iP@(w`9fko2O zEl`7yVtGk2ZGYqSeJ=eTH4Q5%X7S^ZBJFWt)_(~pX7SQ#LjN96*N{R%0g7}4ET~S| z2c*&oLI3LrEY$P?Qj~e1(pv-^P|uKJHs4#8#^ucId?1ng3rGGrwt3$w3d)V2FzOx?$cWJ--U7?j3$&c5E{~Cr!b^gFZBl$kcL3pS~4mLs5@N1#>y(P zfYbw9a}{+Ug_^DtG6bZ)t<4Y&ZwImd|!;svEr8BjRnKKTgxoTPx&^aSHpw_UbQ*Z$( zC4VtBfk*|lrcIQXG`t3o#)93dq^5uhNE<#TExY2Lv~LMQ&$hM9k*BY5l$5JhtyDB; zeA@CM5(IS&NV$O8gh&9uRZ}TD8w0Bv(&PiotyMHfO(#$_pXk>IX^Ee*7CK0r_1emE zdsflAQfeTj{<<2eQc0A0S50#^9OiZueSZW^4Wva6l4yWb(Rft_q*-OsC#{Mg)i737 z0clYcl~$mX8GdLCBQ*gejj8At0aE2kdU9{)lPDFCqTB~mNobXVu937WRX}=jHD$p* zRwkv?r&PIDQtEAr)If?QDCdcr0M+t!6E%>YQ&ea%Uu+zgqNKj}tfVRdt%9?z(tii3 z<9^PzsKnAu(%5$tEoEm0a4ENr0cpy_N@78uQ|bo+ zRMo_Z^n~^dL5hkh8h6I>H;$6dYDz8nt#_U{4^V5{ND5HzB0w68ZL5;XRZ&1%?}-p^ znX6W_!8OOHDS!e}ag^#9kP1UhP=9;0sZR8>S{dwM-ja%%3+O;^Y6?hMBy@Z+AniCgG1FR<`yN8mYSTf;SKUf#gMX?WRONu$ z6sm_*_0KjAaF%okNUf8ae*jnWVdr^tp`5 zIu}g$dO#Y8UUM~7@{W8CjG=o0>DCfftIYtYXfP9iv?QdySBt5T15!YmREDU>`$B7# zoI@pm6p&Vdl(0@oe)k`c0)J92q@;-xak5jgEv_FLu0M$U%En$Ix3vt#QNNOH?InIC z?7T8JfHV@w=ts~vfX;n{Kvnd8`x0DUv;j!BL+aa7wRfd_vF-wTr)%soMe;C1niBBs z^y<690{iE$`F8)ye9ZCA!ut4;d42c8yw7Pz`@Qo+{ZQ`hU+Rlx+J7Sj|KuVt4UY~d z3os^6<$C40)6esiUfZ zbw9wgv~+dIoBDcpcDeU?xqROo6Regj^wpPJ(r3#O;hb;(4;1rcVc(bxd@KL}002ov JPDHLkV1f}wbbkN< delta 2759 zcmV;&3OMz)4%-!wFn;Ug0l>WgP&LpcH)$7vJ^*|R0J{M24G?{SrP7ysL-??}()>JaQ80}L?|bu+ zBX@5fT}Gm30kQ5AGL5FWjrl`h7l@jK`0k-vZ{zy;zuihYghV72CpG@_zwu?@T%a2(&BZ!INKK_~?36 z+mR&o3Hli;M+T;cn;ap~t&|79j3R0%Nxd!p_R&vy1!Xo_K%iZ%^&Sq}`a?;&u)6p@ zpFu554|lPT0MUA9W%KDEI08w^T0Ao+3k1Tn{uz?`cF8{&W1O#paQ z61S~ttyd2+SbH~ngYqS|OCLm%0DisGXua=|lnwWRGz%rwsI&-^DIr~x2#M(r2~S6b zR%no3^4pl>V!nZYpQLL`H#y`Z_$!W^;)T`4cL4FZ$Z|wiIxCyc$F;>f+6+Rwokr_8 z`hTmz-{Sx*+W(dE;D>SEIRX>k3Fl(~_)!Q(+m-U*afP=2Q#7oj#yceZ1+rEj43Pjw zHu5pTol(jaZR+^I3&zBh2rVC_wyOeu+M`3e%Fw;-nrKO(?L+Xf~0pP?0jj?MgH50aKJS!Jlm{i5bg_-l4;;`(Ec_eUKY8E zN&qJShGzirOo(kbHs;3Ucw*wp#1-vTfYQ|=zzr|o7SUPPk84FB=~ksYI5Tah6n|vs z?^RzwAaSiJ5~_VefLqma|1BAtrfH(e%2tF}G}lR*>8faw$N^_rXQO(ZCTSKLx-DW( z_v5lB?o$9U z4^23`k_vFMxPQ-TQeD3HcbcU8M1S26K`=!g;g5?5zwzy@#{uBSeqX{T;-;{@62V_& z-Trjs^fmjduHGCvPf`Hr1L3l2cHua(_$!x41RLh;Jl#a1l-yzR`0jlht`by?y9gu| zI)50+SCr77 z!jLrS+0=x2C#fyhcvB#U#oRe4}n4}VuH%CQcJ(xnoy;qwilU_$Fm1)I2+_W-leCYBFO3pksS?!YcADhO>=RYIPUz(0 z!Y-1u3jqI0!-3hdS5cyC+2|f&`>D+9YO;u$)ab)xNTTTnwdseJ{8QmpOcoxi<(wdClJi{nbz=rk1R01@ zwRx^K#at&yn&vh+0^0IGsvN*P+O;#sJ+G(_6-$+#jE`Ot>8Qo5!bxKv% zxbpKJw$3CCnc<9*X7oFa)}c7kss7I-RYlA|c|+TM7U_plUrBkpH$XU@CXFag*+gEZ zekNtbNrzGC*Ae&M*V8sZWs;LAW$m6#Q0bE`X^W{E^XJ`8%EtVpbTV5%m21%~&T>i4 z)_g5$mQ*HNPvl@?*?(55eI8{86O$CO)V8jxo=v4gSNUUCJxe6A(2cg!bDsT44Lf98 zm=?J%oG|9g3{`ifvOomw56I?9FN3kEGE7nlWKC-NNj#sJq!N=iPeqc3H0Lo|ltY^5 ziGlI5YR3>wHl4S)dm)>pGjlw`21^uSyH7REPVO?8tkxF+iGS1V82m^~H{BC8n=kK06VqAs_KCW976IRum3yAbxxC$L z-{8#C1Ww9o`G(9IOwN)tX`Ur(Gmv6OCM*ysil^NalT;hJ{8eO<=5J2oB;xHp4xu^u z-`SN^;IDfXI)5i~cR);+Z=akdDFE=Zjq~`$P08#!NyTuTegM~Z&i=rKbUI<*QxQli z#?DWP&+&>XNP5@Rl!4mwl5#+tiX52)0ICRRzieNSG>iCh`$JXiF-g3UVprqhS;ST8 z%4)G*SCCXZiY5CNkww|E?J5h)fXfe;c9)C^Hh?7-q-1|=h=kon~0+MXu z!s_BXfcRX3Mx3mwvyyr%A$fZA!Q(PSWyNWBF2rKt<@H=xTe=AV z$5a^ZdVg8j?Oy7*M0BOIviW=*X1v|cAhg?Qw2tqSbTxz2+i^3N{r_9lTCX0JhhHJw zIW}zBPcGJEC3a%seS5V>QvQNNz6hfAN@ed{&{nNTDv|l`S{UiQZGKR`1c*;%|83?+ zg3)f~k<NfGK@AZilgyVdgG8dmSzxPN>5=rRJltZ`r7CV*EC)p{G$KYXIU zS2>MD&jMndk6SY{)~(ih_luouoNJ!7{1j=;O(jNPGJn^Yq`aCY#+4gL4}tM2Dw;g- zY6stVq=z!Mq>sS#ZQsn(GOk7(>O;mgEh8`!_A^V;{frnb%Yt!tjKDqugiD>3=H?_E zfq##5xf*rIJjgc!VPWW@FUhfiKi?DToOBV6=Ak6@wf#>uY=O?<#{)8sK)YJ&Jsfts z7x^Zq@9^*$8~Ke-Qjt4|jp&ViIJOAUh(%^d#$%UqWsD`M7=*X=-@4~LzFW?CN?q!n zXuW#myJw;tct*pOBI&3wYyB>Zw3Ym_xLx+3L8i6(to}adZ#tEfN z(*LijUeeZAS*ePHnEw~)jtJMftP8Sj1s2twC zBy;u*Ln9GMM~w=_%JZ>g-$>_KCo#aW|E=(wO+rpB#YsXyuZRe%D3{;~8V(--#6SlD zK;)7aF;w?*N*`Gib6rkE8hi@?yW(8{Ao>DJr7!n}@L_kQ`FYxs{|~LlzmS>`ua5u# N002ovPDHLkV1h$uS9kyb diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 67654ff..be00581 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -56,6 +56,7 @@ def calculate_lane_width(lane, current_lane, road_edge): class FrogPilotPlanner: def __init__(self, params, params_memory): + self.params_memory = params_memory self.cem = ConditionalExperimentalMode() self.mtsc = MapTurnSpeedController() @@ -105,6 +106,8 @@ class FrogPilotPlanner: self.x_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, mpc.x_solution) self.x_desired_trajectory = self.x_desired_trajectory_full[:CONTROL_N] + self.params_memory.put_int("CSLCSpeed", int(round(self.v_cruise * CV.MS_TO_MPH))) + def update_v_cruise(self, carState, controlsState, modelData, enabled, v_cruise, v_ego): # Pfeiferj's Map Turn Speed Controller if self.map_turn_speed_controller: @@ -168,7 +171,10 @@ class FrogPilotPlanner: else: self.vtsc_target = v_cruise - v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0) + if self.CSLC: + v_ego_diff = 0 + else: + v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0) return min(v_cruise, self.mtsc_target, self.slc_target, self.vtsc_target) - v_ego_diff def publish_lateral(self, sm, pm, DH): @@ -206,6 +212,8 @@ class FrogPilotPlanner: def update_frogpilot_params(self, params, params_memory): self.is_metric = params.get_bool("IsMetric") + self.CSLC = params.get_bool("CSLCEnabled") + self.blindspot_path = params.get_bool("CustomUI") and params.get_bool("BlindSpotPath") self.conditional_experimental_mode = params.get_bool("ConditionalExperimental") diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index fbb93a1..686c040 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -12,7 +12,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"CESlowerLead", "Slower Lead Detected Ahead", "Switch to 'Experimental Mode' when a slower lead vehicle is detected ahead.", ""}, {"CEStopLights", "Stop Lights and Stop Signs", "Switch to 'Experimental Mode' when a stop light or stop sign is detected.", ""}, {"CESignal", "Turn Signal When Below Highway Speeds", "Switch to 'Experimental Mode' when using turn signals below highway speeds to help assit with turns.", ""}, - + {"CSLCEnabled", "Custom Stock Longitudinal Control", "Use cruise control button spamming to adjust cruise set speed based on MTSC, VTSC, and SLC", "../frogpilot/assets/toggle_icons/icon_custom.png"}, {"CustomPersonalities", "Custom Driving Personalities", "Customize the driving personality profiles to your driving style.", "../frogpilot/assets/toggle_icons/icon_custom.png"}, {"DeviceShutdown", "Device Shutdown Timer", "Configure the timer for automatic device shutdown when offroad conserving energy and preventing battery drain.", "../frogpilot/assets/toggle_icons/icon_time.png"}, {"ExperimentalModeActivation", "Experimental Mode Via", "Toggle Experimental Mode by double-clicking the 'Lane Departure'/'LKAS' button or double tapping screen.\n\nOverrides 'Conditional Experimental Mode'.", "../assets/img_experimental_white.svg"},