diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 1ccd8110a6..ec47d12d2e 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -149,6 +149,7 @@ inline static std::unordered_map keys = { {"Version", {PERSISTENT, STRING}}, // --- sunnypilot params --- // + {"AccelBoostSP", {PERSISTENT | BACKUP, BOOL, "1"}}, {"ApiCache_DriveStats", {PERSISTENT, JSON}}, {"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}}, {"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}}, diff --git a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/cruise.py b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/cruise.py index 61254ce624..04f164568a 100644 --- a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/cruise.py +++ b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/cruise.py @@ -57,6 +57,12 @@ class CruiseLayout(Widget): description=tr("Use map data to estimate the appropriate speed to drive through turns ahead."), param="SmartCruiseControlMap") + self.accel_boost_toggle = toggle_item_sp( + title=tr("Acceleration Boost"), + description=tr("Adds a small push on top of sunnypilot's own acceleration, stronger in Aggressive and gentler in Relaxed. " + + "Eases off in curves and when sunnypilot slows or stops. Never goes past your set speed, and lead-car following still comes first."), + param="AccelBoostSP") + self.custom_acc_toggle = toggle_item_sp( title=tr("Custom ACC Speed Increments"), description="", @@ -86,6 +92,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, @@ -136,6 +143,8 @@ class CruiseLayout(Widget): self.icbm_toggle.set_description(new_desc) self.icbm_toggle.show_description(True) + self.accel_boost_toggle.action_item.set_enabled(has_long) + if has_long or has_icbm: self.custom_acc_toggle.action_item.set_enabled(((has_long and not ui_state.CP.pcmCruise) or has_icbm) and ui_state.is_offroad()) self.scc_v_toggle.action_item.set_enabled(True) @@ -151,6 +160,7 @@ class CruiseLayout(Widget): else: has_icbm = has_long = False self.icbm_toggle.action_item.set_enabled(False) + self.accel_boost_toggle.action_item.set_enabled(False) self.icbm_toggle.set_description(tr(ONROAD_ONLY_DESCRIPTION)) show_custom_acc_desc = False diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_boost.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_boost.py new file mode 100644 index 0000000000..d2bb8b3b64 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_boost.py @@ -0,0 +1,67 @@ +""" +Copyright (c) 2021-, 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 numpy as np + +from openpilot.cereal import log +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD + +ACCEL_BOOST_MIN_SPEED = 1.0 +ACCEL_BOOST_MAX = (0.4, 0.25, 0.1) # aggressive, standard, relaxed, m/s^2 +ACCEL_BOOST_BUILD, ACCEL_BOOST_RELEASE = 0.2, 1.0 # m/s^3 +BRAKE_PROB = (0.1, 0.3) # the model's brake press in 4 s: it wants to slow +GAS_PROB = (0.6, 0.8) # predicts a human gas press at this hypothesis at about 23% +CURVE_LAT_ACCEL = (1.5, 2.5) + + +def get_boost_max(personality=log.LongitudinalPersonality.standard): + if personality == log.LongitudinalPersonality.relaxed: + return ACCEL_BOOST_MAX[2] + elif personality == log.LongitudinalPersonality.standard: + return ACCEL_BOOST_MAX[1] + elif personality == log.LongitudinalPersonality.aggressive: + return ACCEL_BOOST_MAX[0] + else: + raise NotImplementedError("Longitudinal personality not supported") + + +class AccelBoostSP: + def __init__(self): + self.params = Params() + self.frame = -1 + self.enabled = False + self.boost = 0.0 + self.total_boost = 0.0 + + def update_params(self): + self.frame += 1 + if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) != 0: + return False + enabled = self.params.get_bool("AccelBoostSP") + if enabled == self.enabled: + return False + self.enabled = enabled + self.boost = self.total_boost = 0.0 + return True + + def update(self, sm, output_a_target_e2e, output_a_target_mpc, a_cruise): + model = sm['modelV2'] + brake_probs = model.meta.disengagePredictions.brakePressProbs + gas_probs = model.meta.disengagePredictions.gasPressProbs + if not sm['selfdriveState'].enabled or len(brake_probs) < 2 or len(gas_probs) < 2: + self.boost = self.total_boost = 0.0 + return output_a_target_e2e + + want = max(float(np.interp(brake_probs[1], BRAKE_PROB, [1.0, 0.0])), float(np.interp(gas_probs[1], GAS_PROB, [0.0, 1.0]))) + want = 0.0 if model.action.shouldStop else want + want *= float(np.interp(abs(model.action.desiredCurvature) * sm['carState'].vEgo ** 2, CURVE_LAT_ACCEL, [1.0, 0.0])) + target = want * get_boost_max(sm['selfdriveState'].personality) + self.boost = float(np.clip(target, self.boost - ACCEL_BOOST_RELEASE * DT_MDL, self.boost + ACCEL_BOOST_BUILD * DT_MDL)) + speed_scale = np.interp(sm['carState'].vEgo, [ACCEL_BOOST_MIN_SPEED, 2 * ACCEL_BOOST_MIN_SPEED], [0.0, 1.0]) + self.total_boost = self.boost * speed_scale * np.interp(output_a_target_e2e, [-0.7, -0.2, 5.0], [0.0, 1.0, 1.0], right=0.0) + return output_a_target_e2e + self.total_boost diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index a579e61c59..f379f29cd8 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -9,6 +9,8 @@ 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.selfdrive.controls.lib.accel_boost import AccelBoost +from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost import AccelBoostSP 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 @@ -29,6 +31,8 @@ class LongitudinalPlannerSP: self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None self.source = LongitudinalPlanSource.cruise self.e2e_alerts_helper = E2EAlertsHelper() + self.accel_boost_sp = AccelBoostSP() + self.openpilot_longitudinal = CP.openpilotLongitudinalControl self.output_v_target = 0. self.output_a_target = 0. @@ -64,6 +68,9 @@ class LongitudinalPlannerSP: return self.output_v_target, self.output_a_target def update(self, sm: messaging.SubMaster) -> None: + if self.accel_boost_sp.update_params(): + self.accel_boost = self.accel_boost_sp if self.accel_boost_sp.enabled and self.openpilot_longitudinal else AccelBoost() + self.events_sp.clear() self.e2e_alerts_helper.update(sm, self.events_sp) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_boost.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_boost.py new file mode 100644 index 0000000000..b054a8f6fb --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_boost.py @@ -0,0 +1,81 @@ +""" +Copyright (c) 2021-, 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 types import SimpleNamespace + +from openpilot.common.parameterized import parameterized +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.common.test import OpenpilotTestCase + +from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD +from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost import AccelBoostSP, ACCEL_BOOST_MAX + + +def make_sm(brake_prob=0.05, gas_prob=0.3, lat_accel=0.0, v_ego=30.0, personality=0, enabled=True, should_stop=False): + meta = SimpleNamespace(disengagePredictions=SimpleNamespace(brakePressProbs=[0.0, brake_prob, 0.0, 0.0, 0.0, 0.0], + gasPressProbs=[0.0, gas_prob, 0.0, 0.0, 0.0, 0.0])) + action = SimpleNamespace(shouldStop=should_stop, desiredCurvature=lat_accel / v_ego ** 2) + return {'modelV2': SimpleNamespace(meta=meta, action=action), 'carState': SimpleNamespace(vEgo=v_ego), + 'selfdriveState': SimpleNamespace(enabled=enabled, personality=personality)} + + +def settle(sm, frames=100): + boost = AccelBoostSP() + for _ in range(frames): + boost.update(sm, 0.0, 9.0, 9.0) + return boost.total_boost + + +class TestAccelBoostSP(OpenpilotTestCase): + @parameterized.expand([(0,), (1,), (2,)]) + def test_personality_max(self, personality): + assert abs(settle(make_sm(personality=personality)) - ACCEL_BOOST_MAX[personality]) < 1e-6 + + def test_brake_gate_closes(self): + assert settle(make_sm(brake_prob=0.5)) == 0.0 + + def test_gas_reopens(self): + assert abs(settle(make_sm(brake_prob=0.5, gas_prob=0.9)) - ACCEL_BOOST_MAX[0]) < 1e-6 + + def test_should_stop(self): + assert settle(make_sm(gas_prob=0.9, should_stop=True)) == 0.0 + + @parameterized.expand([(1.5, 1.0), (2.0, 0.5), (2.5, 0.0), (-2.0, 0.5)]) + def test_curve_fade(self, lat_accel, scale): + assert abs(settle(make_sm(lat_accel=lat_accel)) - scale * ACCEL_BOOST_MAX[0]) < 1e-6 + + def test_low_speed(self): + assert settle(make_sm(v_ego=0.5)) == 0.0 + + def test_disabled_resets(self): + boost = AccelBoostSP() + for _ in range(100): + boost.update(make_sm(), 0.0, 9.0, 9.0) + assert boost.update(make_sm(enabled=False), 0.0, 9.0, 9.0) == 0.0 + assert boost.boost == 0.0 and boost.total_boost == 0.0 + + def test_build_and_release_rates(self): + boost = AccelBoostSP() + boost.update(make_sm(), 0.0, 9.0, 9.0) + assert abs(boost.total_boost - 0.01) < 1e-6 + for _ in range(100): + boost.update(make_sm(), 0.0, 9.0, 9.0) + boost.update(make_sm(brake_prob=0.5), 0.0, 9.0, 9.0) + assert abs(boost.total_boost - (ACCEL_BOOST_MAX[0] - 0.05)) < 1e-6 + + def test_param_switch(self): + params = Params() + boost = AccelBoostSP() + params.put_bool("AccelBoostSP", True, block=True) + assert boost.update_params() and boost.enabled + for _ in range(100): + boost.update(make_sm(), 0.0, 9.0, 9.0) + params.put_bool("AccelBoostSP", False, block=True) + for _ in range(int(PARAMS_UPDATE_PERIOD / DT_MDL) - 1): + assert not boost.update_params() + assert boost.update_params() and not boost.enabled + assert boost.boost == 0.0 and boost.total_boost == 0.0 diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index 215331dde1..06068fca38 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -620,6 +620,19 @@ } ] }, + { + "key": "AccelBoostSP", + "widget": "toggle", + "title": "Acceleration Boost", + "description": "Adds a small push on top of sunnypilot's own acceleration, stronger in Aggressive and gentler in Relaxed. Eases off in curves and when sunnypilot slows or stops. Never goes past your set speed, and lead-car following still comes first.", + "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..efc703ff15 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml @@ -30,6 +30,12 @@ sections: label: Relaxed enablement: - $ref: '#/macros/longitudinal' + - key: AccelBoostSP + widget: toggle + title: Acceleration Boost + description: "Adds a small push on top of sunnypilot's own acceleration, stronger in Aggressive and gentler in Relaxed. Eases off in curves and when sunnypilot slows or stops. Never goes past your set speed, and lead-car following still comes first." + enablement: + - $ref: '#/macros/longitudinal' - key: IntelligentCruiseButtonManagement widget: toggle title: Intelligent Cruise Button Management (ICBM) (Alpha)