Compare commits

..

4 Commits

Author SHA1 Message Date
royjr 8d7995a092 distless 2026-11-02 18:01:59 -05:00
royjr 5845381d1f park 2026-11-02 18:01:59 -05:00
royjr 0ffb700830 summary 2026-11-02 18:01:59 -05:00
royjr a86979d2b2 sunnylink: gate steering arc toggle on MICI (#2085)
sunnylink-hide-steering-arc-mici
2026-11-01 12:02:47 -05:00
15 changed files with 240 additions and 100 deletions
-38
View File
@@ -389,44 +389,6 @@ struct CarControlSP @0xa5cd762cd951a455 {
leadOne @2 :LeadData;
leadTwo @3 :LeadData;
intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
smartCruiseControl @5 :SmartCruiseControl;
speedLimit @6 :SpeedLimit;
laneChange @7 :LaneChange;
driverMonitoring @8 :DriverMonitoring;
leadFollowing @9 :Bool;
struct SmartCruiseControl {
visionState @0 :LongitudinalPlanSP.SmartCruiseControl.VisionState;
mapState @1 :LongitudinalPlanSP.SmartCruiseControl.MapState;
}
struct SpeedLimit {
assistState @0 :LongitudinalPlanSP.SpeedLimit.AssistState;
speedLimitFinalLast @1 :Float32;
}
struct DriverMonitoring {
alertLevel @0 :UInt8;
visionPolicy @1 :Bool;
}
struct LaneChange {
state @0 :State;
direction @1 :Direction;
enum State {
off @0;
preLaneChange @1;
laneChangeStarting @2;
laneChangeFinishing @3;
}
enum Direction {
none @0;
left @1;
right @2;
}
}
struct Param {
key @0 :Text;
+1
View File
@@ -199,6 +199,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
-4
View File
@@ -63,9 +63,5 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct
struct_dataclass.intelligentCruiseButtonManagement = structs.IntelligentCruiseButtonManagement(
**remove_deprecated(struct_dict.get('intelligentCruiseButtonManagement', {}))
)
struct_dataclass.smartCruiseControl = structs.CarControlSP.SmartCruiseControl(**struct_dict.get('smartCruiseControl', {}))
struct_dataclass.speedLimit = structs.CarControlSP.SpeedLimit(**struct_dict.get('speedLimit', {}))
struct_dataclass.laneChange = structs.CarControlSP.LaneChange(**struct_dict.get('laneChange', {}))
struct_dataclass.driverMonitoring = structs.CarControlSP.DriverMonitoring(**struct_dict.get('driverMonitoring', {}))
return struct_dataclass
@@ -57,14 +57,6 @@ COMFORT_BRAKE = 2.5
STOP_DISTANCE = 6.0
MIN_X_LEAD_FACTOR = 0.5
# aggressive follows close to stock Hyundai SCC closest setting: 2.4 m stopped (stock 2.4), 16.3 m @ 31 mph (stock 14.8), 22 m @ 40 (20.2), 27.5 m @ 48 (25.2)
# the MPC settles ~0.8 m short of its target at standstill, so a 3.2 m target stops at ~2.4 m
# the short stop gap fades out by 10 m/s and moving gaps are ~0.1 s over stock: exact stock gaps crash in sim when the lead brakes 2 m/s^2 to a stop
AGGRESSIVE_STOP_DISTANCE_BP = [0., 3., 10.]
AGGRESSIVE_STOP_DISTANCE_V = [3.2, 3.2, STOP_DISTANCE]
AGGRESSIVE_T_FOLLOW_BP = [0., 10., 14., 16., 18., 20., 21.3, 30.]
AGGRESSIVE_T_FOLLOW_V = [0.74, 0.74, 0.74, 0.81, 0.90, 0.945, 1.0, 1.1]
def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
if personality==log.LongitudinalPersonality.relaxed:
return 1.0
@@ -317,11 +309,6 @@ class LongitudinalMpc:
def update(self, radarstate, personality=log.LongitudinalPersonality.standard):
t_follow = get_T_FOLLOW(personality)
stop_distance_offset = 0.
if personality == log.LongitudinalPersonality.aggressive:
t_follow = float(np.interp(self.x0[1], AGGRESSIVE_T_FOLLOW_BP, AGGRESSIVE_T_FOLLOW_V))
# STOP_DISTANCE is compiled into the solver, so move the lead forward instead
stop_distance_offset = STOP_DISTANCE - float(np.interp(self.x0[1], AGGRESSIVE_STOP_DISTANCE_BP, AGGRESSIVE_STOP_DISTANCE_V))
lead_xv_0 = self.process_lead(radarstate.leadOne)
lead_xv_1 = self.process_lead(radarstate.leadTwo)
@@ -329,8 +316,8 @@ class LongitudinalMpc:
# To estimate a safe distance from a moving lead, we calculate how much stopping
# distance that lead needs as a minimum. We can add that to the current distance
# and then treat that as a stopped car/obstacle at this new distance.
lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) + stop_distance_offset
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) + stop_distance_offset
lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle])
self.source = MPC_SOURCES[np.argmin(x_obstacles[0])]
@@ -2,7 +2,6 @@
import numpy as np
import openpilot.cereal.messaging as messaging
from openpilot.cereal import log
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
from openpilot.common.constants import CV
from openpilot.common.filter_simple import FirstOrderFilter
@@ -21,7 +20,6 @@ from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import Lon
J_CRUISE_VALS = [1.6, 1.2, 0.8, 0.6]
J_CRUISE_BP = [0., 10.0, 25., 40.]
A_CRUISE_MIN = -0.5
AGGRESSIVE_LEAD_ONLY_DIST = 40. # m
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
@@ -116,10 +114,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
(self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop),
(output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e)]
# aggressive: with a close radar lead, follow it like stock SCC instead of the model's larger stop gap
lead = sm['radarState'].leadOne
if sm['selfdriveState'].personality == log.LongitudinalPersonality.aggressive and lead.radar and lead.dRel < AGGRESSIVE_LEAD_ONLY_DIST:
candidates.pop()
output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
self.output_should_stop = any(should_stop for _, _, should_stop in candidates)
@@ -4,9 +4,7 @@ from openpilot.common.parameterized import parameterized_class
from openpilot.cereal import log
import numpy as np
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW, \
STOP_DISTANCE, AGGRESSIVE_STOP_DISTANCE_BP, AGGRESSIVE_STOP_DISTANCE_V, AGGRESSIVE_T_FOLLOW_BP, AGGRESSIVE_T_FOLLOW_V
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
@@ -40,12 +38,7 @@ class TestFollowingDistance(OpenpilotTestCase):
def test_following_distance(self):
v_lead = float(self.speed)
simulation_steady_state = run_following_distance_simulation(v_lead, personality=self.personality)
if self.personality == log.LongitudinalPersonality.aggressive:
t_follow = float(np.interp(v_lead, AGGRESSIVE_T_FOLLOW_BP, AGGRESSIVE_T_FOLLOW_V))
stop_distance = float(np.interp(v_lead, AGGRESSIVE_STOP_DISTANCE_BP, AGGRESSIVE_STOP_DISTANCE_V))
correct_steady_state = desired_follow_distance(v_lead, v_lead, t_follow) - (STOP_DISTANCE - stop_distance)
else:
correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality))
correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality))
abs_err_margin = 0.5 if v_lead > 0.0 else 1.15
self.assertAlmostEqual(simulation_steady_state, correct_steady_state,
delta=0.2 * correct_steady_state + abs_err_margin)
+2
View File
@@ -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
@@ -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
@@ -48,6 +48,7 @@ class UIStateSP:
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
@@ -185,6 +186,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")
@@ -31,7 +31,7 @@ class ControlsExt(ModelStateBase):
self.CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP)
cloudlog.info("controlsd_ext got CarParamsSP")
self.sm_services_ext = ['radarState', 'selfdriveStateSP', 'longitudinalPlanSP']
self.sm_services_ext = ['radarState', 'selfdriveStateSP']
self.pm_services_ext = ['carControlSP']
def initialize_lateral_control(self, lac, CI, dt):
@@ -104,31 +104,6 @@ class ControlsExt(ModelStateBase):
CC_SP.intelligentCruiseButtonManagement.sendButton = icbm_src.sendButton
CC_SP.intelligentCruiseButtonManagement.vTarget = icbm_src.vTarget
# Smart Cruise Control state
scc_src = sm['longitudinalPlanSP'].smartCruiseControl
CC_SP.smartCruiseControl.visionState = scc_src.vision.state
CC_SP.smartCruiseControl.mapState = scc_src.map.state
# Speed Limit Assist state
sl_src = sm['longitudinalPlanSP'].speedLimit
CC_SP.speedLimit.assistState = sl_src.assist.state
CC_SP.speedLimit.speedLimitFinalLast = sl_src.resolver.speedLimitFinalLast
# Lane change state
meta = sm['modelV2'].meta
CC_SP.laneChange.state = str(meta.laneChangeState)
CC_SP.laneChange.direction = str(meta.laneChangeDirection)
# Driver monitoring alert
dm = sm['driverMonitoringState']
CC_SP.driverMonitoring.alertLevel = dm.alertLevel.raw
CC_SP.driverMonitoring.visionPolicy = dm.activePolicy == 'vision'
# Planner following a lead, the model path (e2e) wins often while following so count it when there is a lead
long_plan = sm['longitudinalPlan']
CC_SP.leadFollowing = str(long_plan.longitudinalPlanSource) in ('lead0', 'lead1', 'lead2') or \
(str(long_plan.longitudinalPlanSource) == 'e2e' and long_plan.hasLead)
return CC_SP
@staticmethod
@@ -1349,7 +1349,17 @@
"key": "TorqueBar",
"widget": "toggle",
"title": "Steering Arc",
"description": "Display steering arc on the driving screen when lateral control is enabled."
"description": "Display steering arc on the driving screen when lateral control is enabled.",
"visibility": [
{
"type": "not",
"condition": {
"type": "capability",
"field": "device_type",
"equals": "mici"
}
}
]
},
{
"key": "ShowTurnSignals",
@@ -1389,6 +1399,12 @@
}
]
},
{
"key": "TripSummary",
"widget": "toggle",
"title": "Trip Summary",
"description": "Show a summary of distance and engagement when you shift into park after a drive."
},
{
"key": "RocketFuel",
"widget": "toggle",
@@ -20,6 +20,8 @@ sections:
widget: toggle
title: Steering Arc
description: Display steering arc on the driving screen when lateral control is enabled.
visibility:
- $ref: '#/macros/hide_on_mici'
- key: ShowTurnSignals
widget: toggle
title: Display Turn Signals
@@ -37,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