mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-10-16 03:43:43 +08:00
Compare commits
3 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| a775d53cbc | |||
| f7bc31f208 | |||
| 9bebe42f73 |
@@ -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
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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([
|
||||
|
||||
Reference in New Issue
Block a user