mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-11-04 06:04:15 +08:00
Compare commits
5 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 048727fb9f | |||
| 0b68b8be9d | |||
| 88b3ed1a20 | |||
| 8b3a19eb04 | |||
| 367ff603d4 |
@@ -149,6 +149,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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"}},
|
||||
|
||||
@@ -7,7 +7,6 @@ from openpilot.common.constants import CV
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.selfdrive.controls.lib.accel_boost import AccelBoost
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, LongitudinalPlanSource
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
|
||||
@@ -38,7 +37,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc)
|
||||
self.fcw = False
|
||||
self.dt = dt
|
||||
self.accel_boost = AccelBoost()
|
||||
|
||||
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
|
||||
self.a_cruise = init_a
|
||||
|
||||
@@ -22,6 +22,7 @@ from openpilot.common.transformations.orientation import rot_from_euler
|
||||
from enum import IntEnum
|
||||
|
||||
if gui_app.sunnypilot_ui():
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.onroad.accel_boost_indicator import AccelBoostIndicator
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.onroad.hud_renderer import HudRendererSP as HudRenderer
|
||||
from openpilot.selfdrive.ui.sunnypilot.ui_state import OnroadTimerStatus
|
||||
|
||||
@@ -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() if gui_app.sunnypilot_ui() else None
|
||||
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,
|
||||
@@ -274,6 +276,11 @@ class AugmentedRoadView(CameraView):
|
||||
# Use self._content_rect for positioning within camera bounds
|
||||
if show_navigation:
|
||||
self._navigation.render(rl.Rectangle(self._content_rect.x + self._content_rect.width, self.rect.y, PANEL_WIDTH, self.rect.height))
|
||||
elif self._accel_boost_indicator is not None:
|
||||
# The boost bar owns the side panel while the feature is on; the ball only shows otherwise
|
||||
self._accel_boost_indicator.render(self.rect)
|
||||
if not self._accel_boost_indicator.panel_in_use:
|
||||
self._confidence_ball.render(self.rect)
|
||||
else:
|
||||
self._confidence_ball.render(self.rect)
|
||||
|
||||
|
||||
@@ -0,0 +1,146 @@
|
||||
"""
|
||||
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.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.sunnypilot.selfdrive.controls.lib.accel_boost import ACCEL_BOOST_MAX
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
|
||||
BAR_WIDTH = 26
|
||||
BAR_RADIUS = 0.5
|
||||
TRACK_TOP_RATIO = 0.08
|
||||
TRACK_BOTTOM_RATIO = 0.92
|
||||
# Fades to white only near the profile cap, like the stock torque bar fading toward orange at high torque
|
||||
CAP_BLEND_START = 0.70
|
||||
MAX_BOOST_BLEND = 0.55
|
||||
|
||||
COLOR_TRACK = rl.Color(255, 255, 255, 32)
|
||||
COLOR_TRACK_EDGE = rl.Color(255, 255, 255, 70)
|
||||
COLOR_HIGHLIGHT = rl.Color(255, 255, 255, 45)
|
||||
|
||||
AGGRESSIVE_COLOR = rl.Color(255, 80, 60, 255)
|
||||
STANDARD_COLOR = rl.Color(0, 230, 120, 255)
|
||||
RELAXED_COLOR = rl.Color(70, 150, 255, 255)
|
||||
|
||||
# The personality enum is ordered aggressive, standard, relaxed, matching ACCEL_BOOST_MAX
|
||||
PROFILE_STYLES = {
|
||||
0: (AGGRESSIVE_COLOR, ACCEL_BOOST_MAX[0]),
|
||||
1: (STANDARD_COLOR, ACCEL_BOOST_MAX[1]),
|
||||
2: (RELAXED_COLOR, ACCEL_BOOST_MAX[2]),
|
||||
}
|
||||
|
||||
FADE_THRESHOLD = 0.01
|
||||
|
||||
|
||||
def get_boost_style(personality) -> tuple[rl.Color, float]:
|
||||
"""Color and full-scale boost for the active personality."""
|
||||
raw = getattr(personality, "raw", personality)
|
||||
return PROFILE_STYLES.get(int(raw), PROFILE_STYLES[1])
|
||||
|
||||
|
||||
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 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)
|
||||
|
||||
|
||||
class AccelBoostIndicator(Widget):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
fps = max(gui_app.target_fps, 1)
|
||||
self._boost_filter = FirstOrderFilter(0.0, 0.15, 1 / fps)
|
||||
self._alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / fps)
|
||||
# Smoothly rescale the bar when the follow distance profile changes mid-drive
|
||||
self._max_boost_filter = FirstOrderFilter(0.0, 0.25, 1 / fps)
|
||||
self._profile_color, self._max_boost = PROFILE_STYLES[1]
|
||||
|
||||
self.panel_in_use = False
|
||||
|
||||
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']:
|
||||
self._profile_color, self._max_boost = get_boost_style(sm['selfdriveState'].personality)
|
||||
self._max_boost_filter.update(self._max_boost)
|
||||
|
||||
# The bar owns the panel while it is drawn; the confidence ball returns when it fades out
|
||||
self.panel_in_use = self._alpha_filter.x > FADE_THRESHOLD
|
||||
|
||||
def _render(self, _):
|
||||
alpha = self._alpha_filter.x
|
||||
if alpha < FADE_THRESHOLD or self._max_boost <= 0.0:
|
||||
return
|
||||
|
||||
panel_x = self.rect.x + self.rect.width - SIDE_PANEL_WIDTH
|
||||
cx = panel_x + SIDE_PANEL_WIDTH / 2
|
||||
bar_x = cx - BAR_WIDTH / 2
|
||||
track_top = self.rect.y + self.rect.height * TRACK_TOP_RATIO
|
||||
track_bottom = self.rect.y + self.rect.height * TRACK_BOTTOM_RATIO
|
||||
track_h = max(1.0, track_bottom - track_top)
|
||||
track_rect = rl.Rectangle(bar_x, track_top, BAR_WIDTH, track_h)
|
||||
|
||||
# soft well behind the track so the bar reads on the black side panel
|
||||
rl.draw_rectangle_rounded(rl.Rectangle(bar_x - 3, track_top - 3, BAR_WIDTH + 6, track_h + 6), BAR_RADIUS, 12,
|
||||
rl.Color(0, 0, 0, int(80 * alpha)))
|
||||
|
||||
# track, light enough to read on the black side panel
|
||||
rl.draw_rectangle_rounded(track_rect, BAR_RADIUS, 12,
|
||||
rl.Color(COLOR_TRACK.r, COLOR_TRACK.g, COLOR_TRACK.b, int(COLOR_TRACK.a * alpha)))
|
||||
rl.draw_rectangle_rounded_lines_ex(track_rect, BAR_RADIUS, 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
|
||||
if fill_h <= 0.0:
|
||||
return
|
||||
|
||||
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)
|
||||
tip_y = track_bottom - fill_h
|
||||
|
||||
# soft glow around the tip, like the confidence ball's gradient
|
||||
tip_center_y = tip_y + min(fill_h, BAR_WIDTH) / 2
|
||||
glow = rl.Color(fill_color.r, fill_color.g, fill_color.b, int(85 * alpha))
|
||||
rl.draw_circle_gradient(rl.Vector2(cx, tip_center_y), int(BAR_WIDTH * 0.85), glow, rl.BLANK)
|
||||
|
||||
fill = rl.Color(fill_color.r, fill_color.g, fill_color.b, int(255 * alpha))
|
||||
if fill_h < BAR_WIDTH:
|
||||
# Keep the cap inside the track: a fixed radius would paint below the track near zero boost
|
||||
radius = max(1, int(fill_h / 2))
|
||||
rl.draw_circle(int(cx), int(track_bottom - radius), radius, fill)
|
||||
else:
|
||||
rl.draw_rectangle_rounded(rl.Rectangle(bar_x, tip_y, BAR_WIDTH, fill_h), BAR_RADIUS, 12, fill)
|
||||
|
||||
# subtle inner highlight gives the fill a rounded, glassy edge
|
||||
if fill_h > 12:
|
||||
highlight_rect = rl.Rectangle(bar_x + 4, tip_y + 5, 3, fill_h - 10)
|
||||
rl.draw_rectangle_rounded(highlight_rect, 0.5, 8,
|
||||
rl.Color(COLOR_HIGHLIGHT.r, COLOR_HIGHLIGHT.g, COLOR_HIGHLIGHT.b,
|
||||
int(COLOR_HIGHLIGHT.a * alpha)))
|
||||
|
||||
# bright cap at the tip
|
||||
rl.draw_circle(int(cx), int(tip_center_y), 2,
|
||||
rl.Color(255, 255, 255, int(150 * alpha)))
|
||||
@@ -0,0 +1,52 @@
|
||||
"""
|
||||
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.test import OpenpilotTestCase
|
||||
from openpilot.selfdrive.ui.sunnypilot.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 import ACCEL_BOOST_MAX
|
||||
|
||||
|
||||
class TestAccelBoostIndicator(OpenpilotTestCase):
|
||||
def test_profile_styles_match_controller_caps(self):
|
||||
for personality, color, max_boost in (
|
||||
(log.LongitudinalPersonality.aggressive, AGGRESSIVE_COLOR, ACCEL_BOOST_MAX[0]),
|
||||
(log.LongitudinalPersonality.standard, STANDARD_COLOR, ACCEL_BOOST_MAX[1]),
|
||||
(log.LongitudinalPersonality.relaxed, RELAXED_COLOR, ACCEL_BOOST_MAX[2])):
|
||||
style_color, style_max = get_boost_style(personality)
|
||||
assert style_color == color
|
||||
assert style_max == max_boost
|
||||
|
||||
def test_wire_personality_picks_the_right_profile(self):
|
||||
msg = log.SelfdriveState.new_message()
|
||||
msg.personality = log.LongitudinalPersonality.aggressive
|
||||
color, max_boost = get_boost_style(msg.personality)
|
||||
assert color == AGGRESSIVE_COLOR
|
||||
assert max_boost == ACCEL_BOOST_MAX[0]
|
||||
|
||||
def test_bar_fills_at_the_active_profile_cap(self):
|
||||
for cap in ACCEL_BOOST_MAX:
|
||||
assert get_boost_fraction(0.0, cap) == 0.0
|
||||
assert get_boost_fraction(cap * 0.5, cap) == 0.5
|
||||
assert get_boost_fraction(cap, cap) == 1.0
|
||||
|
||||
# A boost beyond the current cap (e.g. after switching to a shorter profile) still fills the bar
|
||||
assert get_boost_fraction(max(ACCEL_BOOST_MAX), min(ACCEL_BOOST_MAX)) == 1.0
|
||||
assert 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)
|
||||
assert relaxed.b > relaxed.r
|
||||
assert relaxed.b >= relaxed.g
|
||||
assert aggressive.r > aggressive.b
|
||||
# lighter in RGB means the dimmest channel came up towards white
|
||||
assert relaxed.r > RELAXED_COLOR.r
|
||||
assert aggressive.g > AGGRESSIVE_COLOR.g
|
||||
@@ -0,0 +1,67 @@
|
||||
"""
|
||||
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 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 2 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
|
||||
@@ -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,10 +31,16 @@ 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_stock = AccelBoost()
|
||||
self.accel_boost_sp = AccelBoostSP()
|
||||
|
||||
self.output_v_target = 0.
|
||||
self.output_a_target = 0.
|
||||
|
||||
@property
|
||||
def accel_boost(self):
|
||||
return self.accel_boost_sp if self.accel_boost_sp.enabled else self.accel_boost_stock
|
||||
|
||||
def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
|
||||
CS = sm['carState']
|
||||
v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
|
||||
@@ -64,6 +72,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_stock = AccelBoost()
|
||||
|
||||
self.events_sp.clear()
|
||||
self.e2e_alerts_helper.update(sm, self.events_sp)
|
||||
|
||||
|
||||
@@ -0,0 +1,81 @@
|
||||
"""
|
||||
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 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
|
||||
@@ -620,6 +620,19 @@
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "AccelBoostSP",
|
||||
"widget": "toggle",
|
||||
"title": "Sunnypilot Accel Boost",
|
||||
"description": "Adds acceleration, sized by driving personality, when the driving model predicts no reason to slow, so it reaches your set speed sooner. Fades out in tight curves.",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "IntelligentCruiseButtonManagement",
|
||||
"widget": "toggle",
|
||||
|
||||
@@ -30,6 +30,13 @@ sections:
|
||||
label: Relaxed
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: AccelBoostSP
|
||||
widget: toggle
|
||||
title: Sunnypilot Accel Boost
|
||||
description: Adds acceleration, sized by driving personality, when the driving model predicts no reason to slow, so it
|
||||
reaches your set speed sooner. Fades out in tight curves.
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: IntelligentCruiseButtonManagement
|
||||
widget: toggle
|
||||
title: Intelligent Cruise Button Management (ICBM) (Alpha)
|
||||
|
||||
Reference in New Issue
Block a user