mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-08-07 15:15:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'dec-redo' into master-dev-c3-new

Browse Source 
# Conflicts:
#	sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
This commit is contained in:
Jason Wen
2025-01-15 13:41:22 -05:00
parent d3ecdbb850 d937062724
commit 353aa611e7
27 changed files with 329 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/custom.capnp
+11 -6
View File
@@ -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;
}
}
}
cereal/log.capnp
+1
View File
@@ -198,6 +198,7 @@ struct OnroadEvent @0xc4fa6047f024e718 {
silentSeatbeltNotLatched @161;
silentParkBrake @162;
controlsMismatchLateral @163;
hyundaiRadarTracksConfirmed @164;
soundsUnavailableDEPRECATED @47;
}
common/params.cc
+6
View File
@@ -224,6 +224,12 @@ std::unordered_map<std::string, uint32_t> keys = {
{"SunnylinkdPid", PERSISTENT},
{"SunnylinkEnabled", PERSISTENT},
// sunnypilot car specific params
{"HyundaiRadarTracks", PERSISTENT},
{"HyundaiRadarTracksConfirmed", PERSISTENT},
{"HyundaiRadarTracksPersistent", PERSISTENT},
{"HyundaiRadarTracksToggle", PERSISTENT},
{"DynamicExperimentalControl", PERSISTENT},
};
opendbc_repo
+1 -1
Submodule opendbc_repo updated: 6ddebc9a03...6a2ad131eb
selfdrive/car/card.py
+3
View File
@@ -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)
selfdrive/controls/lib/longitudinal_planner.py
+6 -5
View File
@@ -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)
selfdrive/selfdrived/events.py
+3
View File
@@ -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")
}
}
selfdrive/selfdrived/selfdrived.py
+7
View File
@@ -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):
selfdrive/ui/qt/offroad/developer_panel.cc
+18
View File
@@ -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<cereal::CarParams>();
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;
selfdrive/ui/qt/offroad/developer_panel.h
+1
View File
@@ -16,6 +16,7 @@ private:
Params params;
ParamControl* joystickToggle;
ParamControl* longManeuverToggle;
ParamControl* hyundaiRadarTracksToggle;
bool is_release;
bool offroad;
selfdrive/ui/translations/main_ar.ts
+8
View File
@@ -131,6 +131,14 @@
<source>Enable GitHub runner service</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Hyundai: Enable Radar Tracks</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>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.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DevicePanel</name>
selfdrive/ui/translations/main_de.ts
+8
View File
@@ -131,6 +131,14 @@
<source>Enable GitHub runner service</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Hyundai: Enable Radar Tracks</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>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.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DevicePanel</name>
selfdrive/ui/translations/main_es.ts
+8
View File
@@ -131,6 +131,14 @@
<source>Enable GitHub runner service</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Hyundai: Enable Radar Tracks</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>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.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DevicePanel</name>
selfdrive/ui/translations/main_fr.ts
+8
View File
@@ -131,6 +131,14 @@
<source>Enable GitHub runner service</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Hyundai: Enable Radar Tracks</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>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.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DevicePanel</name>
selfdrive/ui/translations/main_ja.ts
+8
View File
@@ -131,6 +131,14 @@
<source>Enable GitHub runner service</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Hyundai: Enable Radar Tracks</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>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.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DevicePanel</name>
selfdrive/ui/translations/main_ko.ts
+8
View File
@@ -131,6 +131,14 @@
<source>Enable GitHub runner service</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Hyundai: Enable Radar Tracks</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>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.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DevicePanel</name>
selfdrive/ui/translations/main_pt-BR.ts
+8
View File
@@ -131,6 +131,14 @@
<source>Enable GitHub runner service</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Hyundai: Enable Radar Tracks</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>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.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DevicePanel</name>
selfdrive/ui/translations/main_th.ts
+8
View File
@@ -131,6 +131,14 @@
<source>Enable GitHub runner service</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Hyundai: Enable Radar Tracks</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>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.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DevicePanel</name>
selfdrive/ui/translations/main_tr.ts
+8
View File
@@ -131,6 +131,14 @@
<source>Enable GitHub runner service</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Hyundai: Enable Radar Tracks</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>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.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DevicePanel</name>
selfdrive/ui/translations/main_zh-CHS.ts
+8
View File
@@ -131,6 +131,14 @@
<source>Enable GitHub runner service</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Hyundai: Enable Radar Tracks</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>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.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DevicePanel</name>
selfdrive/ui/translations/main_zh-CHT.ts
+8
View File
@@ -131,6 +131,14 @@
<source>Enable GitHub runner service</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Hyundai: Enable Radar Tracks</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>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.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DevicePanel</name>
sunnypilot/selfdrive/car/car_specific.py
+34
View File
@@ -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
sunnypilot/selfdrive/car/interfaces.py
+41
View File
@@ -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)
sunnypilot/selfdrive/controls/lib/dec/constants.py
+25
View File
@@ -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
sunnypilot/selfdrive/controls/lib/dec/dec.py
+54 -78
View File
@@ -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
sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py
+18 -27
View File
@@ -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()
sunnypilot/selfdrive/controls/lib/dec/helpers.py → sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
+12 -15
View File
@@ -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)