diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index aa6590af0b..275af5a2f5 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -170,6 +170,7 @@ inline static std::unordered_map 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"}}, diff --git a/openpilot/selfdrive/controls/tests/test_accel_boost.py b/openpilot/selfdrive/controls/tests/test_accel_boost.py new file mode 100644 index 0000000000..c9656b1d99 --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_accel_boost.py @@ -0,0 +1,101 @@ +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) diff --git a/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py b/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py index 406e0a77d1..f637e58dae 100644 --- a/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py +++ b/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py @@ -21,6 +21,9 @@ class TogglesLayoutMici(NavScroller): "Use the steering wheel distance button on supported cars.") self._initial_cruise_speed = InitialCruiseSpeedButton() self._initial_cruise_speed.set_visible(lambda: ui_state.CP is not None and ui_state.has_longitudinal_control and not ui_state.CP.pcmCruise) + self._accel_boost_toggle = BigParamControl("extra acceleration boost", "FollowDistanceAccelBoost", + description="Adds extra acceleration on top of the stock gas-override boost.\n" + + "The extra boost follows Following Distance: strongest when Aggressive, weakest when Relaxed.") is_metric_toggle = BigToggle("use metric units", initial_state=ui_state.params.get_bool("IsMetric"), toggle_callback=self._on_metric_toggle) ldw_toggle = BigParamControl("lane departure warnings", "IsLdwEnabled", description="Warn when you drift across a detected lane line.\n" + @@ -40,6 +43,7 @@ class TogglesLayoutMici(NavScroller): self._scroller.add_widgets([ self._personality_toggle, + self._accel_boost_toggle, self._initial_cruise_speed, is_metric_toggle, ldw_toggle, @@ -51,6 +55,7 @@ class TogglesLayoutMici(NavScroller): # Toggle lists self._refresh_toggles = ( + ("FollowDistanceAccelBoost", self._accel_boost_toggle), ("IsMetric", is_metric_toggle), ("IsLdwEnabled", ldw_toggle), ("AlwaysOnDM", always_on_dm_toggle), @@ -92,6 +97,7 @@ class TogglesLayoutMici(NavScroller): if ui_state.CP is not None: self._personality_toggle.set_visible(ui_state.has_longitudinal_control) + self._accel_boost_toggle.set_visible(ui_state.has_longitudinal_control) # Refresh toggles from params to mirror external changes for key, item in self._refresh_toggles: diff --git a/openpilot/selfdrive/ui/mici/onroad/accel_boost_indicator.py b/openpilot/selfdrive/ui/mici/onroad/accel_boost_indicator.py new file mode 100644 index 0000000000..280c032111 --- /dev/null +++ b/openpilot/selfdrive/ui/mici/onroad/accel_boost_indicator.py @@ -0,0 +1,119 @@ +""" +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. +""" +import pyray as rl + +from openpilot.cereal import log +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 gui_app +from openpilot.system.ui.widgets import Widget +from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost_ext import get_accel_boost_max + +# The readout is wider than the side panel, so the value is right-aligned and bleeds over the camera view +BAR_WIDTH = 24 +# Fades to white only near the profile cap, like the stock torque bar fading toward orange at high torque +CAP_BLEND_START = 0.75 +MAX_BOOST_BLEND = 0.45 + +COLOR_TRACK = rl.Color(255, 255, 255, 45) +COLOR_TRACK_EDGE = rl.Color(255, 255, 255, 70) + +AGGRESSIVE_COLOR = rl.Color(255, 80, 60, 255) +STANDARD_COLOR = rl.Color(0, 230, 120, 255) +RELAXED_COLOR = rl.Color(70, 150, 255, 255) + +FADE_THRESHOLD = 0.01 + + +def lighten(color: rl.Color, amount: float) -> rl.Color: + """Lerp towards white in RGB, so the hue stays put the way HSV blending would not.""" + amount = min(max(amount, 0.0), 1.0) + return rl.Color(int(color.r + (255 - color.r) * amount), + int(color.g + (255 - color.g) * amount), + int(color.b + (255 - color.b) * amount), color.a) + + +def get_boost_fraction(boost: float, max_boost: float) -> float: + """How much of the active profile's boost budget is in use, clamped to a full bar.""" + if max_boost <= 0.0: + return 0.0 + return min(max(boost / max_boost, 0.0), 1.0) + + +def get_boost_style(personality) -> tuple[rl.Color, float]: + if personality == log.LongitudinalPersonality.aggressive: + color = AGGRESSIVE_COLOR + elif personality == log.LongitudinalPersonality.relaxed: + color = RELAXED_COLOR + else: + color = STANDARD_COLOR + + return color, get_accel_boost_max(personality) + + +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) + # Smoothly rescale the bar when the follow distance profile changes mid-drive + self._max_boost_filter = FirstOrderFilter(0.0, 0.25, 1 / gui_app.target_fps) + self._profile_color = STANDARD_COLOR + self._max_boost = 0.0 + + self.panel_in_use = False + + def _update_state(self): + sm = ui_state.sm + extra_enabled = ui_state.follow_distance_accel_boost + 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 and extra_enabled else 0.0) + self._boost_filter.update(float(sm['longitudinalPlan'].accelBoost) if visible else 0.0) + + if sm.alive['selfdriveState']: + self._profile_color, self._max_boost = get_boost_style(sm['selfdriveState'].personality) + self._max_boost_filter.update(self._max_boost) + + self.panel_in_use = extra_enabled and ui_state.has_longitudinal_control + + def _render(self, _): + alpha = self._alpha_filter.x + if alpha < FADE_THRESHOLD or self._max_boost <= 0.0: + return + + cx = self.rect.x + self.rect.width - SIDE_PANEL_WIDTH / 2 + bar_x = cx - BAR_WIDTH / 2 + track_top = self.rect.y + self.rect.height * 0.08 + track_bottom = self.rect.y + self.rect.height * 0.92 + track_h = max(1.0, track_bottom - track_top) + + # track, light enough to read on the black side panel + 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))) + rl.draw_rectangle_rounded_lines_ex(rl.Rectangle(bar_x, track_top, BAR_WIDTH, track_h), 0.5, 12, 1, + rl.Color(COLOR_TRACK_EDGE.r, COLOR_TRACK_EDGE.g, COLOR_TRACK_EDGE.b, + int(COLOR_TRACK_EDGE.a * alpha))) + + # Scale to the current profile's cap, so a maxed boost fills the bar and a profile change rescales it + boost = max(self._boost_filter.x, 0.0) + ratio = get_boost_fraction(boost, self._max_boost_filter.x) + fill_h = ratio * track_h + cap_blend = max(0.0, (ratio - CAP_BLEND_START) / (1.0 - CAP_BLEND_START)) + fill_color = lighten(self._profile_color, MAX_BOOST_BLEND * cap_blend) + if fill_h > 0.0: + # soft glow around the tip, like the confidence ball's gradient + tip_y = track_bottom - fill_h + glow = rl.Color(fill_color.r, fill_color.g, fill_color.b, int(70 * alpha)) + rl.draw_circle_gradient(rl.Vector2(cx, tip_y), int(BAR_WIDTH * 0.9), glow, rl.BLANK) + + fill = rl.Color(fill_color.r, fill_color.g, fill_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) diff --git a/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py b/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py index ffd7d94b2e..11f89c117f 100644 --- a/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py +++ b/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py @@ -5,6 +5,7 @@ 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 @@ -181,6 +182,7 @@ class AugmentedRoadView(CameraView): self._alert_renderer = AlertRenderer() self._driver_state_renderer = DriverStateRenderer() self._confidence_ball = ConfidenceBall() + self._accel_boost_indicator = AccelBoostIndicator() 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 +277,10 @@ 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._confidence_ball.render(self.rect) + # The boost bar owns the side panel while the extra boost is on; the ball only shows with the feature off + self._accel_boost_indicator.render(self.rect) + if not self._accel_boost_indicator.panel_in_use: + self._confidence_ball.render(self.rect) self._bookmark_icon.render(self.rect) diff --git a/openpilot/selfdrive/ui/mici/tests/test_accel_boost_indicator.py b/openpilot/selfdrive/ui/mici/tests/test_accel_boost_indicator.py new file mode 100644 index 0000000000..041a949196 --- /dev/null +++ b/openpilot/selfdrive/ui/mici/tests/test_accel_boost_indicator.py @@ -0,0 +1,53 @@ +import unittest + +from openpilot.cereal import log +from openpilot.selfdrive.controls.lib.accel_boost import ACCEL_BOOST_MAX +from openpilot.selfdrive.ui.mici.onroad.accel_boost_indicator import (get_boost_style, get_boost_fraction, lighten, + AGGRESSIVE_COLOR, RELAXED_COLOR, STANDARD_COLOR) +from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost_ext import (SCALE_AGGRESSIVE, SCALE_RELAXED, SCALE_STANDARD, + get_accel_boost_max) + + +class TestAccelBoostIndicator(unittest.TestCase): + def test_bar_scales_with_following_distance(self): + for personality, color, scale in ( + (log.LongitudinalPersonality.relaxed, RELAXED_COLOR, SCALE_RELAXED), + (log.LongitudinalPersonality.standard, STANDARD_COLOR, SCALE_STANDARD), + (log.LongitudinalPersonality.aggressive, AGGRESSIVE_COLOR, SCALE_AGGRESSIVE)): + bar_color, full_scale = get_boost_style(personality) + self.assertEqual(bar_color, color) + self.assertAlmostEqual(full_scale, ACCEL_BOOST_MAX * (1.0 + scale)) + + def test_bar_fills_at_the_active_profile_cap(self): + caps = [get_accel_boost_max(p) for p in (log.LongitudinalPersonality.relaxed, + log.LongitudinalPersonality.standard, + log.LongitudinalPersonality.aggressive)] + self.assertEqual(caps, sorted(caps)) + + for cap in caps: + self.assertAlmostEqual(get_boost_fraction(cap, cap), 1.0) + self.assertAlmostEqual(get_boost_fraction(0.0, cap), 0.0) + self.assertAlmostEqual(get_boost_fraction(cap * 0.5, cap), 0.5) + + # A boost beyond the current cap (e.g. after switching to a shorter profile) still fills the bar + self.assertAlmostEqual(get_boost_fraction(caps[2], caps[0]), 1.0) + self.assertAlmostEqual(get_boost_fraction(0.0, 0.0), 0.0) + + def test_cap_lighten_keeps_the_profile_hue(self): + # An HSV blend towards white sweeps relaxed blue through pink, so the fill lightens in RGB instead + relaxed = lighten(RELAXED_COLOR, 0.45) + aggressive = lighten(AGGRESSIVE_COLOR, 0.45) + self.assertGreater(relaxed.b, relaxed.r) + self.assertGreaterEqual(relaxed.b, relaxed.g) + self.assertGreater(aggressive.r, aggressive.b) + # lighter in RGB means the dimmest channel came up towards white + self.assertGreater(relaxed.r, RELAXED_COLOR.r) + self.assertGreater(aggressive.g, AGGRESSIVE_COLOR.g) + + def test_wire_personality_picks_the_right_profile(self): + # Over the wire personality is a capnp DynamicEnum, which compares equal to the cereal enum but hashes differently + msg = log.SelfdriveState.new_message() + msg.personality = log.LongitudinalPersonality.aggressive + color, full_scale = get_boost_style(msg.personality) + self.assertEqual(color, AGGRESSIVE_COLOR) + self.assertAlmostEqual(full_scale, get_accel_boost_max(log.LongitudinalPersonality.aggressive)) diff --git a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/cruise.py b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/cruise.py index 61254ce624..1570ec1fa6 100644 --- a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/cruise.py +++ b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/cruise.py @@ -30,6 +30,11 @@ 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): @@ -76,6 +81,11 @@ 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, @@ -86,6 +96,7 @@ class CruiseLayout(Widget): self.icbm_toggle, self.scc_v_toggle, self.scc_m_toggle, + self.accel_boost_toggle, self.custom_acc_toggle, self.custom_acc_short_increment, self.custom_acc_long_increment, @@ -104,6 +115,7 @@ 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 @@ -148,10 +160,17 @@ 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) + 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 diff --git a/openpilot/selfdrive/ui/sunnypilot/ui_state.py b/openpilot/selfdrive/ui/sunnypilot/ui_state.py index e5ddd70400..04972b3ded 100644 --- a/openpilot/selfdrive/ui/sunnypilot/ui_state.py +++ b/openpilot/selfdrive/ui/sunnypilot/ui_state.py @@ -64,6 +64,7 @@ class UIStateSP: self.enforce_torque_control: bool = False self.custom_torque_params: bool = False self.torque_override_enabled: bool = False + self.follow_distance_accel_boost: bool = False self._sp_initialized: bool = False def update(self) -> None: @@ -177,6 +178,7 @@ class UIStateSP: self.enforce_torque_control = self.params.get_bool("EnforceTorqueControl") self.custom_torque_params = self.params.get_bool("CustomTorqueParams") self.torque_override_enabled = self.params.get_bool("TorqueParamsOverrideEnabled") + self.follow_distance_accel_boost = self.params.get_bool("FollowDistanceAccelBoost") self.torque_override_lat_accel_factor = float(self.params.get("TorqueParamsOverrideLatAccelFactor", return_default=True)) self.torque_override_friction = float(self.params.get("TorqueParamsOverrideFriction", return_default=True)) self.true_v_ego_ui = self.params.get_bool("TrueVEgoUI") diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_boost_ext.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_boost_ext.py new file mode 100644 index 0000000000..c31b0e7742 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_boost_ext.py @@ -0,0 +1,72 @@ +""" +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, extra_enabled: bool = True) -> float: + if not extra_enabled: + return ACCEL_BOOST_MAX + 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 + + output = self._stock.update(sm, output_a_target_e2e, output_a_target_mpc, a_cruise) + self.stock_boost = output - output_a_target_e2e + return output + + 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 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index a579e61c59..01ee8b8d6d 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -9,6 +9,7 @@ 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 @@ -32,6 +33,16 @@ 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'] diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_boost_ext.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_boost_ext.py new file mode 100644 index 0000000000..6d0a4fbe2c --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_boost_ext.py @@ -0,0 +1,237 @@ +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)) + self.assertAlmostEqual(get_accel_boost_max(personality, extra_enabled=False), ACCEL_BOOST_MAX) + + 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]) diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index 5c70505de4..e34878767d 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -620,6 +620,19 @@ } ] }, + { + "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": "capability", + "field": "has_longitudinal_control", + "equals": true + } + ] + }, { "key": "IntelligentCruiseButtonManagement", "widget": "toggle", diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml index 39d6fbffd4..b5ae458fbe 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml @@ -30,6 +30,14 @@ 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/longitudinal' - key: IntelligentCruiseButtonManagement widget: toggle title: Intelligent Cruise Button Management (ICBM) (Alpha)