mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-11-04 19:03:43 +08:00
Compare commits
3 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 8d7995a092 | |||
| 5845381d1f | |||
| 0ffb700830 |
@@ -149,7 +149,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
|||||||
{"Version", {PERSISTENT, STRING}},
|
{"Version", {PERSISTENT, STRING}},
|
||||||
|
|
||||||
// --- sunnypilot params --- //
|
// --- sunnypilot params --- //
|
||||||
{"AccelBoostSP", {PERSISTENT | BACKUP, BOOL, "1"}},
|
|
||||||
{"ApiCache_DriveStats", {PERSISTENT, JSON}},
|
{"ApiCache_DriveStats", {PERSISTENT, JSON}},
|
||||||
{"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
|
{"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||||
{"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},
|
{"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},
|
||||||
@@ -200,6 +199,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
|||||||
{"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
|
{"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||||
{"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
|
{"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||||
{"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
|
{"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||||
|
{"TripSummary", {PERSISTENT | BACKUP, BOOL, "1"}},
|
||||||
{"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
|
{"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||||
|
|
||||||
// MADS params
|
// MADS params
|
||||||
|
|||||||
@@ -10,6 +10,7 @@ from openpilot.selfdrive.ui.onroad.augmented_road_view import AugmentedRoadView
|
|||||||
from openpilot.selfdrive.ui.ui_state import device, ui_state
|
from openpilot.selfdrive.ui.ui_state import device, ui_state
|
||||||
from openpilot.selfdrive.ui.layouts.onboarding import OnboardingWindow
|
from openpilot.selfdrive.ui.layouts.onboarding import OnboardingWindow
|
||||||
from openpilot.selfdrive.ui.body.layouts.onroad import BodyLayout
|
from openpilot.selfdrive.ui.body.layouts.onroad import BodyLayout
|
||||||
|
from openpilot.selfdrive.ui.sunnypilot.layouts.trip_summary import TripSummary
|
||||||
|
|
||||||
if gui_app.sunnypilot_ui():
|
if gui_app.sunnypilot_ui():
|
||||||
from openpilot.selfdrive.ui.sunnypilot.layouts.settings.settings import SettingsLayoutSP as SettingsLayout
|
from openpilot.selfdrive.ui.sunnypilot.layouts.settings.settings import SettingsLayoutSP as SettingsLayout
|
||||||
@@ -31,6 +32,7 @@ class MainLayout(Widget):
|
|||||||
self._sidebar = Sidebar()
|
self._sidebar = Sidebar()
|
||||||
self._current_mode = MainState.HOME
|
self._current_mode = MainState.HOME
|
||||||
self._prev_onroad = False
|
self._prev_onroad = False
|
||||||
|
self._trip_summary = TripSummary()
|
||||||
|
|
||||||
# Initialize layouts
|
# Initialize layouts
|
||||||
self._home_layout = HomeLayout()
|
self._home_layout = HomeLayout()
|
||||||
|
|||||||
@@ -10,6 +10,7 @@ from openpilot.selfdrive.ui.body.layouts.onroad import BodyLayout
|
|||||||
from openpilot.system.ui.widgets import Widget
|
from openpilot.system.ui.widgets import Widget
|
||||||
from openpilot.system.ui.widgets.scroller import Scroller
|
from openpilot.system.ui.widgets.scroller import Scroller
|
||||||
from openpilot.system.ui.lib.application import gui_app
|
from openpilot.system.ui.lib.application import gui_app
|
||||||
|
from openpilot.selfdrive.ui.sunnypilot.layouts.trip_summary import TripSummary
|
||||||
|
|
||||||
if gui_app.sunnypilot_ui():
|
if gui_app.sunnypilot_ui():
|
||||||
from openpilot.selfdrive.ui.sunnypilot.mici.layouts.settings import SettingsLayoutSP as SettingsLayout
|
from openpilot.selfdrive.ui.sunnypilot.mici.layouts.settings import SettingsLayoutSP as SettingsLayout
|
||||||
@@ -57,6 +58,7 @@ class MiciMainLayout(Scroller):
|
|||||||
self._setup_callbacks()
|
self._setup_callbacks()
|
||||||
|
|
||||||
gui_app.add_nav_stack_tick(self._handle_transitions)
|
gui_app.add_nav_stack_tick(self._handle_transitions)
|
||||||
|
self._trip_summary = TripSummary()
|
||||||
gui_app.push_widget(self)
|
gui_app.push_widget(self)
|
||||||
|
|
||||||
# Start onboarding if terms or training not completed, make sure to push after self
|
# Start onboarding if terms or training not completed, make sure to push after self
|
||||||
|
|||||||
@@ -57,12 +57,6 @@ class CruiseLayout(Widget):
|
|||||||
description=tr("Use map data to estimate the appropriate speed to drive through turns ahead."),
|
description=tr("Use map data to estimate the appropriate speed to drive through turns ahead."),
|
||||||
param="SmartCruiseControlMap")
|
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(
|
self.custom_acc_toggle = toggle_item_sp(
|
||||||
title=tr("Custom ACC Speed Increments"),
|
title=tr("Custom ACC Speed Increments"),
|
||||||
description="",
|
description="",
|
||||||
@@ -92,7 +86,6 @@ class CruiseLayout(Widget):
|
|||||||
self.icbm_toggle,
|
self.icbm_toggle,
|
||||||
self.scc_v_toggle,
|
self.scc_v_toggle,
|
||||||
self.scc_m_toggle,
|
self.scc_m_toggle,
|
||||||
self.accel_boost_toggle,
|
|
||||||
self.custom_acc_toggle,
|
self.custom_acc_toggle,
|
||||||
self.custom_acc_short_increment,
|
self.custom_acc_short_increment,
|
||||||
self.custom_acc_long_increment,
|
self.custom_acc_long_increment,
|
||||||
@@ -143,8 +136,6 @@ class CruiseLayout(Widget):
|
|||||||
self.icbm_toggle.set_description(new_desc)
|
self.icbm_toggle.set_description(new_desc)
|
||||||
self.icbm_toggle.show_description(True)
|
self.icbm_toggle.show_description(True)
|
||||||
|
|
||||||
self.accel_boost_toggle.action_item.set_enabled(has_long)
|
|
||||||
|
|
||||||
if has_long or has_icbm:
|
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.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)
|
self.scc_v_toggle.action_item.set_enabled(True)
|
||||||
@@ -160,7 +151,6 @@ class CruiseLayout(Widget):
|
|||||||
else:
|
else:
|
||||||
has_icbm = has_long = False
|
has_icbm = has_long = False
|
||||||
self.icbm_toggle.action_item.set_enabled(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))
|
self.icbm_toggle.set_description(tr(ONROAD_ONLY_DESCRIPTION))
|
||||||
|
|
||||||
show_custom_acc_desc = False
|
show_custom_acc_desc = False
|
||||||
|
|||||||
@@ -72,6 +72,11 @@ class VisualsLayout(Widget):
|
|||||||
"It is the driver's responsibility to observe their environment and make decisions accordingly."),
|
"It is the driver's responsibility to observe their environment and make decisions accordingly."),
|
||||||
None,
|
None,
|
||||||
),
|
),
|
||||||
|
"TripSummary": (
|
||||||
|
lambda: tr("Trip Summary"),
|
||||||
|
tr("Show a summary of distance and engagement when you shift into park after a drive."),
|
||||||
|
None,
|
||||||
|
),
|
||||||
"TrueVEgoUI": (
|
"TrueVEgoUI": (
|
||||||
lambda: tr("Speedometer: Always Display True Speed"),
|
lambda: tr("Speedometer: Always Display True Speed"),
|
||||||
tr("For applicable vehicles, always display the true vehicle current speed from wheel speed sensors."),
|
tr("For applicable vehicles, always display the true vehicle current speed from wheel speed sensors."),
|
||||||
|
|||||||
@@ -0,0 +1,199 @@
|
|||||||
|
"""
|
||||||
|
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 time
|
||||||
|
import pyray as rl
|
||||||
|
|
||||||
|
from opendbc.car.structs import car
|
||||||
|
|
||||||
|
from openpilot.common.constants import CV
|
||||||
|
from openpilot.selfdrive.ui.onroad.augmented_road_view import BORDER_COLORS
|
||||||
|
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
|
||||||
|
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||||
|
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||||
|
from openpilot.system.ui.widgets import Widget
|
||||||
|
|
||||||
|
AUTO_DISMISS = 20.
|
||||||
|
FADE_IN, FADE_OUT = 0.35, 0.3
|
||||||
|
LAT = (UIStatus.ENGAGED, UIStatus.LAT_ONLY)
|
||||||
|
LONG = (UIStatus.ENGAGED, UIStatus.LONG_ONLY)
|
||||||
|
GEAR = car.CarState.GearShifter
|
||||||
|
FORWARD = (GEAR.drive, GEAR.sport, GEAR.low, GEAR.brake, GEAR.eco, GEAR.manumatic)
|
||||||
|
|
||||||
|
|
||||||
|
class TripTracker:
|
||||||
|
def __init__(self):
|
||||||
|
self.reset()
|
||||||
|
|
||||||
|
def reset(self):
|
||||||
|
self.segments: list[list] = []
|
||||||
|
self.seconds = 0.
|
||||||
|
self.takeovers = 0
|
||||||
|
self._t = None
|
||||||
|
self._status = None
|
||||||
|
|
||||||
|
def update(self, driving: bool):
|
||||||
|
now = time.monotonic()
|
||||||
|
if not driving:
|
||||||
|
self._t = None
|
||||||
|
self._status = None
|
||||||
|
return
|
||||||
|
dt = 0. if self._t is None else min(now - self._t, 0.5)
|
||||||
|
self._t = now
|
||||||
|
self.seconds += dt
|
||||||
|
if self._status not in (None, UIStatus.DISENGAGED) and ui_state.status == UIStatus.DISENGAGED:
|
||||||
|
self.takeovers += 1
|
||||||
|
self._status = ui_state.status
|
||||||
|
d = max(ui_state.sm["carState"].vEgo, 0.) * dt
|
||||||
|
if d <= 0.:
|
||||||
|
return
|
||||||
|
if self.segments and self.segments[-1][0] == ui_state.status:
|
||||||
|
self.segments[-1][1] += d
|
||||||
|
else:
|
||||||
|
self.segments.append([ui_state.status, d])
|
||||||
|
|
||||||
|
def total(self, statuses=None) -> float:
|
||||||
|
return sum(m for s, m in self.segments if statuses is None or s in statuses)
|
||||||
|
|
||||||
|
|
||||||
|
class TripSummary:
|
||||||
|
def __init__(self):
|
||||||
|
self._trip = TripTracker()
|
||||||
|
self._dialog: TripSummaryDialog | None = None
|
||||||
|
self._gear = None
|
||||||
|
self._drove = False
|
||||||
|
gui_app.add_nav_stack_tick(self._tick)
|
||||||
|
|
||||||
|
def _tick(self):
|
||||||
|
gear = ui_state.sm["carState"].gearShifter if ui_state.started else None
|
||||||
|
self._trip.update(gear not in (None, GEAR.park))
|
||||||
|
self._drove |= gear in FORWARD
|
||||||
|
if gear == self._gear:
|
||||||
|
return
|
||||||
|
self._gear = gear
|
||||||
|
|
||||||
|
if gear not in (None, GEAR.park):
|
||||||
|
self._pop()
|
||||||
|
elif gear == GEAR.park and self._drove and ui_state.trip_summary and not ui_state.is_body and self._trip.total() > 0.:
|
||||||
|
self._dialog = TripSummaryDialog(self._trip, gui_app.get_active_widget(), self._pop)
|
||||||
|
gui_app.push_widget(self._dialog)
|
||||||
|
if gear is None:
|
||||||
|
self._trip.reset()
|
||||||
|
if gear in (None, GEAR.park):
|
||||||
|
self._drove = False
|
||||||
|
|
||||||
|
def _pop(self):
|
||||||
|
if self._dialog is not None and gui_app.get_active_widget() is self._dialog:
|
||||||
|
gui_app.pop_widget()
|
||||||
|
self._dialog = None
|
||||||
|
|
||||||
|
|
||||||
|
def _ease(t: float) -> float:
|
||||||
|
t = min(max(t, 0.), 1.)
|
||||||
|
return 1 - (1 - t) ** 3
|
||||||
|
|
||||||
|
|
||||||
|
def _fade(c, a: float) -> rl.Color:
|
||||||
|
r, g, b, al = c if isinstance(c, tuple) else (c.r, c.g, c.b, c.a)
|
||||||
|
return rl.Color(r, g, b, int(al * a))
|
||||||
|
|
||||||
|
|
||||||
|
class TripSummaryDialog(Widget):
|
||||||
|
def __init__(self, trip: TripTracker, below: Widget | None, on_closed):
|
||||||
|
super().__init__()
|
||||||
|
self._below = below
|
||||||
|
self._on_closed = on_closed
|
||||||
|
self._segments = segments = trip.segments
|
||||||
|
total = sum(m for _, m in segments)
|
||||||
|
|
||||||
|
def pct(statuses):
|
||||||
|
return 100 * sum(m for s, m in segments if s in statuses) / total
|
||||||
|
|
||||||
|
dist = total * (CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH) / 3600
|
||||||
|
dist_fmt = "{:.1f}" if dist < 9.95 else "{:.0f}"
|
||||||
|
self._stats = [
|
||||||
|
(dist, dist_fmt, "km" if ui_state.is_metric else "mi", "driven"),
|
||||||
|
(pct(LAT + LONG), "{:.0f}", "%", "engaged"),
|
||||||
|
(pct(LAT), "{:.0f}", "%", "lateral"),
|
||||||
|
(pct(LONG), "{:.0f}", "%", "longitudinal"),
|
||||||
|
]
|
||||||
|
mins = round(trip.seconds / 60)
|
||||||
|
self._details = [
|
||||||
|
(f"{mins // 60}h {mins % 60}" if mins >= 60 else f"{mins}", "min"),
|
||||||
|
(f"{trip.takeovers}", "takeover" if trip.takeovers == 1 else "takeovers"),
|
||||||
|
]
|
||||||
|
self._bold = gui_app.font(FontWeight.BOLD)
|
||||||
|
self._semi = gui_app.font(FontWeight.SEMI_BOLD)
|
||||||
|
self._normal = gui_app.font(FontWeight.NORMAL)
|
||||||
|
self._big = gui_app.big_ui()
|
||||||
|
self._shown_t = rl.get_time()
|
||||||
|
self._closing_t: float | None = None
|
||||||
|
self.set_click_callback(self.close)
|
||||||
|
|
||||||
|
def close(self):
|
||||||
|
if self._closing_t is None:
|
||||||
|
self._closing_t = rl.get_time()
|
||||||
|
|
||||||
|
def _render(self, rect: rl.Rectangle):
|
||||||
|
t = rl.get_time() - self._shown_t
|
||||||
|
if self._closing_t is None and t > AUTO_DISMISS:
|
||||||
|
self.close()
|
||||||
|
out = 0. if self._closing_t is None else (rl.get_time() - self._closing_t) / FADE_OUT
|
||||||
|
if out >= 1.:
|
||||||
|
self._on_closed()
|
||||||
|
return
|
||||||
|
bg = 1 - _ease(out)
|
||||||
|
a = _ease(t / FADE_IN) * bg
|
||||||
|
|
||||||
|
if self._big and self._below is not None and bg < 1.:
|
||||||
|
self._below.render(rect)
|
||||||
|
rl.draw_rectangle_rec(rect, _fade(rl.BLACK, bg))
|
||||||
|
|
||||||
|
k = rect.height / 1080 * (1.0 if self._big else 1.22)
|
||||||
|
pad = 120 * k
|
||||||
|
x, w = rect.x + pad, rect.width - 2 * pad
|
||||||
|
y = rect.y + (rect.height - 850 * k) / 2
|
||||||
|
|
||||||
|
rl.draw_text_ex(self._bold, "trip summary", rl.Vector2(x, y), int(150 * k), 0, _fade(rl.WHITE, a))
|
||||||
|
y += 260 * k
|
||||||
|
|
||||||
|
col_w = w / len(self._stats)
|
||||||
|
for i, (target, fmt, unit, label) in enumerate(self._stats):
|
||||||
|
cx = x + col_w * (i + 0.5)
|
||||||
|
v = measure_text_cached(self._bold, fmt.format(target), int(170 * k))
|
||||||
|
u = measure_text_cached(self._semi, unit, int(80 * k))
|
||||||
|
vx = cx - (v.x + 10 * k + u.x) / 2
|
||||||
|
rl.draw_text_ex(self._bold, fmt.format(target), rl.Vector2(vx, y), int(170 * k), 0, _fade(rl.WHITE, a))
|
||||||
|
rl.draw_text_ex(self._semi, unit, rl.Vector2(vx + v.x + 10 * k, y + 75 * k), int(80 * k), 0, _fade(rl.WHITE, a))
|
||||||
|
lb = measure_text_cached(self._normal, label, int(70 * k))
|
||||||
|
rl.draw_text_ex(self._normal, label, rl.Vector2(cx - lb.x / 2, y + 180 * k), int(70 * k), 0, _fade(rl.Color(200, 200, 200, 255), a))
|
||||||
|
y += 330 * k
|
||||||
|
|
||||||
|
bar = rl.Rectangle(x, y, w, 90 * k)
|
||||||
|
total = sum(m for _, m in self._segments)
|
||||||
|
sx = bar.x
|
||||||
|
for status, m in self._segments:
|
||||||
|
sw = bar.width * m / total
|
||||||
|
rl.draw_rectangle_rec(rl.Rectangle(sx, bar.y, min(sw + 1, bar.x + bar.width - sx), bar.height), _fade(BORDER_COLORS[status], a))
|
||||||
|
sx += sw
|
||||||
|
rl.draw_rectangle_rounded_lines_ex(bar, 1.0, 20, 30 * k, _fade(rl.BLACK, a))
|
||||||
|
|
||||||
|
y += (90 + 70) * k
|
||||||
|
|
||||||
|
size = int(90 * k)
|
||||||
|
parts = []
|
||||||
|
for i, (value, unit) in enumerate(self._details):
|
||||||
|
if i:
|
||||||
|
parts.append((" ", self._normal, None))
|
||||||
|
parts += [(value, self._bold, rl.WHITE), (" " + unit, self._normal, rl.Color(170, 170, 170, 255))]
|
||||||
|
widths = [measure_text_cached(f, t, size).x for t, f, _ in parts]
|
||||||
|
lx = x + (w - sum(widths)) / 2
|
||||||
|
for (text, font, color), tw in zip(parts, widths, strict=True):
|
||||||
|
if color is None:
|
||||||
|
rl.draw_circle(int(lx + tw / 2), int(y + size * 0.6), 12 * k, _fade(rl.Color(110, 110, 110, 255), a))
|
||||||
|
else:
|
||||||
|
rl.draw_text_ex(font, text, rl.Vector2(lx, y), size, 0, _fade(color, a))
|
||||||
|
lx += tw
|
||||||
@@ -48,6 +48,7 @@ class UIStateSP:
|
|||||||
self.model_runner_tinygrad: bool = False
|
self.model_runner_tinygrad: bool = False
|
||||||
self.active_runner_tinygrad: bool = False
|
self.active_runner_tinygrad: bool = False
|
||||||
self.blindspot: bool = False
|
self.blindspot: bool = False
|
||||||
|
self.trip_summary: bool = True
|
||||||
self.chevron_metrics = None
|
self.chevron_metrics = None
|
||||||
self.custom_interactive_timeout: int = 0
|
self.custom_interactive_timeout: int = 0
|
||||||
self.developer_ui = None
|
self.developer_ui = None
|
||||||
@@ -185,6 +186,7 @@ class UIStateSP:
|
|||||||
self.standstill_timer = self.params.get_bool("StandstillTimer")
|
self.standstill_timer = self.params.get_bool("StandstillTimer")
|
||||||
self.sunnylink_enabled = self.params.get_bool("SunnylinkEnabled")
|
self.sunnylink_enabled = self.params.get_bool("SunnylinkEnabled")
|
||||||
self.torque_bar = self.params.get_bool("TorqueBar")
|
self.torque_bar = self.params.get_bool("TorqueBar")
|
||||||
|
self.trip_summary = self.params.get_bool("TripSummary")
|
||||||
self.enforce_torque_control = self.params.get_bool("EnforceTorqueControl")
|
self.enforce_torque_control = self.params.get_bool("EnforceTorqueControl")
|
||||||
self.custom_torque_params = self.params.get_bool("CustomTorqueParams")
|
self.custom_torque_params = self.params.get_bool("CustomTorqueParams")
|
||||||
self.torque_override_enabled = self.params.get_bool("TorqueParamsOverrideEnabled")
|
self.torque_override_enabled = self.params.get_bool("TorqueParamsOverrideEnabled")
|
||||||
|
|||||||
@@ -1,67 +0,0 @@
|
|||||||
"""
|
|
||||||
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
|
|
||||||
@@ -9,8 +9,6 @@ from openpilot.cereal import messaging, custom
|
|||||||
from opendbc.car import structs
|
from opendbc.car import structs
|
||||||
from openpilot.common.constants import CV
|
from openpilot.common.constants import CV
|
||||||
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
|
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.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.smart_cruise_control.smart_cruise_control import SmartCruiseControl
|
||||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist
|
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist
|
||||||
@@ -31,8 +29,6 @@ class LongitudinalPlannerSP:
|
|||||||
self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
|
self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
|
||||||
self.source = LongitudinalPlanSource.cruise
|
self.source = LongitudinalPlanSource.cruise
|
||||||
self.e2e_alerts_helper = E2EAlertsHelper()
|
self.e2e_alerts_helper = E2EAlertsHelper()
|
||||||
self.accel_boost_sp = AccelBoostSP()
|
|
||||||
self.openpilot_longitudinal = CP.openpilotLongitudinalControl
|
|
||||||
|
|
||||||
self.output_v_target = 0.
|
self.output_v_target = 0.
|
||||||
self.output_a_target = 0.
|
self.output_a_target = 0.
|
||||||
@@ -68,9 +64,6 @@ class LongitudinalPlannerSP:
|
|||||||
return self.output_v_target, self.output_a_target
|
return self.output_v_target, self.output_a_target
|
||||||
|
|
||||||
def update(self, sm: messaging.SubMaster) -> None:
|
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.events_sp.clear()
|
||||||
self.e2e_alerts_helper.update(sm, self.events_sp)
|
self.e2e_alerts_helper.update(sm, self.events_sp)
|
||||||
|
|
||||||
|
|||||||
@@ -1,81 +0,0 @@
|
|||||||
"""
|
|
||||||
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
|
|
||||||
@@ -620,19 +620,6 @@
|
|||||||
}
|
}
|
||||||
]
|
]
|
||||||
},
|
},
|
||||||
{
|
|
||||||
"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",
|
"key": "IntelligentCruiseButtonManagement",
|
||||||
"widget": "toggle",
|
"widget": "toggle",
|
||||||
@@ -1412,6 +1399,12 @@
|
|||||||
}
|
}
|
||||||
]
|
]
|
||||||
},
|
},
|
||||||
|
{
|
||||||
|
"key": "TripSummary",
|
||||||
|
"widget": "toggle",
|
||||||
|
"title": "Trip Summary",
|
||||||
|
"description": "Show a summary of distance and engagement when you shift into park after a drive."
|
||||||
|
},
|
||||||
{
|
{
|
||||||
"key": "RocketFuel",
|
"key": "RocketFuel",
|
||||||
"widget": "toggle",
|
"widget": "toggle",
|
||||||
|
|||||||
@@ -30,12 +30,6 @@ sections:
|
|||||||
label: Relaxed
|
label: Relaxed
|
||||||
enablement:
|
enablement:
|
||||||
- $ref: '#/macros/longitudinal'
|
- $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
|
- key: IntelligentCruiseButtonManagement
|
||||||
widget: toggle
|
widget: toggle
|
||||||
title: Intelligent Cruise Button Management (ICBM) (Alpha)
|
title: Intelligent Cruise Button Management (ICBM) (Alpha)
|
||||||
|
|||||||
@@ -39,6 +39,10 @@ sections:
|
|||||||
description: Show a timer on the HUD when the car is at a standstill.
|
description: Show a timer on the HUD when the car is at a standstill.
|
||||||
visibility:
|
visibility:
|
||||||
- $ref: '#/macros/hide_on_mici'
|
- $ref: '#/macros/hide_on_mici'
|
||||||
|
- key: TripSummary
|
||||||
|
widget: toggle
|
||||||
|
title: Trip Summary
|
||||||
|
description: Show a summary of distance and engagement when you shift into park after a drive.
|
||||||
- key: RocketFuel
|
- key: RocketFuel
|
||||||
widget: toggle
|
widget: toggle
|
||||||
title: Real-time Acceleration Bar
|
title: Real-time Acceleration Bar
|
||||||
|
|||||||
Reference in New Issue
Block a user