diff --git a/cereal/custom.capnp b/cereal/custom.capnp index bffc1e4d3b..9c83b0f6fc 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -83,12 +83,17 @@ struct ModelManagerSP @0xaedffd8f31e7b55d { } struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { - mpcSource @0 :MpcSource; - dynamicExperimentalControl @1 :Bool; - - enum MpcSource { - acc @0; - blended @1; + dec @0 :DynamicExperimentalControl; + + struct DynamicExperimentalControl { + state @0 :DynamicExperimentalControlState; + enabled @1 :Bool; + active @2 :Bool; + + enum DynamicExperimentalControlState { + acc @0; + blended @1; + } } } diff --git a/cereal/log.capnp b/cereal/log.capnp index 80a5034392..eea8c8ba45 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -198,6 +198,7 @@ struct OnroadEvent @0xc4fa6047f024e718 { silentSeatbeltNotLatched @161; silentParkBrake @162; controlsMismatchLateral @163; + hyundaiRadarTracksConfirmed @164; soundsUnavailableDEPRECATED @47; } diff --git a/common/params.cc b/common/params.cc index 3c11e034cb..e8e05ff6b2 100644 --- a/common/params.cc +++ b/common/params.cc @@ -224,6 +224,12 @@ std::unordered_map keys = { {"SunnylinkdPid", PERSISTENT}, {"SunnylinkEnabled", PERSISTENT}, + // sunnypilot car specific params + {"HyundaiRadarTracks", PERSISTENT}, + {"HyundaiRadarTracksConfirmed", PERSISTENT}, + {"HyundaiRadarTracksPersistent", PERSISTENT}, + {"HyundaiRadarTracksToggle", PERSISTENT}, + {"DynamicExperimentalControl", PERSISTENT}, }; diff --git a/opendbc_repo b/opendbc_repo index 6ddebc9a03..6a2ad131eb 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 6ddebc9a0365d3421bee7cceddb0263c88fb78dd +Subproject commit 6a2ad131ebfe7a15dca5aae391ef5225189c3c2d diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 1852dccf74..e093b5e0d0 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -23,6 +23,7 @@ from openpilot.selfdrive.car.cruise import VCruiseHelper from openpilot.selfdrive.car.car_specific import MockCarState from openpilot.sunnypilot.mads.mads import MadsParams +from openpilot.sunnypilot.selfdrive.car.interfaces import setup_car_interface_sp, initialize_car_interface_sp REPLAY = "REPLAY" in os.environ @@ -100,6 +101,7 @@ class Car: cached_params = _cached_params self.CI = get_car(*self.can_callbacks, obd_callback(self.params), experimental_long_allowed, num_pandas, cached_params) + setup_car_interface_sp(self.CI.CP, self.params) self.RI = get_radar_interface(self.CI.CP) self.CP = self.CI.CP @@ -230,6 +232,7 @@ class Car: # Initialize CarInterface, once controls are ready # TODO: this can make us miss at least a few cycles when doing an ECU knockout self.CI.init(self.CP, *self.can_callbacks) + initialize_car_interface_sp(self.CP, self.params, *self.can_callbacks) # signal pandad to switch to car safety mode self.params.put_bool_nonblocking("ControlsReady", True) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 2272ebfeb8..c15a65bf49 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -16,7 +16,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_ from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog -from openpilot.sunnypilot.selfdrive.controls.lib.dec.helpers import DecPlanner +from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MIN = -1.2 @@ -69,11 +69,11 @@ def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05): return a_target, should_stop -class LongitudinalPlanner(DecPlanner): +class LongitudinalPlanner(LongitudinalPlannerSP): def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP self.mpc = LongitudinalMpc(dt=dt) - DecPlanner.__init__(self, self.CP, self.mpc) + LongitudinalPlannerSP.__init__(self, self.CP, self.mpc) self.fcw = False self.dt = dt self.allow_throttle = True @@ -108,9 +108,9 @@ class LongitudinalPlanner(DecPlanner): return x, v, a, j, throttle_prob def update(self, sm): - DecPlanner.update(self, sm) + LongitudinalPlannerSP.update(self, sm) self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' - if dec_mpc_mode := self.get_mpc_mode(sm): + if dec_mpc_mode := self.get_mpc_mode(): self.mpc.mode = dec_mpc_mode if len(sm['carControl'].orientationNED) == 3: @@ -212,4 +212,5 @@ class LongitudinalPlanner(DecPlanner): longitudinalPlan.allowThrottle = self.allow_throttle pm.send('longitudinalPlan', plan_send) + self.publish_longitudinal_plan_sp(sm, pm) diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 672d809dd9..481a33079b 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -1061,6 +1061,9 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { ET.NO_ENTRY: NoEntryAlert("Controls Mismatch: Lateral"), }, + EventName.hyundaiRadarTracksConfirmed: { + ET.PERMANENT: NormalPermanentAlert("Radar tracks available. Restart the car to initialize") + } } diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 8328bf67f0..759682f44f 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -24,6 +24,7 @@ from openpilot.selfdrive.controls.lib.latcontrol import MIN_LATERAL_CONTROL_SPEE from openpilot.system.version import get_build_metadata from openpilot.sunnypilot.mads.mads import ModularAssistiveDrivingSystem +from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP REPLAY = "REPLAY" in os.environ SIMULATION = "SIMULATION" in os.environ @@ -137,6 +138,8 @@ class SelfdriveD: sock_services = list(self.pm.sock.keys()) + ['selfdriveStateSP'] self.pm = messaging.PubMaster(sock_services) + self.car_events_sp = CarSpecificEventsSP(self.CP, self.params) + def update_events(self, CS): """Compute onroadEvents from carState""" @@ -177,6 +180,9 @@ class SelfdriveD: car_events = self.car_events.update(CS, self.CS_prev, self.sm['carControl']).to_msg() self.events.add_from_msg(car_events) + car_events_sp = self.car_events_sp.update().to_msg() + self.events.add_from_msg(car_events_sp) + if self.CP.notCar: # wait for everything to init first if self.sm.frame > int(5. / DT_CTRL) and self.initialized: @@ -495,6 +501,7 @@ class SelfdriveD: self.personality = self.read_personality_param() self.mads.read_params() + self.car_events_sp.read_params() time.sleep(0.1) def run(self): diff --git a/selfdrive/ui/qt/offroad/developer_panel.cc b/selfdrive/ui/qt/offroad/developer_panel.cc index eee48a6bdb..1a362f62c6 100644 --- a/selfdrive/ui/qt/offroad/developer_panel.cc +++ b/selfdrive/ui/qt/offroad/developer_panel.cc @@ -29,6 +29,18 @@ DeveloperPanel::DeveloperPanel(SettingsWindow *parent) : ListWidget(parent) { }); addItem(longManeuverToggle); + // TODO-SP: Move to Vehicles panel when ported back + hyundaiRadarTracksToggle = new ParamControl( + "HyundaiRadarTracksToggle", + tr("Hyundai: Enable Radar Tracks"), + tr("Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. " + "This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance."), ""); + hyundaiRadarTracksToggle->setConfirmation(true, false); + QObject::connect(hyundaiRadarTracksToggle, &ParamControl::toggleFlipped, [=](bool state) { + updateToggles(offroad); + }); + addItem(hyundaiRadarTracksToggle); + auto enableGithubRunner = new ParamControl("EnableGithubRunner", tr("Enable GitHub runner service"), tr("Enables or disables the github runner service."), ""); addItem(enableGithubRunner); @@ -51,9 +63,15 @@ void DeveloperPanel::updateToggles(bool _offroad) { AlignedBuffer aligned_buf; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); cereal::CarParams::Reader CP = cmsg.getRoot(); + + auto hyundai = CP.getCarName() == "hyundai"; + auto hyundai_mando_radar = hyundai && (CP.getFlags() & 4096); + longManeuverToggle->setEnabled(hasLongitudinalControl(CP) && _offroad); + hyundaiRadarTracksToggle->setVisible(hyundai_mando_radar && hasLongitudinalControl(CP)); } else { longManeuverToggle->setEnabled(false); + hyundaiRadarTracksToggle->setVisible(false); } offroad = _offroad; diff --git a/selfdrive/ui/qt/offroad/developer_panel.h b/selfdrive/ui/qt/offroad/developer_panel.h index 0351cd045c..5dc5a5c494 100644 --- a/selfdrive/ui/qt/offroad/developer_panel.h +++ b/selfdrive/ui/qt/offroad/developer_panel.h @@ -16,6 +16,7 @@ private: Params params; ParamControl* joystickToggle; ParamControl* longManeuverToggle; + ParamControl* hyundaiRadarTracksToggle; bool is_release; bool offroad; diff --git a/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts index 4f7a2222bd..2599845376 100644 --- a/selfdrive/ui/translations/main_ar.ts +++ b/selfdrive/ui/translations/main_ar.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index 6eaab99989..dbedef9326 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_es.ts b/selfdrive/ui/translations/main_es.ts index 58fb2696c6..017df12e08 100644 --- a/selfdrive/ui/translations/main_es.ts +++ b/selfdrive/ui/translations/main_es.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_fr.ts b/selfdrive/ui/translations/main_fr.ts index b9be52acd8..0cbf515c5d 100644 --- a/selfdrive/ui/translations/main_fr.ts +++ b/selfdrive/ui/translations/main_fr.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index 4f757d5f0c..529d9e3209 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index ebc217e556..6ef5a3a2ae 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index 4d24c47d48..879a95e307 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts index 08ce8b7647..94dd75473e 100644 --- a/selfdrive/ui/translations/main_th.ts +++ b/selfdrive/ui/translations/main_th.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_tr.ts b/selfdrive/ui/translations/main_tr.ts index 0037777958..9bfdb2fb21 100644 --- a/selfdrive/ui/translations/main_tr.ts +++ b/selfdrive/ui/translations/main_tr.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index 517ad5fd1d..ba3841ddeb 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index 6f82c97e9f..6a48443d69 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/sunnypilot/selfdrive/car/car_specific.py b/sunnypilot/selfdrive/car/car_specific.py new file mode 100644 index 0000000000..5abb5603d6 --- /dev/null +++ b/sunnypilot/selfdrive/car/car_specific.py @@ -0,0 +1,34 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +from cereal import log +from opendbc.car import structs + +from openpilot.selfdrive.selfdrived.events import Events + +EventName = log.OnroadEvent.EventName + + +class CarSpecificEventsSP: + def __init__(self, CP: structs.CarParams, params): + self.CP = CP + self.params = params + + self.hyundai_radar_tracks = self.params.get_bool("HyundaiRadarTracks") + self.hyundai_radar_tracks_confirmed = self.params.get_bool("HyundaiRadarTracksConfirmed") + + def read_params(self): + self.hyundai_radar_tracks = self.params.get_bool("HyundaiRadarTracks") + self.hyundai_radar_tracks_confirmed = self.params.get_bool("HyundaiRadarTracksConfirmed") + + def update(self): + events = Events() + if self.CP.carName == 'hyundai': + if self.hyundai_radar_tracks and not self.hyundai_radar_tracks_confirmed: + events.add(EventName.hyundaiRadarTracksConfirmed) + + return events diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py new file mode 100644 index 0000000000..a92f8f60be --- /dev/null +++ b/sunnypilot/selfdrive/car/interfaces.py @@ -0,0 +1,41 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +from opendbc.car import Bus, structs +from opendbc.car.can_definitions import CanRecvCallable, CanSendCallable +from opendbc.car.car_helpers import can_fingerprint +from opendbc.car.hyundai.radar_interface import RADAR_START_ADDR +from opendbc.car.hyundai.values import HyundaiFlags, DBC as HYUNDAI_DBC +from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP + + +def setup_car_interface_sp(CP: structs.CarParams, params): + if CP.carName == 'hyundai': + if CP.flags & HyundaiFlags.MANDO_RADAR and CP.radarUnavailable: + # Having this automatic without a toggle causes a weird process replay diff because + # somehow it sees fewer logs than intended + if params.get_bool("HyundaiRadarTracksToggle"): + CP.sunnypilotFlags |= HyundaiFlagsSP.ENABLE_RADAR_TRACKS.value + if params.get_bool("HyundaiRadarTracks"): + CP.radarUnavailable = False + + +def initialize_car_interface_sp(CP: structs.CarParams, params, can_recv: CanRecvCallable, can_send: CanSendCallable): + if CP.carName == 'hyundai': + if CP.sunnypilotFlags & HyundaiFlagsSP.ENABLE_RADAR_TRACKS: + can_recv() + _, fingerprint = can_fingerprint(can_recv) + radar_unavailable = RADAR_START_ADDR not in fingerprint[1] or Bus.radar not in HYUNDAI_DBC[CP.carFingerprint] + + radar_tracks = params.get_bool("HyundaiRadarTracks") + radar_tracks_persistent = params.get_bool("HyundaiRadarTracksPersistent") + + params.put_bool_nonblocking("HyundaiRadarTracksConfirmed", radar_tracks) + + if not radar_tracks_persistent: + params.put_bool_nonblocking("HyundaiRadarTracks", not radar_unavailable) + params.put_bool_nonblocking("HyundaiRadarTracksPersistent", True) diff --git a/sunnypilot/selfdrive/controls/lib/dec/constants.py b/sunnypilot/selfdrive/controls/lib/dec/constants.py new file mode 100644 index 0000000000..1922ab85db --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/dec/constants.py @@ -0,0 +1,25 @@ +class WMACConstants: + LEAD_WINDOW_SIZE = 4 + LEAD_PROB = 0.6 + + SLOW_DOWN_WINDOW_SIZE = 4 + SLOW_DOWN_PROB = 0.6 + + SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.] + SLOW_DOWN_DIST = [25., 38., 55., 75., 95., 115., 130., 150.] + + SLOWNESS_WINDOW_SIZE = 12 + SLOWNESS_PROB = 0.5 + SLOWNESS_CRUISE_OFFSET = 1.05 + + DANGEROUS_TTC_WINDOW_SIZE = 3 + DANGEROUS_TTC = 2.3 + + MPC_FCW_WINDOW_SIZE = 10 + MPC_FCW_PROB = 0.5 + + +class SNG_State: + off = 0 + stopped = 1 + going = 2 diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index eb7660965e..f7bc028ee4 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -25,47 +25,24 @@ import numpy as np from cereal import messaging +from opendbc.car import structs from openpilot.common.numpy_fast import interp from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL +from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants, SNG_State # d-e2e, from modeldata.h TRAJECTORY_SIZE = 33 -LEAD_WINDOW_SIZE = 4 -LEAD_PROB = 0.6 - -SLOW_DOWN_WINDOW_SIZE = 4 -SLOW_DOWN_PROB = 0.6 - -SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.] -SLOW_DOWN_DIST = [25., 38., 55., 75., 95., 115., 130., 150.] - -SLOWNESS_WINDOW_SIZE = 12 -SLOWNESS_PROB = 0.5 -SLOWNESS_CRUISE_OFFSET = 1.05 - -DANGEROUS_TTC_WINDOW_SIZE = 3 -DANGEROUS_TTC = 2.3 - HIGHWAY_CRUISE_KPH = 70 STOP_AND_GO_FRAME = 60 SET_MODE_TIMEOUT = 10 -MPC_FCW_WINDOW_SIZE = 10 -MPC_FCW_PROB = 0.5 - V_ACC_MIN = 9.72 -class SNG_State: - off = 0 - stopped = 1 - going = 2 - - class GenericMovingAverageCalculator: def __init__(self, window_size): self.window_size = window_size @@ -109,31 +86,32 @@ class WeightedMovingAverageCalculator: class DynamicExperimentalController: - def __init__(self, params=None): + def __init__(self, CP: structs.CarParams, mpc, params=None): + self._CP = CP + self._mpc = mpc self._params = params or Params() - self._is_enabled: bool = self._params.get_bool("DynamicExperimentalControl") + self._enabled: bool = self._params.get_bool("DynamicExperimentalControl") + self._active: bool = False self._mode: str = 'acc' - self._mode_prev: str = 'acc' - self._mode_changed: bool = False self._frame: int = 0 # Use weighted moving average for filtering leads - self._lead_gmac = WeightedMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE) + self._lead_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.LEAD_WINDOW_SIZE) self._has_lead_filtered = False self._has_lead_filtered_prev = False - self._slow_down_gmac = WeightedMovingAverageCalculator(window_size=SLOW_DOWN_WINDOW_SIZE) - self._has_slow_down = False + self._slow_down_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.SLOW_DOWN_WINDOW_SIZE) + self._has_slow_down: bool = False self._has_blinkers = False - self._slowness_gmac = WeightedMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE) - self._has_slowness = False + self._slowness_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.SLOWNESS_WINDOW_SIZE) + self._has_slowness: bool = False self._has_nav_instruction = False - self._dangerous_ttc_gmac = WeightedMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE) - self._has_dangerous_ttc = False + self._dangerous_ttc_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.DANGEROUS_TTC_WINDOW_SIZE) + self._has_dangerous_ttc: bool = False self._v_ego_kph = 0. self._v_cruise_kph = 0. @@ -146,12 +124,25 @@ class DynamicExperimentalController: self._sng_transit_frame = 0 self._sng_state = SNG_State.off - self._mpc_fcw_gmac = WeightedMovingAverageCalculator(window_size=MPC_FCW_WINDOW_SIZE) - self._has_mpc_fcw = False + self._mpc_fcw_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.MPC_FCW_WINDOW_SIZE) + self._has_mpc_fcw: bool = False self._mpc_fcw_crash_cnt = 0 self._set_mode_timeout = 0 + def _read_params(self) -> None: + if self._frame % int(1. / DT_MDL) == 0: + self._enabled = self._params.get_bool("DynamicExperimentalControl") + + def mode(self) -> str: + return str(self._mode) + + def enabled(self) -> bool: + return self._enabled + + def active(self) -> bool: + return self._active + @staticmethod def _anomaly_detection(recent_data: list[float], threshold: float = 2.0, context_check: bool = True) -> bool: """ @@ -165,34 +156,35 @@ class DynamicExperimentalController: # Context check to ensure repeated anomaly if context_check: - return bool(np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1) + return np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1 return anomaly def _adaptive_slowdown_threshold(self) -> float: """ Adapts the slow-down threshold based on vehicle speed and recent behavior. """ + slowdown_scaling_factor: float = (1.0 + 0.05 * np.log(1 + len(self._slow_down_gmac.data))) adaptive_threshold: float = float( - interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.05 * np.log(1 + len(self._slow_down_gmac.data))) + interp(self._v_ego_kph, WMACConstants.SLOW_DOWN_BP, WMACConstants.SLOW_DOWN_DIST) * slowdown_scaling_factor ) return adaptive_threshold - def _smoothed_lead_detection(self, lead_prob: float, smoothing_factor: float = 0.2) -> bool: + def _smoothed_lead_detection(self, lead_prob: float, smoothing_factor: float = 0.2): """ Smoothing the lead detection to avoid erratic behavior. """ self._has_lead_filtered = (1 - smoothing_factor) * self._has_lead_filtered + smoothing_factor * lead_prob - return bool(self._has_lead_filtered > LEAD_PROB) + return self._has_lead_filtered > WMACConstants.LEAD_PROB def _adaptive_lead_prob_threshold(self) -> float: """ Adapts lead probability threshold based on driving conditions. """ if self._v_ego_kph > HIGHWAY_CRUISE_KPH: - return float(LEAD_PROB + 0.1) # Increase the threshold on highways - return float(LEAD_PROB) + return float(WMACConstants.LEAD_PROB + 0.1) # Increase the threshold on highways + return float(WMACConstants.LEAD_PROB) - def _update(self, sm: messaging.SubMaster) -> None: + def _update_calculations(self, sm: messaging.SubMaster) -> None: car_state = sm['carState'] lead_one = sm['radarState'].leadOne md = sm['modelV2'] @@ -204,14 +196,14 @@ class DynamicExperimentalController: # fcw detection self._mpc_fcw_gmac.add_data(self._mpc_fcw_crash_cnt > 0) - self._has_mpc_fcw = self._mpc_fcw_gmac.get_weighted_average() > MPC_FCW_PROB + self._has_mpc_fcw = self._mpc_fcw_gmac.get_weighted_average() > WMACConstants.MPC_FCW_PROB # nav enable detection # self._has_nav_instruction = md.navEnabledDEPRECATED and maneuver_distance / max(car_state.vEgo, 1) < 13 # lead detection with smoothing self._lead_gmac.add_data(lead_one.status) - #self._has_lead_filtered = self._lead_gmac.get_weighted_average() > LEAD_PROB + #self._has_lead_filtered = self._lead_gmac.get_weighted_average() > WMACConstants.LEAD_PROB lead_prob = self._lead_gmac.get_weighted_average() or 0 self._has_lead_filtered = self._smoothed_lead_detection(lead_prob) @@ -219,7 +211,7 @@ class DynamicExperimentalController: adaptive_threshold = self._adaptive_slowdown_threshold() slow_down_trigger = len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE and md.position.x[TRAJECTORY_SIZE - 1] < adaptive_threshold self._slow_down_gmac.add_data(slow_down_trigger) - self._has_slow_down = self._slow_down_gmac.get_weighted_average() > SLOW_DOWN_PROB + self._has_slow_down = self._slow_down_gmac.get_weighted_average() > WMACConstants.SLOW_DOWN_PROB # anomaly detection for slow down events if self._anomaly_detection(self._slow_down_gmac.data): @@ -245,8 +237,8 @@ class DynamicExperimentalController: # slowness detection if not self._has_standstill: - self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph * SLOWNESS_CRUISE_OFFSET)) - self._has_slowness = self._slowness_gmac.get_weighted_average() > SLOWNESS_PROB + self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET)) + self._has_slowness = self._slowness_gmac.get_weighted_average() > WMACConstants.SLOWNESS_PROB # dangerous TTC detection if not self._has_lead_filtered and self._has_lead_filtered_prev: @@ -256,7 +248,7 @@ class DynamicExperimentalController: if self._has_lead and car_state.vEgo >= 0.01: self._dangerous_ttc_gmac.add_data(lead_one.dRel / car_state.vEgo) - self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_weighted_average() is not None and self._dangerous_ttc_gmac.get_weighted_average() <= DANGEROUS_TTC + self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_weighted_average() is not None and self._dangerous_ttc_gmac.get_weighted_average() <= WMACConstants.DANGEROUS_TTC # keep prev values self._has_standstill_prev = self._has_standstill @@ -354,20 +346,8 @@ class DynamicExperimentalController: self._set_mode('acc') - def get_mpc_mode(self) -> str: - return str(self._mode) - - def has_changed(self) -> bool: - return bool(self._mode_changed) - - def set_enabled(self, enabled: bool) -> None: - self._is_enabled = enabled - - def is_enabled(self) -> bool: - return self._is_enabled - - def set_mpc_fcw_crash_cnt(self, crash_cnt: float) -> None: - self._mpc_fcw_crash_cnt = crash_cnt + def set_mpc_fcw_crash_cnt(self) -> None: + self._mpc_fcw_crash_cnt = self._mpc.crash_cnt def _set_mode(self, mode: str) -> None: if self._set_mode_timeout == 0: @@ -378,22 +358,18 @@ class DynamicExperimentalController: if self._set_mode_timeout > 0: self._set_mode_timeout -= 1 - def _read_params(self) -> None: - if self._frame % int(1. / DT_MDL) == 0: - self._is_enabled = self._params.get_bool("DynamicExperimentalControl") - - def update(self, radar_unavailable: bool, sm: messaging.SubMaster) -> None: + def update(self, sm: messaging.SubMaster) -> None: self._read_params() - if self._is_enabled: - self._update(sm) + self.set_mpc_fcw_crash_cnt() - if radar_unavailable: - self._radarless_mode() - else: - self._radar_mode() + self._update_calculations(sm) - self._mode_changed = self._mode != self._mode_prev - self._mode_prev = self._mode + if self._CP.radarUnavailable: + self._radarless_mode() + else: + self._radar_mode() + + self._active = sm['selfdriveState'].experimentalMode and self._enabled self._frame += 1 diff --git a/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py index af77639424..741d9e0170 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py @@ -1,18 +1,10 @@ -from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import ( - DynamicExperimentalController, - TRAJECTORY_SIZE, - LEAD_WINDOW_SIZE, - SLOW_DOWN_WINDOW_SIZE, - DANGEROUS_TTC_WINDOW_SIZE, - MPC_FCW_WINDOW_SIZE, - SNG_State, - STOP_AND_GO_FRAME -) - import pytest import numpy as np from openpilot.common.params import Params +from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants, SNG_State +from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, TRAJECTORY_SIZE, STOP_AND_GO_FRAME + class MockInterp: def __call__(self, x, xp, fp): return np.interp(x, xp, fp) @@ -48,8 +40,7 @@ def interp(monkeypatch): def controller(interp): params = Params() params.put_bool("DynamicExperimentalControl", True) - controller = DynamicExperimentalController() - return controller + return DynamicExperimentalController() def test_initial_state(controller): """Test initial state of the controller""" @@ -94,7 +85,7 @@ def test_lead_detection(controller, has_radar): controls_state = MockControlState(v_cruise=72) # Let moving average stabilize - for _ in range(LEAD_WINDOW_SIZE + 1): + for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1): controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._has_lead_filtered @@ -103,7 +94,7 @@ def test_lead_detection(controller, has_radar): # Test lead loss detection lead_one.status = False - for _ in range(LEAD_WINDOW_SIZE + 1): + for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1): controller.update(not has_radar, car_state, lead_one, md, controls_state) assert not controller._has_lead_filtered @@ -119,7 +110,7 @@ def test_slow_down_detection(controller, has_radar): controls_state = MockControlState(v_cruise=30) # Test slow down detection - for _ in range(SLOW_DOWN_WINDOW_SIZE + 1): + for _ in range(WMACConstants.SLOW_DOWN_WINDOW_SIZE + 1): controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._has_slow_down @@ -128,7 +119,7 @@ def test_slow_down_detection(controller, has_radar): # Test slow down recovery positions = [200] * TRAJECTORY_SIZE # Position outside slow down threshold md = MockModelData(x_vals=x_vals, positions=positions) - for _ in range(SLOW_DOWN_WINDOW_SIZE + 1): + for _ in range(WMACConstants.SLOW_DOWN_WINDOW_SIZE + 1): controller.update(not has_radar, car_state, lead_one, md, controls_state) assert not controller._has_slow_down @@ -143,7 +134,7 @@ def test_dangerous_ttc_detection(controller, has_radar): # First establish normal conditions with lead lead_one.dRel = 100 # Safe distance - for _ in range(LEAD_WINDOW_SIZE + 1): # First establish lead detection + for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1): # First establish lead detection controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._has_lead_filtered # Verify lead is detected @@ -153,7 +144,7 @@ def test_dangerous_ttc_detection(controller, has_radar): # TTC = dRel/vEgo = 10/10 = 1s (which is less than DANGEROUS_TTC = 2.3s) # Need to update multiple times to allow the weighted average to stabilize - for _ in range(DANGEROUS_TTC_WINDOW_SIZE * 2): + for _ in range(WMACConstants.DANGEROUS_TTC_WINDOW_SIZE * 2): controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._has_dangerous_ttc, "TTC of 1s should be considered dangerous" @@ -171,8 +162,8 @@ def test_mode_transitions(controller, has_radar): def stabilize_filters(): """Helper to let all moving averages stabilize""" - for _ in range(max(LEAD_WINDOW_SIZE, SLOW_DOWN_WINDOW_SIZE, - DANGEROUS_TTC_WINDOW_SIZE, MPC_FCW_WINDOW_SIZE) + 1): + for _ in range(max(WMACConstants.LEAD_WINDOW_SIZE, WMACConstants.SLOW_DOWN_WINDOW_SIZE, + WMACConstants.DANGEROUS_TTC_WINDOW_SIZE, WMACConstants.MPC_FCW_WINDOW_SIZE) + 1): controller.update(not has_radar, car_state, lead_one, md, controls_state) # Test 1: Normal driving -> ACC mode @@ -198,7 +189,7 @@ def test_mode_transitions(controller, has_radar): lead_one.dRel = 50 # First establish normal lead detection # First establish lead detection - for _ in range(LEAD_WINDOW_SIZE + 1): + for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1): controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._has_lead_filtered # Verify lead is detected @@ -206,7 +197,7 @@ def test_mode_transitions(controller, has_radar): # Now create dangerous TTC condition lead_one.dRel = 20 # This creates a TTC of 1s, well below DANGEROUS_TTC - for _ in range(DANGEROUS_TTC_WINDOW_SIZE * 2): + for _ in range(WMACConstants.DANGEROUS_TTC_WINDOW_SIZE * 2): controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._has_dangerous_ttc, "Should detect dangerous TTC condition" @@ -223,7 +214,7 @@ def test_mpc_fcw_handling(controller, has_radar): # Test FCW activation controller.set_mpc_fcw_crash_cnt(5) - for _ in range(MPC_FCW_WINDOW_SIZE + 1): + for _ in range(WMACConstants.MPC_FCW_WINDOW_SIZE + 1): controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._has_mpc_fcw @@ -231,7 +222,7 @@ def test_mpc_fcw_handling(controller, has_radar): # Test FCW recovery controller.set_mpc_fcw_crash_cnt(0) - for _ in range(MPC_FCW_WINDOW_SIZE + 1): + for _ in range(WMACConstants.MPC_FCW_WINDOW_SIZE + 1): controller.update(not has_radar, car_state, lead_one, md, controls_state) assert not controller._has_mpc_fcw @@ -244,12 +235,12 @@ def test_radar_unavailable_handling(controller): controls_state = MockControlState(v_cruise=100) # Test with radar available - for _ in range(LEAD_WINDOW_SIZE + 1): + for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1): controller.update(False, car_state, lead_one, md, controls_state) radar_mode = controller.get_mpc_mode() # Test with radar unavailable - for _ in range(LEAD_WINDOW_SIZE + 1): + for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1): controller.update(True, car_state, lead_one, md, controls_state) radarless_mode = controller.get_mpc_mode() diff --git a/sunnypilot/selfdrive/controls/lib/dec/helpers.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py similarity index 52% rename from sunnypilot/selfdrive/controls/lib/dec/helpers.py rename to sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 734cec3492..56f32373d5 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/helpers.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -9,25 +9,21 @@ from cereal import messaging, custom from opendbc.car import structs from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController -MpcSource = custom.LongitudinalPlanSP.MpcSource +DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState -class DecPlanner: +class LongitudinalPlannerSP: def __init__(self, CP: structs.CarParams, mpc): - self.CP = CP - self.mpc = mpc + self.dec = DynamicExperimentalController(CP, mpc) - self.dynamic_experimental_controller = DynamicExperimentalController() - - def get_mpc_mode(self, sm: messaging.SubMaster): - if not self.dynamic_experimental_controller.is_enabled() or not sm['selfdriveState'].experimentalMode: + def get_mpc_mode(self) -> str | None: + if not self.dec.active(): return None - return self.dynamic_experimental_controller.get_mpc_mode() + return self.dec.mode() def update(self, sm: messaging.SubMaster) -> None: - self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt) - self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm) + self.dec.update(sm) def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: plan_sp_send = messaging.new_message('longitudinalPlanSP') @@ -36,9 +32,10 @@ class DecPlanner: longitudinalPlanSP = plan_sp_send.longitudinalPlanSP - # DEC - longitudinalPlanSP.mpcSource = MpcSource.blended if self.mpc.mode == 'blended' else MpcSource.acc - - longitudinalPlanSP.dynamicExperimentalControl = self.dynamic_experimental_controller.is_enabled() + # Dynamic Experimental Control + dec = longitudinalPlanSP.dec + dec.state = DecState.blended if self.dec.mode() == 'blended' else DecState.acc + dec.enabled = self.dec.enabled() + dec.active = self.dec.active() pm.send('longitudinalPlanSP', plan_sp_send)