mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-11-22 05:33:43 +08:00
Compare commits
16 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| faac3152ba | |||
| e0c97365da | |||
| 5064bb581d | |||
| a598f43690 | |||
| 7f8db47fd0 | |||
| 1291d14108 | |||
| c831334d76 | |||
| 5958b2d25b | |||
| 73c8b30fa9 | |||
| 8bdb2f54d9 | |||
| 9c1bdde65b | |||
| 914aaefac1 | |||
| d05649c0a4 | |||
| 3521140920 | |||
| 56628fc770 | |||
| 60a9cc8657 |
@@ -4,6 +4,7 @@
|
||||
[submodule "opendbc"]
|
||||
path = opendbc_repo
|
||||
url = https://github.com/sunnypilot/opendbc.git
|
||||
branch = toyotabsmhack
|
||||
[submodule "msgq"]
|
||||
path = msgq_repo
|
||||
url = https://github.com/sunnypilot/msgq.git
|
||||
|
||||
+1
-1
Submodule opendbc_repo updated: c0e74738a8...08aa33c019
@@ -200,6 +200,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"TripSummary", {PERSISTENT | BACKUP, BOOL, "1"}},
|
||||
{"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
// MADS params
|
||||
@@ -245,6 +246,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"TeslaCoopSteering", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"TeslaMadsScreenButton", {PERSISTENT | BACKUP, INT, "0"}},
|
||||
{"ToyotaEnhancedBsm", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ToyotaEnforceStockLongitudinal", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ToyotaStopAndGoHack", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ToyotaTSS2Smooth", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
@@ -3,7 +3,7 @@ from collections.abc import Callable
|
||||
import base64
|
||||
import ctypes
|
||||
import gc
|
||||
from functools import cached_property, partial
|
||||
from functools import cached_property
|
||||
import os
|
||||
os.environ['GMMU'] = '0' # for chestnut fast loading, noop for qcom
|
||||
os.environ.setdefault('AM_POWER_LIMIT', '100')
|
||||
@@ -43,7 +43,6 @@ from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
|
||||
from openpilot.selfdrive.modeld.helpers import MODELS_DIR, chestnut_present, chestnut_compiled, modeld_pkl_path, load_oob, wait_for_chestnut
|
||||
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
||||
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
|
||||
from openpilot.sunnypilot.modeld_v2.turn_planner import TurnPlanner
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
|
||||
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
@@ -55,8 +54,7 @@ BIG_MODEL_TIMEOUT = 60
|
||||
|
||||
|
||||
def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
|
||||
lat_action_t: float, long_action_t: float, v_ego: float,
|
||||
turn_shaper: Callable[[float], float] | None = None) -> log.ModelDataV2.Action:
|
||||
lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action:
|
||||
if 'action' not in model_output:
|
||||
plan = model_output['plan'][0]
|
||||
desired_accel = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0],
|
||||
@@ -71,8 +69,6 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
|
||||
else:
|
||||
desired_accel = model_output['action'][0,1]
|
||||
desired_curvature = model_output['action'][0,0] / (max(1.0, v_ego))**2
|
||||
if turn_shaper is not None:
|
||||
desired_curvature = turn_shaper(desired_curvature)
|
||||
stop = should_stop(v_ego, desired_accel)
|
||||
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
|
||||
if v_ego > MIN_LAT_CONTROL_SPEED:
|
||||
@@ -344,7 +340,6 @@ def main(demo=False):
|
||||
|
||||
DH = DesireHelper()
|
||||
RELC = RoadEdgeLaneChangeController()
|
||||
turn_planner = TurnPlanner(CP.wheelbase)
|
||||
|
||||
while True:
|
||||
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
|
||||
@@ -453,9 +448,7 @@ def main(demo=False):
|
||||
|
||||
if not sm['carControl'].latActive:
|
||||
prev_action.desiredCurvature = sm['controlsState'].curvature
|
||||
action = get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego,
|
||||
partial(turn_planner.update, model_output, v_ego, sm['controlsState'].curvature,
|
||||
sm['carControl'].latActive, is_rhd))
|
||||
action = get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
|
||||
prev_action = action
|
||||
fill_model_msg(modelv2_send, model_output, action,
|
||||
publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
|
||||
|
||||
@@ -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.layouts.onboarding import OnboardingWindow
|
||||
from openpilot.selfdrive.ui.body.layouts.onroad import BodyLayout
|
||||
from openpilot.selfdrive.ui.sunnypilot.layouts.trip_summary import TripSummary
|
||||
|
||||
if gui_app.sunnypilot_ui():
|
||||
from openpilot.selfdrive.ui.sunnypilot.layouts.settings.settings import SettingsLayoutSP as SettingsLayout
|
||||
@@ -31,6 +32,7 @@ class MainLayout(Widget):
|
||||
self._sidebar = Sidebar()
|
||||
self._current_mode = MainState.HOME
|
||||
self._prev_onroad = False
|
||||
self._trip_summary = TripSummary()
|
||||
|
||||
# Initialize layouts
|
||||
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.scroller import Scroller
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.selfdrive.ui.sunnypilot.layouts.trip_summary import TripSummary
|
||||
|
||||
if gui_app.sunnypilot_ui():
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.layouts.settings import SettingsLayoutSP as SettingsLayout
|
||||
@@ -57,6 +58,7 @@ class MiciMainLayout(Scroller):
|
||||
self._setup_callbacks()
|
||||
|
||||
gui_app.add_nav_stack_tick(self._handle_transitions)
|
||||
self._trip_summary = TripSummary()
|
||||
gui_app.push_widget(self)
|
||||
|
||||
# Start onboarding if terms or training not completed, make sure to push after self
|
||||
|
||||
@@ -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.augmented_road_view import AugmentedRoadViewSP
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.onroad.hud_renderer import HudRendererSP as HudRenderer
|
||||
from openpilot.selfdrive.ui.sunnypilot.ui_state import OnroadTimerStatus
|
||||
|
||||
@@ -156,9 +157,10 @@ class BookmarkIcon(Widget):
|
||||
rl.draw_texture_ex(icon, rl.Vector2(icon_x, icon_y), 0.0, 1.0, rl.Color(255, 255, 255, round(255 * alpha)))
|
||||
|
||||
|
||||
class AugmentedRoadView(CameraView):
|
||||
class AugmentedRoadView(CameraView, AugmentedRoadViewSP):
|
||||
def __init__(self, bookmark_callback=None, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_NARROW_ROAD):
|
||||
super().__init__("camerad", stream_type)
|
||||
AugmentedRoadViewSP.__init__(self)
|
||||
self._bookmark_callback = bookmark_callback
|
||||
self._set_placeholder_color(rl.BLACK)
|
||||
|
||||
@@ -275,7 +277,7 @@ class AugmentedRoadView(CameraView):
|
||||
if show_navigation:
|
||||
self._navigation.render(rl.Rectangle(self._content_rect.x + self._content_rect.width, self.rect.y, PANEL_WIDTH, self.rect.height))
|
||||
else:
|
||||
self._confidence_ball.render(self.rect)
|
||||
AugmentedRoadViewSP.render_side_panel(self, self.rect)
|
||||
|
||||
self._bookmark_icon.render(self.rect)
|
||||
|
||||
|
||||
@@ -249,11 +249,7 @@ class ModelsLayout(Widget):
|
||||
return {"name": name, "downloading": True, "progress": progress, "status_text": tr("verifying")}
|
||||
if ds.downloading in statuses:
|
||||
speed = max((p.speed for p in progresses), default=0.0)
|
||||
speed_text = f"{speed / 1024 ** 2:.1f} MB/s" if speed > 0 else "--.- MB/s"
|
||||
if any(p.resuming for p in progresses):
|
||||
status_text = tr("resuming...") + f" {speed_text}"
|
||||
else:
|
||||
status_text = speed_text
|
||||
status_text = f"{speed / 1024 ** 2:.1f} MB/s" if speed > 0 else "--.- MB/s"
|
||||
return {"name": name, "downloading": True, "progress": progress, "status_text": status_text}
|
||||
if statuses <= {ds.downloaded, ds.cached}:
|
||||
return {"name": name, "text_color": ON_COLOR, "icon": "icons/checkmark.png"}
|
||||
|
||||
@@ -11,11 +11,17 @@ from openpilot.system.ui.lib.multilang import tr, tr_noop
|
||||
from openpilot.system.ui.widgets import DialogResult
|
||||
from openpilot.system.ui.widgets.confirm_dialog import ConfirmDialog
|
||||
from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp
|
||||
from opendbc.car.toyota.values import ToyotaFlags
|
||||
from opendbc.car.toyota.values import CAR, ToyotaFlags
|
||||
from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
|
||||
|
||||
|
||||
ONROAD_ONLY_DESCRIPTION = tr_noop("Start the vehicle to check vehicle compatibility.")
|
||||
SNG_HACK_UNAVAILABLE = tr_noop("sunnypilot Longitudinal Control must be available and enabled for your vehicle to use this feature.")
|
||||
BSM_UNAVAILABLE = tr_noop("Enhanced Blind Spot Monitoring requires a TSS2 Toyota/Lexus without SecOC, " +
|
||||
"or a smartDSU with sunnypilot longitudinal control.")
|
||||
BSM_STOCK_LONG_UNAVAILABLE = tr_noop("Enhanced Blind Spot Monitoring is not available while Enforce Factory Longitudinal Control is enabled.")
|
||||
BSM_REQUIRED = tr_noop("This vehicle does not report blind spot status over the standard CAN signals. " +
|
||||
"Enhanced Blind Spot Monitoring is required for blind spot detection.")
|
||||
PLATFORM_UNAVAILABLE = tr_noop("This feature is currently not available on this platform.")
|
||||
|
||||
DESCRIPTIONS = {
|
||||
@@ -26,6 +32,10 @@ DESCRIPTIONS = {
|
||||
'sunnypilot will allow some Toyota/Lexus cars to auto resume during stop and go traffic. ' +
|
||||
'This feature is only applicable to certain models that are able to use longitudinal control. This is an alpha feature. Use at your own risk.'
|
||||
),
|
||||
'enhanced_bsm': tr_noop(
|
||||
'sunnypilot will poll the blind spot monitor diagnostics directly from the radar. Use this on vehicles whose blind spot monitors ' +
|
||||
'do not report blind spot status over the standard CAN signals. This is an alpha feature. Use at your own risk.'
|
||||
),
|
||||
'tss2_smooth': tr_noop(
|
||||
'Enable smoother longitudinal control for TSS2 vehicles. ' +
|
||||
'Uses a custom speed-dependent integral gain curve for smoother traffic driving.'
|
||||
@@ -53,6 +63,14 @@ class ToyotaSettings(BrandSettings):
|
||||
enabled=lambda: not ui_state.engaged,
|
||||
)
|
||||
|
||||
self.enhanced_bsm = toggle_item_sp(
|
||||
lambda: tr("Enhanced Blind Spot Monitoring (Alpha)"),
|
||||
description=lambda: tr(DESCRIPTIONS["enhanced_bsm"]),
|
||||
initial_state=ui_state.params.get_bool("ToyotaEnhancedBsm"),
|
||||
callback=self._on_enable_enhanced_bsm,
|
||||
enabled=lambda: not ui_state.engaged,
|
||||
)
|
||||
|
||||
self.tss2_smooth = toggle_item_sp(
|
||||
lambda: tr("TSS2 Smooth Longitudinal (Alpha)"),
|
||||
description=lambda: tr(DESCRIPTIONS["tss2_smooth"]),
|
||||
@@ -64,6 +82,7 @@ class ToyotaSettings(BrandSettings):
|
||||
self.items = [
|
||||
self.enforce_stock_longitudinal,
|
||||
self.stop_and_go_hack,
|
||||
self.enhanced_bsm,
|
||||
self.tss2_smooth,
|
||||
]
|
||||
|
||||
@@ -130,6 +149,25 @@ class ToyotaSettings(BrandSettings):
|
||||
ui_state.params.put_bool("ToyotaStopAndGoHack", False)
|
||||
ui_state.params.put_bool("OnroadCycleRequested", True)
|
||||
|
||||
def _on_enable_enhanced_bsm(self, state: bool):
|
||||
if state:
|
||||
def confirm_callback(result: int):
|
||||
if result == DialogResult.CONFIRM:
|
||||
ui_state.params.put_bool("ToyotaEnhancedBsm", True)
|
||||
ui_state.params.put_bool("OnroadCycleRequested", True)
|
||||
else:
|
||||
self.enhanced_bsm.action_item.set_state(False)
|
||||
|
||||
content = (f"<h1>{self.enhanced_bsm.title}</h1><br>" +
|
||||
f"<p>{self.enhanced_bsm.description}</p>")
|
||||
|
||||
dlg = ConfirmDialog(content, tr("Enable"), rich=True, callback=confirm_callback)
|
||||
gui_app.push_widget(dlg)
|
||||
|
||||
else:
|
||||
ui_state.params.put_bool("ToyotaEnhancedBsm", False)
|
||||
ui_state.params.put_bool("OnroadCycleRequested", True)
|
||||
|
||||
def update_settings(self):
|
||||
if ui_state.CP is not None:
|
||||
longitudinal = ui_state.CP.openpilotLongitudinalControl
|
||||
@@ -150,6 +188,33 @@ class ToyotaSettings(BrandSettings):
|
||||
if show_desc:
|
||||
self.stop_and_go_hack.show_description(True)
|
||||
|
||||
# Enhanced BSM needs the radar diagnostic address, which is only in the TX allowlist for
|
||||
# TSS2 cars without SecOC, or for TSS-P cars with a smartDSU. Both need openpilot long.
|
||||
sdsu_bsm_supported = (ui_state.CP_SP is not None and
|
||||
bool(ui_state.CP_SP.flags & ToyotaFlagsSP.SMART_DSU))
|
||||
bsm_supported = (not bool(ui_state.CP.flags & ToyotaFlags.SECOC) and
|
||||
(bool(ui_state.CP.flags & ToyotaFlags.TSS2) or sdsu_bsm_supported))
|
||||
bsm_available = bsm_supported and longitudinal and not enforce_stock
|
||||
self.enhanced_bsm.action_item.set_enabled(bsm_available and not ui_state.engaged)
|
||||
|
||||
bsm_desc = tr(DESCRIPTIONS["enhanced_bsm"])
|
||||
if not bsm_supported:
|
||||
self.enhanced_bsm.action_item.set_state(False)
|
||||
bsm_desc = "<b>" + tr(BSM_UNAVAILABLE) + "</b>\n\n" + bsm_desc
|
||||
elif enforce_stock:
|
||||
self.enhanced_bsm.action_item.set_state(False)
|
||||
bsm_desc = "<b>" + tr(BSM_STOCK_LONG_UNAVAILABLE) + "</b>\n\n" + bsm_desc
|
||||
elif not longitudinal:
|
||||
self.enhanced_bsm.action_item.set_state(False)
|
||||
bsm_desc = "<b>" + tr(SNG_HACK_UNAVAILABLE) + "</b>\n\n" + bsm_desc
|
||||
elif ui_state.CP.carFingerprint == CAR.TOYOTA_PRIUS_TSS2:
|
||||
bsm_desc = "<b>" + tr(BSM_REQUIRED) + "</b>\n\n" + bsm_desc
|
||||
|
||||
if self.enhanced_bsm.description != bsm_desc:
|
||||
self.enhanced_bsm.set_description(bsm_desc)
|
||||
if not bsm_available:
|
||||
self.enhanced_bsm.show_description(True)
|
||||
|
||||
tss2 = bool(ui_state.CP.flags & ToyotaFlags.TSS2)
|
||||
if longitudinal and not enforce_stock and tss2:
|
||||
self.tss2_smooth.action_item.set_enabled(not ui_state.engaged)
|
||||
@@ -173,6 +238,12 @@ class ToyotaSettings(BrandSettings):
|
||||
self.stop_and_go_hack.set_description(new_desc)
|
||||
self.stop_and_go_hack.show_description(True)
|
||||
|
||||
self.enhanced_bsm.action_item.set_enabled(False)
|
||||
bsm_desc = "<b>" + tr(ONROAD_ONLY_DESCRIPTION) + "</b>\n\n" + tr(DESCRIPTIONS["enhanced_bsm"])
|
||||
if self.enhanced_bsm.description != bsm_desc:
|
||||
self.enhanced_bsm.set_description(bsm_desc)
|
||||
self.enhanced_bsm.show_description(True)
|
||||
|
||||
self.tss2_smooth.action_item.set_enabled(False)
|
||||
new_desc = "<b>" + tr(ONROAD_ONLY_DESCRIPTION) + "</b>\n\n" + tr(DESCRIPTIONS["tss2_smooth"])
|
||||
if self.tss2_smooth.description != new_desc:
|
||||
|
||||
@@ -72,6 +72,11 @@ class VisualsLayout(Widget):
|
||||
"It is the driver's responsibility to observe their environment and make decisions accordingly."),
|
||||
None,
|
||||
),
|
||||
"TripSummary": (
|
||||
lambda: tr("Trip Summary"),
|
||||
tr("Show a summary of distance and engagement when you shift into park after a drive."),
|
||||
None,
|
||||
),
|
||||
"TrueVEgoUI": (
|
||||
lambda: tr("Speedometer: Always Display True Speed"),
|
||||
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
|
||||
@@ -267,8 +267,8 @@ class ModelsLayoutMici(NavScroller):
|
||||
self.current_model_info.info_text.set_text(info_text)
|
||||
|
||||
if manager.selectedBundle and manager.selectedBundle.status == custom.ModelManagerSP.DownloadStatus.failed:
|
||||
self.current_model_info.info_header.set_text(tr("error"))
|
||||
self.current_model_info.info_text.set_text(f"{manager.selectedBundle.internalName.lower()} | {tr('download failed')}")
|
||||
self.current_model_info.info_header.set_text(tr("error") + self._download_progress)
|
||||
self.current_model_info.info_text.set_text(tr("download failed"))
|
||||
|
||||
elif manager.selectedBundle and manager.selectedBundle.status == custom.ModelManagerSP.DownloadStatus.downloading:
|
||||
self.cancel_download_btn.set_visible(ui_state.params.get("ModelManager_DownloadRef") is not None)
|
||||
@@ -277,7 +277,6 @@ class ModelsLayoutMici(NavScroller):
|
||||
count = 0
|
||||
verifying = False
|
||||
canceling = False
|
||||
resuming = False
|
||||
speed = 0.0
|
||||
for model in manager.selectedBundle.models:
|
||||
count += 1
|
||||
@@ -287,13 +286,12 @@ class ModelsLayoutMici(NavScroller):
|
||||
progress += p.progress
|
||||
verifying = verifying or p.status == custom.ModelManagerSP.DownloadStatus.verifying
|
||||
canceling = canceling or p.canceling
|
||||
resuming = resuming or p.resuming
|
||||
speed = max(speed, p.speed) # only one file downloads at a time, so the non-zero speed is the live one
|
||||
elif p.status in (custom.ModelManagerSP.DownloadStatus.downloaded,
|
||||
custom.ModelManagerSP.DownloadStatus.cached):
|
||||
progress += 100.0
|
||||
|
||||
header = tr("canceling...") if canceling else tr("verifying") if verifying else tr("resuming...") if resuming else tr("downloading")
|
||||
header = tr("canceling...") if canceling else tr("verifying") if verifying else tr("downloading")
|
||||
self.current_model_info.current_model_header.set_text(header)
|
||||
self.cancel_download_btn.set_text(tr("cancel verification") if verifying else tr("cancel download"))
|
||||
self.current_model_info.current_model_header._shimmer = True
|
||||
|
||||
@@ -0,0 +1,95 @@
|
||||
"""
|
||||
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 math
|
||||
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.sunnypilot.selfdrive.controls.lib.accel_boost import get_boost_max
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
|
||||
BAR_WIDTH = 26
|
||||
TRACK_TOP_RATIO = 0.08
|
||||
TRACK_BOTTOM_RATIO = 0.92
|
||||
CAP_BLEND_START = 0.45
|
||||
MAX_BOOST_BLEND = 0.55
|
||||
FADE_THRESHOLD = 0.01
|
||||
|
||||
COLOR_TRACK = rl.Color(255, 255, 255, 28)
|
||||
COLOR_FILL = rl.Color(0, 230, 120, 255)
|
||||
|
||||
|
||||
def get_boost_fraction(boost: float, max_boost: float) -> float:
|
||||
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:
|
||||
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_color(fraction: float) -> rl.Color:
|
||||
blend = max(0.0, (fraction - CAP_BLEND_START) / (1.0 - CAP_BLEND_START))
|
||||
return lighten(COLOR_FILL, MAX_BOOST_BLEND * blend)
|
||||
|
||||
|
||||
class AccelBoostIndicator(Widget):
|
||||
def __init__(self, panel_width: float = SIDE_PANEL_WIDTH, bar_width: float = BAR_WIDTH):
|
||||
super().__init__()
|
||||
self._panel_width = panel_width
|
||||
self._bar_width = bar_width
|
||||
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)
|
||||
self._max_boost_filter = FirstOrderFilter(get_boost_max(), 0.25, 1 / fps)
|
||||
|
||||
@property
|
||||
def holding_panel(self) -> bool:
|
||||
return self._alpha_filter.x > FADE_THRESHOLD
|
||||
|
||||
def _update_state(self):
|
||||
sm = ui_state.sm
|
||||
visible = (ui_state.accel_boost and ui_state.has_longitudinal_control and sm.alive['selfdriveState'] and
|
||||
sm.alive['longitudinalPlan'] and sm['selfdriveState'].enabled)
|
||||
plan = sm['longitudinalPlan']
|
||||
applied = visible and plan.longitudinalPlanSource == log.LongitudinalPlan.LongitudinalPlanSource.e2e
|
||||
self._boost_filter.update(float(plan.accelBoost) if applied else 0.0)
|
||||
self._alpha_filter.update(1.0 if visible else 0.0)
|
||||
|
||||
if sm.alive['selfdriveState']:
|
||||
self._max_boost_filter.update(get_boost_max(sm['selfdriveState'].personality))
|
||||
|
||||
def _render(self, _):
|
||||
alpha = self._alpha_filter.x
|
||||
if alpha < FADE_THRESHOLD:
|
||||
return
|
||||
|
||||
panel_x = max(self.rect.x, self.rect.x + self.rect.width - self._panel_width)
|
||||
bar_x = panel_x + (self._panel_width - self._bar_width) / 2
|
||||
track_top = self.rect.y + self.rect.height * TRACK_TOP_RATIO
|
||||
track = rl.Rectangle(bar_x, track_top, self._bar_width,
|
||||
max(1.0, self.rect.height * (TRACK_BOTTOM_RATIO - TRACK_TOP_RATIO)))
|
||||
rl.draw_rectangle_rounded(track, 0.5, 12,
|
||||
rl.Color(COLOR_TRACK.r, COLOR_TRACK.g, COLOR_TRACK.b, int(COLOR_TRACK.a * alpha)))
|
||||
|
||||
fraction = get_boost_fraction(self._boost_filter.x, self._max_boost_filter.x)
|
||||
fill_h = fraction * track.height
|
||||
if fill_h < 1.0:
|
||||
return
|
||||
|
||||
fill_color = get_boost_color(fraction)
|
||||
tip = track.y + track.height - fill_h
|
||||
rl.begin_scissor_mode(int(track.x), int(tip), int(math.ceil(track.width)) + 1, int(math.ceil(fill_h)))
|
||||
rl.draw_rectangle_rounded(track, 0.5, 12, rl.Color(fill_color.r, fill_color.g, fill_color.b, int(235 * alpha)))
|
||||
rl.end_scissor_mode()
|
||||
@@ -0,0 +1,19 @@
|
||||
"""
|
||||
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.selfdrive.ui.sunnypilot.mici.onroad.accel_boost_indicator import AccelBoostIndicator
|
||||
|
||||
|
||||
class AugmentedRoadViewSP:
|
||||
def __init__(self):
|
||||
self._accel_boost_indicator = AccelBoostIndicator()
|
||||
|
||||
def render_side_panel(self, rect: rl.Rectangle) -> None:
|
||||
self._accel_boost_indicator.render(rect)
|
||||
if not self._accel_boost_indicator.holding_panel:
|
||||
self._confidence_ball.render(rect)
|
||||
@@ -0,0 +1,36 @@
|
||||
"""
|
||||
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 (
|
||||
CAP_BLEND_START, COLOR_FILL, get_boost_color, get_boost_fraction,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost import ACCEL_BOOST_MAX, get_boost_max
|
||||
|
||||
|
||||
class TestAccelBoostIndicator(OpenpilotTestCase):
|
||||
def test_bar_fills_at_the_active_profile_cap(self):
|
||||
for personality in (log.LongitudinalPersonality.aggressive, log.LongitudinalPersonality.standard,
|
||||
log.LongitudinalPersonality.relaxed):
|
||||
cap = get_boost_max(personality)
|
||||
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
|
||||
|
||||
assert get_boost_fraction(max(ACCEL_BOOST_MAX), min(ACCEL_BOOST_MAX)) == 1.0
|
||||
assert get_boost_fraction(0.0, 0.0) == 0.0
|
||||
assert get_boost_fraction(-1.0, get_boost_max()) == 0.0
|
||||
|
||||
def test_fill_lightens_as_boost_builds(self):
|
||||
base = get_boost_color(0.0)
|
||||
assert (base.r, base.g, base.b) == (COLOR_FILL.r, COLOR_FILL.g, COLOR_FILL.b)
|
||||
assert (get_boost_color(CAP_BLEND_START).r, get_boost_color(CAP_BLEND_START).b) == (COLOR_FILL.r, COLOR_FILL.b)
|
||||
|
||||
full = get_boost_color(1.0)
|
||||
assert full.r > base.r
|
||||
assert full.b > base.b
|
||||
assert full.g > full.r and full.g > full.b
|
||||
@@ -44,10 +44,12 @@ class UIStateSP:
|
||||
self.screensaver = ScreenSaverSP(params=self.params)
|
||||
self.screensaver_enabled: bool = False
|
||||
|
||||
self.accel_boost: bool = False
|
||||
self.active_bundle = None
|
||||
self.model_runner_tinygrad: bool = False
|
||||
self.active_runner_tinygrad: bool = False
|
||||
self.blindspot: bool = False
|
||||
self.trip_summary: bool = True
|
||||
self.chevron_metrics = None
|
||||
self.custom_interactive_timeout: int = 0
|
||||
self.developer_ui = None
|
||||
@@ -171,6 +173,7 @@ class UIStateSP:
|
||||
# stock only counts the default big model's compiled pkl. a downloaded big bundle runs on the
|
||||
# chestnut just the same, so ChestnutState has to see it as available too.
|
||||
self.chestnut_compiled = self.chestnut_compiled or self.model_runner_tinygrad
|
||||
self.accel_boost = self.params.get_bool("AccelBoostSP")
|
||||
self.blindspot = self.params.get_bool("BlindSpot")
|
||||
self.chevron_metrics = self.params.get("ChevronInfo")
|
||||
self.custom_interactive_timeout = self.params.get("InteractivityTimeout", return_default=True)
|
||||
@@ -185,6 +188,7 @@ class UIStateSP:
|
||||
self.standstill_timer = self.params.get_bool("StandstillTimer")
|
||||
self.sunnylink_enabled = self.params.get_bool("SunnylinkEnabled")
|
||||
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.custom_torque_params = self.params.get_bool("CustomTorqueParams")
|
||||
self.torque_override_enabled = self.params.get_bool("TorqueParamsOverrideEnabled")
|
||||
|
||||
@@ -41,7 +41,6 @@ from openpilot.selfdrive.modeld.modeld import ChestnutGpuState
|
||||
from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser
|
||||
from openpilot.sunnypilot.modeld_v2.blendful import Blendful
|
||||
from openpilot.sunnypilot.modeld_v2.turn_planner import TurnPlanner
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
|
||||
from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants
|
||||
from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
|
||||
@@ -232,8 +231,7 @@ class ModelState(ModelStateBase):
|
||||
|
||||
def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
|
||||
lat_action_t: float, long_action_t: float, v_ego: float,
|
||||
blendful: Callable[[float], float] | None = None,
|
||||
turn_shaper: Callable[[float], float] | None = None) -> log.ModelDataV2.Action:
|
||||
blendful: Callable[[float], float] | None = None) -> log.ModelDataV2.Action:
|
||||
if 'action' not in model_output:
|
||||
plan = model_output['plan'][0]
|
||||
desired_accel = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS,
|
||||
@@ -247,8 +245,6 @@ class ModelState(ModelStateBase):
|
||||
desired_curvature = model_output['action'][0, 0] / (max(2.5, v_ego))**2
|
||||
if blendful is not None:
|
||||
desired_curvature += blendful(desired_curvature)
|
||||
if turn_shaper is not None:
|
||||
desired_curvature = turn_shaper(desired_curvature)
|
||||
|
||||
stop = v_ego < 0.3 and desired_accel < 0.1
|
||||
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
|
||||
@@ -377,7 +373,6 @@ def main(demo=False):
|
||||
RELC = RoadEdgeLaneChangeController()
|
||||
blendful = Blendful()
|
||||
blendful_enabled = False
|
||||
turn_planner = TurnPlanner(CP.wheelbase)
|
||||
|
||||
while True:
|
||||
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
|
||||
@@ -497,9 +492,7 @@ def main(demo=False):
|
||||
prev_action.desiredCurvature = sm['controlsState'].curvature
|
||||
blendful_active = desire == log.Desire.none and DH.lane_change_state == log.LaneChangeState.off
|
||||
action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego,
|
||||
partial(blendful.update, model_output, v_ego, blendful_active, enabled=blendful_enabled),
|
||||
partial(turn_planner.update, model_output, v_ego, sm['controlsState'].curvature,
|
||||
sm['carControl'].latActive, is_rhd))
|
||||
partial(blendful.update, model_output, v_ego, blendful_active, enabled=blendful_enabled))
|
||||
prev_action = action
|
||||
fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
|
||||
publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
|
||||
|
||||
@@ -1,239 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
|
||||
from openpilot.sunnypilot.modeld_v2.turn_planner import TurnPlanner, compute_plan_peak_curv
|
||||
|
||||
|
||||
def make_plan(curv: float, speed: float = 4.0) -> dict[str, np.ndarray]:
|
||||
"""Build a synthetic model_output dict with a constant-curvature arc plan."""
|
||||
t = np.array(ModelConstants.T_IDXS, dtype=np.float32)
|
||||
s = speed * t
|
||||
plan = np.zeros((1, len(t), ModelConstants.PLAN_WIDTH), dtype=np.float32)
|
||||
if abs(curv) < 1e-6:
|
||||
plan[0, :, Plan.POSITION][:, 0] = s
|
||||
plan[0, :, Plan.POSITION][:, 1] = 0.0
|
||||
plan[0, :, Plan.T_FROM_CURRENT_EULER][:, 2] = 0.0
|
||||
else:
|
||||
r = 1.0 / curv
|
||||
theta = s * curv
|
||||
plan[0, :, Plan.POSITION][:, 0] = r * np.sin(theta)
|
||||
plan[0, :, Plan.POSITION][:, 1] = r * (1.0 - np.cos(theta))
|
||||
plan[0, :, Plan.T_FROM_CURRENT_EULER][:, 2] = theta
|
||||
return {'plan': plan}
|
||||
|
||||
|
||||
class TestTurnPlanner(OpenpilotTestCase):
|
||||
def test_plan_peak_curv_extraction(self):
|
||||
assert abs(compute_plan_peak_curv(make_plan(0.0, 5.0))) < 1e-5
|
||||
pk = compute_plan_peak_curv(make_plan(0.12, 4.0))
|
||||
assert abs(pk - 0.12) < 0.01
|
||||
pk_l = compute_plan_peak_curv(make_plan(-0.15, 4.0))
|
||||
assert abs(pk_l + 0.15) < 0.01
|
||||
assert compute_plan_peak_curv({}) == 0.0
|
||||
|
||||
def test_untouched_at_highway_speed(self):
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
for i in range(60):
|
||||
k = 0.01 * math.sin(i * 0.1)
|
||||
out = tp.update(make_plan(k, 25.0), v_ego=25.0, actual_curv=k, active=True, is_rhd=False, model_curv=k)
|
||||
assert out == k
|
||||
|
||||
def test_untouched_on_gentle_low_speed_curve(self):
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
for i in range(80):
|
||||
k = 0.025 * min(1.0, i / 20.0)
|
||||
out = tp.update(make_plan(k, 6.0), v_ego=6.0, actual_curv=k, active=True, is_rhd=False, model_curv=k)
|
||||
assert out == k
|
||||
|
||||
def test_sign_preservation_and_magnitude_bound_under_jitter(self):
|
||||
"""Output must never oppose model_curv or exceed APEX_PATH_BOOST_CAP * |model_curv|."""
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
# Reproduces a5 seg31 Turn 1: brief negative jitter then sharp positive right turn
|
||||
ks = [-0.015] * 15 + [0.016, 0.05, 0.11, 0.14, 0.17, 0.18, 0.14, 0.08, 0.02, -0.01]
|
||||
plan_right = make_plan(0.15, 3.0)
|
||||
for k in ks:
|
||||
out = tp.update(plan_right, v_ego=3.0, actual_curv=k * 0.7, active=True, is_rhd=False, model_curv=k)
|
||||
assert abs(out) <= abs(k) * 1.35 + 1e-9
|
||||
if k > 0:
|
||||
assert out >= 0.0
|
||||
elif k < 0:
|
||||
assert out <= 0.0
|
||||
|
||||
def test_zero_lag_on_turn_exit_unwinding(self):
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
plan_tight = make_plan(0.14, 4.0)
|
||||
# Ramp up entry and hold through apex so buffer is at peak curvature
|
||||
for i in range(25):
|
||||
k = 0.14 * (i / 25.0)
|
||||
tp.update(plan_tight, v_ego=4.0, actual_curv=k * 0.8, active=True, is_rhd=False, model_curv=k)
|
||||
for _ in range(15):
|
||||
tp.update(plan_tight, v_ego=4.0, actual_curv=0.14, active=True, is_rhd=False, model_curv=0.14)
|
||||
# Now unwind monotonically: output must equal model_curv with zero lag
|
||||
for i in range(1, 20):
|
||||
k = max(0.0, 0.14 * (1.0 - i / 15.0))
|
||||
out = tp.update(make_plan(k, 4.0), v_ego=4.0, actual_curv=k + 0.01, active=True, is_rhd=False, model_curv=k)
|
||||
assert abs(out - k) < 1e-3
|
||||
|
||||
def test_standstill_launch_and_mid_turn_pause(self):
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
# Stopped straight at stop bar, then launches into a sharp right turn
|
||||
for _ in range(10):
|
||||
tp.update(make_plan(0.12, 0.0), v_ego=0.0, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.002)
|
||||
# First few steps of launch should hold back early curvature ramp
|
||||
outs = []
|
||||
for i in range(1, 20):
|
||||
k = 0.006 * i
|
||||
outs.append(tp.update(make_plan(0.12, 2.5), v_ego=2.5, actual_curv=0.004 * i, active=True, is_rhd=False, model_curv=k))
|
||||
assert outs[10] < 0.006 * 11 - 0.01
|
||||
|
||||
# Pause mid-turn (actual_curv = 0.05) should preserve turn_psi and not dip below 0.05 on resume
|
||||
tp.turn_psi = math.radians(30.0)
|
||||
tp.update(make_plan(0.08, 0.0), v_ego=0.0, actual_curv=0.05, active=True, is_rhd=False, model_curv=0.05)
|
||||
assert tp.turn_psi >= math.radians(29.0)
|
||||
resumed = tp.update(make_plan(0.08, 2.0), v_ego=2.0, actual_curv=0.05, active=True, is_rhd=False, model_curv=0.06)
|
||||
assert resumed >= 0.05 - 1e-6
|
||||
|
||||
def test_wheelbase_and_rhd_scaling(self):
|
||||
tp_short = TurnPlanner(wheelbase=2.4)
|
||||
tp_long = TurnPlanner(wheelbase=3.4)
|
||||
plan_r = make_plan(0.14, 4.2)
|
||||
for _ in range(25):
|
||||
tp_short.update(plan_r, 4.2, 0.0, True, False, 0.02)
|
||||
tp_long.update(plan_r, 4.2, 0.0, True, False, 0.02)
|
||||
assert tp_long.shift_dist > tp_short.shift_dist * 1.3
|
||||
|
||||
# RHD gives larger shift on left turns (k < 0) than right turns (k > 0)
|
||||
tp_rhd_l = TurnPlanner(wheelbase=2.89)
|
||||
tp_rhd_r = TurnPlanner(wheelbase=2.89)
|
||||
for _ in range(25):
|
||||
tp_rhd_l.update(make_plan(-0.14, 3.5), 3.5, 0.0, True, True, -0.02)
|
||||
tp_rhd_r.update(make_plan(0.14, 3.5), 3.5, 0.0, True, True, 0.02)
|
||||
assert tp_rhd_l.shift_dist > tp_rhd_r.shift_dist * 1.2
|
||||
|
||||
def test_closed_loop_90deg_turn_clears_curb_without_oncoming_overshoot(self):
|
||||
"""Closed-loop Tesla Model Y 90-deg right turn around R=2.5 m curb."""
|
||||
L, W_half, x_cam, lane_w, curb_r = 2.89, 0.96, 1.50, 3.60, 2.5
|
||||
y_curb = lane_w / 2.0
|
||||
x_dest = curb_r + lane_w / 2.0
|
||||
x_yellow = x_dest + lane_w / 2.0
|
||||
|
||||
pts = [(x, 0.0) for x in np.linspace(-20, x_dest - 2.0, 200)]
|
||||
pts += [(x_dest - 2.0 + 2.0 * math.cos(th), 2.0 + 2.0 * math.sin(th)) for th in np.linspace(-math.pi / 2, 0, 50)]
|
||||
pts += [(x_dest, y) for y in np.linspace(2.0, 25.0, 200)]
|
||||
ref = np.array(pts)
|
||||
|
||||
def run(use_planner: bool) -> tuple[float, float]:
|
||||
X, Y, psi, k_act = -15.0, 0.0, 0.0, 0.0
|
||||
v = 3.5
|
||||
tp = TurnPlanner(wheelbase=L, dt=DT_MDL) if use_planner else None
|
||||
min_clr, max_ovr = 99.0, -99.0
|
||||
for _ in range(400):
|
||||
xc, yc = X + x_cam * math.cos(psi), Y + x_cam * math.sin(psi)
|
||||
i_near = int(np.argmin(np.hypot(ref[:, 0] - xc, ref[:, 1] - yc)))
|
||||
i_tgt = i_near
|
||||
while i_tgt < len(ref) - 1 and np.hypot(ref[i_tgt, 0] - xc, ref[i_tgt, 1] - yc) < 6.5:
|
||||
i_tgt += 1
|
||||
dx, dy = ref[i_tgt, 0] - xc, ref[i_tgt, 1] - yc
|
||||
lx = dx * math.cos(psi) + dy * math.sin(psi)
|
||||
ly = -dx * math.sin(psi) + dy * math.cos(psi)
|
||||
k_mod = float(np.clip(2.0 * ly / max(lx**2 + ly**2, 1.0), -0.19, 0.19))
|
||||
plan = make_plan(0.14 if (max(0.0, x_dest - xc) < 15.0 and psi < math.radians(75)) else 0.0, v)
|
||||
|
||||
k_cmd = tp.update(plan, v, k_act, True, False, k_mod) if tp is not None else k_mod
|
||||
k_act += (float(np.clip(k_cmd, -0.19, 0.19)) - k_act) * (DT_MDL / (0.22 + DT_MDL))
|
||||
|
||||
ds = v * DT_MDL
|
||||
psi += k_act * ds
|
||||
X += ds * math.cos(psi)
|
||||
Y += ds * math.sin(psi)
|
||||
|
||||
x_ir, y_ir = X - W_half * math.sin(psi), Y + W_half * math.cos(psi)
|
||||
x_of, y_of = X + L * math.cos(psi) + W_half * math.sin(psi), Y + L * math.sin(psi) - W_half * math.cos(psi)
|
||||
if x_ir <= 0.0:
|
||||
clr = y_curb - y_ir
|
||||
elif y_ir >= y_curb + curb_r:
|
||||
clr = x_ir - curb_r
|
||||
else:
|
||||
clr = math.hypot(x_ir, y_ir - (y_curb + curb_r)) - curb_r
|
||||
min_clr = min(min_clr, clr)
|
||||
if y_of > 2.0:
|
||||
max_ovr = max(max_ovr, x_of - x_yellow)
|
||||
if Y > 15.0:
|
||||
break
|
||||
return min_clr, max_ovr
|
||||
|
||||
base_clr, _ = run(False)
|
||||
plan_clr, plan_ovr = run(True)
|
||||
assert base_clr < -0.5 # baseline curbs the rear wheel badly
|
||||
assert plan_clr > 0.20 # TurnPlanner clears the curb by > 20 cm
|
||||
assert plan_ovr < 0.0 # stays inside the oncoming yellow line
|
||||
|
||||
def test_rolling_speed_taper(self):
|
||||
"""Shift is strong at low speed, light at rolling speed, 0 at 19+ mph, and time-capped at crawl."""
|
||||
tp_crawl = TurnPlanner(wheelbase=2.89)
|
||||
tp_low = TurnPlanner(wheelbase=2.89)
|
||||
tp_roll = TurnPlanner(wheelbase=2.89)
|
||||
tp_fast = TurnPlanner(wheelbase=2.89)
|
||||
for _ in range(30):
|
||||
tp_crawl.update(make_plan(0.14, 1.8), v_ego=1.8, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02)
|
||||
tp_low.update(make_plan(0.14, 3.5), v_ego=3.5, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02)
|
||||
tp_roll.update(make_plan(0.14, 6.0), v_ego=6.0, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02)
|
||||
tp_fast.update(make_plan(0.14, 9.0), v_ego=9.0, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02)
|
||||
assert tp_crawl.shift_dist <= 1.8 * 0.48 + 0.05 # time-capped so crawl starts don't stall
|
||||
assert tp_low.shift_dist > 1.3
|
||||
assert 0.3 < tp_roll.shift_dist < 0.7
|
||||
assert tp_fast.shift_dist == 0.0
|
||||
|
||||
def test_apex_1s_path_boost_and_exit_unwind(self):
|
||||
"""Once past entry, blends with 1s path to boost under-steering apex and unwind hanging exit."""
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
# Drive past entry (psi > 30 deg)
|
||||
for _ in range(35):
|
||||
tp.update(make_plan(0.14, 4.0), v_ego=4.0, actual_curv=0.12, active=True, is_rhd=False, model_curv=0.10)
|
||||
# Apex (psi ~ 48 deg): action under-steers at 0.10 while 1s path says 0.14 -> boosted cleanly above 0.10
|
||||
apex_out = tp.update(make_plan(0.14, 4.0), v_ego=4.0, actual_curv=0.12, active=True, is_rhd=False, model_curv=0.10)
|
||||
assert apex_out > 0.115
|
||||
# Advance past 60 deg into late-turn exit
|
||||
for _ in range(20):
|
||||
tp.update(make_plan(0.14, 4.0), v_ego=4.0, actual_curv=0.12, active=True, is_rhd=False, model_curv=0.14)
|
||||
# Exit hang: action still stuck at 0.20 while 1s path already unwound to 0.10 -> pulls down cleanly
|
||||
exit_out = tp.update(make_plan(0.10, 4.0), v_ego=4.0, actual_curv=0.15, active=True, is_rhd=False, model_curv=0.20)
|
||||
assert exit_out < 0.15
|
||||
|
||||
def test_road_edge_curb_guard_holds_until_opening(self):
|
||||
"""Confident straight curb beside car prolongs turn-in until curb flares open or disappears."""
|
||||
x = np.array(ModelConstants.X_IDXS, dtype=np.float32)
|
||||
|
||||
def with_curb(plan_dict, e0=2.4, flare_5m=0.1, std=0.4):
|
||||
edges = np.zeros((1, 2, len(x), 2), dtype=np.float32)
|
||||
edges[0, 0, :, 0] = -5.0
|
||||
edges[0, 1, :, 0] = e0 + (flare_5m / 25.0) * (x ** 2)
|
||||
stds = np.full((1, 2, len(x), 2), std, dtype=np.float32)
|
||||
return {**plan_dict, 'road_edges': edges, 'road_edges_stds': stds}
|
||||
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
# Rolling 15 mph (6.7 m/s) right turn with solid straight curb right beside car -> curb guard holds it down
|
||||
mo_curb = with_curb(make_plan(0.03, 6.7), e0=2.4, flare_5m=0.1, std=0.4)
|
||||
for _ in range(5):
|
||||
out_guarded = tp.update(mo_curb, v_ego=6.7, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.035)
|
||||
assert out_guarded < 0.016
|
||||
|
||||
# Curb flares open (intersection/turn lane opens) -> releases immediately!
|
||||
mo_open = with_curb(make_plan(0.06, 6.7), e0=2.4, flare_5m=1.2, std=1.1)
|
||||
out_open = tp.update(mo_open, v_ego=6.7, actual_curv=0.01, active=True, is_rhd=False, model_curv=0.06)
|
||||
assert out_open > 0.04
|
||||
|
||||
# Normal curved road (curb itself curves at k=0.03 => flare_5m = 0.5 * 0.03 * 25 = 0.375 m) -> untouched
|
||||
tp2 = TurnPlanner(wheelbase=2.89)
|
||||
mo_curve = with_curb(make_plan(0.03, 6.7), e0=2.4, flare_5m=0.375, std=0.4)
|
||||
out_curve = tp2.update(mo_curve, v_ego=6.7, actual_curv=0.03, active=True, is_rhd=False, model_curv=0.03)
|
||||
assert abs(out_curve - 0.03) < 1e-6
|
||||
@@ -1,272 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import math
|
||||
from collections import deque
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
|
||||
|
||||
# Two-stage rear-axle protected turn planner:
|
||||
# Stage 1 (Entry): wheelbase-scaled spatial delay on curvature buildup so the
|
||||
# inside rear tire clears the corner curb instead of off-tracking over it.
|
||||
# Capped in time (MAX_SHIFT_TIME) so crawl-speed starts never feel stalled.
|
||||
# Stage 2 (Apex & Exit): once past initial corner entry, smoothly blends
|
||||
# action curvature with the model's own 1-second path curvature so tight
|
||||
# apexes don't under-steer/go wide and exits unwind cleanly.
|
||||
SHIFT_WHEELBASE_RATIO = 0.58 # fraction of wheelbase to delay turn-in (~1.68 m for 2.89 m WB)
|
||||
WHEELBASE_CLIP = (2.0, 4.0) # m
|
||||
OPPOSITE_SIDE_WEIGHT = 0.70 # lighter shift for non-curb-side turns
|
||||
MAX_SHIFT_TIME = 0.48 # s, caps entry delay at crawl speeds (2-5 mph)
|
||||
|
||||
TURN_SPEED_TAPER = (7.5, 10.5) # m/s (~17-23.5 mph): overall active window
|
||||
SHIFT_SPEED_BP = (3.5, 6.0, 8.5) # m/s (~7.8, 13.4, 19.0 mph)
|
||||
SHIFT_SPEED_V = (1.0, 0.35, 0.0) # full from stop, light on rolling turns, 0 above 19 mph
|
||||
CURV_GATE = (0.04, 0.09) # 1/m (R=25 m -> R=11 m): ignores gentle curves
|
||||
|
||||
ENTRY_PHASE_RELEASE_DEG = (8.0, 22.0) # deg: release entry shift crisply once rotating
|
||||
APEX_BLEND_PHASE_DEG = (10.0, 25.0) # deg: ramp in 1s-path blend through apex & exit
|
||||
APEX_PATH_BLEND_MAX = 0.65 # max weight on 1s path curvature during apex/exit
|
||||
APEX_PATH_BOOST_CAP = 1.35 # max ratio vs |model_curv| for safety
|
||||
PATH_AVG_HORIZON_S = 1.0 # s, horizon for average path curvature
|
||||
|
||||
SHIFT_BUILD_TAU = 0.15 # s
|
||||
SHIFT_RELEASE_TAU = 0.07 # s
|
||||
APEX_BLEND_TAU = 0.12 # s
|
||||
BUFFER_MAX_DIST = 4.5 # m
|
||||
MIN_MOVE_SPEED = 0.3 # m/s
|
||||
MID_TURN_CURV_THRESH = 0.02 # 1/m
|
||||
PLAN_S_RANGE = (1.0, 18.0) # m
|
||||
PLAN_MIN_SEG_DS = 0.8 # m
|
||||
|
||||
# Road-edge curb guard: prevents premature turn-in (e.g. immediately on blinker)
|
||||
# while a confident, unbroken curb still runs straight beside the vehicle.
|
||||
CURB_STD_GATE = (0.55, 0.95) # m: confident curb below 0.55, gone/opening above 0.95
|
||||
CURB_CLOSE_GATE = (2.5, 3.1) # m: curb close to lane below 2.5, open turn bay above 3.1
|
||||
CURB_OPENING_GATE = (0.35, 0.90) # m: (edge_5m - edge_0m) flare indicating intersection opening
|
||||
CURB_FOLLOW_MARGIN_K = 0.005 # 1/m: small lane-keeping allowance above curb's own curvature
|
||||
CURB_CHECK_X = 5.0 # m: lookahead along road edge for curb wall check
|
||||
CURB_GUARD_PHASE_DEG = (10.0, 22.0) # deg: only guards turn entry before car has rotated in
|
||||
|
||||
T_IDXS = np.array(ModelConstants.T_IDXS, dtype=np.float32)
|
||||
X_IDXS = np.array(ModelConstants.X_IDXS, dtype=np.float32)
|
||||
|
||||
|
||||
def compute_plan_metrics(model_output: dict[str, np.ndarray]) -> tuple[float, float]:
|
||||
"""Return (peak_upcoming_curv, avg_1s_path_curv) from model_output['plan']."""
|
||||
plan_arr = model_output.get('plan')
|
||||
if plan_arr is None or plan_arr.ndim < 3 or plan_arr.shape[1] < 3:
|
||||
return 0.0, 0.0
|
||||
|
||||
plan = plan_arr[0]
|
||||
pos = plan[:, Plan.POSITION]
|
||||
yaws = plan[:, Plan.T_FROM_CURRENT_EULER][:, 2]
|
||||
ds = np.hypot(np.diff(pos[:, 0]), np.diff(pos[:, 1]))
|
||||
s = np.concatenate(([0.0], np.cumsum(ds)))
|
||||
|
||||
peak_k = 0.0
|
||||
n = len(yaws)
|
||||
for i in range(n - 2):
|
||||
if s[i] < PLAN_S_RANGE[0]:
|
||||
continue
|
||||
if s[i] > PLAN_S_RANGE[1]:
|
||||
break
|
||||
seg_ds = s[i + 2] - s[i]
|
||||
if seg_ds < PLAN_MIN_SEG_DS:
|
||||
continue
|
||||
k = float((yaws[i + 2] - yaws[i]) / seg_ds)
|
||||
if abs(k) > abs(peak_k):
|
||||
peak_k = k
|
||||
peak_k = float(np.clip(peak_k, -0.25, 0.25))
|
||||
|
||||
if len(yaws) == len(T_IDXS):
|
||||
s_1s = float(np.interp(PATH_AVG_HORIZON_S, T_IDXS, s))
|
||||
yaw_1s = float(np.interp(PATH_AVG_HORIZON_S, T_IDXS, yaws))
|
||||
path_k_1s = float(np.clip(yaw_1s / s_1s, -0.25, 0.25)) if s_1s >= PLAN_MIN_SEG_DS else 0.0
|
||||
else:
|
||||
path_k_1s = 0.0
|
||||
|
||||
return peak_k, path_k_1s
|
||||
|
||||
|
||||
def compute_plan_peak_curv(model_output: dict[str, np.ndarray]) -> float:
|
||||
return compute_plan_metrics(model_output)[0]
|
||||
|
||||
|
||||
def compute_curb_guard(model_output: dict[str, np.ndarray], turn_sign: float) -> tuple[float, float]:
|
||||
"""
|
||||
Check if a confident, unbroken road edge (curb) still runs straight beside the car
|
||||
on the turning side (turn_sign > 0 = right edge, turn_sign < 0 = left edge).
|
||||
Returns (curb_weight in [0, 1], max_safe_curv_abs).
|
||||
"""
|
||||
edges = model_output.get('road_edges')
|
||||
stds = model_output.get('road_edges_stds')
|
||||
if edges is None or stds is None or edges.ndim < 4 or edges.shape[1] < 2:
|
||||
return 0.0, 1.0
|
||||
|
||||
idx = 1 if turn_sign > 0.0 else 0
|
||||
edge_y = edges[0, idx, :, 0]
|
||||
if len(edge_y) != len(X_IDXS):
|
||||
return 0.0, 1.0
|
||||
edge_std = float(stds[0, idx, 0, 0])
|
||||
|
||||
std_w = float(np.interp(edge_std, CURB_STD_GATE, [1.0, 0.0]))
|
||||
if std_w <= 0.0:
|
||||
return 0.0, 1.0
|
||||
|
||||
e0 = float(edge_y[0]) * turn_sign
|
||||
e5 = float(np.interp(CURB_CHECK_X, X_IDXS, edge_y)) * turn_sign
|
||||
if e0 <= 0.0:
|
||||
return 0.0, 1.0
|
||||
|
||||
close_w = float(np.interp(e0, CURB_CLOSE_GATE, [1.0, 0.0]))
|
||||
flare = e5 - e0
|
||||
unopened_w = float(np.interp(flare, CURB_OPENING_GATE, [1.0, 0.0]))
|
||||
curb_w = std_w * close_w * unopened_w
|
||||
if curb_w <= 0.0:
|
||||
return 0.0, 1.0
|
||||
|
||||
# Allow following the curb's own curvature (so normal curved roads are untouched) plus small margin.
|
||||
k_curb = max(0.0, 2.0 * flare / (CURB_CHECK_X * CURB_CHECK_X))
|
||||
return curb_w, k_curb + CURB_FOLLOW_MARGIN_K
|
||||
|
||||
|
||||
class TurnPlanner:
|
||||
"""
|
||||
Two-stage rear-axle protected turn planner for tight low-speed turns.
|
||||
|
||||
Stage 1 (Entry): delays curvature buildup by a wheelbase-proportional distance
|
||||
(time-capped at MAX_SHIFT_TIME) so the rear axle clears the inside curb.
|
||||
Stage 2 (Apex & Exit): blends action curvature with the model's 1-second path
|
||||
curvature once past entry, preventing wide apexes and exit hang-ups.
|
||||
"""
|
||||
|
||||
def __init__(self, wheelbase: float = 2.7, dt: float = DT_MDL):
|
||||
self.wheelbase = float(np.clip(wheelbase, *WHEELBASE_CLIP))
|
||||
self.dt = dt
|
||||
self.s = 0.0
|
||||
self.buf_s: deque[float] = deque()
|
||||
self.buf_k: deque[float] = deque()
|
||||
self.turn_sign = 0.0
|
||||
self.turn_psi = 0.0
|
||||
self.shift_dist = 0.0
|
||||
self.apex_blend = 0.0
|
||||
self.plan_peak_curv = 0.0
|
||||
self.path_curv_1s = 0.0
|
||||
|
||||
def reset(self) -> None:
|
||||
self.s = 0.0
|
||||
self.buf_s.clear()
|
||||
self.buf_k.clear()
|
||||
self.turn_sign = 0.0
|
||||
self.turn_psi = 0.0
|
||||
self.shift_dist = 0.0
|
||||
self.apex_blend = 0.0
|
||||
self.plan_peak_curv = 0.0
|
||||
self.path_curv_1s = 0.0
|
||||
|
||||
def update(self, model_output: dict[str, np.ndarray], v_ego: float, actual_curv: float,
|
||||
active: bool, is_rhd: bool, model_curv: float, enabled: bool = True) -> float:
|
||||
if not enabled or not active:
|
||||
self.reset()
|
||||
return model_curv
|
||||
|
||||
if v_ego < MIN_MOVE_SPEED:
|
||||
# Stopped straight (e.g., at a stop bar): reset turn phase so launch gets full protection.
|
||||
# Stopped mid-turn (e.g., yielding to a pedestrian): preserve turn_psi so resume doesn't re-delay.
|
||||
if abs(actual_curv) < MID_TURN_CURV_THRESH:
|
||||
self.turn_psi = 0.0
|
||||
self.turn_sign = 0.0
|
||||
self.shift_dist = 0.0
|
||||
self.apex_blend = 0.0
|
||||
self.buf_s.clear()
|
||||
self.buf_k.clear()
|
||||
self.buf_s.append(self.s)
|
||||
self.buf_k.append(float(actual_curv))
|
||||
return model_curv
|
||||
|
||||
ds = v_ego * self.dt
|
||||
self.s += ds
|
||||
self.buf_s.append(self.s)
|
||||
self.buf_k.append(float(model_curv))
|
||||
while len(self.buf_s) > 2 and (self.s - self.buf_s[0]) > BUFFER_MAX_DIST:
|
||||
self.buf_s.popleft()
|
||||
self.buf_k.popleft()
|
||||
|
||||
turn_speed_w = float(np.interp(v_ego, TURN_SPEED_TAPER, [1.0, 0.0]))
|
||||
if turn_speed_w <= 0.0 and self.shift_dist < 1e-3 and self.apex_blend < 1e-3:
|
||||
self.turn_psi = 0.0
|
||||
self.turn_sign = 0.0
|
||||
self.plan_peak_curv = 0.0
|
||||
self.path_curv_1s = 0.0
|
||||
return model_curv
|
||||
|
||||
plan_peak, path_k_1s = compute_plan_metrics(model_output)
|
||||
self.plan_peak_curv = plan_peak
|
||||
self.path_curv_1s = path_k_1s
|
||||
|
||||
tightness = max(abs(model_curv), abs(plan_peak))
|
||||
curv_w = float(np.interp(tightness, CURV_GATE, [0.0, 1.0]))
|
||||
|
||||
ref_k = model_curv if abs(model_curv) > 0.015 else plan_peak
|
||||
if abs(ref_k) > 0.035 and turn_speed_w > 0.05:
|
||||
new_sign = 1.0 if ref_k > 0.0 else -1.0
|
||||
if new_sign != self.turn_sign:
|
||||
self.turn_sign = new_sign
|
||||
self.turn_psi = 0.0
|
||||
self.turn_psi = max(0.0, self.turn_psi + actual_curv * self.turn_sign * ds)
|
||||
else:
|
||||
self.turn_psi = max(0.0, self.turn_psi - 0.5 * self.dt)
|
||||
if self.turn_psi < 0.02:
|
||||
self.turn_sign = 0.0
|
||||
|
||||
psi_deg = math.degrees(self.turn_psi)
|
||||
|
||||
# Stage 1: Entry rear-axle spatial shift (time-capped so crawl-speed starts stay responsive)
|
||||
shift_speed_w = float(np.interp(v_ego, SHIFT_SPEED_BP, SHIFT_SPEED_V))
|
||||
entry_phase_w = float(np.interp(psi_deg, ENTRY_PHASE_RELEASE_DEG, [1.0, 0.0]))
|
||||
is_curb_side = (ref_k < 0.0) if is_rhd else (ref_k > 0.0)
|
||||
side_w = 1.0 if is_curb_side else OPPOSITE_SIDE_WEIGHT
|
||||
|
||||
raw_shift = SHIFT_WHEELBASE_RATIO * self.wheelbase * shift_speed_w * curv_w * entry_phase_w * side_w
|
||||
target_shift = min(raw_shift, v_ego * MAX_SHIFT_TIME)
|
||||
tau = SHIFT_RELEASE_TAU if target_shift < self.shift_dist else SHIFT_BUILD_TAU
|
||||
self.shift_dist += (target_shift - self.shift_dist) * (self.dt / (tau + self.dt))
|
||||
|
||||
out_k = model_curv
|
||||
if self.shift_dist >= 0.05 and len(self.buf_s) >= 2:
|
||||
k_delayed = float(np.interp(self.s - self.shift_dist, self.buf_s, self.buf_k))
|
||||
# Same sign as model_curv (or 0), never exceeds |model_curv|, zero lag on unwind.
|
||||
if model_curv >= 0.0:
|
||||
out_k = float(np.clip(k_delayed, 0.0, model_curv))
|
||||
else:
|
||||
out_k = float(np.clip(k_delayed, model_curv, 0.0))
|
||||
|
||||
# Stage 1b: Road-edge curb guard during turn entry (prolongs straight travel while
|
||||
# a confident, unbroken curb is still beside the car before the turn lane/intersection opens).
|
||||
curb_phase_w = float(np.interp(psi_deg, CURB_GUARD_PHASE_DEG, [1.0, 0.0]))
|
||||
if turn_speed_w > 0.05 and curb_phase_w > 0.01 and abs(out_k) > CURB_FOLLOW_MARGIN_K:
|
||||
curb_w, k_cap = compute_curb_guard(model_output, 1.0 if out_k > 0.0 else -1.0)
|
||||
guard_w = curb_w * turn_speed_w * curb_phase_w
|
||||
if guard_w > 0.01 and abs(out_k) > k_cap:
|
||||
capped = math.copysign(k_cap, out_k)
|
||||
out_k = (1.0 - guard_w) * out_k + guard_w * capped
|
||||
|
||||
# Stage 2: Apex & Exit 1s-path blend (prevents wide apexes and exit hang-ups)
|
||||
apex_phase_w = float(np.interp(psi_deg, APEX_BLEND_PHASE_DEG, [0.0, 1.0]))
|
||||
target_blend = APEX_PATH_BLEND_MAX * turn_speed_w * curv_w * apex_phase_w
|
||||
self.apex_blend += (target_blend - self.apex_blend) * (self.dt / (APEX_BLEND_TAU + self.dt))
|
||||
|
||||
if self.apex_blend > 0.01 and out_k * path_k_1s > 0.0:
|
||||
# Through the apex, boost when 1s path asks for tighter curvature than action (prevents wide turns).
|
||||
# Only pull curvature down toward 1s path late in the turn (psi > 60 deg) to prevent exit hang-ups.
|
||||
if abs(path_k_1s) > abs(out_k) or psi_deg > 60.0:
|
||||
max_boosted = abs(model_curv) * APEX_PATH_BOOST_CAP
|
||||
target_k = float(np.clip(path_k_1s, -max_boosted, max_boosted))
|
||||
out_k = (1.0 - self.apex_blend) * out_k + self.apex_blend * target_k
|
||||
|
||||
return out_k
|
||||
@@ -6,7 +6,6 @@ See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import json
|
||||
import os
|
||||
import re
|
||||
import threading
|
||||
@@ -29,11 +28,8 @@ DOWNLOAD_TIMEOUT = (30, 30)
|
||||
# HuggingFace throttles each connection to ~1-2 MB/s but not the connection count; 12 saturates the device link
|
||||
MAX_CONCURRENT_PIECES = 12
|
||||
PIECE_SIZE = 8 * 1024 * 1024
|
||||
PIECE_ATTEMPTS = 3 # per piece, before the transfer as a whole is retried
|
||||
PIECE_ATTEMPTS = 3
|
||||
PIECE_BACKOFF = 1.0 # seconds, growing linearly: waits 1 s then 2 s between a piece's three attempts
|
||||
DOWNLOAD_ATTEMPTS = 4 # whole-transfer attempts, each resuming from the sidecar
|
||||
RETRY_BACKOFF = 2.0 # seconds, doubling: waits 2, 4 then 8 s between the four transfer attempts
|
||||
RETRY_POLL = 0.25 # how often a backoff checks for a cancel
|
||||
TRANSIENT_HTTP_STATUS = frozenset({408, 429, 500, 502, 503, 504})
|
||||
REPORT_INTERVAL = 0.5 # seconds between progress publications
|
||||
# byte ranges count encoded bytes but iter_content yields decoded ones, so ask for an unencoded body
|
||||
@@ -53,69 +49,26 @@ def _is_transient(e: BaseException) -> bool:
|
||||
|
||||
|
||||
class _Progress:
|
||||
def __init__(self, done: set[int], done_bytes: int):
|
||||
self._bytes = done_bytes
|
||||
self._done = set(done)
|
||||
def __init__(self):
|
||||
self._bytes = 0
|
||||
self._lock = threading.Lock()
|
||||
|
||||
def add_bytes(self, n: int) -> None:
|
||||
with self._lock:
|
||||
self._bytes += n
|
||||
|
||||
def mark_done(self, index: int) -> None:
|
||||
def snapshot(self) -> int:
|
||||
with self._lock:
|
||||
self._done.add(index)
|
||||
|
||||
def snapshot(self) -> tuple[int, set[int]]:
|
||||
with self._lock:
|
||||
return self._bytes, set(self._done)
|
||||
return self._bytes
|
||||
|
||||
|
||||
def _piece_ranges(total: int, piece_size: int) -> list[tuple[int, int]]:
|
||||
return [(start, min(start + piece_size, total)) for start in range(0, total, piece_size)]
|
||||
|
||||
|
||||
def _sidecar_path(path: str) -> str:
|
||||
return f"{path}.download"
|
||||
|
||||
|
||||
def _download_in_progress(path: str) -> bool:
|
||||
return os.path.isfile(_sidecar_path(path))
|
||||
|
||||
|
||||
def _save_resume_state(path: str, layout: dict, done: set[int]) -> None:
|
||||
tmp = _sidecar_path(path) + ".tmp"
|
||||
with open(tmp, "w") as f:
|
||||
json.dump({**layout, "done": sorted(done)}, f)
|
||||
f.flush()
|
||||
os.fsync(f.fileno())
|
||||
os.replace(tmp, _sidecar_path(path)) # a power loss leaves the old sidecar or the new one, never a torn file
|
||||
dir_fd = os.open(os.path.dirname(path) or ".", os.O_RDONLY)
|
||||
try:
|
||||
os.fsync(dir_fd)
|
||||
finally:
|
||||
os.close(dir_fd)
|
||||
|
||||
|
||||
def _remove_download_state(path: str) -> None:
|
||||
for stale in (_sidecar_path(path), _sidecar_path(path) + ".tmp"):
|
||||
if os.path.isfile(stale):
|
||||
os.remove(stale)
|
||||
|
||||
|
||||
def _prepare_target(path: str, layout: dict) -> set[int]:
|
||||
total = layout["total"]
|
||||
try:
|
||||
with open(_sidecar_path(path)) as f:
|
||||
state = json.load(f)
|
||||
if isinstance(state, dict) and all(state.get(k) == v for k, v in layout.items()) and os.path.getsize(path) == total:
|
||||
return {i for i in state.get("done", []) if isinstance(i, int)}
|
||||
except (OSError, ValueError, TypeError): # missing, unreadable or malformed sidecar: start over
|
||||
pass
|
||||
_save_resume_state(path, layout, set())
|
||||
def _prepare_target(path: str, total: int) -> None:
|
||||
with open(path, "wb") as f:
|
||||
f.truncate(total)
|
||||
return set()
|
||||
|
||||
|
||||
def _content_range_total(response: requests.Response) -> int | None:
|
||||
@@ -157,9 +110,6 @@ def _fetch_piece(url: str, path: str, index: int, start: int, end: int | None, p
|
||||
f.truncate() # unknown length: the body defines the file size
|
||||
if end is not None and written != end - start:
|
||||
raise requests.exceptions.ConnectionError(f"short read: {written} of {end - start} bytes")
|
||||
f.flush()
|
||||
os.fsync(f.fileno()) # before it is recorded, so a power loss never leaves the sidecar naming unsynced bytes
|
||||
progress.mark_done(index)
|
||||
return
|
||||
except requests.RequestException as e:
|
||||
progress.add_bytes(-written) # the piece restarts from scratch
|
||||
@@ -170,11 +120,6 @@ def _fetch_piece(url: str, path: str, index: int, start: int, end: int | None, p
|
||||
raise DownloadCancelled("Download cancelled") from None
|
||||
|
||||
|
||||
def _remove_file(path: str) -> None:
|
||||
if os.path.isfile(path):
|
||||
os.remove(path)
|
||||
|
||||
|
||||
class ModelManagerSP:
|
||||
"""Manages model downloads and status reporting"""
|
||||
|
||||
@@ -241,12 +186,11 @@ class ModelManagerSP:
|
||||
else:
|
||||
eta = self._calculate_eta(artifact.fileName, progress)
|
||||
self._set_progress(artifact, custom.ModelManagerSP.DownloadStatus.downloading, progress, eta, speed)
|
||||
artifact.downloadProgress.resuming = False # shown only for the first report, when the bar jumps to the resumed point
|
||||
|
||||
async def _report_until_done(self, tasks: list[asyncio.Future], artifact, progress: _Progress, path: str, layout: dict,
|
||||
started: tuple[int, set[int], float]) -> None:
|
||||
async def _report_until_done(self, tasks: list[asyncio.Future], artifact, progress: _Progress, total: int,
|
||||
started: tuple[int, float]) -> None:
|
||||
pending = set(tasks)
|
||||
last_bytes, saved, last_time = started
|
||||
last_bytes, last_time = started
|
||||
speed = 0.0
|
||||
while pending:
|
||||
done, pending = await asyncio.wait(pending, timeout=REPORT_INTERVAL)
|
||||
@@ -255,17 +199,14 @@ class ModelManagerSP:
|
||||
if self._download_interrupted():
|
||||
raise DownloadCancelled("Download cancelled")
|
||||
now = time.monotonic()
|
||||
done_bytes, done_pieces = progress.snapshot()
|
||||
if done_pieces != saved:
|
||||
_save_resume_state(path, layout, done_pieces)
|
||||
saved = done_pieces
|
||||
done_bytes = progress.snapshot()
|
||||
if (dt := now - last_time) > 0:
|
||||
instant = max(0.0, (done_bytes - last_bytes) / dt) # bytes/s this tick; clamped since retried pieces can lower the byte count
|
||||
speed = instant if speed == 0 else SPEED_SMOOTHING * speed + (1 - SPEED_SMOOTHING) * instant
|
||||
if speed < SPEED_FLOOR: # a stall decays toward 0 without reaching it, and a near-0 speed overflows the eta
|
||||
speed = 0.0
|
||||
last_time, last_bytes = now, done_bytes
|
||||
self._publish_progress(artifact, done_bytes, layout["total"], speed)
|
||||
self._publish_progress(artifact, done_bytes, total, speed)
|
||||
|
||||
async def _download_file(self, url: str, path: str, artifact) -> None:
|
||||
self._download_start_times[artifact.fileName] = time.monotonic()
|
||||
@@ -273,18 +214,16 @@ class ModelManagerSP:
|
||||
|
||||
total, ranged = await loop.run_in_executor(None, _probe_size, url)
|
||||
pieces: list[tuple[int, int | None]] = list(_piece_ranges(total, PIECE_SIZE)) if ranged else [(0, None)]
|
||||
layout = {"total": total, "piece_size": PIECE_SIZE, "ranged": ranged, "sha256": artifact.downloadUri.sha256}
|
||||
done = await loop.run_in_executor(None, _prepare_target, path, layout)
|
||||
resumed_bytes = sum(end - start for i, (start, end) in enumerate(pieces) if i in done and end is not None)
|
||||
progress = _Progress(done, resumed_bytes) # the bar and eta start from the resumed pieces
|
||||
await loop.run_in_executor(None, _prepare_target, path, total)
|
||||
progress = _Progress()
|
||||
cancel = threading.Event()
|
||||
started = *progress.snapshot(), time.monotonic() # before any worker can land bytes
|
||||
started = progress.snapshot(), time.monotonic() # before any worker can land bytes
|
||||
|
||||
with ThreadPoolExecutor(max_workers=MAX_CONCURRENT_PIECES) as pool:
|
||||
tasks = [loop.run_in_executor(pool, _fetch_piece, url, path, i, start, end, progress, cancel, self._block_size)
|
||||
for i, (start, end) in enumerate(pieces) if i not in done]
|
||||
for i, (start, end) in enumerate(pieces)]
|
||||
try:
|
||||
await self._report_until_done(tasks, artifact, progress, path, layout, started)
|
||||
await self._report_until_done(tasks, artifact, progress, total, started)
|
||||
except BaseException as e:
|
||||
cancel.set()
|
||||
if isinstance(e, DownloadCancelled):
|
||||
@@ -293,32 +232,9 @@ class ModelManagerSP:
|
||||
for task in tasks:
|
||||
task.cancel() # drops queued pieces; running ones see the event at their next block
|
||||
await asyncio.gather(*tasks, return_exceptions=True)
|
||||
await loop.run_in_executor(None, pool.shutdown) # let running pieces stop so none lands after the save
|
||||
_save_resume_state(path, layout, progress.snapshot()[1])
|
||||
raise
|
||||
|
||||
_remove_download_state(path)
|
||||
artifact.downloadProgress.resuming = False
|
||||
del self._download_start_times[artifact.fileName]
|
||||
|
||||
async def _download_with_retries(self, url: str, path: str, artifact) -> None:
|
||||
for attempt in range(DOWNLOAD_ATTEMPTS):
|
||||
try:
|
||||
await self._download_file(url, path, artifact)
|
||||
return
|
||||
except Exception as e:
|
||||
if not _is_transient(e) or attempt == DOWNLOAD_ATTEMPTS - 1:
|
||||
raise
|
||||
delay = RETRY_BACKOFF * 2 ** attempt
|
||||
cloudlog.warning(f"Retrying {artifact.fileName} in {delay:g}s after {type(e).__name__}: {e}")
|
||||
progress = artifact.downloadProgress
|
||||
self._set_progress(artifact, progress.status, progress.progress, progress.eta, 0.0) # stalled: no speed
|
||||
deadline = time.monotonic() + delay
|
||||
while (remaining := deadline - time.monotonic()) > 0:
|
||||
if self._download_interrupted():
|
||||
raise DownloadCancelled("Download cancelled") from e
|
||||
await asyncio.sleep(min(RETRY_POLL, remaining))
|
||||
|
||||
async def _process_artifact(self, artifact, destination_path: str) -> None:
|
||||
if not artifact.downloadUri.uri:
|
||||
return None
|
||||
@@ -330,39 +246,37 @@ class ModelManagerSP:
|
||||
filename = artifact.fileName
|
||||
full_path = os.path.join(destination_path, filename)
|
||||
status = custom.ModelManagerSP.DownloadStatus
|
||||
artifact.downloadProgress.resuming = False
|
||||
artifact.downloadProgress.canceling = False
|
||||
|
||||
try:
|
||||
if _download_in_progress(full_path):
|
||||
cloudlog.info(f"Resuming interrupted download of {filename}")
|
||||
artifact.downloadProgress.resuming = True
|
||||
else:
|
||||
self._set_progress(artifact, status.verifying, 0)
|
||||
if await verify_file(full_path, expected_hash):
|
||||
self._set_progress(artifact, status.cached, 100)
|
||||
return
|
||||
self._set_progress(artifact, status.verifying, 0)
|
||||
if await verify_file(full_path, expected_hash):
|
||||
self._set_progress(artifact, status.cached, 100)
|
||||
return
|
||||
|
||||
self._set_progress(artifact, status.downloading, 0)
|
||||
await self._download_with_retries(url, full_path, artifact)
|
||||
await self._download_file(url, full_path, artifact)
|
||||
self._set_progress(artifact, status.verifying, 99)
|
||||
if not await verify_file(full_path, expected_hash):
|
||||
_remove_file(full_path)
|
||||
_remove_download_state(full_path)
|
||||
raise ValueError(f"Hash validation failed for {filename}")
|
||||
|
||||
self._set_progress(artifact, status.downloaded, 100)
|
||||
|
||||
except DownloadCancelled: # not a failure: the request handler clears the row
|
||||
except DownloadCancelled:
|
||||
self._download_start_times.pop(artifact.fileName, None)
|
||||
artifact.downloadProgress.status = status.failed
|
||||
artifact.downloadProgress.eta = 0
|
||||
artifact.downloadProgress.speed = 0
|
||||
self._sync_artifact_progress(artifact)
|
||||
if self.selected_bundle:
|
||||
self.selected_bundle.status = status.failed
|
||||
self._report_status()
|
||||
raise
|
||||
|
||||
except Exception as e:
|
||||
cloudlog.error(f"Error downloading {filename}: {str(e)}")
|
||||
if os.path.isfile(full_path): # noqa: ASYNC240
|
||||
os.remove(full_path)
|
||||
artifact.downloadProgress.status = status.failed
|
||||
artifact.downloadProgress.eta = 0
|
||||
artifact.downloadProgress.speed = 0
|
||||
@@ -419,8 +333,6 @@ class ModelManagerSP:
|
||||
self.params.put(ACTIVE_BUNDLE_KEYS[source], model_bundle.to_dict(), block=True)
|
||||
self.active_bundle = get_active_bundle(self.params, chestnut=self.chestnut_present)
|
||||
|
||||
except DownloadCancelled:
|
||||
raise # not a failure
|
||||
except Exception:
|
||||
if self.selected_bundle is not None:
|
||||
self.selected_bundle.status = custom.ModelManagerSP.DownloadStatus.failed
|
||||
@@ -448,14 +360,11 @@ class ModelManagerSP:
|
||||
self._download_ref = ref_to_download
|
||||
try:
|
||||
self.download(model_to_download, Paths.model_root(), source)
|
||||
except DownloadCancelled:
|
||||
self.selected_bundle = None
|
||||
except Exception as e:
|
||||
cloudlog.exception(e) # a failure stays on the row until the next request
|
||||
else:
|
||||
self.selected_bundle = None
|
||||
cloudlog.exception(e)
|
||||
finally:
|
||||
self._release_download_ref()
|
||||
self.selected_bundle = None
|
||||
|
||||
def main_thread(self) -> None:
|
||||
"""Main thread for model management"""
|
||||
|
||||
@@ -11,13 +11,11 @@ import contextlib
|
||||
import dataclasses
|
||||
import hashlib
|
||||
import http.server
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import random
|
||||
import re
|
||||
import requests
|
||||
import stat
|
||||
import tempfile
|
||||
import threading
|
||||
import time
|
||||
@@ -57,8 +55,6 @@ class ServerState:
|
||||
fail_ranges: dict[tuple[int, int], int] = dataclasses.field(default_factory=dict) # range -> HTTP status, every time
|
||||
fail_times: dict[tuple[int, int], int] = dataclasses.field(default_factory=dict) # range -> how many more requests 503
|
||||
short_once: set[tuple[int, int]] = dataclasses.field(default_factory=set) # ranges whose first answer is cut short
|
||||
fail_after_others: bool = False # hold each failure until every other piece has been sent
|
||||
sent: set[tuple[int, int]] = dataclasses.field(default_factory=set) # ranges sent in full
|
||||
stall_ranges: set[tuple[int, int]] = dataclasses.field(default_factory=set)
|
||||
stall_event: threading.Event | None = None
|
||||
stall_released_by_event: bool | None = None
|
||||
@@ -92,10 +88,6 @@ class DownloadHandler(http.server.BaseHTTPRequestHandler):
|
||||
state.fail_times[rng] -= 1
|
||||
status = 503
|
||||
if status is not None:
|
||||
others = {piece_range(i) for i in range(NUM_PIECES)} - {rng}
|
||||
deadline = time.monotonic() + 5
|
||||
while state.fail_after_others and not others <= state.sent and time.monotonic() < deadline:
|
||||
time.sleep(0.01)
|
||||
self.send_response(status)
|
||||
self.end_headers()
|
||||
return
|
||||
@@ -123,8 +115,6 @@ class DownloadHandler(http.server.BaseHTTPRequestHandler):
|
||||
self.wfile.write(payload[100:])
|
||||
else:
|
||||
self.wfile.write(payload)
|
||||
if rng is not None:
|
||||
state.sent.add(rng)
|
||||
|
||||
|
||||
@contextlib.contextmanager
|
||||
@@ -143,10 +133,7 @@ def threaded_server(handler):
|
||||
class InlineExecutor(concurrent.futures.Executor):
|
||||
def submit(self, fn, /, *args, **kwargs):
|
||||
future: concurrent.futures.Future = concurrent.futures.Future()
|
||||
try:
|
||||
future.set_result(fn(*args, **kwargs)) # the piece lands before the reporter starts
|
||||
except BaseException as e:
|
||||
future.set_exception(e)
|
||||
future.set_result(fn(*args, **kwargs)) # the piece lands before the reporter starts
|
||||
return future
|
||||
|
||||
|
||||
@@ -160,12 +147,11 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
|
||||
self.addCleanup(self._tmp.cleanup)
|
||||
self.dest = self._tmp.name
|
||||
|
||||
for name, value in (('PIECE_SIZE', PIECE_SIZE), ('REPORT_INTERVAL', 0.05), ('PIECE_BACKOFF', 0.01), ('SPEED_FLOOR', 100), ('RETRY_BACKOFF', 0.01)):
|
||||
for name, value in (('PIECE_SIZE', PIECE_SIZE), ('REPORT_INTERVAL', 0.05), ('PIECE_BACKOFF', 0.01), ('SPEED_FLOOR', 100)):
|
||||
self.enterContext(mock.patch.object(manager_module, name, value))
|
||||
|
||||
self.reported: list[float] = []
|
||||
self.reported_statuses: list[int] = []
|
||||
self.reported_resuming: list[bool] = []
|
||||
|
||||
self.manager = ModelManagerSP.__new__(ModelManagerSP)
|
||||
self.params = self.manager.params = mock.MagicMock()
|
||||
@@ -186,7 +172,6 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
|
||||
self.reported.append(float(artifact.downloadProgress.progress))
|
||||
status = artifact.downloadProgress.status
|
||||
self.reported_statuses.append(getattr(status, 'raw', status)) # .raw: _DynamicEnum equals its int but does not hash like it
|
||||
self.reported_resuming.append(bool(artifact.downloadProgress.resuming))
|
||||
|
||||
def make_artifact(self):
|
||||
bundle = custom.ModelManagerSP.ModelBundle.new_message()
|
||||
@@ -203,24 +188,6 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
|
||||
def path(self) -> str:
|
||||
return os.path.join(self.dest, FILE_NAME)
|
||||
|
||||
@property
|
||||
def sidecar(self) -> str:
|
||||
return manager_module._sidecar_path(self.path)
|
||||
|
||||
def resume_state(self) -> dict:
|
||||
with open(self.sidecar) as f:
|
||||
return json.load(f)
|
||||
|
||||
def write_resume_state(self, done: list[int], total: int = len(WHOLE_BODY), file_size: int | None = None,
|
||||
file_sha256: str = sha256(WHOLE_BODY)) -> None:
|
||||
with open(self.path, 'wb') as f:
|
||||
f.truncate(len(WHOLE_BODY) if file_size is None else file_size)
|
||||
for i in done:
|
||||
f.seek(i * PIECE_SIZE)
|
||||
f.write(WHOLE_BODY[i * PIECE_SIZE:(i + 1) * PIECE_SIZE])
|
||||
with open(self.sidecar, 'w') as f:
|
||||
json.dump({"total": total, "piece_size": PIECE_SIZE, "ranged": True, "sha256": file_sha256, "done": done}, f)
|
||||
|
||||
def piece_requests(self) -> list[tuple[int, int]]:
|
||||
return [r for r in self.server.request_ranges if r is not None and r != (0, 0)]
|
||||
|
||||
@@ -242,17 +209,6 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
|
||||
def download_bundle(self, source: str):
|
||||
asyncio.run(self.manager._download_bundle(self._bundle, self.dest, source))
|
||||
|
||||
def request_download(self) -> dict:
|
||||
self.make_artifact()
|
||||
self._bundle.ref = "ref"
|
||||
self._bundle.minimumSelectorVersion = helpers.REQUIRED_JSON_VERSION
|
||||
self.manager.source_models = {"qcom": [self._bundle], "chestnut": []}
|
||||
self.manager.params, store = self._make_params_with_store()
|
||||
store["ModelManager_DownloadRef"] = "ref"
|
||||
with mock.patch.object(manager_module.Paths, 'model_root', return_value=self.dest):
|
||||
self.manager._process_download_requests()
|
||||
return store
|
||||
|
||||
def read_path(self) -> bytes:
|
||||
with open(self.path, 'rb') as f:
|
||||
return f.read()
|
||||
@@ -269,7 +225,6 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
|
||||
|
||||
params.get.side_effect = get
|
||||
params.put.side_effect = put
|
||||
params.remove.side_effect = lambda key: put(key, None)
|
||||
return params, store
|
||||
|
||||
|
||||
@@ -283,7 +238,6 @@ class TestManagerDownload(ManagerDownloadTestBase):
|
||||
assert max(self.reported) <= 99.0, "progress holds below 100 until the hash check"
|
||||
assert artifact.fileName not in self.manager._download_start_times
|
||||
assert artifact.downloadProgress.speed > 0, "a running download shows its speed"
|
||||
assert not os.path.exists(self.sidecar), "the resume state goes once every piece has landed"
|
||||
|
||||
def test_speed_counts_bytes_landed_before_the_first_report(self):
|
||||
self.enterContext(mock.patch.object(manager_module, 'ThreadPoolExecutor', lambda max_workers: InlineExecutor()))
|
||||
@@ -342,42 +296,9 @@ class TestManagerDownload(ManagerDownloadTestBase):
|
||||
|
||||
def test_permanent_failure_is_not_retried(self):
|
||||
self.server.fail_ranges = {piece_range(1): 404}
|
||||
self.enterContext(mock.patch.object(manager_module, 'ThreadPoolExecutor', lambda max_workers: InlineExecutor()))
|
||||
with self.assertRaises(requests.exceptions.HTTPError):
|
||||
self.process_artifact()
|
||||
self.download_file()
|
||||
assert self.piece_requests().count(piece_range(1)) == 1
|
||||
assert os.path.isfile(self.path)
|
||||
assert self.resume_state()["done"] == [i for i in range(NUM_PIECES) if i != 1]
|
||||
|
||||
def test_failed_transfer_resumes_on_retry(self):
|
||||
self.server.fail_times = {piece_range(1): manager_module.PIECE_ATTEMPTS} # the first transfer gives up on piece 1
|
||||
self.server.fail_after_others = True # by then every other piece is saved
|
||||
self.process_artifact()
|
||||
assert self.read_path() == WHOLE_BODY
|
||||
assert self.piece_requests().count(piece_range(1)) == manager_module.PIECE_ATTEMPTS + 1, "the retry fetched piece 1"
|
||||
others = [i for i in range(NUM_PIECES) if i != 1]
|
||||
assert all(self.piece_requests().count(piece_range(i)) == 1 for i in others), "the retry skipped every saved piece"
|
||||
|
||||
def test_lasting_transient_failure_gives_up(self):
|
||||
self.server.fail_ranges = {piece_range(1): 503}
|
||||
with self.assertRaises(requests.exceptions.HTTPError):
|
||||
self.process_artifact()
|
||||
assert self.piece_requests().count(piece_range(1)) == manager_module.PIECE_ATTEMPTS * manager_module.DOWNLOAD_ATTEMPTS
|
||||
|
||||
def test_cancel_ends_retry_backoff(self):
|
||||
self.enterContext(mock.patch.object(manager_module, 'RETRY_BACKOFF', 30.0))
|
||||
self.manager._download_ref = b"ref"
|
||||
|
||||
def fail_then_cancel(*args):
|
||||
self.params.get.return_value = None # the user cancels while the manager waits to retry
|
||||
raise requests.exceptions.ConnectionError("dropped")
|
||||
transfer = self.enterContext(mock.patch.object(self.manager, '_download_file', side_effect=fail_then_cancel))
|
||||
started = time.monotonic()
|
||||
with self.assertRaises(DownloadCancelled):
|
||||
self.process_artifact()
|
||||
assert time.monotonic() - started < 5, "the wait must end at the cancel, not after 30 s"
|
||||
assert transfer.call_count == 1, "no second transfer after a cancel"
|
||||
assert self.artifact.downloadProgress.status != custom.ModelManagerSP.DownloadStatus.failed
|
||||
|
||||
def test_removing_download_ref_cancels(self):
|
||||
stall_event = self.stall(3)
|
||||
@@ -395,51 +316,6 @@ class TestManagerDownload(ManagerDownloadTestBase):
|
||||
assert self.server.stall_released_by_event is True, "the cancel must have caught the piece mid-transfer"
|
||||
end = piece_range(3)[1] + 1
|
||||
assert self.read_path()[end - 100:end] == bytes(100), "the cancelled piece must stop before its last bytes land"
|
||||
done = self.resume_state()["done"]
|
||||
assert done and 3 not in done, "a cancel records the finished pieces, and only those, for resume"
|
||||
|
||||
def test_resume_skips_complete_pieces(self):
|
||||
self.write_resume_state(done=[0, 4])
|
||||
self.download_file()
|
||||
assert piece_range(0) not in self.piece_requests() and piece_range(4) not in self.piece_requests()
|
||||
assert self.read_path() == WHOLE_BODY
|
||||
assert min(self.reported) >= 2 * PIECE_SIZE / len(WHOLE_BODY) * 100 - 1, "progress must not restart below the resumed share"
|
||||
|
||||
def test_unusable_resume_state_starts_over(self):
|
||||
def malformed(content: bytes):
|
||||
self.write_resume_state(done=[0])
|
||||
with open(self.sidecar, 'wb') as f:
|
||||
f.write(content)
|
||||
|
||||
layout = {"total": len(WHOLE_BODY), "piece_size": PIECE_SIZE, "ranged": True, "sha256": sha256(WHOLE_BODY)}
|
||||
cases = {
|
||||
"other size": lambda: self.write_resume_state(done=[0], total=len(WHOLE_BODY) + 1),
|
||||
"other bytes": lambda: self.write_resume_state(done=[0], file_sha256="0" * 64),
|
||||
"truncated file": lambda: self.write_resume_state(done=[0], file_size=PIECE_SIZE),
|
||||
"not an object": lambda: malformed(b'[]'),
|
||||
"cut-off json": lambda: malformed(b'{"total": '),
|
||||
"done not a list": lambda: malformed(json.dumps({**layout, "done": 3}).encode()),
|
||||
"done not ints": lambda: malformed(json.dumps({**layout, "done": ["0", None]}).encode()),
|
||||
}
|
||||
for name, write_state in cases.items():
|
||||
with self.subTest(name):
|
||||
self.server.request_ranges = []
|
||||
write_state()
|
||||
self.download_file()
|
||||
assert sorted(self.piece_requests()) == [piece_range(i) for i in range(NUM_PIECES)], "every piece is fetched again"
|
||||
assert self.read_path() == WHOLE_BODY
|
||||
|
||||
def test_pieces_and_resume_state_are_synced_to_disk(self):
|
||||
real_fsync, synced = os.fsync, []
|
||||
|
||||
def record_sync(fd):
|
||||
st = os.fstat(fd)
|
||||
synced.append("dir" if stat.S_ISDIR(st.st_mode) else "model" if os.path.exists(self.path) and st.st_ino == os.stat(self.path).st_ino else "sidecar")
|
||||
real_fsync(fd)
|
||||
with mock.patch.object(manager_module.os, 'fsync', side_effect=record_sync):
|
||||
self.download_file()
|
||||
assert synced.count("model") == NUM_PIECES, f"expected one fsync per piece, got {synced.count('model')}"
|
||||
assert "sidecar" in synced and "dir" in synced, "sidecar file and directory must be fsync'd"
|
||||
|
||||
def test_replaced_download_ref_queues_instead_of_canceling(self):
|
||||
self.params.get.return_value = b"other-ref"
|
||||
@@ -469,26 +345,9 @@ class TestProcessArtifact(ManagerDownloadTestBase):
|
||||
self.server.corrupt = True
|
||||
with self.assertRaises(ValueError):
|
||||
self.process_artifact()
|
||||
assert not os.path.isfile(self.path) and not os.path.exists(self.sidecar), "a corrupt file must not be resumed from"
|
||||
assert not os.path.isfile(self.path)
|
||||
assert self.artifact.downloadProgress.status == custom.ModelManagerSP.DownloadStatus.failed
|
||||
|
||||
def test_interrupted_download_resumes_without_reverifying(self):
|
||||
self.write_resume_state(done=[0, 1, 2])
|
||||
artifact = self.process_artifact()
|
||||
ds = custom.ModelManagerSP.DownloadStatus
|
||||
assert self.reported_statuses[0] == ds.downloading, "an in-progress download is not verified first"
|
||||
assert piece_range(0) not in self.piece_requests()
|
||||
assert artifact.downloadProgress.status == ds.downloaded
|
||||
assert any(self.reported_resuming), "a resumed download is flagged resuming until its first progress report"
|
||||
assert not artifact.downloadProgress.resuming
|
||||
|
||||
def test_resume_with_every_piece_done_clears_resuming(self):
|
||||
self.write_resume_state(done=list(range(NUM_PIECES)))
|
||||
artifact = self.process_artifact()
|
||||
assert self.piece_requests() == []
|
||||
assert artifact.downloadProgress.status == custom.ModelManagerSP.DownloadStatus.downloaded
|
||||
assert not artifact.downloadProgress.resuming
|
||||
|
||||
def test_probe_failure_moves_past_verifying(self):
|
||||
self.server.fail_ranges = {(0, 0): 404} # the size probe
|
||||
with self.assertRaises(requests.exceptions.HTTPError):
|
||||
@@ -515,23 +374,6 @@ class TestProcessArtifact(ManagerDownloadTestBase):
|
||||
self.download_bundle("qcom")
|
||||
assert ACTIVE_BUNDLE_KEYS["qcom"] not in store
|
||||
assert os.path.isfile(self.path), "cancel must not delete a cached model"
|
||||
assert self.manager.selected_bundle.status != custom.ModelManagerSP.DownloadStatus.failed, "a cancel is not a failure"
|
||||
|
||||
def test_failed_download_stays_on_the_row(self):
|
||||
self.server.fail_ranges = {piece_range(1): 404}
|
||||
store = self.request_download()
|
||||
selected = self.manager.selected_bundle
|
||||
assert selected is not None and selected.status == custom.ModelManagerSP.DownloadStatus.failed, "a failure stays on the row"
|
||||
assert store["ModelManager_DownloadRef"] is None, "the failed request is released"
|
||||
|
||||
def test_finished_download_clears_the_row(self):
|
||||
self.request_download()
|
||||
assert self.manager.selected_bundle is None
|
||||
|
||||
def test_cancelled_download_clears_the_row(self):
|
||||
self.pm.send.side_effect = lambda *args: self.manager.params.remove("ModelManager_DownloadRef") # cancel at the first report
|
||||
self.request_download()
|
||||
assert self.manager.selected_bundle is None
|
||||
|
||||
def test_download_ref_is_released_only_if_unchanged(self):
|
||||
for current, removed in ((b"new-ref", False), (b"ref", True)):
|
||||
|
||||
@@ -126,6 +126,7 @@ def initialize_params(params) -> list[dict[str, Any]]:
|
||||
keys.extend([
|
||||
"ToyotaEnforceStockLongitudinal",
|
||||
"ToyotaStopAndGoHack",
|
||||
"ToyotaEnhancedBsm",
|
||||
"ToyotaTSS2Smooth",
|
||||
])
|
||||
|
||||
|
||||
@@ -13,6 +13,7 @@ from opendbc.car.subaru.values import CAR as SUBARU_CAR, SubaruFlags
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA_CAR, ToyotaFlags
|
||||
from opendbc.sunnypilot.car.interfaces import has_bsm
|
||||
from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
|
||||
from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.hardware import HARDWARE
|
||||
@@ -44,6 +45,7 @@ CAPABILITY_FIELDS = (
|
||||
"device_type",
|
||||
"subaru_has_sng",
|
||||
"hyundai_alpha_long_available",
|
||||
"toyota_enhanced_bsm_available",
|
||||
"toyota_tss2",
|
||||
"custom_model_active",
|
||||
)
|
||||
@@ -68,6 +70,7 @@ CAPABILITY_LABELS: dict[str, str] = {
|
||||
"device_type": "Device type",
|
||||
"subaru_has_sng": "Subaru Stop-and-Go available",
|
||||
"hyundai_alpha_long_available": "Hyundai Alpha Longitudinal available",
|
||||
"toyota_enhanced_bsm_available": "Toyota Enhanced BSM available",
|
||||
"toyota_tss2": "Toyota TSS2 platform",
|
||||
"custom_model_active": "Non-Default model selected",
|
||||
}
|
||||
@@ -89,6 +92,7 @@ CAR_CAPABILITY_FIELDS = (
|
||||
"subaru_has_sng",
|
||||
"hyundai_alpha_long_available",
|
||||
"toyota_tss2",
|
||||
"toyota_enhanced_bsm_available",
|
||||
)
|
||||
|
||||
# Explicit defaults for non-boolean capability fields
|
||||
@@ -104,7 +108,7 @@ def _bundle_field(bundle: dict | None, key: str) -> str:
|
||||
return bundle.get(key, "") if isinstance(bundle, dict) else ""
|
||||
|
||||
|
||||
def _resolve_brand_capabilities(caps: dict, bundle_platform: str, CP) -> None:
|
||||
def _resolve_brand_capabilities(caps: dict, bundle_platform: str, CP, CP_SP=None) -> None:
|
||||
"""Set brand-specific capabilities from bundle platform or CarParams fallback.
|
||||
|
||||
Bundle (manual car selection) is a pre-fingerprint approximation.
|
||||
@@ -138,11 +142,19 @@ def _resolve_brand_capabilities(caps: dict, bundle_platform: str, CP) -> None:
|
||||
elif brand == "toyota":
|
||||
if bundle_platform:
|
||||
try:
|
||||
caps["toyota_tss2"] = bool(TOYOTA_CAR[bundle_platform].config.flags & ToyotaFlags.TSS2)
|
||||
flags = TOYOTA_CAR[bundle_platform].config.flags
|
||||
caps["toyota_tss2"] = bool(flags & ToyotaFlags.TSS2)
|
||||
caps["toyota_enhanced_bsm_available"] = bool(flags & ToyotaFlags.TSS2) and not bool(flags & ToyotaFlags.SECOC)
|
||||
except KeyError:
|
||||
cloudlog.exception(f"capabilities: unknown toyota platform {bundle_platform!r}")
|
||||
elif CP is not None:
|
||||
caps["toyota_tss2"] = bool(CP.flags & ToyotaFlags.TSS2)
|
||||
# same gate as _initialize_toyota: openpilot longitudinal, not SecOC, and either TSS2 or a
|
||||
# smartDSU. The bundle path can't see 0x2FF or longitudinal, so it stays TSS2-only.
|
||||
sdsu_bsm = CP_SP is not None and bool(CP_SP.flags & ToyotaFlagsSP.SMART_DSU)
|
||||
caps["toyota_enhanced_bsm_available"] = (bool(CP.openpilotLongitudinalControl) and
|
||||
not bool(CP.flags & ToyotaFlags.SECOC) and
|
||||
(bool(CP.flags & ToyotaFlags.TSS2) or sdsu_bsm))
|
||||
|
||||
|
||||
def generate_capabilities(params: Params | None = None) -> dict:
|
||||
@@ -217,7 +229,7 @@ def generate_capabilities(params: Params | None = None) -> dict:
|
||||
if CP is not None:
|
||||
caps["has_bsm"] = has_bsm(CP, CP_SP)
|
||||
|
||||
_resolve_brand_capabilities(caps, bundle_platform, CP)
|
||||
_resolve_brand_capabilities(caps, bundle_platform, CP, CP_SP)
|
||||
|
||||
return caps
|
||||
|
||||
|
||||
@@ -156,7 +156,7 @@ The tables below describe the **compiled** `settings_ui.json` schema — what th
|
||||
|
||||
**Enablement rules**: Grayed out (disabled) when rules fail. Frontend shows a contextual badge explaining why.
|
||||
|
||||
**Capability fields** (referenced in rules): `has_longitudinal_control`, `has_icbm`, `icbm_available`, `torque_allowed`, `brand`, `pcm_cruise`, `alpha_long_available`, `steer_control_type`, `has_bsm`, `is_release`, `is_sp_release`, `is_development`, `tesla_has_vehicle_bus`, `has_stop_and_go`, `stock_longitudinal`
|
||||
**Capability fields** (referenced in rules): `has_longitudinal_control`, `has_icbm`, `icbm_available`, `torque_allowed`, `brand`, `pcm_cruise`, `alpha_long_available`, `steer_control_type`, `has_bsm`, `is_release`, `is_sp_release`, `is_development`, `tesla_has_vehicle_bus`, `has_stop_and_go`, `stock_longitudinal`, `toyota_enhanced_bsm_available`
|
||||
|
||||
---
|
||||
|
||||
|
||||
@@ -1412,6 +1412,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",
|
||||
"widget": "toggle",
|
||||
@@ -2361,6 +2367,28 @@
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "ToyotaEnhancedBsm",
|
||||
"widget": "toggle",
|
||||
"needs_onroad_cycle": true,
|
||||
"title": "Enhanced Blind Spot Monitoring (Alpha)",
|
||||
"description": "sunnypilot will poll the blind spot monitor diagnostics directly from the radar. Use this on vehicles whose blind spot monitors do not report blind spot status over the standard CAN signals. This is an alpha feature. Use at your own risk.",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "not_engaged"
|
||||
},
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "toyota_enhanced_bsm_available",
|
||||
"equals": true
|
||||
},
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "stock_longitudinal",
|
||||
"equals": false
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "ToyotaTSS2Smooth",
|
||||
"widget": "toggle",
|
||||
|
||||
@@ -101,6 +101,21 @@ sections:
|
||||
- type: param
|
||||
key: ToyotaEnforceStockLongitudinal
|
||||
equals: false
|
||||
- key: ToyotaEnhancedBsm
|
||||
widget: toggle
|
||||
needs_onroad_cycle: true
|
||||
title: Enhanced Blind Spot Monitoring (Alpha)
|
||||
description: sunnypilot will poll the blind spot monitor diagnostics directly from the radar. Use this on vehicles whose
|
||||
blind spot monitors do not report blind spot status over the standard CAN signals. This is an alpha feature. Use at
|
||||
your own risk.
|
||||
enablement:
|
||||
- $ref: '#/macros/not_engaged'
|
||||
- type: capability
|
||||
field: toyota_enhanced_bsm_available
|
||||
equals: true
|
||||
- type: capability
|
||||
field: stock_longitudinal
|
||||
equals: false
|
||||
- key: ToyotaTSS2Smooth
|
||||
widget: toggle
|
||||
needs_onroad_cycle: true
|
||||
|
||||
@@ -39,6 +39,10 @@ sections:
|
||||
description: Show a timer on the HUD when the car is at a standstill.
|
||||
visibility:
|
||||
- $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
|
||||
widget: toggle
|
||||
title: Real-time Acceleration Bar
|
||||
|
||||
@@ -70,12 +70,18 @@ class TestOpaquePerBrandFlags(OpenpilotTestCase):
|
||||
def test_hyundai_alpha_long_available_field_present(self):
|
||||
assert "hyundai_alpha_long_available" in CAPABILITY_FIELDS
|
||||
|
||||
def test_toyota_enhanced_bsm_available_field_present(self):
|
||||
assert "toyota_enhanced_bsm_available" in CAPABILITY_FIELDS
|
||||
|
||||
def test_subaru_has_sng_default_false(self, caps):
|
||||
assert caps["subaru_has_sng"] is False
|
||||
|
||||
def test_hyundai_alpha_long_available_default_false(self, caps):
|
||||
assert caps["hyundai_alpha_long_available"] is False
|
||||
|
||||
def test_toyota_enhanced_bsm_available_default_false(self, caps):
|
||||
assert caps["toyota_enhanced_bsm_available"] is False
|
||||
|
||||
def test_toyota_tss2_field_present(self):
|
||||
assert "toyota_tss2" in CAPABILITY_FIELDS
|
||||
|
||||
|
||||
@@ -207,6 +207,7 @@ class TestNotEngagedReplacement(OpenpilotTestCase):
|
||||
"AlphaLongitudinalEnabled",
|
||||
"ToyotaEnforceStockLongitudinal",
|
||||
"ToyotaStopAndGoHack",
|
||||
"ToyotaEnhancedBsm",
|
||||
"ToyotaTSS2Smooth",
|
||||
], names=["key"])
|
||||
def test_offroad_only_replaced_with_not_engaged(self, schema, key):
|
||||
|
||||
@@ -286,6 +286,7 @@ class TestKnownVehicleSettings(OpenpilotTestCase):
|
||||
keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("toyota"))}
|
||||
assert "ToyotaEnforceStockLongitudinal" in keys
|
||||
assert "ToyotaStopAndGoHack" in keys
|
||||
assert "ToyotaEnhancedBsm" in keys
|
||||
assert "ToyotaTSS2Smooth" in keys
|
||||
|
||||
def test_tesla_has_coop_steering(self, schema):
|
||||
|
||||
Reference in New Issue
Block a user