mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-10-21 02:53:42 +08:00
Compare commits
49 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 6e5c616242 | |||
| 3629ca27a4 | |||
| 7f9439e89e | |||
| 9a013d56f9 | |||
| 4674f5bbec | |||
| 662b29eb6b | |||
| d811c8ddde | |||
| e693544fd7 | |||
| f9c876ca2d | |||
| 046a3ead4e | |||
| 723ebf374e | |||
| b24f546a0f | |||
| 3163577bc9 | |||
| 84fda467b5 | |||
| 04a8fb81ab | |||
| 05099ae19c | |||
| e6d035b639 | |||
| 45bfd88e7e | |||
| 86f2e7b255 | |||
| eb7a6fc0f8 | |||
| 033ba0d973 | |||
| 32460cd5b1 | |||
| 0042016791 | |||
| f6e2fb1c33 | |||
| 43cbdc65cd | |||
| 5823cf4f84 | |||
| 023ef9419a | |||
| 374659bf43 | |||
| 07f755a4c7 | |||
| f4996a0529 | |||
| 2f6e2560ea | |||
| 0414843e2f | |||
| 5acb9d2c8e | |||
| 2bf8f88978 | |||
| e6e5b1c165 | |||
| b7d06aa440 | |||
| a9ccd8fedf | |||
| 6220734b2d | |||
| f3f7296776 | |||
| 9f3a2607ee | |||
| b4c3a19db1 | |||
| 04098893d9 | |||
| bc064e892f | |||
| be51bc793d | |||
| f7c65f7d6a | |||
| bd27f3b54f | |||
| d7c1827134 | |||
| 1344824746 | |||
| 75b5926f4f |
@@ -170,7 +170,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}},
|
||||
{"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
|
||||
{"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
|
||||
{"FollowDistanceAccelBoost", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}},
|
||||
{"HasAcceptedTermsSP", {PERSISTENT, STRING, "0"}},
|
||||
|
||||
@@ -1,101 +0,0 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
from openpilot.cereal import log
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.selfdrive.controls.lib.accel_boost import (
|
||||
AccelBoost, ACCEL_BOOST_MAX, ACCEL_BOOST_PER_OVERRIDE, ACCEL_BOOST_RATE
|
||||
)
|
||||
|
||||
E2E_ACCEL = 0.5
|
||||
PLANNER_ACCEL = 2.0
|
||||
RATE_PER_STEP = ACCEL_BOOST_RATE * DT_MDL
|
||||
|
||||
|
||||
class FakeSubMaster:
|
||||
def __init__(self, enabled=True, gas_pressed=False, v_ego=15.0,
|
||||
personality=log.LongitudinalPersonality.standard):
|
||||
self._messages = {
|
||||
'selfdriveState': SimpleNamespace(enabled=enabled, personality=personality),
|
||||
'carState': SimpleNamespace(gasPressed=gas_pressed, vEgo=v_ego),
|
||||
}
|
||||
|
||||
def __getitem__(self, key):
|
||||
return self._messages[key]
|
||||
|
||||
|
||||
def run_boost(boost, sm, steps=1):
|
||||
out = E2E_ACCEL
|
||||
for _ in range(steps):
|
||||
out = boost.update(sm, E2E_ACCEL, PLANNER_ACCEL, PLANNER_ACCEL)
|
||||
return out
|
||||
|
||||
|
||||
class TestAccelBoost(OpenpilotTestCase):
|
||||
def _boost_to_max(self):
|
||||
boost = AccelBoost()
|
||||
sm = FakeSubMaster()
|
||||
steps = round(ACCEL_BOOST_PER_OVERRIDE / RATE_PER_STEP)
|
||||
# Eligibility is latched on a frame without gas, then applied on the next override.
|
||||
run_boost(boost, sm)
|
||||
for _ in range(10):
|
||||
sm['carState'].gasPressed = True
|
||||
run_boost(boost, sm, steps)
|
||||
sm['carState'].gasPressed = False
|
||||
run_boost(boost, sm)
|
||||
return boost
|
||||
|
||||
def test_no_boost_without_gas(self):
|
||||
boost = AccelBoost()
|
||||
sm = FakeSubMaster(gas_pressed=False)
|
||||
out = run_boost(boost, sm)
|
||||
self.assertEqual(boost.total_boost, 0.0)
|
||||
self.assertAlmostEqual(out, E2E_ACCEL)
|
||||
|
||||
def test_boost_requires_e2e_to_be_limiting(self):
|
||||
boost = AccelBoost()
|
||||
sm = FakeSubMaster(gas_pressed=False)
|
||||
# Eligibility is latched before the override, so keep the e2e plan at the planner limit here.
|
||||
boost.update(sm, E2E_ACCEL, E2E_ACCEL, E2E_ACCEL)
|
||||
sm['carState'].gasPressed = True
|
||||
for _ in range(10):
|
||||
boost.update(sm, E2E_ACCEL, E2E_ACCEL, E2E_ACCEL)
|
||||
self.assertEqual(boost.total_boost, 0.0)
|
||||
|
||||
def test_per_override_cap(self):
|
||||
boost = AccelBoost()
|
||||
sm = FakeSubMaster(gas_pressed=False)
|
||||
run_boost(boost, sm)
|
||||
sm['carState'].gasPressed = True
|
||||
run_boost(boost, sm, 1000)
|
||||
self.assertAlmostEqual(boost.total_boost, ACCEL_BOOST_PER_OVERRIDE)
|
||||
|
||||
def test_max_boost(self):
|
||||
boost = self._boost_to_max()
|
||||
self.assertAlmostEqual(boost.total_boost, ACCEL_BOOST_MAX)
|
||||
|
||||
def test_boost_is_applied_to_e2e_accel(self):
|
||||
boost = self._boost_to_max()
|
||||
sm = FakeSubMaster(gas_pressed=True)
|
||||
out = run_boost(boost, sm)
|
||||
self.assertAlmostEqual(out, E2E_ACCEL + boost.total_boost)
|
||||
|
||||
def test_boost_does_not_depend_on_following_distance(self):
|
||||
totals = []
|
||||
for personality in (log.LongitudinalPersonality.relaxed,
|
||||
log.LongitudinalPersonality.standard,
|
||||
log.LongitudinalPersonality.aggressive):
|
||||
boost = AccelBoost()
|
||||
sm = FakeSubMaster(personality=personality)
|
||||
steps = round(ACCEL_BOOST_PER_OVERRIDE / RATE_PER_STEP)
|
||||
run_boost(boost, sm)
|
||||
for _ in range(10):
|
||||
sm['carState'].gasPressed = True
|
||||
run_boost(boost, sm, steps)
|
||||
sm['carState'].gasPressed = False
|
||||
run_boost(boost, sm)
|
||||
totals.append(boost.total_boost)
|
||||
|
||||
self.assertEqual(len(totals), 3)
|
||||
for total in totals:
|
||||
self.assertAlmostEqual(total, ACCEL_BOOST_MAX)
|
||||
@@ -1,86 +0,0 @@
|
||||
import pyray as rl
|
||||
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.selfdrive.ui.mici.onroad import SIDE_PANEL_WIDTH
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.lib.application import FontWeight, gui_app
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost_ext import get_accel_boost_max
|
||||
|
||||
BAR_WIDTH = 10
|
||||
VALUE_FONT_SIZE = 17
|
||||
LABEL_FONT_SIZE = 10
|
||||
|
||||
COLOR_TRACK = rl.Color(255, 255, 255, 40)
|
||||
COLOR_TEXT = rl.Color(255, 255, 255, 235)
|
||||
|
||||
PROFILE_STYLES = {
|
||||
0: ("AGGR", rl.Color(255, 80, 60, 255)),
|
||||
1: ("STD", rl.Color(0, 230, 120, 255)),
|
||||
2: ("RELAX", rl.Color(70, 150, 255, 255)),
|
||||
}
|
||||
|
||||
|
||||
class AccelBoostIndicator(Widget):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self._boost_filter = FirstOrderFilter(0.0, 0.15, 1 / gui_app.target_fps)
|
||||
self._alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
|
||||
self._font = gui_app.font(FontWeight.BOLD)
|
||||
self._label_font = gui_app.font(FontWeight.MEDIUM)
|
||||
self._profile_label = "STD"
|
||||
self._profile_color = PROFILE_STYLES[1][1]
|
||||
self._max_boost = 0.0
|
||||
|
||||
def _update_state(self):
|
||||
sm = ui_state.sm
|
||||
visible = (ui_state.has_longitudinal_control and sm.alive['selfdriveState'] and sm.alive['longitudinalPlan'] and
|
||||
sm['selfdriveState'].enabled)
|
||||
self._alpha_filter.update(1.0 if visible else 0.0)
|
||||
self._boost_filter.update(float(sm['longitudinalPlan'].accelBoost) if visible else 0.0)
|
||||
|
||||
if sm.alive['selfdriveState']:
|
||||
personality = sm['selfdriveState'].personality
|
||||
self._profile_label, self._profile_color = PROFILE_STYLES.get(personality.raw, PROFILE_STYLES[1])
|
||||
self._max_boost = get_accel_boost_max(personality)
|
||||
|
||||
def _render(self, _):
|
||||
alpha = self._alpha_filter.x
|
||||
if alpha < 0.01:
|
||||
return
|
||||
|
||||
panel_x = self.rect.x + self.rect.width - SIDE_PANEL_WIDTH
|
||||
margin = min(120.0, max(56.0, self.rect.height * 0.14))
|
||||
track_top = self.rect.y + margin
|
||||
track_bottom = self.rect.y + self.rect.height - margin
|
||||
track_h = max(1.0, track_bottom - track_top)
|
||||
cx = panel_x + SIDE_PANEL_WIDTH / 2
|
||||
bar_x = cx - BAR_WIDTH / 2
|
||||
|
||||
# track
|
||||
rl.draw_rectangle_rounded(rl.Rectangle(bar_x, track_top, BAR_WIDTH, track_h), 0.5, 12,
|
||||
rl.Color(COLOR_TRACK.r, COLOR_TRACK.g, COLOR_TRACK.b, int(COLOR_TRACK.a * alpha)))
|
||||
|
||||
if self._max_boost <= 0.0:
|
||||
return
|
||||
|
||||
# Scale to the current profile's cap, so a maxed boost fills the bar.
|
||||
boost = min(max(self._boost_filter.x, 0.0), self._max_boost)
|
||||
fill_h = boost / self._max_boost * track_h
|
||||
if fill_h > 0.0:
|
||||
fill = rl.Color(self._profile_color.r, self._profile_color.g, self._profile_color.b, int(255 * alpha))
|
||||
if fill_h < BAR_WIDTH:
|
||||
rl.draw_circle(int(cx), int(track_bottom - fill_h / 2), max(1, int(BAR_WIDTH / 2)), fill)
|
||||
else:
|
||||
rl.draw_rectangle_rounded(rl.Rectangle(bar_x, track_bottom - fill_h, BAR_WIDTH, fill_h), 0.5, 12, fill)
|
||||
|
||||
label_size = measure_text_cached(self._label_font, self._profile_label, LABEL_FONT_SIZE)
|
||||
rl.draw_text_ex(self._label_font, self._profile_label, rl.Vector2(cx - label_size.x / 2, track_top - 42),
|
||||
LABEL_FONT_SIZE, 0,
|
||||
rl.Color(self._profile_color.r, self._profile_color.g, self._profile_color.b, int(230 * alpha)))
|
||||
|
||||
value = f"+{boost:.2f}"
|
||||
value_size = measure_text_cached(self._font, value, VALUE_FONT_SIZE)
|
||||
rl.draw_text_ex(self._font, value, rl.Vector2(cx - value_size.x / 2, track_top - 30), VALUE_FONT_SIZE, 0,
|
||||
rl.Color(COLOR_TEXT.r, COLOR_TEXT.g, COLOR_TEXT.b, int(COLOR_TEXT.a * alpha)))
|
||||
@@ -5,13 +5,13 @@ from opendbc.car.structs import car
|
||||
from openpilot.cereal.visionipc import VisionStreamType
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
|
||||
from openpilot.selfdrive.ui.mici.onroad import SIDE_PANEL_WIDTH
|
||||
from openpilot.selfdrive.ui.mici.onroad.accel_boost_indicator import AccelBoostIndicator
|
||||
from openpilot.selfdrive.ui.mici.onroad.alert_renderer import AlertRenderer
|
||||
from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer
|
||||
from openpilot.selfdrive.ui.mici.onroad.hud_renderer import HudRenderer
|
||||
from openpilot.selfdrive.ui.mici.onroad.long_indicator import LongIndicator
|
||||
from openpilot.selfdrive.ui.mici.onroad.model_renderer import ModelRenderer
|
||||
from openpilot.selfdrive.ui.mici.onroad.navigation import NavigationPanel, PANEL_WIDTH
|
||||
from openpilot.selfdrive.ui.mici.onroad.confidence_ball import ConfidenceBall
|
||||
from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
|
||||
from openpilot.system.ui.lib.application import FontWeight, gui_app, MousePos, MouseEvent, TextAlignment, TextAlignmentVertical
|
||||
from openpilot.system.ui.widgets.label import UnifiedLabel
|
||||
@@ -180,7 +180,7 @@ class AugmentedRoadView(CameraView):
|
||||
self._long_indicator = LongIndicator()
|
||||
self._alert_renderer = AlertRenderer()
|
||||
self._driver_state_renderer = DriverStateRenderer()
|
||||
self._accel_boost_indicator = AccelBoostIndicator()
|
||||
self._confidence_ball = ConfidenceBall()
|
||||
self._offroad_label = UnifiedLabel("start the car to\nuse sunnypilot", 54, FontWeight.DISPLAY,
|
||||
text_color=rl.Color(255, 255, 255, int(255 * 0.9)),
|
||||
alignment=TextAlignment.CENTER,
|
||||
@@ -275,7 +275,7 @@ class AugmentedRoadView(CameraView):
|
||||
if show_navigation:
|
||||
self._navigation.render(rl.Rectangle(self._content_rect.x + self._content_rect.width, self.rect.y, PANEL_WIDTH, self.rect.height))
|
||||
else:
|
||||
self._accel_boost_indicator.render(self.rect)
|
||||
self._confidence_ball.render(self.rect)
|
||||
|
||||
self._bookmark_icon.render(self.rect)
|
||||
|
||||
|
||||
@@ -30,11 +30,6 @@ ACC_NOLONG_DESCRIPTION = tr_noop("This feature can only be used with sunnypilot
|
||||
ACC_PCMCRUISE_DISABLED_DESCRIPTION = tr_noop("This feature is not supported on this platform due to vehicle limitations.")
|
||||
ONROAD_ONLY_DESCRIPTION = tr_noop("Start the vehicle to check vehicle compatibility.")
|
||||
|
||||
ACCEL_BOOST_EXT_DESCRIPTION = tr_noop("Adds extra acceleration on top of the stock gas-override boost when you hold the gas pedal " +
|
||||
"while sunnypilot longitudinal control is active. The extra boost follows the Following Distance setting: " +
|
||||
"strongest when Aggressive, weakest when Relaxed. The stock boost remains active when this is off.")
|
||||
ACCEL_BOOST_EXT_NOLONG_DESCRIPTION = tr_noop("This feature requires sunnypilot longitudinal control to be available.")
|
||||
|
||||
|
||||
class CruiseLayout(Widget):
|
||||
def __init__(self):
|
||||
@@ -81,11 +76,6 @@ class CruiseLayout(Widget):
|
||||
min_value=1, max_value=3, value_change_step=1,
|
||||
inline=True)
|
||||
|
||||
self.accel_boost_toggle = toggle_item_sp(
|
||||
title=tr("Extra Acceleration Boost by Following Distance"),
|
||||
description="",
|
||||
param="FollowDistanceAccelBoost")
|
||||
|
||||
self.sla_settings_button = simple_button_item_sp(
|
||||
button_text=lambda: tr("Speed Limit"),
|
||||
button_width=800,
|
||||
@@ -99,7 +89,6 @@ class CruiseLayout(Widget):
|
||||
self.custom_acc_toggle,
|
||||
self.custom_acc_short_increment,
|
||||
self.custom_acc_long_increment,
|
||||
self.accel_boost_toggle,
|
||||
self.sla_settings_button,
|
||||
]
|
||||
return items
|
||||
@@ -115,7 +104,6 @@ class CruiseLayout(Widget):
|
||||
self._scroller.show_event()
|
||||
self.icbm_toggle.show_description(True)
|
||||
self.custom_acc_toggle.show_description(True)
|
||||
self.accel_boost_toggle.show_description(True)
|
||||
|
||||
def _set_current_panel(self, panel: PanelType):
|
||||
self._current_panel = panel
|
||||
@@ -160,17 +148,10 @@ class CruiseLayout(Widget):
|
||||
self.scc_v_toggle.action_item.set_enabled(False)
|
||||
self.scc_m_toggle.action_item.set_enabled(False)
|
||||
|
||||
self.accel_boost_toggle.action_item.set_enabled(has_long and ui_state.is_offroad())
|
||||
boost_desc = tr(ACCEL_BOOST_EXT_DESCRIPTION) if has_long else tr(ACCEL_BOOST_EXT_NOLONG_DESCRIPTION)
|
||||
if self.accel_boost_toggle.description != boost_desc:
|
||||
self.accel_boost_toggle.set_description(boost_desc)
|
||||
self.accel_boost_toggle.show_description(True)
|
||||
|
||||
else:
|
||||
has_icbm = has_long = False
|
||||
self.icbm_toggle.action_item.set_enabled(False)
|
||||
self.icbm_toggle.set_description(tr(ONROAD_ONLY_DESCRIPTION))
|
||||
self.accel_boost_toggle.action_item.set_enabled(False)
|
||||
|
||||
show_custom_acc_desc = False
|
||||
|
||||
|
||||
@@ -10,6 +10,7 @@ import builtins
|
||||
import contextlib
|
||||
import dataclasses
|
||||
import functools
|
||||
import gc
|
||||
import hashlib
|
||||
import io
|
||||
import itertools
|
||||
@@ -111,17 +112,21 @@ def read_parts(path: str) -> tuple[dict, dict]:
|
||||
return parts, _MetadataUnpickler(io.BytesIO(bytes(blob[slice(*header["extras"])]))).load()
|
||||
|
||||
|
||||
def _extended(path: str, extend) -> tuple[dict, dict, str]:
|
||||
def _extended(path: str, extend) -> tuple[dict, dict, str, dict]:
|
||||
parts, extras = read_parts(path)
|
||||
added = extend(parts, extras) if extend else {}
|
||||
return parts | added, extras, _parts_digest(added) if added else ""
|
||||
return parts | added, extras, _parts_digest(added) if added else "", {key: parts[key] for key in added if key in parts}
|
||||
|
||||
|
||||
def load_model(path: str, device_map: dict | None = None, verify: bool = True, cache: bool = True, extend=None) -> dict:
|
||||
parts, extras, extra = _extended(path, extend)
|
||||
parts, extras, extra, replaced = _extended(path, extend)
|
||||
if cache and (cached := _read_cache(path, parts, device_map, extra)) is not None:
|
||||
return extras | cached
|
||||
shared: dict = {}
|
||||
if verify and replaced:
|
||||
for header, blob in replaced.values():
|
||||
PlainJit(header, blob, device_map, verify, shared)
|
||||
gc.collect()
|
||||
jits = {key: PlainJit(header, blob, device_map, verify, shared) for key, (header, blob) in parts.items()}
|
||||
if cache:
|
||||
_write_cache(path, jits, device_map, extra)
|
||||
|
||||
@@ -1,67 +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 openpilot.cereal import log
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.controls.lib.accel_boost import AccelBoost, ACCEL_BOOST_MAX
|
||||
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
|
||||
|
||||
SCALE_RELAXED = 0.3
|
||||
SCALE_STANDARD = 0.5
|
||||
SCALE_AGGRESSIVE = 0.7
|
||||
|
||||
|
||||
def get_accel_boost_scale(personality) -> float:
|
||||
if personality == log.LongitudinalPersonality.aggressive:
|
||||
return SCALE_AGGRESSIVE
|
||||
if personality == log.LongitudinalPersonality.relaxed:
|
||||
return SCALE_RELAXED
|
||||
return SCALE_STANDARD
|
||||
|
||||
|
||||
def get_accel_boost_max(personality) -> float:
|
||||
"""Maximum reported boost: the stock cap plus the profile-scaled extra cap."""
|
||||
return ACCEL_BOOST_MAX * (1.0 + get_accel_boost_scale(personality))
|
||||
|
||||
|
||||
class AccelBoostExt:
|
||||
def __init__(self, stock: AccelBoost | None = None):
|
||||
self._stock = stock if stock is not None else AccelBoost()
|
||||
self._extra = AccelBoost()
|
||||
self.params = Params()
|
||||
self.frame = -1
|
||||
self.enabled = self.params.get_bool("FollowDistanceAccelBoost")
|
||||
|
||||
self.stock_boost = 0.0
|
||||
self.extra_boost = 0.0
|
||||
|
||||
@property
|
||||
def total_boost(self) -> float:
|
||||
return self._stock.total_boost + self.extra_boost
|
||||
|
||||
def _update_params(self) -> None:
|
||||
if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
|
||||
self.enabled = self.params.get_bool("FollowDistanceAccelBoost")
|
||||
|
||||
def update(self, sm, output_a_target_e2e, output_a_target_mpc, a_cruise):
|
||||
self.frame += 1
|
||||
self._update_params()
|
||||
|
||||
if self.enabled and sm['selfdriveState'].enabled:
|
||||
scale = get_accel_boost_scale(sm['selfdriveState'].personality)
|
||||
extra_output = self._extra.update(sm, output_a_target_e2e, output_a_target_mpc, a_cruise)
|
||||
self.extra_boost = (extra_output - output_a_target_e2e) * scale
|
||||
else:
|
||||
self.extra_boost = 0.0
|
||||
self._extra.total_boost = 0.0
|
||||
self._extra.boost_this_override = 0.0
|
||||
self._extra.boost_eligible = False
|
||||
|
||||
target = output_a_target_e2e + self.extra_boost
|
||||
output = self._stock.update(sm, target, output_a_target_mpc, a_cruise)
|
||||
self.stock_boost = output - target
|
||||
return output
|
||||
@@ -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.accel_boost_ext import AccelBoostExt
|
||||
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
|
||||
@@ -33,16 +32,6 @@ class LongitudinalPlannerSP:
|
||||
|
||||
self.output_v_target = 0.
|
||||
self.output_a_target = 0.
|
||||
self._accel_boost: AccelBoostExt | None = None
|
||||
|
||||
@property
|
||||
def accel_boost(self) -> AccelBoostExt:
|
||||
assert self._accel_boost is not None
|
||||
return self._accel_boost
|
||||
|
||||
@accel_boost.setter
|
||||
def accel_boost(self, controller) -> None:
|
||||
self._accel_boost = AccelBoostExt(controller)
|
||||
|
||||
def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
|
||||
CS = sm['carState']
|
||||
|
||||
@@ -1,236 +0,0 @@
|
||||
from types import SimpleNamespace
|
||||
from unittest import mock
|
||||
|
||||
from openpilot.cereal import log
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.selfdrive.controls.lib.accel_boost import AccelBoost, ACCEL_BOOST_MAX, ACCEL_BOOST_PER_OVERRIDE
|
||||
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost_ext import AccelBoostExt, get_accel_boost_max
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP
|
||||
|
||||
E2E_ACCEL = 0.5
|
||||
PLANNER_ACCEL = 2.0
|
||||
STEPS_PER_OVERRIDE = round(ACCEL_BOOST_PER_OVERRIDE / (0.025 * DT_MDL))
|
||||
EXPECTED_SCALE = {
|
||||
log.LongitudinalPersonality.relaxed: 0.3,
|
||||
log.LongitudinalPersonality.standard: 0.5,
|
||||
log.LongitudinalPersonality.aggressive: 0.7,
|
||||
}
|
||||
|
||||
|
||||
class FakeParams:
|
||||
def __init__(self, enabled=True):
|
||||
self.enabled = enabled
|
||||
|
||||
def get_bool(self, key):
|
||||
return self.enabled
|
||||
|
||||
|
||||
class FakeSubMaster:
|
||||
def __init__(self, enabled=True, gas_pressed=False, v_ego=15.0,
|
||||
personality=log.LongitudinalPersonality.standard):
|
||||
self._messages = {
|
||||
'selfdriveState': SimpleNamespace(enabled=enabled, personality=personality),
|
||||
'carState': SimpleNamespace(gasPressed=gas_pressed, vEgo=v_ego),
|
||||
}
|
||||
|
||||
def __getitem__(self, key):
|
||||
return self._messages[key]
|
||||
|
||||
|
||||
def update(boost, sm, e2e=E2E_ACCEL):
|
||||
return boost.update(sm, e2e, PLANNER_ACCEL, PLANNER_ACCEL)
|
||||
|
||||
|
||||
class PlannerStub(LongitudinalPlannerSP):
|
||||
def __init__(self):
|
||||
self._accel_boost = None
|
||||
|
||||
|
||||
class TestAccelBoostExt(OpenpilotTestCase):
|
||||
def setUp(self):
|
||||
super().setUp()
|
||||
self.params = FakeParams()
|
||||
patcher = mock.patch("openpilot.sunnypilot.selfdrive.controls.lib.accel_boost_ext.Params", return_value=self.params)
|
||||
patcher.start()
|
||||
self.addCleanup(patcher.stop)
|
||||
|
||||
def _boost_to_max(self, personality):
|
||||
boost = AccelBoostExt()
|
||||
sm = FakeSubMaster(personality=personality)
|
||||
# Eligibility is latched on a frame without gas, then applied on the next override.
|
||||
update(boost, sm)
|
||||
for _ in range(10):
|
||||
sm['carState'].gasPressed = True
|
||||
for _ in range(STEPS_PER_OVERRIDE):
|
||||
update(boost, sm)
|
||||
sm['carState'].gasPressed = False
|
||||
update(boost, sm)
|
||||
return boost, sm
|
||||
|
||||
def _applied_extra(self, boost, sm):
|
||||
update(boost, sm)
|
||||
return boost.extra_boost
|
||||
|
||||
def test_off_matches_stock_boost_exactly(self):
|
||||
self.params.enabled = False
|
||||
sm = FakeSubMaster()
|
||||
stock = AccelBoost()
|
||||
ext = AccelBoostExt()
|
||||
|
||||
stock_out = update(stock, sm)
|
||||
ext_out = update(ext, sm)
|
||||
self.assertAlmostEqual(ext_out, stock_out)
|
||||
|
||||
for _ in range(10):
|
||||
sm['carState'].gasPressed = True
|
||||
for _ in range(STEPS_PER_OVERRIDE):
|
||||
stock_out = update(stock, sm)
|
||||
ext_out = update(ext, sm)
|
||||
self.assertAlmostEqual(ext_out, stock_out)
|
||||
sm['carState'].gasPressed = False
|
||||
stock_out = update(stock, sm)
|
||||
ext_out = update(ext, sm)
|
||||
self.assertAlmostEqual(ext_out, stock_out)
|
||||
|
||||
self.assertAlmostEqual(ext.total_boost, stock.total_boost)
|
||||
self.assertAlmostEqual(ext.stock_boost, stock_out - E2E_ACCEL)
|
||||
self.assertEqual(ext.extra_boost, 0.0)
|
||||
self.assertEqual(ext._extra.total_boost, 0.0)
|
||||
|
||||
def test_no_boost_without_gas(self):
|
||||
ext = AccelBoostExt()
|
||||
sm = FakeSubMaster(gas_pressed=False)
|
||||
out = update(ext, sm)
|
||||
self.assertEqual(ext.total_boost, 0.0)
|
||||
self.assertEqual(ext._extra.total_boost, 0.0)
|
||||
self.assertAlmostEqual(out, E2E_ACCEL)
|
||||
|
||||
def test_boost_requires_e2e_to_be_limiting(self):
|
||||
ext = AccelBoostExt()
|
||||
sm = FakeSubMaster(gas_pressed=False)
|
||||
ext.update(sm, E2E_ACCEL, E2E_ACCEL, E2E_ACCEL)
|
||||
sm['carState'].gasPressed = True
|
||||
for _ in range(10):
|
||||
ext.update(sm, E2E_ACCEL, E2E_ACCEL, E2E_ACCEL)
|
||||
self.assertEqual(ext.total_boost, 0.0)
|
||||
self.assertEqual(ext.stock_boost, 0.0)
|
||||
self.assertEqual(ext.extra_boost, 0.0)
|
||||
self.assertEqual(ext._extra.total_boost, 0.0)
|
||||
|
||||
def test_extra_layer_per_override_cap(self):
|
||||
ext = AccelBoostExt()
|
||||
sm = FakeSubMaster(gas_pressed=False)
|
||||
update(ext, sm)
|
||||
sm['carState'].gasPressed = True
|
||||
for _ in range(1000):
|
||||
update(ext, sm)
|
||||
|
||||
self.assertAlmostEqual(ext._extra.total_boost, ACCEL_BOOST_PER_OVERRIDE)
|
||||
applied = self._applied_extra(ext, sm)
|
||||
expected = ACCEL_BOOST_PER_OVERRIDE * EXPECTED_SCALE[log.LongitudinalPersonality.standard]
|
||||
self.assertAlmostEqual(applied, expected)
|
||||
|
||||
def test_extra_scales_with_following_distance(self):
|
||||
applied = {}
|
||||
for personality in (log.LongitudinalPersonality.relaxed,
|
||||
log.LongitudinalPersonality.standard,
|
||||
log.LongitudinalPersonality.aggressive):
|
||||
ext, sm = self._boost_to_max(personality)
|
||||
sm['carState'].gasPressed = True
|
||||
applied[personality] = self._applied_extra(ext, sm)
|
||||
|
||||
for personality, value in applied.items():
|
||||
self.assertAlmostEqual(value, ACCEL_BOOST_MAX * EXPECTED_SCALE[personality])
|
||||
|
||||
self.assertLess(applied[log.LongitudinalPersonality.relaxed], applied[log.LongitudinalPersonality.standard])
|
||||
self.assertLess(applied[log.LongitudinalPersonality.standard], applied[log.LongitudinalPersonality.aggressive])
|
||||
|
||||
def test_capnp_dynamic_enum_personality_scales(self):
|
||||
# Real SubMaster messages expose personality as a capnp DynamicEnum; equality still matches the enum value.
|
||||
msg = log.SelfdriveState.new_message()
|
||||
msg.personality = log.LongitudinalPersonality.aggressive
|
||||
ext, sm = self._boost_to_max(msg.personality)
|
||||
sm['carState'].gasPressed = True
|
||||
applied = self._applied_extra(ext, sm)
|
||||
self.assertAlmostEqual(applied, ACCEL_BOOST_MAX * EXPECTED_SCALE[log.LongitudinalPersonality.aggressive])
|
||||
|
||||
def test_extra_feeds_into_stock_last(self):
|
||||
ext, sm = self._boost_to_max(log.LongitudinalPersonality.standard)
|
||||
sm['carState'].gasPressed = True
|
||||
inputs = []
|
||||
original_update = AccelBoost.update
|
||||
|
||||
def spy(boost, sm_, e2e, mpc, cruise):
|
||||
inputs.append(e2e)
|
||||
return original_update(boost, sm_, e2e, mpc, cruise)
|
||||
|
||||
with mock.patch.object(AccelBoost, "update", spy):
|
||||
out = update(ext, sm)
|
||||
|
||||
# First call is our extra layer on the raw target, last call is stock on the boosted target.
|
||||
self.assertAlmostEqual(inputs[0], E2E_ACCEL)
|
||||
self.assertAlmostEqual(inputs[-1], E2E_ACCEL + ext.extra_boost)
|
||||
self.assertAlmostEqual(out, inputs[-1] + ext.stock_boost)
|
||||
|
||||
def test_extra_is_added_on_top_of_stock_boost(self):
|
||||
ext, sm = self._boost_to_max(log.LongitudinalPersonality.aggressive)
|
||||
sm['carState'].gasPressed = True
|
||||
out = update(ext, sm)
|
||||
extra = out - E2E_ACCEL - ext.stock_boost
|
||||
self.assertAlmostEqual(ext._stock.total_boost, ACCEL_BOOST_MAX)
|
||||
self.assertAlmostEqual(ext.total_boost, ACCEL_BOOST_MAX + ext.extra_boost)
|
||||
self.assertAlmostEqual(ext.stock_boost, ACCEL_BOOST_MAX)
|
||||
self.assertAlmostEqual(ext.extra_boost, ACCEL_BOOST_MAX * EXPECTED_SCALE[log.LongitudinalPersonality.aggressive])
|
||||
self.assertAlmostEqual(extra, ext.extra_boost)
|
||||
|
||||
def test_sp_planner_wraps_assigned_stock_controller(self):
|
||||
planner = PlannerStub()
|
||||
stock = AccelBoost()
|
||||
planner.accel_boost = stock
|
||||
|
||||
self.assertIsInstance(planner.accel_boost, AccelBoostExt)
|
||||
self.assertIs(planner.accel_boost._stock, stock)
|
||||
|
||||
def test_wraps_existing_stock_controller(self):
|
||||
stock = AccelBoost()
|
||||
sm = FakeSubMaster()
|
||||
update(stock, sm)
|
||||
sm['carState'].gasPressed = True
|
||||
update(stock, sm)
|
||||
|
||||
ext = AccelBoostExt(stock)
|
||||
self.assertIs(ext._stock, stock)
|
||||
self.assertAlmostEqual(ext.total_boost, stock.total_boost)
|
||||
|
||||
def test_boost_bar_max_matches_profile_cap(self):
|
||||
for personality, scale in EXPECTED_SCALE.items():
|
||||
self.assertAlmostEqual(get_accel_boost_max(personality), ACCEL_BOOST_MAX * (1.0 + scale))
|
||||
|
||||
def test_total_boost_is_stock_accumulated_plus_extra_applied(self):
|
||||
ext, sm = self._boost_to_max(log.LongitudinalPersonality.aggressive)
|
||||
sm['carState'].gasPressed = True
|
||||
update(ext, sm)
|
||||
self.assertAlmostEqual(ext.total_boost, ext._stock.total_boost + ext.extra_boost)
|
||||
|
||||
def test_turning_toggle_off_restores_stock_only(self):
|
||||
ext, sm = self._boost_to_max(log.LongitudinalPersonality.aggressive)
|
||||
self.assertGreater(ext._extra.total_boost, 0.0)
|
||||
|
||||
self.params.enabled = False
|
||||
sm['carState'].gasPressed = True
|
||||
# Params are re-read every PARAMS_UPDATE_PERIOD; the extra layer must clear on the next poll.
|
||||
for _ in range(int(PARAMS_UPDATE_PERIOD / DT_MDL) + 1):
|
||||
out = update(ext, sm)
|
||||
|
||||
self.assertEqual(ext._extra.total_boost, 0.0)
|
||||
self.assertEqual(ext.extra_boost, 0.0)
|
||||
self.assertAlmostEqual(out, E2E_ACCEL + ext.stock_boost)
|
||||
|
||||
def test_relaxing_following_distance_applies_immediately(self):
|
||||
ext, sm = self._boost_to_max(log.LongitudinalPersonality.aggressive)
|
||||
sm['selfdriveState'].personality = log.LongitudinalPersonality.relaxed
|
||||
sm['carState'].gasPressed = True
|
||||
applied = self._applied_extra(ext, sm)
|
||||
self.assertAlmostEqual(applied, ACCEL_BOOST_MAX * EXPECTED_SCALE[log.LongitudinalPersonality.relaxed])
|
||||
@@ -620,22 +620,6 @@
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "FollowDistanceAccelBoost",
|
||||
"widget": "toggle",
|
||||
"title": "Extra Acceleration Boost by Following Distance",
|
||||
"description": "Adds extra acceleration on top of the stock gas-override boost. The extra boost follows the Following Distance setting, strongest when Aggressive and weakest when Relaxed. The stock boost remains active when this is off.",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "offroad_only"
|
||||
},
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "IntelligentCruiseButtonManagement",
|
||||
"widget": "toggle",
|
||||
|
||||
@@ -30,15 +30,6 @@ sections:
|
||||
label: Relaxed
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: FollowDistanceAccelBoost
|
||||
widget: toggle
|
||||
title: Extra Acceleration Boost by Following Distance
|
||||
description: Adds extra acceleration on top of the stock gas-override boost. The extra boost follows
|
||||
the Following Distance setting, strongest when Aggressive and weakest when Relaxed. The stock boost
|
||||
remains active when this is off.
|
||||
enablement:
|
||||
- $ref: '#/macros/offroad'
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: IntelligentCruiseButtonManagement
|
||||
widget: toggle
|
||||
title: Intelligent Cruise Button Management (ICBM) (Alpha)
|
||||
|
||||
Reference in New Issue
Block a user