Compare commits

...

3 Commits

Author SHA1 Message Date
rav4kumar a775d53cbc Sunset Dynamic Experimental Mode (DEC) 2026-10-15 10:44:19 -07:00
James Vecellio-Grant f7bc31f208 ci: no hook (#2055) 2026-10-15 07:41:21 -07:00
James Vecellio-Grant 9bebe42f73 default models: fix big (#2046)
* fix big

* whoops
2026-10-13 23:45:39 -04:00
23 changed files with 74 additions and 726 deletions
+34 -3
View File
@@ -47,6 +47,7 @@ jobs:
if [ "${{ inputs.target }}" = "big" ]; then
NAME=$(python3 -c "from openpilot.sunnypilot.models.model_name import DEFAULT_BIG_MODEL; print(DEFAULT_BIG_MODEL)")
HF_DEFAULTS_PATH="models/defaults/big"
ONNX_PATH="openpilot/selfdrive/modeld/models/big_driving_tinygrad.pkl"
elif [ "${{ inputs.target }}" = "dm" ]; then
ONNX_PATH="openpilot/selfdrive/modeld/models/dmonitoring_model.onnx"
HF_DEFAULTS_PATH="models/defaults/dm"
@@ -185,7 +186,6 @@ jobs:
- uses: actions/checkout@v4
with:
submodules: recursive
lfs: true
- name: Pull big PKL via LFS
run: git lfs pull -I "${{ env.BIG_PKL }}"
@@ -235,6 +235,34 @@ jobs:
print(f'Chunked into {len(targets)} files')
"
- name: Compile big warp
run: |
source ${UV_PROJECT_ENVIRONMENT}/bin/activate
export PYTHONPATH="${PYTHONPATH}:${{ github.workspace }}/tinygrad_repo:${{ github.workspace }}"
CHESTNUT_TG_FLAGS="DEBUG=1 DEV=USB+AMD:LLVM FRAME_DEV=CPU FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0 TC_OPT=2 TC_MIN_GLOBALS=32"
COMPILER="${{ github.workspace }}/tinygrad_repo/examples/openpilot"
MODELS_DIR="${{ github.workspace }}/openpilot/selfdrive/modeld/models"
MODEL_SIZE=$(python3 -c "from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE as s; print(f'{s[0]}x{s[1]}')")
for res in $(python3 -c "from openpilot.common.transformations.camera import _ar_ox_fisheye as a, _os_fisheye as o; print(f'{a.width}x{a.height} {o.width}x{o.height}')"); do
CAM_W=$(echo $res | cut -d x -f1)
CAM_H=$(echo $res | cut -d x -f2)
STRIDE_INFO=$(python3 -c "
import sys
from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
s,y,u,sz = get_nv12_info(int(sys.argv[1]), int(sys.argv[2]))
print(f'{s},{y},{u},{sz}')
" $CAM_W $CAM_H)
WARP_PKL="${MODELS_DIR}/big_driving_warp_${res}_tinygrad.pkl"
env ${CHESTNUT_TG_FLAGS} python3 "${COMPILER}/compile_warp.py" \
--frame ${CAM_W},${CAM_H},${STRIDE_INFO} \
--warp-to ${MODEL_SIZE} \
--layout yuv420 \
--frames 2 \
--output "${WARP_PKL}"
done
- name: Prepare output
env:
MODEL_NAME: ${{ needs.resolve.outputs.safe_model_name }}
@@ -249,7 +277,7 @@ jobs:
cp "$MODELS_DIR/${PKL_BASE}".chunk* "$OUTPUT_DIR/"
cp "$MODELS_DIR/${PKL_BASE}.chunkmanifest" "$OUTPUT_DIR/"
PKL_LFS_SHA256=$(sha256sum "$MODELS_DIR/${PKL_BASE}" | cut -d' ' -f1)
PKL_LFS_SHA256=$(git cat-file blob "$(git ls-files -s "${{ github.workspace }}/${{ env.BIG_PKL }}" | awk '{print $2}')" | grep '^oid sha256:' | cut -d: -f2)
python3 "${{ github.workspace }}/release/ci/model_generator.py" \
--model-dir "$MODELS_DIR" \
@@ -258,6 +286,8 @@ jobs:
--upstream-branch "${{ needs.resolve.outputs.onnx_ref }}" \
--onnx-sha256 "$PKL_LFS_SHA256"
cp "$MODELS_DIR"/big_driving_warp_* "$OUTPUT_DIR/" || true
echo "model-${MODEL_NAME}-${{ github.run_number }}" > "$OUTPUT_DIR/artifact_name.txt"
- name: Upload big model artifact
@@ -335,6 +365,8 @@ jobs:
--hf-defaults-path "${{ needs.resolve.outputs.hf_defaults_path }}" \
--artifact-name "$ARTIFACT_NAME" \
--model-dir output \
--onnx-path "${{ needs.resolve.outputs.onnx_path }}" \
--onnx-ref "${{ needs.resolve.outputs.onnx_ref }}" \
--model-name "${{ needs.resolve.outputs.model_name }}" \
--tinygrad-ref "${{ needs.resolve.outputs.tinygrad_ref }}" \
--run-number "${{ github.run_number }}"
@@ -514,4 +546,3 @@ jobs:
run: |
source ${UV_PROJECT_ENVIRONMENT}/bin/activate
PYTHONPATH=$PYTHONPATH:${{ github.workspace }}/ ${{ github.workspace }}/scripts/manage-powersave.py --enable
@@ -71,6 +71,11 @@ runs:
ENCODED_URL=$(python3 -c "import urllib.parse; print(urllib.parse.quote('${BASE_URL}/${warp}', safe=':/'))")
printf '%s\t%s\n' "$ENCODED_URL" "${DEST_DIR}/${warp}" >> "$DOWNLOAD_LIST"
done
elif [ "$CANONICAL" = "big_driving_tinygrad.pkl" ]; then
for warp in big_driving_warp_1928x1208_tinygrad.pkl big_driving_warp_1344x760_tinygrad.pkl; do
ENCODED_URL=$(python3 -c "import urllib.parse; print(urllib.parse.quote('${BASE_URL}/${warp}', safe=':/'))")
printf '%s\t%s\n' "$ENCODED_URL" "${DEST_DIR}/${warp}" >> "$DOWNLOAD_LIST"
done
fi
}
+7 -19
View File
@@ -196,25 +196,13 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
}
struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
dec @0 :DynamicExperimentalControl;
longitudinalPlanSource @1 :LongitudinalPlanSource;
smartCruiseControl @2 :SmartCruiseControl;
speedLimit @3 :SpeedLimit;
vTarget @4 :Float32;
aTarget @5 :Float32;
events @6 :List(OnroadEventSP.Event);
e2eAlerts @7 :E2eAlerts;
struct DynamicExperimentalControl {
state @0 :DynamicExperimentalControlState;
enabled @1 :Bool;
active @2 :Bool;
enum DynamicExperimentalControlState {
acc @0;
blended @1;
}
}
longitudinalPlanSource @0 :LongitudinalPlanSource;
smartCruiseControl @1 :SmartCruiseControl;
speedLimit @2 :SpeedLimit;
vTarget @3 :Float32;
aTarget @4 :Float32;
events @5 :List(OnroadEventSP.Event);
e2eAlerts @6 :E2eAlerts;
struct SmartCruiseControl {
vision @0 :Vision;
-1
View File
@@ -240,7 +240,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"ToyotaEnforceStockLongitudinal", {PERSISTENT | BACKUP, BOOL, "0"}},
{"ToyotaStopAndGoHack", {PERSISTENT | BACKUP, BOOL, "0"}},
{"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}},
{"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
// sunnypilot model params
+1 -5
View File
@@ -127,9 +127,6 @@ class Car:
set_alternative_experience(self.CP, self.CP_SP, self.params)
set_car_specific_params(self.CP, self.CP_SP, self.params)
# Dynamic Experimental Control
self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl")
openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle")
controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly
self.CP.passive = not controller_available or self.CP.dashcamOnly
@@ -214,7 +211,7 @@ class Car:
self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric)
if self.sm['carControl'].enabled and not self.CC_prev.enabled:
# Use CarState w/ buttons from the step selfdrived enables on
self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode, self.dynamic_experimental_control)
self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode)
# TODO: mirror the carState.cruiseState struct?
CS.vCruise = float(self.v_cruise_helper.v_cruise_kph)
@@ -302,7 +299,6 @@ class Car:
self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
# sunnypilot
self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl")
self.v_cruise_helper.read_custom_set_speed_params()
time.sleep(0.1)
+2 -3
View File
@@ -138,13 +138,12 @@ class VCruiseHelper(VCruiseHelperSP):
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, dynamic_experimental_control: bool) -> None:
def initialize_v_cruise(self, CS, experimental_mode: bool) -> None:
# initializing is handled by the PCM
if self.CP.pcmCruise:
return
initial_experimental_mode = experimental_mode and not dynamic_experimental_control
initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if initial_experimental_mode else V_CRUISE_INITIAL
initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL
if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_initialized:
self.v_cruise_kph = self.v_cruise_kph_last
@@ -59,16 +59,16 @@ class TestVCruiseHelper(OpenpilotTestCase):
for _ in range(2):
self.v_cruise_helper.update_v_cruise(car.CarState(cruiseState={"available": False}), enabled=False, is_metric=False)
def enable(self, v_ego, experimental_mode, dynamic_experimental_control):
def enable(self, v_ego, experimental_mode):
# Simulates user pressing set with a current speed
self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode, dynamic_experimental_control)
self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode)
def test_adjust_speed(self):
"""
Asserts speed changes on falling edges of buttons.
"""
self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False)
self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
for btn in (ButtonType.accelCruise, ButtonType.decelCruise):
for pressed in (True, False):
@@ -92,7 +92,7 @@ class TestVCruiseHelper(OpenpilotTestCase):
CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=pressed)]
self.v_cruise_helper.update_v_cruise(CS, enabled=enabled, is_metric=False)
if pressed:
self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False)
self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
# Expected diff on enabling. Speed should not change on falling edge of pressed
assert (not pressed) == (self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last)
@@ -102,7 +102,7 @@ class TestVCruiseHelper(OpenpilotTestCase):
Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill.
"""
self.enable(0, False, False)
self.enable(0, False)
for standstill in (True, False):
for pressed in (True, False):
@@ -122,7 +122,7 @@ class TestVCruiseHelper(OpenpilotTestCase):
for v_ego in np.linspace(0, 100, 101):
self.reset_cruise_speed_state()
self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False)
self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
# first decrement speed, then perform gas pressed logic
expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT
@@ -144,11 +144,10 @@ class TestVCruiseHelper(OpenpilotTestCase):
"""
for experimental_mode in (True, False):
for dynamic_experimental_control in (True, False):
for v_ego in np.linspace(0, 100, 101):
self.reset_cruise_speed_state()
assert not self.v_cruise_helper.v_cruise_initialized
for v_ego in np.linspace(0, 100, 101):
self.reset_cruise_speed_state()
assert not self.v_cruise_helper.v_cruise_initialized
self.enable(float(v_ego), experimental_mode, dynamic_experimental_control)
assert V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX
assert self.v_cruise_helper.v_cruise_initialized
self.enable(float(v_ego), experimental_mode)
assert V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX
assert self.v_cruise_helper.v_cruise_initialized
@@ -138,16 +138,16 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
output_should_stop_e2e = sm['modelV2'].action.shouldStop
is_e2e = self.is_e2e(sm)
experimental_mode = sm['selfdriveState'].experimentalMode
self.a_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego,
self.a_cruise = get_cruise_accel(experimental_mode, v_cruise, v_ego,
self.a_cruise, steer_angle_without_offset, self.CP, self.dt,
accel_coast, self.allow_throttle)
cruise_should_stop = should_stop(v_ego, self.a_cruise)
candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
(self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)]
if is_e2e:
if experimental_mode:
candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e))
output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
@@ -82,14 +82,8 @@ class CruiseLayout(Widget):
callback=lambda: self._set_current_panel(PanelType.SLA)
)
self.dec_toggle = toggle_item_sp(
title=tr("Enable Dynamic Experimental Control"),
description=tr("Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal."),
param="DynamicExperimentalControl")
items = [
self.icbm_toggle,
self.dec_toggle,
self.scc_v_toggle,
self.scc_m_toggle,
self.custom_acc_toggle,
@@ -144,16 +138,13 @@ class CruiseLayout(Widget):
if has_long or has_icbm:
self.custom_acc_toggle.action_item.set_enabled(((has_long and not ui_state.CP.pcmCruise) or has_icbm) and ui_state.is_offroad())
self.dec_toggle.action_item.set_enabled(has_long)
self.scc_v_toggle.action_item.set_enabled(True)
self.scc_m_toggle.action_item.set_enabled(True)
else:
ui_state.params.remove("CustomAccIncrementsEnabled")
ui_state.params.remove("DynamicExperimentalControl")
ui_state.params.remove("SmartCruiseControlVision")
ui_state.params.remove("SmartCruiseControlMap")
self.custom_acc_toggle.action_item.set_enabled(False)
self.dec_toggle.action_item.set_enabled(False)
self.scc_v_toggle.action_item.set_enabled(False)
self.scc_m_toggle.action_item.set_enabled(False)
@@ -221,10 +221,9 @@ class UIStateSP:
self.params.remove("LateralJerkTorqueController")
self.params.remove("AlphaLongitudinalEnabled")
# No longitudinal control: no experimental mode or DEC
# No longitudinal control: no experimental mode
if not has_long:
self.params.remove("ExperimentalMode")
self.params.remove("DynamicExperimentalControl")
# ICBM: clear if not available or if full longitudinal control is active
if self.CP_SP is not None:
@@ -81,7 +81,6 @@ def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsS
if not CP.openpilotLongitudinalControl and CP_SP.pcmCruiseSpeed:
cloudlog.warning("openpilot Longitudinal Control and ICBM not available, cleaning up params")
params.remove("DynamicExperimentalControl")
params.remove("CustomAccIncrementsEnabled")
params.remove("SmartCruiseControlVision")
params.remove("SmartCruiseControlMap")
@@ -57,7 +57,7 @@ class TestCustomAccIncrements(TestVCruiseHelper):
def test_default_behavior_when_disabled(self):
"""Test that default increments are used when custom ACC is disabled"""
self.set_custom_increments(enabled=False, short_inc=5, long_inc=10)
self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False)
self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
initial_speed = self.v_cruise_helper.v_cruise_kph
@@ -69,7 +69,7 @@ class TestCustomAccIncrements(TestVCruiseHelper):
def test_custom_short_press_increments(self, increment):
"""Test custom short press increments (1-10)"""
self.set_custom_increments(enabled=True, short_inc=increment, long_inc=5)
self.enable(50 * CV.KPH_TO_MS, False, False)
self.enable(50 * CV.KPH_TO_MS, False)
initial_speed = self.v_cruise_helper.v_cruise_kph
self.press_button_short(ButtonType.accelCruise)
@@ -86,7 +86,7 @@ class TestCustomAccIncrements(TestVCruiseHelper):
def test_custom_long_press_increments(self, increment):
"""Test custom long press increments (1, 5, 10)"""
self.set_custom_increments(enabled=True, short_inc=1, long_inc=increment)
self.enable(50 * CV.KPH_TO_MS, False, False)
self.enable(50 * CV.KPH_TO_MS, False)
initial_speed = self.v_cruise_helper.v_cruise_kph
self.press_button_long(ButtonType.accelCruise)
@@ -103,7 +103,7 @@ class TestCustomAccIncrements(TestVCruiseHelper):
def test_accel_decel_symmetry(self, button_type):
"""Test that acceleration and deceleration work symmetrically"""
self.set_custom_increments(enabled=True, short_inc=3, long_inc=5)
self.enable(50 * CV.KPH_TO_MS, False, False)
self.enable(50 * CV.KPH_TO_MS, False)
initial_speed = self.v_cruise_helper.v_cruise_kph
self.press_button_short(button_type)
@@ -123,7 +123,7 @@ class TestCustomAccIncrements(TestVCruiseHelper):
for initial, increment, expected in test_cases:
self.set_custom_increments(enabled=True, short_inc=increment, long_inc=increment)
self.reset_cruise_speed_state()
self.enable(initial * CV.KPH_TO_MS, False, False)
self.enable(initial * CV.KPH_TO_MS, False)
self.press_button_short(ButtonType.accelCruise)
assert self.v_cruise_helper.v_cruise_kph == expected
@@ -132,7 +132,7 @@ class TestCustomAccIncrements(TestVCruiseHelper):
"""Test that invalid values fallback to safe defaults"""
# Test invalid short increment
self.set_custom_increments(enabled=True, short_inc=-1, long_inc=5)
self.enable(50 * CV.KPH_TO_MS, False, False)
self.enable(50 * CV.KPH_TO_MS, False)
initial_speed = self.v_cruise_helper.v_cruise_kph
self.press_button_short(ButtonType.accelCruise)
@@ -141,7 +141,7 @@ class TestCustomAccIncrements(TestVCruiseHelper):
# Test invalid long increment
self.reset_cruise_speed_state()
self.set_custom_increments(enabled=True, short_inc=1, long_inc=99)
self.enable(50 * CV.KPH_TO_MS, False, False)
self.enable(50 * CV.KPH_TO_MS, False)
initial_speed = self.v_cruise_helper.v_cruise_kph
self.press_button_long(ButtonType.accelCruise)
@@ -1,17 +0,0 @@
class WMACConstants:
# Lead detection parameters
LEAD_WINDOW_SIZE = 6 # Stable detection window
LEAD_PROB = 0.45 # Balanced threshold for lead detection
# Slow down detection parameters
SLOW_DOWN_WINDOW_SIZE = 5 # Responsive but stable
SLOW_DOWN_PROB = 0.3 # Balanced threshold for slow down scenarios
# Optimized slow down distance curve - smooth and progressive
SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
SLOW_DOWN_DIST = [32., 46., 64., 86., 108., 130., 145., 165.]
# Slowness detection parameters
SLOWNESS_WINDOW_SIZE = 10 # Stable slowness detection
SLOWNESS_PROB = 0.55 # Clear threshold for slowness
SLOWNESS_CRUISE_OFFSET = 1.025 # Conservative cruise speed offset
@@ -1,388 +0,0 @@
"""
Copyright (c) 2021-, rav4kumar, 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.
"""
# Version = 2025-6-30
from openpilot.cereal import messaging
from opendbc.car import structs
from numpy 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
from typing import Literal
# d-e2e, from modeldata.h
TRAJECTORY_SIZE = 33
SET_MODE_TIMEOUT = 15
# Define the valid mode types
ModeType = Literal['acc', 'blended']
class SmoothKalmanFilter:
"""Enhanced Kalman filter with smoothing for stable decision making."""
def __init__(self, initial_value=0, measurement_noise=0.1, process_noise=0.01,
alpha=1.0, smoothing_factor=0.85):
self.x = initial_value
self.P = 1.0
self.R = measurement_noise
self.Q = process_noise
self.alpha = alpha
self.smoothing_factor = smoothing_factor
self.initialized = False
self.history = []
self.max_history = 10
self.confidence = 0.0
def add_data(self, measurement):
if len(self.history) >= self.max_history:
self.history.pop(0)
self.history.append(measurement)
if not self.initialized:
self.x = measurement
self.initialized = True
self.confidence = 0.1
return
self.P = self.alpha * self.P + self.Q
K = self.P / (self.P + self.R)
effective_K = K * (1.0 - self.smoothing_factor) + self.smoothing_factor * 0.1
innovation = measurement - self.x
self.x = self.x + effective_K * innovation
self.P = (1 - effective_K) * self.P
if abs(innovation) < 0.1:
self.confidence = min(1.0, self.confidence + 0.05)
else:
self.confidence = max(0.1, self.confidence - 0.02)
def get_value(self):
return self.x if self.initialized else None
def get_confidence(self):
return self.confidence
def reset_data(self):
self.initialized = False
self.history = []
self.confidence = 0.0
class ModeTransitionManager:
"""Manages smooth transitions between driving modes with hysteresis."""
def __init__(self):
self.current_mode: ModeType = 'acc'
self.mode_confidence = {'acc': 1.0, 'blended': 0.0}
self.transition_timeout = 0
self.min_mode_duration = 10
self.mode_duration = 0
self.emergency_override = False
def request_mode(self, mode: ModeType, confidence: float = 1.0, emergency: bool = False):
# Emergency override for critical situations (stops, collisions)
if emergency:
self.emergency_override = True
self.current_mode = mode
self.transition_timeout = SET_MODE_TIMEOUT
self.mode_duration = 0
return
self.mode_confidence[mode] = min(1.0, self.mode_confidence[mode] + 0.1 * confidence)
for m in self.mode_confidence:
if m != mode:
self.mode_confidence[m] = max(0.0, self.mode_confidence[m] - 0.05)
# Require minimum duration in current mode (unless emergency)
if self.mode_duration < self.min_mode_duration and not self.emergency_override:
return
# Hysteresis: higher threshold for mode changes
confidence_threshold = 0.6 if mode != self.current_mode else 0.3 # Lower threshold for faster response
if self.mode_confidence[mode] > confidence_threshold:
if mode != self.current_mode and self.transition_timeout == 0:
self.transition_timeout = SET_MODE_TIMEOUT
self.current_mode = mode
self.mode_duration = 0
def update(self):
if self.transition_timeout > 0:
self.transition_timeout -= 1
self.mode_duration += 1
# Reset emergency override after some time
if self.emergency_override and self.mode_duration > 20:
self.emergency_override = False
# Gradual confidence decay
for mode in self.mode_confidence:
self.mode_confidence[mode] *= 0.98
def get_mode(self) -> ModeType:
return self.current_mode
class DynamicExperimentalController:
def __init__(self, CP: structs.CarParams, mpc, params=None):
self._CP = CP
self._mpc = mpc
self._params = params or Params()
self._enabled: bool = self._params.get_bool("DynamicExperimentalControl")
self._active: bool = False
self._frame: int = 0
self._urgency = 0.0
self._mode_manager = ModeTransitionManager()
# Smooth filters for stable decision making with faster response for critical scenarios
self._lead_filter = SmoothKalmanFilter(
measurement_noise=0.15,
process_noise=0.05,
alpha=1.02,
smoothing_factor=0.8
)
self._slow_down_filter = SmoothKalmanFilter(
measurement_noise=0.1,
process_noise=0.1,
alpha=1.05,
smoothing_factor=0.7
)
self._slowness_filter = SmoothKalmanFilter(
measurement_noise=0.1,
process_noise=0.06,
alpha=1.015,
smoothing_factor=0.92
)
self._mpc_fcw_filter = SmoothKalmanFilter(
measurement_noise=0.2,
process_noise=0.1,
alpha=1.1,
smoothing_factor=0.5
)
self._has_lead_filtered = False
self._has_slow_down = False
self._has_slowness = False
self._has_mpc_fcw = False
self._v_ego_kph = 0.0
self._v_cruise_kph = 0.0
self._has_standstill = False
self._mpc_fcw_crash_cnt = 0
self._standstill_count = 0
# debug
self._endpoint_x = float('inf')
self._expected_distance = 0.0
self._trajectory_valid = False
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 self._mode_manager.get_mode()
def enabled(self) -> bool:
return self._enabled
def active(self) -> bool:
return self._active
def set_mpc_fcw_crash_cnt(self) -> None:
"""Set MPC FCW crash count"""
self._mpc_fcw_crash_cnt = self._mpc.crash_cnt
def _update_calculations(self, sm: messaging.SubMaster) -> None:
car_state = sm['carState']
lead_one = sm['radarState'].leadOne
md = sm['modelV2']
self._v_ego_kph = car_state.vEgo * 3.6
self._v_cruise_kph = car_state.vCruise
self._has_standstill = car_state.standstill
# standstill detection
if self._has_standstill:
self._standstill_count = min(20, self._standstill_count + 1)
else:
self._standstill_count = max(0, self._standstill_count - 1)
# Lead detection
self._lead_filter.add_data(float(lead_one.present))
lead_value = self._lead_filter.get_value() or 0.0
self._has_lead_filtered = lead_value > WMACConstants.LEAD_PROB
# MPC FCW detection
fcw_filtered_value = self._mpc_fcw_filter.get_value() or 0.0
self._mpc_fcw_filter.add_data(float(self._mpc_fcw_crash_cnt > 0))
self._has_mpc_fcw = fcw_filtered_value > 0.5
# Slow down detection
self._calculate_slow_down(md)
# Slowness detection
if not (self._standstill_count > 5) and not self._has_slow_down:
current_slowness = float(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET))
self._slowness_filter.add_data(current_slowness)
slowness_value = self._slowness_filter.get_value() or 0.0
# Hysteresis for slowness
threshold = WMACConstants.SLOWNESS_PROB * (0.8 if self._has_slowness else 1.1)
self._has_slowness = slowness_value > threshold
def _calculate_slow_down(self, md):
"""Calculate urgency based on trajectory endpoint vs expected distance."""
# Reset to safe defaults
urgency = 0.0
self._endpoint_x = float('inf')
self._trajectory_valid = False
#Require exact trajectory size
position_valid = len(md.position.x) == TRAJECTORY_SIZE
orientation_valid = len(md.orientation.x) == TRAJECTORY_SIZE
if not (position_valid and orientation_valid):
# Invalid trajectory - this itself might indicate a stop scenario
# Apply moderate urgency for incomplete trajectories at speed
if self._v_ego_kph > 20.0:
urgency = 0.3
self._slow_down_filter.add_data(urgency)
urgency_filtered = self._slow_down_filter.get_value() or 0.0
self._has_slow_down = urgency_filtered > WMACConstants.SLOW_DOWN_PROB
self._urgency = urgency_filtered
return
# We have a valid full trajectory
self._trajectory_valid = True
# Use the exact endpoint (33rd point, index 32)
endpoint_x = md.position.x[TRAJECTORY_SIZE - 1]
self._endpoint_x = endpoint_x
# Get expected distance based on current speed using tuned constants
expected_distance = interp(self._v_ego_kph,
WMACConstants.SLOW_DOWN_BP,
WMACConstants.SLOW_DOWN_DIST)
self._expected_distance = expected_distance
# Calculate urgency based on trajectory shortage
if endpoint_x < expected_distance:
shortage = expected_distance - endpoint_x
shortage_ratio = shortage / expected_distance
# Base urgency on shortage ratio
urgency = min(1.0, shortage_ratio * 2.0)
# Increase urgency for very short trajectories (imminent stops)
critical_distance = expected_distance * 0.3
if endpoint_x < critical_distance:
urgency = min(1.0, urgency * 2.0)
# Speed-based urgency adjustment
if self._v_ego_kph > 25.0:
speed_factor = 1.0 + (self._v_ego_kph - 25.0) / 80.0
urgency = min(1.0, urgency * speed_factor)
# Apply filtering but with less smoothing for stops
self._slow_down_filter.add_data(urgency)
urgency_filtered = self._slow_down_filter.get_value() or 0.0
# Update state with lower threshold for better stop detection
self._has_slow_down = urgency_filtered > (WMACConstants.SLOW_DOWN_PROB * 0.8)
self._urgency = urgency_filtered
def _radarless_mode(self) -> None:
"""Radarless mode decision logic with emergency handling."""
# EMERGENCY: MPC FCW - immediate blended mode
if self._has_mpc_fcw:
self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
return
# Standstill: use blended
if self._standstill_count > 3:
self._mode_manager.request_mode('blended', confidence=0.9)
return
# Slow down scenarios: emergency for high urgency, normal for lower urgency
if self._has_slow_down:
if self._urgency > 0.7:
# Emergency: immediate blended mode for high urgency stops
self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
else:
# Normal: blended with urgency-based confidence
confidence = min(1.0, self._urgency * 1.5)
self._mode_manager.request_mode('blended', confidence=confidence)
return
# Driving slow: use ACC (but not if actively slowing down)
if self._has_slowness and not self._has_slow_down:
self._mode_manager.request_mode('acc', confidence=0.8)
return
# Default: ACC
self._mode_manager.request_mode('acc', confidence=0.7)
def _radar_mode(self) -> None:
"""Radar mode with emergency handling."""
# EMERGENCY: MPC FCW - immediate blended mode
if self._has_mpc_fcw:
self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
return
# If lead detected and not in standstill: always use ACC
if self._has_lead_filtered and not (self._standstill_count > 3):
self._mode_manager.request_mode('acc', confidence=1.0)
return
# Slow down scenarios: emergency for high urgency, normal for lower urgency
if self._has_slow_down:
if self._urgency > 0.7:
# Emergency: immediate blended mode for high urgency stops
self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
else:
# Normal: blended with urgency-based confidence
confidence = min(1.0, self._urgency * 1.3)
self._mode_manager.request_mode('blended', confidence=confidence)
return
# Standstill: use blended
if self._standstill_count > 3:
self._mode_manager.request_mode('blended', confidence=0.9)
return
# Driving slow: use ACC (but not if actively slowing down)
if self._has_slowness and not self._has_slow_down:
self._mode_manager.request_mode('acc', confidence=0.8)
return
# Default: ACC
self._mode_manager.request_mode('acc', confidence=0.7)
def update(self, sm: messaging.SubMaster) -> None:
self._read_params()
self.set_mpc_fcw_crash_cnt()
self._update_calculations(sm)
if self._CP.radarUnavailable:
self._radarless_mode()
else:
self._radar_mode()
self._mode_manager.update()
self._active = sm['selfdriveState'].experimentalMode and self._enabled
self._frame += 1
@@ -1,112 +0,0 @@
"""
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 typing import cast
from openpilot.cereal import custom, messaging
from opendbc.car import structs
from openpilot.common.test import OpenpilotTestCase
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, LongitudinalPlanSource
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
V_EGO = 20.0
E2E_ACCEL = -3.0 # low enough that e2e wins the min() whenever it is a candidate
class MockDec:
def __init__(self, active: bool, mode: str):
self._active = active
self._mode = mode
def update(self, sm):
pass
def active(self) -> bool:
return self._active
def mode(self) -> str:
return self._mode
def enabled(self) -> bool:
return True
class MockSubMaster(dict):
def __init__(self, services: dict):
super().__init__(services)
self.valid = dict.fromkeys(services, True)
self.logMonoTime = dict.fromkeys(services, 0)
self.updated = dict.fromkeys(services, True)
self.recv_frame = dict.fromkeys(services, 1)
def all_checks(self, service_list=None) -> bool:
return True
def build_sm(experimental_mode: bool) -> MockSubMaster:
services = {}
for service in ("radarState", "controlsState", "vehicleParameters", "carStateSP",
"liveMapDataSP", "gpsLocationExternal", "gpsLocation"):
services[service] = getattr(messaging.new_message(service), service)
car_state = messaging.new_message('carState')
car_state.carState.vEgo = V_EGO
car_state.carState.vCruise = 100.0
car_state.carState.vCruiseCluster = 100.0
services['carState'] = car_state.carState.as_reader()
selfdrive_state = messaging.new_message('selfdriveState')
selfdrive_state.selfdriveState.experimentalMode = experimental_mode
selfdrive_state.selfdriveState.enabled = True
services['selfdriveState'] = selfdrive_state.selfdriveState.as_reader()
car_control = messaging.new_message('carControl')
car_control.carControl.enabled = True
services['carControl'] = car_control.carControl.as_reader()
model = messaging.new_message('modelV2')
model.modelV2.orientationRate.z = [0.01] * 33 # nonzero: a straight path divides by zero in SCC vision
model.modelV2.velocity.x = [V_EGO] * 33
model.modelV2.position.x = [float(i) for i in range(33)]
model.modelV2.action.desiredAcceleration = E2E_ACCEL
services['modelV2'] = model.modelV2.as_reader()
return MockSubMaster(services)
def build_planner(dec_active: bool, dec_mode: str) -> LongitudinalPlanner:
CP = structs.CarParams()
CP.steerRatio = 15.0
CP.wheelbase = 2.7
CP.longitudinalActuatorDelay = 0.2
CP_SP = custom.CarParamsSP.new_message().as_reader()
planner = LongitudinalPlanner(CP, CP_SP, init_v=V_EGO)
planner.dec = cast(DynamicExperimentalController, MockDec(dec_active, dec_mode))
return planner
class TestDecPlannerGate(OpenpilotTestCase):
"""The e2e candidate must be gated on is_e2e(), not raw experimentalMode."""
def _source(self, experimental_mode: bool, dec_active: bool, dec_mode: str) -> LongitudinalPlanSource:
planner = build_planner(dec_active, dec_mode)
planner.update(build_sm(experimental_mode))
return planner.mpc.source
def test_no_e2e_when_experimental_mode_off(self):
assert self._source(False, False, 'acc') != LongitudinalPlanSource.e2e
def test_e2e_when_dec_inactive(self):
# DEC off: behavior must match upstream
assert self._source(True, False, 'acc') == LongitudinalPlanSource.e2e
def test_e2e_when_dec_blended(self):
assert self._source(True, True, 'blended') == LongitudinalPlanSource.e2e
def test_no_e2e_when_dec_holds_acc(self):
# the regression
assert self._source(True, True, 'acc') != LongitudinalPlanSource.e2e
@@ -1,91 +0,0 @@
from openpilot.common.test import OpenpilotTestCase
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
class MockLeadOne:
def __init__(self, present=0.0):
self.present = present
class MockRadarState:
def __init__(self, present=0.0):
self.leadOne = MockLeadOne(present=present)
class MockCarState:
def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False):
self.vEgo = vEgo
self.vCruise = vCruise
self.standstill = standstill
class MockModelData:
def __init__(self, valid=True):
size = 33 if valid else 10 # incomplete if invalid
self.position = type("Pos", (), {"x": [0.0] * size})()
self.orientation = type("Ori", (), {"x": [0.0] * size})()
class MockSelfDriveState:
def __init__(self, experimentalMode=False):
self.experimentalMode = experimentalMode
class MockParams:
def get_bool(self, name):
return True
def default_sm():
sm = {
'carState': MockCarState(vEgo=10.0, vCruise=20.0),
'radarState': MockRadarState(present=1.0),
'modelV2': MockModelData(valid=True),
'selfdriveState': MockSelfDriveState(experimentalMode=True),
}
return sm
def mock_cp():
class CP:
radarUnavailable = False
return CP()
def mock_mpc():
class MPC:
crash_cnt = 0
return MPC()
# Fake Kalman Filter that always returns a given value
class FakeKalman:
def __init__(self, value=1.0):
self.value = value
def add_data(self, v): pass
def get_value(self): return self.value
def get_confidence(self): return 1.0
def reset_data(self): pass
class TestDynamicExperimentalController(OpenpilotTestCase):
def test_initial_mode_is_acc(self, mock_cp, mock_mpc):
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
assert controller.mode() == "acc"
def test_standstill_triggers_blended(self, mock_cp, mock_mpc, default_sm):
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
default_sm['carState'].standstill = True
for _ in range(10):
controller.update(default_sm)
assert controller.mode() == "blended"
def test_emergency_blended_on_fcw(self, mock_cp, mock_mpc, default_sm):
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
mock_mpc.crash_cnt = 1 # simulate FCW
for _ in range(2):
controller.update(default_sm)
assert controller.mode() == "blended"
def test_radarless_slowdown_triggers_blended(self, mock_cp, mock_mpc, default_sm):
mock_cp.radarUnavailable = True
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
# Force conditions to simulate slowdown
controller._slow_down_filter = FakeKalman(value=1.0) # ty: ignore[invalid-assignment]
controller._v_ego_kph = 35.0
default_sm['modelV2'] = MockModelData(valid=False) # Incomplete trajectory
for _ in range(3):
controller.update(default_sm)
assert controller.mode() == "blended"
@@ -9,7 +9,6 @@ from openpilot.cereal import messaging, custom
from opendbc.car import structs
from openpilot.common.constants import CV
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist
@@ -17,7 +16,6 @@ from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolve
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
from openpilot.sunnypilot.models.helpers import get_active_bundle
DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
@@ -25,7 +23,6 @@ class LongitudinalPlannerSP:
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
self.events_sp = EventsSP()
self.resolver = SpeedLimitResolver()
self.dec = DynamicExperimentalController(CP, mpc)
self.scc = SmartCruiseControl()
self.resolver = SpeedLimitResolver()
self.sla = SpeedLimitAssist(CP, CP_SP)
@@ -36,13 +33,6 @@ class LongitudinalPlannerSP:
self.output_v_target = 0.
self.output_a_target = 0.
def is_e2e(self, sm: messaging.SubMaster) -> bool:
experimental_mode = sm['selfdriveState'].experimentalMode
if not self.dec.active():
return experimental_mode
return experimental_mode and self.dec.mode() == "blended"
def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
CS = sm['carState']
v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
@@ -75,7 +65,6 @@ class LongitudinalPlannerSP:
def update(self, sm: messaging.SubMaster) -> None:
self.events_sp.clear()
self.dec.update(sm)
self.e2e_alerts_helper.update(sm, self.events_sp)
def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
@@ -89,12 +78,6 @@ class LongitudinalPlannerSP:
longitudinalPlanSP.aTarget = float(self.output_a_target)
longitudinalPlanSP.events = self.events_sp.to_msg()
# 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()
# Smart Cruise Control
smartCruiseControl = longitudinalPlanSP.smartCruiseControl
# Vision Control
@@ -367,8 +367,8 @@ enablement:
enablement:
- type: any
conditions:
- {type: param, key: ExperimentalMode, equals: true}
- {type: param, key: DynamicExperimentalControl, equals: true}
- {type: param, key: SmartCruiseControlVision, equals: true}
- {type: param, key: SmartCruiseControlMap, equals: true}
```
**Mixed: capability AND param** (only on longitudinal cars when ShowAdvancedControls is on):
@@ -599,26 +599,6 @@
}
]
},
{
"key": "DynamicExperimentalControl",
"widget": "toggle",
"title": "Dynamic Experimental Control",
"description": "Let the model decide when to use sunnypilot ACC or sunnypilot End to End Longitudinal.",
"visibility": [
{
"type": "capability",
"field": "has_longitudinal_control",
"equals": true
}
],
"enablement": [
{
"type": "capability",
"field": "has_longitudinal_control",
"equals": true
}
]
},
{
"key": "DisengageOnAccelerator",
"widget": "toggle",
@@ -16,14 +16,6 @@ sections:
title: Experimental Mode
enablement:
- $ref: '#/macros/longitudinal'
- key: DynamicExperimentalControl
widget: toggle
title: Dynamic Experimental Control
description: Let the model decide when to use sunnypilot ACC or sunnypilot End to End Longitudinal.
visibility:
- $ref: '#/macros/longitudinal'
enablement:
- $ref: '#/macros/longitudinal'
- key: DisengageOnAccelerator
widget: toggle
title: Disengage Cruise on Accelerator Pedal
@@ -201,11 +201,6 @@ class TestSpuriousOffroadGatesDropped(OpenpilotTestCase):
assert item is not None
assert "offroad_only" not in _flatten_rule_types(item.get("enablement"))
def test_dynamic_experimental_has_no_offroad_only(self, schema):
item = _find_item(schema, "DynamicExperimentalControl")
assert item is not None
assert "offroad_only" not in _flatten_rule_types(item.get("enablement"))
class TestNotEngagedReplacement(OpenpilotTestCase):
@parameterized.expand([