mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-06-27 03:02:05 +08:00
Longitudinal: Dynamic Experimental Control (#564)
* init dec * Update sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py Co-authored-by: sourcery-ai[bot] <58596630+sourcery-ai[bot]@users.noreply.github.com> * Update sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py Co-authored-by: sourcery-ai[bot] <58596630+sourcery-ai[bot]@users.noreply.github.com> * fix static test * ff * fix static test * unitee testt * Refactor test_dynamic_controller and fix formatting issues Added a new import for STOP_AND_GO_FRAME and corrected a float initialization for v_ego in MockCarState. Also fixed indentation in the test_standstill_detection method for consistency. * Refactor test indentation for dynamic controller tests Adjust indentation and formatting in test_dynamic_controller.py to ensure consistency and readability. This change does not alter functionality but improves the maintainability of the test code. * Migrated to pytest using claude * Integrate radar parameter into dynamic controller's pytest tests Added a `has_radar` parameter to the test functions in the dynamic controller's pytest file. This allows each function to run both with and without radar inputs, thus enhancing the coverage of our test cases. * Disabling unittest file to allow checks on the pipeline to succeed. Pending to remove this, but leaving it to validate the move to pytest is okay before merging * Replace unittest with pytest for dynamic controller tests Migrated dynamic controller tests from unittest to pytest for improved readability and maintainability. Refactored mock setup using pytest fixtures and monkeypatching while preserving test coverage. * new line... * Refactor and modularize DynamicExperimentalController logic Moved DynamicExperimentalController logic and helper functions to a dedicated module for better readability and maintainability. Simplified longitudinal planner logic by introducing reusable methods to manage MPC mode and longitudinal plan publishing. Adjusted file structure for dynamic controller-related components and updated relevant imports. * Add missing import for messaging in helpers.py The `messaging` module was added to resolve potential issues with undefined references. This change ensures all required imports are present, improving the reliability and maintainability of the code. * Format * Formatting * rebase fix * Refactor MpcSource definition and update references. Moved MpcSource enum into LongitudinalPlanSP for better encapsulation. Updated references in helpers.py to use the new path. This change improves code organization and maintains functionality. * Format * Refactor DEC into a dedicated longitudinal planner class Move Dynamic Experimental Control (DEC) logic to a new `DecLongitudinalPlanner` class for better modularity and maintainability. This simplifies the `LongitudinalPlanner` by delegating DEC-specific behavior and consolidates related methods into a single file. Additionally, redundant code was removed to improve readability and reduce complexity. * **Refactor DEC module structure for better organization** Moved DEC-related files from `dec` to `lib` for improved clarity and consistency within the project structure. Updated all relevant import paths to reflect the new locations. Ensured functionality remains unaffected with these changes. * static test * static * had moved to car_state * cleanup * some more * static method * move around * more cleanup * stuff * into their own * rename * check live param * sync with stock * type hint * unused * smoother trans * window time * fix type hint * pass sm.frame from plannerd * more fixes * more * more explicit * fix test * Revert "fix test" This reverts commit635b15f2bc. * Revert "pass sm.frame from plannerd" This reverts commita8deaa69b8. * use internal frame --------- Co-authored-by: sourcery-ai[bot] <58596630+sourcery-ai[bot]@users.noreply.github.com> Co-authored-by: DevTekVE <devtekve@gmail.com> Co-authored-by: Jason Wen <haibin.wen3@gmail.com>
This commit is contained in:
Kumar
+8
-1
@@ -82,7 +82,14 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
|
||||
}
|
||||
}
|
||||
|
||||
struct CustomReserved2 @0xf35cc4560bbf6ec2 {
|
||||
struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
mpcSource @0 :MpcSource;
|
||||
dynamicExperimentalControl @1 :Bool;
|
||||
|
||||
enum MpcSource {
|
||||
acc @0;
|
||||
blended @1;
|
||||
}
|
||||
}
|
||||
|
||||
struct CustomReserved3 @0xda96579883444c35 {
|
||||
|
||||
+1
-1
@@ -2633,7 +2633,7 @@ struct Event {
|
||||
# *********** Custom: reserved for forks ***********
|
||||
selfdriveStateSP @107 :Custom.SelfdriveStateSP;
|
||||
modelManagerSP @108 :Custom.ModelManagerSP;
|
||||
customReserved2 @109 :Custom.CustomReserved2;
|
||||
longitudinalPlanSP @109 :Custom.LongitudinalPlanSP;
|
||||
customReserved3 @110 :Custom.CustomReserved3;
|
||||
customReserved4 @111 :Custom.CustomReserved4;
|
||||
customReserved5 @112 :Custom.CustomReserved5;
|
||||
|
||||
@@ -77,6 +77,7 @@ _services: dict[str, tuple] = {
|
||||
# sunnypilot
|
||||
"modelManagerSP": (False, 1., 1),
|
||||
"selfdriveStateSP": (True, 100., 10),
|
||||
"longitudinalPlanSP": (True, 20., 10),
|
||||
|
||||
# debug
|
||||
"uiDebug": (True, 0., 1),
|
||||
|
||||
@@ -223,6 +223,8 @@ std::unordered_map<std::string, uint32_t> keys = {
|
||||
{"SunnylinkDongleId", PERSISTENT},
|
||||
{"SunnylinkdPid", PERSISTENT},
|
||||
{"SunnylinkEnabled", PERSISTENT},
|
||||
|
||||
{"DynamicExperimentalControl", PERSISTENT},
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
@@ -16,6 +16,8 @@ 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
|
||||
|
||||
LON_MPC_STEP = 0.2 # first step is 0.2s
|
||||
A_CRUISE_MIN = -1.2
|
||||
A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
|
||||
@@ -67,10 +69,11 @@ def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05):
|
||||
return a_target, should_stop
|
||||
|
||||
|
||||
class LongitudinalPlanner:
|
||||
class LongitudinalPlanner(DecPlanner):
|
||||
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)
|
||||
self.fcw = False
|
||||
self.dt = dt
|
||||
self.allow_throttle = True
|
||||
@@ -105,7 +108,10 @@ class LongitudinalPlanner:
|
||||
return x, v, a, j, throttle_prob
|
||||
|
||||
def update(self, sm):
|
||||
DecPlanner.update(self, sm)
|
||||
self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
|
||||
if dec_mpc_mode := self.get_mpc_mode(sm):
|
||||
self.mpc.mode = dec_mpc_mode
|
||||
|
||||
if len(sm['carControl'].orientationNED) == 3:
|
||||
accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
|
||||
@@ -206,3 +212,4 @@ class LongitudinalPlanner:
|
||||
longitudinalPlan.allowThrottle = self.allow_throttle
|
||||
|
||||
pm.send('longitudinalPlan', plan_send)
|
||||
self.publish_longitudinal_plan_sp(sm, pm)
|
||||
|
||||
@@ -18,7 +18,7 @@ def main():
|
||||
|
||||
ldw = LaneDepartureWarning()
|
||||
longitudinal_planner = LongitudinalPlanner(CP)
|
||||
pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance'])
|
||||
pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
|
||||
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'],
|
||||
poll='modelV2', ignore_avg_freq=['radarState'])
|
||||
|
||||
|
||||
@@ -40,6 +40,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
|
||||
"",
|
||||
"../assets/img_experimental_white.svg",
|
||||
},
|
||||
{
|
||||
"DynamicExperimentalControl",
|
||||
tr("Enable Dynamic Experimental Control"),
|
||||
tr("Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal."),
|
||||
"../assets/offroad/icon_blank.png",
|
||||
},
|
||||
{
|
||||
"DisengageOnAccelerator",
|
||||
tr("Disengage on Accelerator Pedal"),
|
||||
|
||||
@@ -1411,6 +1411,14 @@ This may take up to a minute.</source>
|
||||
<source>Enable driver monitoring even when openpilot is not engaged.</source>
|
||||
<translation>تمكين مراقبة السائق حتى عندما لا يكون نظام OpenPilot مُفعّلاً.</translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable Dynamic Experimental Control</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>Updater</name>
|
||||
|
||||
@@ -1395,6 +1395,14 @@ This may take up to a minute.</source>
|
||||
<source>Enable driver monitoring even when openpilot is not engaged.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable Dynamic Experimental Control</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>Updater</name>
|
||||
|
||||
@@ -1395,6 +1395,14 @@ Esto puede tardar un minuto.</translation>
|
||||
<source>Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode.</source>
|
||||
<translation>Activar el control longitudinal (fase experimental) para permitir el modo Experimental.</translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable Dynamic Experimental Control</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>Updater</name>
|
||||
|
||||
@@ -1395,6 +1395,14 @@ Cela peut prendre jusqu'à une minute.</translation>
|
||||
<source>Enable driver monitoring even when openpilot is not engaged.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable Dynamic Experimental Control</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>Updater</name>
|
||||
|
||||
@@ -1389,6 +1389,14 @@ This may take up to a minute.</source>
|
||||
<source>Enable driver monitoring even when openpilot is not engaged.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable Dynamic Experimental Control</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>Updater</name>
|
||||
|
||||
@@ -1391,6 +1391,14 @@ This may take up to a minute.</source>
|
||||
<source>Enable driver monitoring even when openpilot is not engaged.</source>
|
||||
<translation>Openpilot이 활성화되지 않은 경우에도 드라이버 모니터링을 활성화합니다.</translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable Dynamic Experimental Control</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>Updater</name>
|
||||
|
||||
@@ -1395,6 +1395,14 @@ Isso pode levar até um minuto.</translation>
|
||||
<source>Enable driver monitoring even when openpilot is not engaged.</source>
|
||||
<translation>Habilite o monitoramento do motorista mesmo quando o openpilot não estiver acionado.</translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable Dynamic Experimental Control</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>Updater</name>
|
||||
|
||||
@@ -1391,6 +1391,14 @@ This may take up to a minute.</source>
|
||||
<source>Enable driver monitoring even when openpilot is not engaged.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable Dynamic Experimental Control</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>Updater</name>
|
||||
|
||||
@@ -1389,6 +1389,14 @@ This may take up to a minute.</source>
|
||||
<source>Enable driver monitoring even when openpilot is not engaged.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable Dynamic Experimental Control</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>Updater</name>
|
||||
|
||||
@@ -1391,6 +1391,14 @@ This may take up to a minute.</source>
|
||||
<source>Enable driver monitoring even when openpilot is not engaged.</source>
|
||||
<translation>即使在openpilot未激活时也启用驾驶员监控。</translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable Dynamic Experimental Control</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>Updater</name>
|
||||
|
||||
@@ -1391,6 +1391,14 @@ This may take up to a minute.</source>
|
||||
<source>Enable driver monitoring even when openpilot is not engaged.</source>
|
||||
<translation>即使在openpilot未激活時也啟用駕駛監控。</translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable Dynamic Experimental Control</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
<message>
|
||||
<source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
|
||||
<translation type="unfinished"></translation>
|
||||
</message>
|
||||
</context>
|
||||
<context>
|
||||
<name>Updater</name>
|
||||
|
||||
@@ -0,0 +1,399 @@
|
||||
# The MIT License
|
||||
#
|
||||
# Copyright (c) 2019-, Rick Lan, dragonpilot community, and a number of other of contributors.
|
||||
#
|
||||
# Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
# of this software and associated documentation files (the "Software"), to deal
|
||||
# in the Software without restriction, including without limitation the rights
|
||||
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
# copies of the Software, and to permit persons to whom the Software is
|
||||
# furnished to do so, subject to the following conditions:
|
||||
#
|
||||
# The above copyright notice and this permission notice shall be included in
|
||||
# all copies or substantial portions of the Software.
|
||||
#
|
||||
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
# THE SOFTWARE.
|
||||
#
|
||||
# Version = 2024-7-11
|
||||
|
||||
import numpy as np
|
||||
|
||||
from cereal import messaging
|
||||
from openpilot.common.numpy_fast import interp
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
|
||||
# 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
|
||||
self.data = []
|
||||
self.total = 0
|
||||
|
||||
def add_data(self, value: float) -> None:
|
||||
if len(self.data) == self.window_size:
|
||||
self.total -= self.data.pop(0)
|
||||
self.data.append(value)
|
||||
self.total += value
|
||||
|
||||
def get_moving_average(self) -> float | None:
|
||||
return None if len(self.data) == 0 else self.total / len(self.data)
|
||||
|
||||
def reset_data(self) -> None:
|
||||
self.data = []
|
||||
self.total = 0
|
||||
|
||||
|
||||
class WeightedMovingAverageCalculator:
|
||||
def __init__(self, window_size):
|
||||
self.window_size = window_size
|
||||
self.data = []
|
||||
self.weights = np.linspace(1, 3, window_size) # Linear weights, adjust as needed
|
||||
|
||||
def add_data(self, value: float) -> None:
|
||||
if len(self.data) == self.window_size:
|
||||
self.data.pop(0)
|
||||
self.data.append(value)
|
||||
|
||||
def get_weighted_average(self) -> float | None:
|
||||
if len(self.data) == 0:
|
||||
return None
|
||||
weighted_sum: float = float(np.dot(self.data, self.weights[-len(self.data):]))
|
||||
weight_total: float = float(np.sum(self.weights[-len(self.data):]))
|
||||
return weighted_sum / weight_total
|
||||
|
||||
def reset_data(self) -> None:
|
||||
self.data = []
|
||||
|
||||
|
||||
class DynamicExperimentalController:
|
||||
def __init__(self, params=None):
|
||||
self._params = params or Params()
|
||||
self._is_enabled: bool = self._params.get_bool("DynamicExperimentalControl")
|
||||
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._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._has_blinkers = False
|
||||
|
||||
self._slowness_gmac = WeightedMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE)
|
||||
self._has_slowness = False
|
||||
|
||||
self._has_nav_instruction = False
|
||||
|
||||
self._dangerous_ttc_gmac = WeightedMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE)
|
||||
self._has_dangerous_ttc = False
|
||||
|
||||
self._v_ego_kph = 0.
|
||||
self._v_cruise_kph = 0.
|
||||
|
||||
self._has_lead = False
|
||||
|
||||
self._has_standstill = False
|
||||
self._has_standstill_prev = False
|
||||
|
||||
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_crash_cnt = 0
|
||||
|
||||
self._set_mode_timeout = 0
|
||||
|
||||
@staticmethod
|
||||
def _anomaly_detection(recent_data: list[float], threshold: float = 2.0, context_check: bool = True) -> bool:
|
||||
"""
|
||||
Basic anomaly detection using standard deviation.
|
||||
"""
|
||||
if len(recent_data) < 5:
|
||||
return False
|
||||
mean: float = float(np.mean(recent_data))
|
||||
std_dev: float = float(np.std(recent_data))
|
||||
anomaly: bool = bool(recent_data[-1] > mean + threshold * std_dev)
|
||||
|
||||
# Context check to ensure repeated anomaly
|
||||
if context_check:
|
||||
return bool(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.
|
||||
"""
|
||||
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)))
|
||||
)
|
||||
return adaptive_threshold
|
||||
|
||||
def _smoothed_lead_detection(self, lead_prob: float, smoothing_factor: float = 0.2) -> bool:
|
||||
"""
|
||||
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)
|
||||
|
||||
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)
|
||||
|
||||
def _update(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_lead = lead_one.status
|
||||
self._has_standstill = car_state.standstill
|
||||
|
||||
# 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
|
||||
|
||||
# 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
|
||||
lead_prob = self._lead_gmac.get_weighted_average() or 0
|
||||
self._has_lead_filtered = self._smoothed_lead_detection(lead_prob)
|
||||
|
||||
# adaptive slow down detection
|
||||
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
|
||||
|
||||
# anomaly detection for slow down events
|
||||
if self._anomaly_detection(self._slow_down_gmac.data):
|
||||
# Handle anomaly: potentially log it, adjust behavior, or issue a warning
|
||||
self._has_slow_down = False # Reset slow down if anomaly detected
|
||||
|
||||
# blinker detection
|
||||
self._has_blinkers = car_state.leftBlinker or car_state.rightBlinker
|
||||
|
||||
# sng detection
|
||||
if self._has_standstill:
|
||||
self._sng_state = SNG_State.stopped
|
||||
self._sng_transit_frame = 0
|
||||
else:
|
||||
if self._sng_transit_frame == 0:
|
||||
if self._sng_state == SNG_State.stopped:
|
||||
self._sng_state = SNG_State.going
|
||||
self._sng_transit_frame = STOP_AND_GO_FRAME
|
||||
elif self._sng_state == SNG_State.going:
|
||||
self._sng_state = SNG_State.off
|
||||
elif self._sng_transit_frame > 0:
|
||||
self._sng_transit_frame -= 1
|
||||
|
||||
# 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
|
||||
|
||||
# dangerous TTC detection
|
||||
if not self._has_lead_filtered and self._has_lead_filtered_prev:
|
||||
self._dangerous_ttc_gmac.reset_data()
|
||||
self._has_dangerous_ttc = False
|
||||
|
||||
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
|
||||
|
||||
# keep prev values
|
||||
self._has_standstill_prev = self._has_standstill
|
||||
self._has_lead_filtered_prev = self._has_lead_filtered
|
||||
|
||||
def _radarless_mode(self) -> None:
|
||||
# when mpc fcw crash prob is high
|
||||
# use blended to slow down quickly
|
||||
if self._has_mpc_fcw:
|
||||
self._set_mode('blended')
|
||||
return
|
||||
|
||||
# Nav enabled and distance to upcoming turning is 300 or below
|
||||
# if self._has_nav_instruction:
|
||||
# self._set_mode('blended')
|
||||
# return
|
||||
|
||||
# when blinker is on and speed is driving below V_ACC_MIN: blended
|
||||
# we don't want it to switch mode at higher speed, blended may trigger hard brake
|
||||
# if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
|
||||
# self._set_mode('blended')
|
||||
# return
|
||||
|
||||
# when at highway cruise and SNG: blended
|
||||
# ensuring blended mode is used because acc is bad at catching SNG lead car
|
||||
# especially those who accel very fast and then brake very hard.
|
||||
# if self._sng_state == SNG_State.going and self._v_cruise_kph >= V_ACC_MIN:
|
||||
# self._set_mode('blended')
|
||||
# return
|
||||
|
||||
# when standstill: blended
|
||||
# in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
|
||||
if self._has_standstill:
|
||||
self._set_mode('blended')
|
||||
return
|
||||
|
||||
# when detecting slow down scenario: blended
|
||||
# e.g. traffic light, curve, stop sign etc.
|
||||
if self._has_slow_down:
|
||||
self._set_mode('blended')
|
||||
return
|
||||
|
||||
# when detecting lead slow down: blended
|
||||
# use blended for higher braking capability
|
||||
if self._has_dangerous_ttc:
|
||||
self._set_mode('blended')
|
||||
return
|
||||
|
||||
# car driving at speed lower than set speed: acc
|
||||
if self._has_slowness:
|
||||
self._set_mode('acc')
|
||||
return
|
||||
|
||||
self._set_mode('acc')
|
||||
|
||||
def _radar_mode(self) -> None:
|
||||
# when mpc fcw crash prob is high
|
||||
# use blended to slow down quickly
|
||||
if self._has_mpc_fcw:
|
||||
self._set_mode('blended')
|
||||
return
|
||||
|
||||
# If there is a filtered lead, the vehicle is not in standstill, and the lead vehicle's yRel meets the condition,
|
||||
if self._has_lead_filtered and not self._has_standstill:
|
||||
self._set_mode('acc')
|
||||
return
|
||||
|
||||
# when blinker is on and speed is driving below V_ACC_MIN: blended
|
||||
# we don't want it to switch mode at higher speed, blended may trigger hard brake
|
||||
# if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
|
||||
# self._set_mode('blended')
|
||||
# return
|
||||
|
||||
# when standstill: blended
|
||||
# in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
|
||||
if self._has_standstill:
|
||||
self._set_mode('blended')
|
||||
return
|
||||
|
||||
# when detecting slow down scenario: blended
|
||||
# e.g. traffic light, curve, stop sign etc.
|
||||
if self._has_slow_down:
|
||||
self._set_mode('blended')
|
||||
return
|
||||
|
||||
# car driving at speed lower than set speed: acc
|
||||
if self._has_slowness:
|
||||
self._set_mode('acc')
|
||||
return
|
||||
|
||||
# Nav enabled and distance to upcoming turning is 300 or below
|
||||
# if self._has_nav_instruction:
|
||||
# self._set_mode('blended')
|
||||
# return
|
||||
|
||||
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_mode(self, mode: str) -> None:
|
||||
if self._set_mode_timeout == 0:
|
||||
self._mode = mode
|
||||
if mode == 'blended':
|
||||
self._set_mode_timeout = SET_MODE_TIMEOUT
|
||||
|
||||
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:
|
||||
self._read_params()
|
||||
|
||||
if self._is_enabled:
|
||||
self._update(sm)
|
||||
|
||||
if radar_unavailable:
|
||||
self._radarless_mode()
|
||||
else:
|
||||
self._radar_mode()
|
||||
|
||||
self._mode_changed = self._mode != self._mode_prev
|
||||
self._mode_prev = self._mode
|
||||
|
||||
self._frame += 1
|
||||
@@ -0,0 +1,46 @@
|
||||
"""
|
||||
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 messaging, custom
|
||||
from opendbc.car import structs
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
|
||||
|
||||
MpcSource = custom.LongitudinalPlanSP.MpcSource
|
||||
|
||||
|
||||
class DecPlanner:
|
||||
def __init__(self, CP: structs.CarParams, mpc):
|
||||
self.CP = CP
|
||||
self.mpc = mpc
|
||||
|
||||
self.is_enabled = False
|
||||
|
||||
self.dynamic_experimental_controller = DynamicExperimentalController()
|
||||
|
||||
def get_mpc_mode(self, sm: messaging.SubMaster):
|
||||
if not self.is_enabled or not sm['selfdriveState'].experimentalMode:
|
||||
return None
|
||||
|
||||
return self.dynamic_experimental_controller.get_mpc_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)
|
||||
|
||||
def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
|
||||
plan_sp_send = messaging.new_message('longitudinalPlanSP')
|
||||
|
||||
plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
|
||||
|
||||
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()
|
||||
|
||||
pm.send('longitudinalPlanSP', plan_sp_send)
|
||||
@@ -0,0 +1,257 @@
|
||||
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
|
||||
|
||||
class MockInterp:
|
||||
def __call__(self, x, xp, fp):
|
||||
return np.interp(x, xp, fp)
|
||||
|
||||
class MockCarState:
|
||||
def __init__(self, v_ego=0., standstill=False, left_blinker=False, right_blinker=False):
|
||||
self.vEgo = v_ego
|
||||
self.standstill = standstill
|
||||
self.leftBlinker = left_blinker
|
||||
self.rightBlinker = right_blinker
|
||||
|
||||
class MockLeadOne:
|
||||
def __init__(self, status=False, d_rel=0):
|
||||
self.status = status
|
||||
self.dRel = d_rel
|
||||
|
||||
class MockModelData:
|
||||
def __init__(self, x_vals=None, positions=None):
|
||||
self.orientation = type('Orientation', (), {'x': x_vals})()
|
||||
self.position = type('Position', (), {'x': positions})()
|
||||
|
||||
class MockControlState:
|
||||
def __init__(self, v_cruise=0):
|
||||
self.vCruise = v_cruise
|
||||
|
||||
@pytest.fixture
|
||||
def interp(monkeypatch):
|
||||
mock_interp = MockInterp()
|
||||
monkeypatch.setattr('openpilot.common.numpy_fast.interp', mock_interp)
|
||||
return mock_interp
|
||||
|
||||
@pytest.fixture
|
||||
def controller(interp):
|
||||
params = Params()
|
||||
params.put_bool("DynamicExperimentalControl", True)
|
||||
controller = DynamicExperimentalController()
|
||||
return controller
|
||||
|
||||
def test_initial_state(controller):
|
||||
"""Test initial state of the controller"""
|
||||
assert controller._mode == 'acc'
|
||||
assert not controller._has_lead
|
||||
assert not controller._has_standstill
|
||||
assert controller._sng_state == SNG_State.off
|
||||
assert not controller._has_lead_filtered
|
||||
assert not controller._has_slow_down
|
||||
assert not controller._has_dangerous_ttc
|
||||
assert not controller._has_mpc_fcw
|
||||
|
||||
@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
|
||||
def test_standstill_detection(controller, has_radar):
|
||||
"""Test standstill detection and state transitions"""
|
||||
car_state = MockCarState(standstill=True)
|
||||
lead_one = MockLeadOne()
|
||||
md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
|
||||
controls_state = MockControlState()
|
||||
|
||||
# Test transition to standstill
|
||||
controller.update(not has_radar, car_state, lead_one, md, controls_state)
|
||||
assert controller._sng_state == SNG_State.stopped
|
||||
assert controller.get_mpc_mode() == 'blended'
|
||||
|
||||
# Test transition from standstill to moving
|
||||
car_state.standstill = False
|
||||
controller.update(not has_radar, car_state, lead_one, md, controls_state)
|
||||
assert controller._sng_state == SNG_State.going
|
||||
|
||||
# Test complete transition to normal driving
|
||||
for _ in range(STOP_AND_GO_FRAME + 1):
|
||||
controller.update(not has_radar, car_state, lead_one, md, controls_state)
|
||||
assert controller._sng_state == SNG_State.off
|
||||
|
||||
@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
|
||||
def test_lead_detection(controller, has_radar):
|
||||
"""Test lead vehicle detection and filtering"""
|
||||
car_state = MockCarState(v_ego=20) # 72 kph
|
||||
lead_one = MockLeadOne(status=True, d_rel=50) # Safe distance
|
||||
md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
|
||||
controls_state = MockControlState(v_cruise=72)
|
||||
|
||||
# Let moving average stabilize
|
||||
for _ in range(LEAD_WINDOW_SIZE + 1):
|
||||
controller.update(not has_radar, car_state, lead_one, md, controls_state)
|
||||
|
||||
assert controller._has_lead_filtered
|
||||
expected_mode = 'acc' if has_radar else 'blended'
|
||||
assert controller.get_mpc_mode() == expected_mode
|
||||
|
||||
# Test lead loss detection
|
||||
lead_one.status = False
|
||||
for _ in range(LEAD_WINDOW_SIZE + 1):
|
||||
controller.update(not has_radar, car_state, lead_one, md, controls_state)
|
||||
|
||||
assert not controller._has_lead_filtered
|
||||
|
||||
@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
|
||||
def test_slow_down_detection(controller, has_radar):
|
||||
"""Test slow down detection based on trajectory"""
|
||||
car_state = MockCarState(v_ego=10/3.6) # 10 kph
|
||||
lead_one = MockLeadOne()
|
||||
x_vals = [0] * TRAJECTORY_SIZE
|
||||
positions = [20] * TRAJECTORY_SIZE # Position within slow down threshold
|
||||
md = MockModelData(x_vals=x_vals, positions=positions)
|
||||
controls_state = MockControlState(v_cruise=30)
|
||||
|
||||
# Test slow down detection
|
||||
for _ in range(SLOW_DOWN_WINDOW_SIZE + 1):
|
||||
controller.update(not has_radar, car_state, lead_one, md, controls_state)
|
||||
|
||||
assert controller._has_slow_down
|
||||
assert controller.get_mpc_mode() == 'blended'
|
||||
|
||||
# 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):
|
||||
controller.update(not has_radar, car_state, lead_one, md, controls_state)
|
||||
|
||||
assert not controller._has_slow_down
|
||||
|
||||
@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
|
||||
def test_dangerous_ttc_detection(controller, has_radar):
|
||||
"""Test Time-To-Collision detection and handling"""
|
||||
car_state = MockCarState(v_ego=10) # 36 kph
|
||||
lead_one = MockLeadOne(status=True)
|
||||
md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
|
||||
controls_state = MockControlState(v_cruise=36)
|
||||
|
||||
# First establish normal conditions with lead
|
||||
lead_one.dRel = 100 # Safe distance
|
||||
for _ in range(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
|
||||
|
||||
# Now test dangerous TTC detection
|
||||
lead_one.dRel = 10 # 10m distance - should trigger dangerous TTC
|
||||
# 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):
|
||||
controller.update(not has_radar, car_state, lead_one, md, controls_state)
|
||||
|
||||
assert controller._has_dangerous_ttc, "TTC of 1s should be considered dangerous"
|
||||
expected_mode = 'acc' if has_radar else 'blended'
|
||||
assert controller.get_mpc_mode() == expected_mode, f"Should be in [{expected_mode}] mode with dangerous TTC"
|
||||
|
||||
@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
|
||||
def test_mode_transitions(controller, has_radar):
|
||||
"""Test comprehensive mode transitions under different conditions"""
|
||||
# Initialize with normal driving conditions
|
||||
car_state = MockCarState(v_ego=25) # 90 kph
|
||||
lead_one = MockLeadOne(status=False)
|
||||
md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[200] * TRAJECTORY_SIZE)
|
||||
controls_state = MockControlState(v_cruise=100)
|
||||
|
||||
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):
|
||||
controller.update(not has_radar, car_state, lead_one, md, controls_state)
|
||||
|
||||
# Test 1: Normal driving -> ACC mode
|
||||
stabilize_filters()
|
||||
assert controller.get_mpc_mode() == 'acc', "Should be in ACC mode under normal driving conditions"
|
||||
|
||||
# Test 2: Standstill -> Blended mode
|
||||
car_state.standstill = True
|
||||
controller.update(not has_radar, car_state, lead_one, md, controls_state)
|
||||
assert controller.get_mpc_mode() == 'blended', "Should be in blended mode during standstill"
|
||||
|
||||
# Test 3: Lead car appears -> ACC mode
|
||||
car_state = MockCarState(v_ego=20) # Reset car state
|
||||
lead_one.status = True
|
||||
lead_one.dRel = 50 # Safe distance
|
||||
stabilize_filters()
|
||||
assert not controller._has_dangerous_ttc, "Should not have dangerous TTC"
|
||||
assert controller.get_mpc_mode() == 'acc', "Should be in ACC mode with safe lead distance"
|
||||
|
||||
# Test 4: Dangerous TTC -> Blended mode
|
||||
car_state = MockCarState(v_ego=20) # 72 kph
|
||||
lead_one.status = True
|
||||
lead_one.dRel = 50 # First establish normal lead detection
|
||||
|
||||
# First establish lead detection
|
||||
for _ in range(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
|
||||
|
||||
# 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):
|
||||
controller.update(not has_radar, car_state, lead_one, md, controls_state)
|
||||
|
||||
assert controller._has_dangerous_ttc, "Should detect dangerous TTC condition"
|
||||
expected_mode = 'acc' if has_radar else 'blended'
|
||||
assert controller.get_mpc_mode() == expected_mode, f"Should be in [{expected_mode}] mode with dangerous TTC"
|
||||
|
||||
@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
|
||||
def test_mpc_fcw_handling(controller, has_radar):
|
||||
"""Test MPC FCW crash count handling and mode transitions"""
|
||||
car_state = MockCarState(v_ego=20)
|
||||
lead_one = MockLeadOne()
|
||||
md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
|
||||
controls_state = MockControlState(v_cruise=72)
|
||||
|
||||
# Test FCW activation
|
||||
controller.set_mpc_fcw_crash_cnt(5)
|
||||
for _ in range(MPC_FCW_WINDOW_SIZE + 1):
|
||||
controller.update(not has_radar, car_state, lead_one, md, controls_state)
|
||||
|
||||
assert controller._has_mpc_fcw
|
||||
assert controller.get_mpc_mode() == 'blended'
|
||||
|
||||
# Test FCW recovery
|
||||
controller.set_mpc_fcw_crash_cnt(0)
|
||||
for _ in range(MPC_FCW_WINDOW_SIZE + 1):
|
||||
controller.update(not has_radar, car_state, lead_one, md, controls_state)
|
||||
|
||||
assert not controller._has_mpc_fcw
|
||||
|
||||
def test_radar_unavailable_handling(controller):
|
||||
"""Test behavior transitions between radar available and unavailable states"""
|
||||
car_state = MockCarState(v_ego=27.78) # 100 kph
|
||||
lead_one = MockLeadOne(status=True, d_rel=50)
|
||||
md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
|
||||
controls_state = MockControlState(v_cruise=100)
|
||||
|
||||
# Test with radar available
|
||||
for _ in range(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):
|
||||
controller.update(True, car_state, lead_one, md, controls_state)
|
||||
radarless_mode = controller.get_mpc_mode()
|
||||
|
||||
assert radar_mode is not None
|
||||
assert radarless_mode is not None
|
||||
@@ -43,6 +43,7 @@ def manager_init() -> None:
|
||||
]
|
||||
|
||||
sunnypilot_default_params: list[tuple[str, str | bytes]] = [
|
||||
("DynamicExperimentalControl", "0"),
|
||||
("Mads", "1"),
|
||||
("MadsMainCruiseAllowed", "1"),
|
||||
("MadsPauseLateralOnBrake", "0"),
|
||||
|
||||
Reference in New Issue
Block a user