feat: Squash all min-features into full

This commit is contained in:
Rick Lan
2026-01-08 12:22:12 +08:00
parent 7950dee9a1
commit ef7cd06332
387 changed files with 105494 additions and 141 deletions
+21 -2
View File
@@ -20,6 +20,7 @@ from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase
from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
from openpilot.selfdrive.car.cruise import VCruiseHelper
from openpilot.selfdrive.car.car_specific import MockCarState
from opendbc.safety import ALTERNATIVE_EXPERIENCE
REPLAY = "REPLAY" in os.environ
@@ -66,7 +67,7 @@ class Car:
def __init__(self, CI=None, RI=None) -> None:
self.can_sock = messaging.sub_sock('can', timeout=20)
self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'])
self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'])
self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks', 'carStateExt'])
self.can_rcv_cum_timeout_counter = 0
@@ -101,7 +102,11 @@ class Car:
with car.CarParams.from_bytes(cached_params_raw) as _cached_params:
cached_params = _cached_params
self.CI = get_car(*self.can_callbacks, obd_callback(self.params), alpha_long_allowed, is_release, num_pandas, dp_params, cached_params)
if self.params.get_bool("dp_lat_alka"):
dp_params |= structs.DPFlags.LatALKA
dp_fingerprint = str(self.params.get("dp_dev_model_selected") or "")
self.CI = get_car(*self.can_callbacks, obd_callback(self.params), alpha_long_allowed, is_release, num_pandas, dp_params, cached_params, dp_fingerprint=dp_fingerprint)
self.RI = interfaces[self.CI.CP.carFingerprint].RadarInterface(self.CI.CP)
self.CP = self.CI.CP
@@ -111,7 +116,15 @@ class Car:
self.CI, self.CP = CI, CI.CP
self.RI = RI
if self.params.get_bool("dp_lon_ext_radar"):
from opendbc.car.radar_interface import RadarInterface
self.RI = RadarInterface(self.CI.CP)
self.CP.alternativeExperience = 0
# dp - ALKA: set alternative experience flag if ALKA is enabled
if dp_params & structs.DPFlags.LatALKA:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALKA
openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle")
controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly
self.CP.passive = not controller_available or self.CP.dashcamOnly
@@ -221,6 +234,12 @@ class Car:
cs_send.carState.cumLagMs = -self.rk.remaining * 1000.
self.pm.send('carState', cs_send)
# dp - ALKA: publish lkas_on state from carstate
cs_ext = messaging.new_message('carStateExt')
cs_ext.valid = CS.canValid
cs_ext.carStateExt.lkasOn = getattr(self.CI.CS, 'lkas_on', False)
self.pm.send('carStateExt', cs_ext)
if RD is not None:
tracks_msg = messaging.new_message('liveTracks')
tracks_msg.valid = not any(RD.errors.to_dict().values())
+22 -3
View File
@@ -11,6 +11,7 @@ from openpilot.common.swaglog import cloudlog
from opendbc.car.car_helpers import interfaces
from opendbc.car.vehicle_model import VehicleModel
from opendbc.safety import ALTERNATIVE_EXPERIENCE
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
@@ -38,8 +39,8 @@ class Controls:
self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
self.pm = messaging.PubMaster(['carControl', 'controlsState'])
'driverMonitoringState', 'onroadEvents', 'driverAssistance', 'carStateExt'], poll='selfdriveState')
self.pm = messaging.PubMaster(['carControl', 'controlsState', 'controlsStateExt'])
self.steer_limited_by_safety = False
self.curvature = 0.0
@@ -58,6 +59,10 @@ class Controls:
elif self.CP.lateralTuning.which() == 'torque':
self.LaC = LatControlTorque(self.CP, self.CI, DT_CTRL)
# dp - ALKA: cache enabled state (CP doesn't change after init)
self.alka_enabled = bool(self.CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALKA)
self.alka_active = False
def update(self):
self.sm.update(15)
if self.sm.updated["liveCalibration"]:
@@ -93,7 +98,15 @@ class Controls:
# Check which actuators can be enabled
standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill
CC.latActive = self.sm['selfdriveState'].active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
# dp - ALKA: check conditions (alka_enabled is cached in __init__)
if self.alka_enabled:
# Read lkas_on state from carstate (published via carStateExt)
lkas_on = self.sm['carStateExt'].lkasOn
# Conditions: lkas_on, gear not in P/N/R, calibration complete, seatbelt latched, doors closed
calibrated = self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated
gear_ok = CS.gearShifter not in (car.CarState.GearShifter.park, car.CarState.GearShifter.neutral, car.CarState.GearShifter.reverse)
self.alka_active = lkas_on and gear_ok and calibrated and not CS.seatbeltUnlatched and not CS.doorOpen
CC.latActive = (self.sm['selfdriveState'].active or self.alka_active) and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
(not standstill or self.CP.steerAtStandstill)
CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl
@@ -203,6 +216,12 @@ class Controls:
self.pm.send('controlsState', dat)
# controlsStateExt
dat = messaging.new_message('controlsStateExt')
dat.valid = True
dat.controlsStateExt.alkaActive = self.alka_active
self.pm.send('controlsStateExt', dat)
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid
+21 -5
View File
@@ -1,6 +1,7 @@
from cereal import log
from openpilot.common.constants import CV
from openpilot.common.realtime import DT_MDL
import time
LaneChangeState = log.LaneChangeState
LaneChangeDirection = log.LaneChangeDirection
@@ -31,7 +32,7 @@ DESIRES = {
class DesireHelper:
def __init__(self):
def __init__(self, dp_lat_lca_speed=LANE_CHANGE_SPEED_MIN, dp_lat_lca_auto_sec=0.):
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
self.lane_change_timer = 0.0
@@ -39,24 +40,31 @@ class DesireHelper:
self.keep_pulse_timer = 0.0
self.prev_one_blinker = False
self.desire = log.Desire.none
self.dp_lat_lca_speed = float(dp_lat_lca_speed * CV.MPH_TO_MS)
self.dp_lat_lca_auto_sec = dp_lat_lca_auto_sec
self.dp_lat_lca_auto_sec_start = 0.
@staticmethod
def get_lane_change_direction(CS):
return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
def update(self, carstate, lateral_active, lane_change_prob):
def update(self, carstate, lateral_active, lane_change_prob, left_edge_detected, right_edge_detected):
v_ego = carstate.vEgo
one_blinker = carstate.leftBlinker != carstate.rightBlinker
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
below_lane_change_speed = True if self.dp_lat_lca_speed == 0. else v_ego < self.dp_lat_lca_speed
if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
else:
# LaneChangeState.off
c_time = time.monotonic()
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
if self.dp_lat_lca_auto_sec > 0.:
self.dp_lat_lca_auto_sec_start = c_time
# Initialize lane change direction to prevent UI alert flicker
self.lane_change_direction = self.get_lane_change_direction(carstate)
@@ -69,8 +77,16 @@ class DesireHelper:
((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
(carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
blindspot_detected = (((carstate.leftBlindspot or left_edge_detected) and self.lane_change_direction == LaneChangeDirection.left) or
((carstate.rightBlindspot or right_edge_detected) and self.lane_change_direction == LaneChangeDirection.right))
# reset timer
if self.dp_lat_lca_auto_sec > 0.:
if blindspot_detected:
self.dp_lat_lca_auto_sec_start = c_time
else:
if (c_time - self.dp_lat_lca_auto_sec_start) >= self.dp_lat_lca_auto_sec:
torque_applied = True
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
@@ -14,6 +14,9 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDX
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_from_plan
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
from openpilot.common.swaglog import cloudlog
from dragonpilot.selfdrive.controls.lib.acm import ACM
from dragonpilot.selfdrive.controls.lib.aem import AEM
from dragonpilot.selfdrive.controls.lib.dtsc import DTSC
LON_MPC_STEP = 0.2 # first step is 0.2s
A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
@@ -27,6 +30,9 @@ _A_TOTAL_MAX_V = [1.7, 3.2]
_A_TOTAL_MAX_BP = [20., 40.]
class DPFlags:
ACM = 1
AEM = 2
DTSC = 2 ** 2
pass
def get_max_accel(v_ego):
@@ -70,6 +76,9 @@ class LongitudinalPlanner:
self.a_desired_trajectory = np.zeros(CONTROL_N)
self.j_desired_trajectory = np.zeros(CONTROL_N)
self.solverExecutionTime = 0.0
self.acm = ACM()
self.aem = AEM()
self.dtsc = DTSC(aggressiveness=0.8)
@staticmethod
def parse_model(model_msg):
@@ -94,6 +103,10 @@ class LongitudinalPlanner:
def update(self, sm, dp_flags = 0):
mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
if dp_flags & DPFlags.AEM:
self.aem.update_states(model_msg=sm['modelV2'], radar_msg=sm['radarState'], v_ego=sm['carState'].vEgo)
mode = self.aem.get_mode(mode)
if len(sm['carControl'].orientationNED) == 3:
accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
else:
@@ -143,10 +156,29 @@ class LongitudinalPlanner:
self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
# Apply DTSC curve speed constraints if enabled
if dp_flags & DPFlags.DTSC:
# Get modified acceleration constraints based on curvature
a_min_dtsc, a_max_dtsc = self.dtsc.get_mpc_constraints(
sm['modelV2'], v_ego, accel_clip[0], accel_clip[1])
# Update MPC parameters with curve constraints
# This directly modifies the acceleration bounds in the MPC solver
for i in range(len(a_min_dtsc)):
# Apply the more restrictive constraint
self.mpc.params[i, 0] = max(accel_clip[0], a_min_dtsc[i]) # a_min
self.mpc.params[i, 1] = min(accel_clip[1], a_max_dtsc[i]) # a_max
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality)
self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
# ACM - Adaptive Coasting Module
if dp_flags & DPFlags.ACM:
user_control = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled
self.acm.update_states(sm['carControl'], sm['radarState'], user_control, v_ego, v_cruise)
self.a_desired_trajectory = self.acm.update_a_desired_trajectory(self.a_desired_trajectory)
self.j_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution)
# TODO counter is only needed because radar is glitchy, remove once radar is gone
+7
View File
@@ -24,6 +24,13 @@ def main():
dp_flags = 0
if params.get_bool("dp_lon_acm"):
dp_flags |= DPFlags.ACM
if params.get_bool("dp_lon_aem"):
dp_flags |= DPFlags.AEM
if params.get_bool("dp_lon_dtsc"):
dp_flags |= DPFlags.DTSC
while True:
sm.update()
if sm.updated['modelV2']:
+285
View File
@@ -0,0 +1,285 @@
#!/usr/bin/env python3
"""
Tests for ALKA (Always-on Lane Keeping Assist) Python layer.
Tests the controlsd logic for computing latActive when ALKA is enabled.
Matches the logic in selfdrive/controls/controlsd.py.
"""
import unittest
from cereal import log
from opendbc.safety import ALTERNATIVE_EXPERIENCE
class TestALKAAlternativeExperience(unittest.TestCase):
"""Test ALTERNATIVE_EXPERIENCE.ALKA constant."""
def test_alka_constant_value(self):
"""ALKA constant should be 1024 (2^10)."""
self.assertEqual(ALTERNATIVE_EXPERIENCE.ALKA, 1024)
def test_alka_flag_bitwise(self):
"""ALKA flag should work with bitwise operations."""
# Test setting ALKA flag
exp = ALTERNATIVE_EXPERIENCE.DEFAULT | ALTERNATIVE_EXPERIENCE.ALKA
self.assertTrue(exp & ALTERNATIVE_EXPERIENCE.ALKA)
# Test combining with other flags
exp = ALTERNATIVE_EXPERIENCE.DISABLE_STOCK_AEB | ALTERNATIVE_EXPERIENCE.ALKA
self.assertTrue(exp & ALTERNATIVE_EXPERIENCE.ALKA)
self.assertTrue(exp & ALTERNATIVE_EXPERIENCE.DISABLE_STOCK_AEB)
# Test without ALKA
exp = ALTERNATIVE_EXPERIENCE.DISABLE_STOCK_AEB
self.assertFalse(exp & ALTERNATIVE_EXPERIENCE.ALKA)
class TestALKALatActive(unittest.TestCase):
"""Test latActive computation with ALKA.
This mirrors the logic in controlsd.py:
alka_enabled = (self.CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALKA) != 0
lkas_on = self.sm['carStateExt'].lkasOn
calibrated = self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated
gear_ok = CS.gearShifter not in (park, neutral, reverse)
alka_active = lkas_on and gear_ok and calibrated and not CS.seatbeltUnlatched and not CS.doorOpen
CC.latActive = (self.sm['selfdriveState'].active or alka_active) and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
(not standstill or self.CP.steerAtStandstill)
"""
def _compute_alka_active(self, alka_enabled, lkas_on, gear_ok, calibrated, seatbelt_unlatched, door_open):
"""Compute alka_active matching controlsd.py logic."""
return alka_enabled and lkas_on and gear_ok and calibrated and \
not seatbelt_unlatched and not door_open
def _compute_lat_active(self, selfdrive_active, alka_active, steer_fault_temp, steer_fault_perm, standstill, steer_at_standstill=False):
"""Compute latActive matching controlsd.py logic."""
return (selfdrive_active or alka_active) and not steer_fault_temp and not steer_fault_perm and \
(not standstill or steer_at_standstill)
def test_lat_active_normal_mode(self):
"""Without ALKA, latActive should follow selfdriveState.active."""
alka_active = self._compute_alka_active(
alka_enabled=False, lkas_on=True, gear_ok=True,
calibrated=True, seatbelt_unlatched=False, door_open=False)
lat_active = self._compute_lat_active(
selfdrive_active=True, alka_active=alka_active,
steer_fault_temp=False, steer_fault_perm=False, standstill=False)
self.assertTrue(lat_active)
# When selfdrive not active, lat should be inactive
lat_active = self._compute_lat_active(
selfdrive_active=False, alka_active=alka_active,
steer_fault_temp=False, steer_fault_perm=False, standstill=False)
self.assertFalse(lat_active)
def test_lat_active_alka_mode(self):
"""With ALKA, latActive can be true even when selfdriveState.active is false."""
alka_active = self._compute_alka_active(
alka_enabled=True, lkas_on=True, gear_ok=True,
calibrated=True, seatbelt_unlatched=False, door_open=False)
lat_active = self._compute_lat_active(
selfdrive_active=False, alka_active=alka_active,
steer_fault_temp=False, steer_fault_perm=False, standstill=False)
self.assertTrue(lat_active)
def test_lat_active_alka_requires_lkas_on(self):
"""ALKA requires lkasOn (ACC Main ON)."""
alka_active = self._compute_alka_active(
alka_enabled=True, lkas_on=False, gear_ok=True,
calibrated=True, seatbelt_unlatched=False, door_open=False)
lat_active = self._compute_lat_active(
selfdrive_active=False, alka_active=alka_active,
steer_fault_temp=False, steer_fault_perm=False, standstill=False)
self.assertFalse(lat_active)
def test_lat_active_alka_requires_gear_ok(self):
"""ALKA requires gear not in P/N/R."""
alka_active = self._compute_alka_active(
alka_enabled=True, lkas_on=True, gear_ok=False,
calibrated=True, seatbelt_unlatched=False, door_open=False)
lat_active = self._compute_lat_active(
selfdrive_active=False, alka_active=alka_active,
steer_fault_temp=False, steer_fault_perm=False, standstill=False)
self.assertFalse(lat_active)
def test_lat_active_alka_requires_calibration(self):
"""ALKA requires calibration to be complete."""
alka_active = self._compute_alka_active(
alka_enabled=True, lkas_on=True, gear_ok=True,
calibrated=False, seatbelt_unlatched=False, door_open=False)
lat_active = self._compute_lat_active(
selfdrive_active=False, alka_active=alka_active,
steer_fault_temp=False, steer_fault_perm=False, standstill=False)
self.assertFalse(lat_active)
def test_lat_active_alka_requires_seatbelt(self):
"""ALKA requires seatbelt to be latched."""
alka_active = self._compute_alka_active(
alka_enabled=True, lkas_on=True, gear_ok=True,
calibrated=True, seatbelt_unlatched=True, door_open=False)
lat_active = self._compute_lat_active(
selfdrive_active=False, alka_active=alka_active,
steer_fault_temp=False, steer_fault_perm=False, standstill=False)
self.assertFalse(lat_active)
def test_lat_active_alka_requires_doors_closed(self):
"""ALKA requires all doors to be closed."""
alka_active = self._compute_alka_active(
alka_enabled=True, lkas_on=True, gear_ok=True,
calibrated=True, seatbelt_unlatched=False, door_open=True)
lat_active = self._compute_lat_active(
selfdrive_active=False, alka_active=alka_active,
steer_fault_temp=False, steer_fault_perm=False, standstill=False)
self.assertFalse(lat_active)
def test_lat_active_blocked_by_steer_fault_temporary(self):
"""Temporary steer fault should block lateral control regardless of ALKA."""
alka_active = self._compute_alka_active(
alka_enabled=True, lkas_on=True, gear_ok=True,
calibrated=True, seatbelt_unlatched=False, door_open=False)
lat_active = self._compute_lat_active(
selfdrive_active=False, alka_active=alka_active,
steer_fault_temp=True, steer_fault_perm=False, standstill=False)
self.assertFalse(lat_active)
def test_lat_active_blocked_by_steer_fault_permanent(self):
"""Permanent steer fault should block lateral control regardless of ALKA."""
alka_active = self._compute_alka_active(
alka_enabled=True, lkas_on=True, gear_ok=True,
calibrated=True, seatbelt_unlatched=False, door_open=False)
lat_active = self._compute_lat_active(
selfdrive_active=False, alka_active=alka_active,
steer_fault_temp=False, steer_fault_perm=True, standstill=False)
self.assertFalse(lat_active)
def test_lat_active_blocked_by_standstill(self):
"""ALKA should be blocked at standstill (latActive checks standstill)."""
alka_active = self._compute_alka_active(
alka_enabled=True, lkas_on=True, gear_ok=True,
calibrated=True, seatbelt_unlatched=False, door_open=False)
lat_active = self._compute_lat_active(
selfdrive_active=False, alka_active=alka_active,
steer_fault_temp=False, steer_fault_perm=False, standstill=True)
self.assertFalse(lat_active)
class TestALKASettings(unittest.TestCase):
"""Test ALKA settings configuration."""
def test_alka_setting_in_lateral_section(self):
"""ALKA setting should be in the Lateral section."""
from dragonpilot.settings import SETTINGS
lateral_section = None
for section in SETTINGS:
if section["title"] == "Lateral":
lateral_section = section
break
self.assertIsNotNone(lateral_section, "Lateral section not found in SETTINGS")
# Find ALKA setting
alka_setting = None
for setting in lateral_section["settings"]:
if setting.get("key") == "dp_lat_alka":
alka_setting = setting
break
self.assertIsNotNone(alka_setting, "dp_lat_alka setting not found")
def test_alka_setting_has_brands(self):
"""ALKA setting brands should match safety modes with alka_allowed=true."""
from dragonpilot.settings import SETTINGS
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
# Map of brand names to their safety modes
brand_to_safety_mode = {
"toyota": CarParams.SafetyModel.toyota,
"hyundai": CarParams.SafetyModel.hyundai,
"honda": CarParams.SafetyModel.hondaNidec,
"volkswagen": CarParams.SafetyModel.volkswagen,
"subaru": CarParams.SafetyModel.subaru,
"mazda": CarParams.SafetyModel.mazda,
"nissan": CarParams.SafetyModel.nissan,
"ford": CarParams.SafetyModel.ford,
"chrysler": CarParams.SafetyModel.chrysler,
}
# Find ALKA setting
alka_setting = None
for section in SETTINGS:
for setting in section.get("settings", []):
if setting.get("key") == "dp_lat_alka":
alka_setting = setting
break
self.assertIsNotNone(alka_setting)
self.assertIn("brands", alka_setting)
self.assertIsInstance(alka_setting["brands"], list)
# Verify each brand in settings has alka_allowed=true in safety mode
safety = libsafety_py.libsafety
for brand in alka_setting["brands"]:
self.assertIn(brand, brand_to_safety_mode, f"Unknown brand: {brand}")
safety_mode = brand_to_safety_mode[brand]
safety.set_safety_hooks(safety_mode, 0)
safety.init_tests()
self.assertTrue(safety.get_alka_allowed(), f"Brand {brand} should have alka_allowed=true")
class TestALKAAllConditions(unittest.TestCase):
"""Comprehensive truth table tests for all ALKA conditions."""
def test_alka_all_conditions_truth_table(self):
"""Test all combinations of ALKA conditions."""
# Test cases: (alka_enabled, lkas_on, gear_ok, calibrated, seatbelt_unlatched, door_open) -> expected_alka_active
test_cases = [
# All conditions met
(True, True, True, True, False, False, True),
# Missing one condition each
(False, True, True, True, False, False, False), # ALKA disabled
(True, False, True, True, False, False, False), # lkas_on false
(True, True, False, True, False, False, False), # gear not ok (P/N/R)
(True, True, True, False, False, False, False), # Not calibrated
(True, True, True, True, True, False, False), # Seatbelt unlatched
(True, True, True, True, False, True, False), # Door open
# Multiple conditions missing
(True, False, False, True, False, False, False), # No lkas_on + bad gear
(True, True, True, False, True, True, False), # Not calibrated + seatbelt + door
]
for alka_enabled, lkas_on, gear_ok, calibrated, seatbelt_unlatched, door_open, expected in test_cases:
alka_active = alka_enabled and lkas_on and gear_ok and calibrated and \
not seatbelt_unlatched and not door_open
self.assertEqual(alka_active, expected,
f"Failed for alka_enabled={alka_enabled}, lkas_on={lkas_on}, gear_ok={gear_ok}, "
f"calibrated={calibrated}, seatbelt_unlatched={seatbelt_unlatched}, "
f"door_open={door_open}")
def test_lat_active_truth_table(self):
"""Test latActive computation with various inputs."""
# Test cases: (selfdrive_active, alka_active, steer_fault, standstill) -> expected_lat_active
test_cases = [
(False, False, False, False, False), # Nothing active
(True, False, False, False, True), # Selfdrive only
(False, True, False, False, True), # ALKA only
(True, True, False, False, True), # Both active
(True, False, True, False, False), # Selfdrive but fault
(False, True, True, False, False), # ALKA but fault
(True, False, False, True, False), # Selfdrive but standstill (no steerAtStandstill)
(False, True, False, True, False), # ALKA but standstill
]
for selfdrive_active, alka_active, steer_fault, standstill, expected in test_cases:
lat_active = (selfdrive_active or alka_active) and not steer_fault and not standstill
self.assertEqual(lat_active, expected,
f"Failed for selfdrive_active={selfdrive_active}, alka_active={alka_active}, "
f"steer_fault={steer_fault}, standstill={standstill}")
if __name__ == "__main__":
unittest.main()
+26 -6
View File
@@ -30,7 +30,9 @@ from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext
from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
from dragonpilot.selfdrive.controls.lib.road_edge_detector import RoadEdgeDetector
LITE = os.getenv("LITE") is not None
PROCESS_NAME = "selfdrive.modeld.modeld"
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -46,7 +48,7 @@ MIN_LAT_CONTROL_SPEED = 0.3
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) -> log.ModelDataV2.Action:
lat_action_t: float, long_action_t: float, v_ego: float, dp_lat_offset_cm: int) -> log.ModelDataV2.Action:
plan = model_output['plan'][0]
desired_accel, should_stop = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0],
plan[:,Plan.ACCELERATION][:,0],
@@ -64,6 +66,12 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
else:
desired_curvature = prev_action.desiredCurvature
# Apply lateral offset (driving style adjustment)
if dp_lat_offset_cm != 0:
lat_offset_m = dp_lat_offset_cm / 100.0
curvature_offset = 2.0 * lat_offset_m / 900.0
desired_curvature += curvature_offset
return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),
desiredAcceleration=float(desired_accel),
shouldStop=bool(should_stop))
@@ -261,7 +269,7 @@ def main(demo=False):
cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})")
# messaging
pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelExt"])
sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"])
publish_state = PublishState()
@@ -292,7 +300,14 @@ def main(demo=False):
long_delay = CP.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS
prev_action = log.ModelDataV2.Action()
DH = DesireHelper()
dp_lat_lca_speed = int(params.get("dp_lat_lca_speed"))
dp_lat_lca_auto_sec = float(params.get("dp_lat_lca_auto_sec"))
DH = DesireHelper(dp_lat_lca_speed=dp_lat_lca_speed, dp_lat_lca_auto_sec=dp_lat_lca_auto_sec)
dp_dev_is_rhd = params.get_bool("dp_dev_is_rhd")
RED = RoadEdgeDetector(params.get_bool("dp_lat_road_edge_detection"))
dp_lat_offset_cm = int(params.get("dp_lat_offset_cm") or 0)
while True:
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -329,7 +344,7 @@ def main(demo=False):
sm.update(0)
desire = DH.desire
is_rhd = sm["driverMonitoringState"].isRHD
is_rhd = dp_dev_is_rhd if LITE else sm["driverMonitoringState"].isRHD
frame_id = sm["roadCameraState"].frameId
v_ego = max(sm["carState"].vEgo, 0.)
lat_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
@@ -376,8 +391,9 @@ def main(demo=False):
modelv2_send = messaging.new_message('modelV2')
drivingdata_send = messaging.new_message('drivingModelData')
posenet_send = messaging.new_message('cameraOdometry')
model_ext_send = messaging.new_message('modelExt')
action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego)
action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego, dp_lat_offset_cm)
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,
@@ -387,7 +403,10 @@ def main(demo=False):
l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
lane_change_prob = l_lane_change_prob + r_lane_change_prob
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
RED.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs)
model_ext_send.modelExt.leftEdgeDetected = RED.left_edge_detected
model_ext_send.modelExt.rightEdgeDetected = RED.right_edge_detected
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RED.left_edge_detected, RED.right_edge_detected)
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
@@ -397,6 +416,7 @@ def main(demo=False):
pm.send('modelV2', modelv2_send)
pm.send('drivingModelData', drivingdata_send)
pm.send('cameraOdometry', posenet_send)
pm.send('modelExt', model_ext_send)
last_vipc_frame_id = meta_main.frame_id
+10 -1
View File
@@ -136,9 +136,18 @@ std::optional<std::string> Panda::get_serial() {
}
bool Panda::up_to_date() {
const bool tici_hw = getenv("TICI_HW");
const bool tici_tres = getenv("TICI_TRES");
if (auto fw_sig = get_firmware_version()) {
for (auto fn : { "panda.bin.signed", "panda_h7.bin.signed" }) {
auto content = util::read_file(std::string("../../panda/board/obj/") + fn);
// auto content = util::read_file(std::string("../../panda/board/obj/") + fn);
// rick - for tici
std::string content;
if (tici_hw && !tici_tres) {
content = util::read_file(std::string("../../panda_tici/board/obj/") + fn);
} else {
content = util::read_file(std::string("../../panda/board/obj/") + fn);
}
if (content.size() >= fw_sig->size() &&
memcmp(content.data() + content.size() - fw_sig->size(), fw_sig->data(), fw_sig->size()) == 0) {
return true;
+17 -8
View File
@@ -23,9 +23,10 @@ from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroa
from openpilot.system.version import get_build_metadata
from openpilot.system.hardware import HARDWARE
from opendbc.safety import ALTERNATIVE_EXPERIENCE
REPLAY = "REPLAY" in os.environ
SIMULATION = "SIMULATION" in os.environ
SIMULATION = "SIMULATION" in os.environ or os.getenv("LITE") is not None
TESTING_CLOSET = "TESTING_CLOSET" in os.environ
LONGITUDINAL_PERSONALITY_MAP = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()}
@@ -58,6 +59,9 @@ class SelfdriveD:
self.car_events = CarSpecificEvents(self.CP)
# dp - ALKA: check if ALKA is enabled
self.alka = bool(self.CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALKA)
self.pose_calibrator = PoseCalibrator()
self.calibrated_pose: Pose | None = None
self.excessive_actuation_check = ExcessiveActuationCheck()
@@ -74,16 +78,16 @@ class SelfdriveD:
# TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches
self.car_state_sock = messaging.sub_sock('carState', timeout=20)
ignore = self.sensor_packets + self.gps_packets + ['alertDebug']
ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ['modelExt']
if SIMULATION:
ignore += ['driverCameraState', 'managerState']
ignore += ['driverMonitoringState', 'driverCameraState', 'managerState']
if REPLAY:
# no vipc in replay will make them ignored anyways
ignore += ['roadCameraState', 'wideRoadCameraState']
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback'] + \
'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback', 'modelExt'] + \
self.camera_packets + self.sensor_packets + self.gps_packets,
ignore_alive=ignore, ignore_avg_freq=ignore,
ignore_valid=ignore, frequency=int(1/DT_CTRL))
@@ -119,9 +123,12 @@ class SelfdriveD:
self.experimental_mode = False
self.personality = self.params.get("LongitudinalPersonality", return_default=True)
self.recalibrating_seen = False
self.state_machine = StateMachine()
self.state_machine = StateMachine(self.alka)
self.rk = Ratekeeper(100, print_delay_threshold=None)
# dp
self.dp_lat_road_edge_detection_cooldown: float = 0.
# Determine startup event
self.startup_event = EventName.startup #if build_metadata.openpilot.comma_remote and build_metadata.tested_channel else EventName.startupMaster
if HARDWARE.get_device_type() == 'mici':
@@ -256,9 +263,11 @@ class SelfdriveD:
# Handle lane change
if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
direction = self.sm['modelV2'].meta.laneChangeDirection
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right):
self.events.add(EventName.laneChangeBlocked)
if ((CS.leftBlindspot or self.sm['modelExt'].leftEdgeDetected) and direction == LaneChangeDirection.left) or \
((CS.rightBlindspot or self.sm['modelExt'].rightEdgeDetected) and direction == LaneChangeDirection.right):
self.dp_lat_road_edge_detection_cooldown = time.monotonic() + 0.5
if time.monotonic() <= self.dp_lat_road_edge_detection_cooldown:
self.events.add(EventName.laneChangeBlocked)
else:
if direction == LaneChangeDirection.left:
self.events.add(EventName.preLaneChangeLeft)
+4 -2
View File
@@ -9,10 +9,11 @@ ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding)
ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES)
class StateMachine:
def __init__(self):
def __init__(self, alka=False):
self.current_alert_types = [ET.PERMANENT]
self.state = State.disabled
self.soft_disable_timer = 0
self.alka = alka
def update(self, events: Events):
# decrement the soft disable timer at every step, as it's reset on
@@ -92,7 +93,8 @@ class StateMachine:
# Check if openpilot is engaged and actuators are enabled
enabled = self.state in ENABLED_STATES
active = self.state in ACTIVE_STATES
if active:
# dp - ALKA: show warnings when ALKA is enabled (for safety alerts during lane keeping)
if active or self.alka:
self.current_alert_types.append(ET.WARNING)
return enabled, active
+11 -1
View File
@@ -12,6 +12,7 @@ from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos
from openpilot.system.ui.lib.multilang import tr, trn
from openpilot.system.ui.widgets.label import gui_label
from openpilot.system.ui.widgets import Widget
import os
HEADER_HEIGHT = 80
HEAD_BUTTON_FONT_SIZE = 40
@@ -20,6 +21,8 @@ SPACING = 25
RIGHT_COLUMN_WIDTH = 750
REFRESH_INTERVAL = 10.0
LITE = os.getenv("LITE") is not None
class HomeLayoutState(IntEnum):
HOME = 0
@@ -228,5 +231,12 @@ class HomeLayout(Widget):
def _get_version_text(self) -> str:
brand = "dragonpilot"
if LITE:
if "TICI_TRES" in os.environ:
device_type = " - XLite"
else:
device_type = " - Lite"
else:
device_type = ""
description = self.params.get("UpdaterCurrentDescription")
return f"{brand} {description}" if description else brand
return f"{brand}{device_type} {description}" if description else f"{brand}{device_type}"
+91
View File
@@ -1,5 +1,6 @@
import os
import math
import json
from cereal import messaging, log
from openpilot.common.basedir import BASEDIR
@@ -39,6 +40,12 @@ class DeviceLayout(Widget):
self._fcc_dialog: HtmlModal | None = None
self._training_guide: TrainingGuide | None = None
# dp - vehicle selector
self._dp_vehicle_selector_btn = button_item(lambda: tr("Vehicle Model"), lambda: tr("SELECT"), callback=self._on_vehicle_selector_btn_pressed)
self._dp_vehicle_selector_btn.action_item.set_value(ui_state.params.get("dp_dev_model_selected") or tr("[AUTO DETECT]"))
self._dp_vehicle_selector_make_dialog: MultiOptionDialog | None = None
self._dp_vehicle_selector_model_dialog: MultiOptionDialog | None = None
items = self._initialize_items()
self._scroller = Scroller(items, line_separator=True, spacing=0)
@@ -55,7 +62,12 @@ class DeviceLayout(Widget):
self._power_off_btn = dual_button_item(lambda: tr("Reboot"), lambda: tr("Power Off"),
left_callback=self._reboot_prompt, right_callback=self._power_off_prompt)
self._dp_on_off_road_btn = button_item(lambda: tr("On/Off Road"), lambda: tr("Go Offroad"), lambda: tr("Force openpilot to go into onroad/offroad state.<br>(e.g. for update purpose)"),
callback=self._dp_on_off_road_prompt)
items = [
self._dp_vehicle_selector_btn,
self._dp_on_off_road_btn,
text_item(lambda: tr("Dongle ID"), self._params.get("DongleId") or (lambda: tr("N/A"))),
text_item(lambda: tr("Serial"), self._params.get("HardwareSerial") or (lambda: tr("N/A"))),
self._pair_device_btn,
@@ -208,3 +220,82 @@ class DeviceLayout(Widget):
self._training_guide = TrainingGuide(completed_callback=completed_callback)
gui_app.set_modal_overlay(self._training_guide)
def _update_state(self):
self._dp_vehicle_selector_btn.action_item.set_value(ui_state.params.get("dp_dev_model_selected") or tr("[AUTO DETECT]"))
def _on_vehicle_selector_btn_pressed(self):
models_json = self._params.get("dp_dev_model_list")
if not models_json:
gui_app.set_modal_overlay(alert_dialog(tr("Vehicle Model list not found.")))
return
try:
car_models_list = json.loads(models_json)
except json.JSONDecodeError:
gui_app.set_modal_overlay(alert_dialog(tr("Vehicle Model list is not a valid format.")))
return
models_by_make = car_models_list
makes = sorted(models_by_make.keys())
all_makes = [tr("[AUTO DETECT]")] + makes
selected_model = self._params.get("dp_dev_model_selected")
current_selection = tr("[AUTO DETECT]")
if selected_model:
for make, models in models_by_make.items():
if selected_model in models:
current_selection = make
break
def _on_vehicle_selector_make_selected(result: int):
if result == DialogResult.CONFIRM and self._dp_vehicle_selector_make_dialog:
make = self._dp_vehicle_selector_make_dialog.selection
if make == tr("[AUTO DETECT]"):
if self._params.get("dp_dev_model_selected"):
self._params.remove("dp_dev_model_selected")
self._dp_vehicle_selector_btn.action_item.set_value(tr("[AUTO DETECT]"))
self._params.put_bool("OnroadCycleRequested", True)
else:
self._show_model_selection(make, models_by_make[make])
self._dp_vehicle_selector_make_dialog = None
self._dp_vehicle_selector_make_dialog = MultiOptionDialog(
tr("Select a Make"),
all_makes,
current_selection,
)
gui_app.set_modal_overlay(self._dp_vehicle_selector_make_dialog, callback=_on_vehicle_selector_make_selected)
def _show_model_selection(self, make, models):
selected_model = self._params.get("dp_dev_model_selected") or ""
def _on_vehicle_selector_model_selected(result: int):
if result == DialogResult.CONFIRM and self._dp_vehicle_selector_model_dialog:
selection = self._dp_vehicle_selector_model_dialog.selection
self._params.put("dp_dev_model_selected", selection)
self._dp_vehicle_selector_btn.action_item.set_value(selection)
self._params.put_bool("OnroadCycleRequested", True)
self._dp_vehicle_selector_model_dialog = None
self._dp_vehicle_selector_model_dialog = MultiOptionDialog(
tr("Select a Model") + f" ({make})",
models,
selected_model if selected_model in models else "",
)
gui_app.set_modal_overlay(self._dp_vehicle_selector_model_dialog, callback=_on_vehicle_selector_model_selected)
def _dp_on_off_road_prompt(self):
def on_off_road(result: int):
if result != DialogResult.CONFIRM:
return
val = self._params.get_bool("dp_dev_go_off_road")
self._params.put_bool("dp_dev_go_off_road", not val)
self._dp_on_off_road_btn.action_item.set_text(tr("Go Onroad") if not val else tr("Go Offroad"))
dialog = ConfirmDialog(tr("Are you sure you want to switch?"), tr("CONFIRM"))
gui_app.set_modal_overlay(dialog, callback=on_off_road)
+8
View File
@@ -8,9 +8,12 @@ from openpilot.system.ui.lib.application import gui_app
from openpilot.system.ui.lib.multilang import tr, tr_noop
from openpilot.system.ui.widgets import DialogResult
from openpilot.selfdrive.ui.ui_state import ui_state
import os
PERSONALITY_TO_INT = log.LongitudinalPersonality.schema.enumerants
LITE = os.getenv("LITE") is not None
# Description constants
DESCRIPTIONS = {
"OpenpilotEnabledToggle": tr_noop(
@@ -118,6 +121,11 @@ class TogglesLayout(Widget):
self._toggles = {}
self._locked_toggles = set()
if LITE:
for key in ['RecordAudio']:
self._toggle_defs.pop(key, None)
for param, (title, desc, icon, needs_restart) in self._toggle_defs.items():
toggle = toggle_item(
title,
+22 -7
View File
@@ -8,6 +8,7 @@ from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos, F
from openpilot.system.ui.lib.multilang import tr, tr_noop
from openpilot.system.ui.lib.text_measure import measure_text_cached
from openpilot.system.ui.widgets import Widget
from dragonpilot.selfdrive.ui.dashy_qr import DashyQR
SIDEBAR_WIDTH = 300
METRIC_HEIGHT = 126
@@ -78,6 +79,10 @@ class Sidebar(Widget):
self._settings_img = gui_app.texture("images/button_settings.png", SETTINGS_BTN.width, SETTINGS_BTN.height)
self._mic_img = gui_app.texture("icons/microphone.png", 30, 30)
self._mic_indicator_rect = rl.Rectangle(0, 0, 0, 0)
# QR code for dashy
self._qr = DashyQR()
self._font_regular = gui_app.font(FontWeight.NORMAL)
self._font_bold = gui_app.font(FontWeight.SEMI_BOLD)
@@ -110,7 +115,8 @@ class Sidebar(Widget):
self._recording_audio = ui_state.recording_audio
self._update_network_status(device_state)
self._update_temperature_status(device_state)
self._update_connection_status(device_state)
if not ui_state.dp_dev_disable_connect:
self._update_connection_status(device_state)
self._update_panda_status()
def _update_network_status(self, device_state):
@@ -163,12 +169,19 @@ class Sidebar(Widget):
tint = Colors.BUTTON_PRESSED if settings_down else Colors.BUTTON_NORMAL
rl.draw_texture(self._settings_img, int(SETTINGS_BTN.x), int(SETTINGS_BTN.y), tint)
# Home/Flag button
flag_pressed = mouse_down and rl.check_collision_point_rec(mouse_pos, HOME_BTN)
button_img = self._flag_img if ui_state.started else self._home_img
# Show QR code when offroad
self._qr.update()
if self._qr.texture:
scale = HOME_BTN.width / self._qr.texture.width
pos = rl.Vector2(HOME_BTN.x, HOME_BTN.y)
rl.draw_texture_ex(self._qr.texture, pos, 0.0, scale, rl.WHITE)
else:
# Home/Flag button
flag_pressed = mouse_down and rl.check_collision_point_rec(mouse_pos, HOME_BTN)
button_img = self._flag_img if ui_state.started else self._home_img
tint = Colors.BUTTON_PRESSED if (ui_state.started and flag_pressed) else Colors.BUTTON_NORMAL
rl.draw_texture(button_img, int(HOME_BTN.x), int(HOME_BTN.y), tint)
tint = Colors.BUTTON_PRESSED if (ui_state.started and flag_pressed) else Colors.BUTTON_NORMAL
rl.draw_texture(button_img, int(HOME_BTN.x), int(HOME_BTN.y), tint)
# Microphone button
if self._recording_audio:
@@ -200,7 +213,9 @@ class Sidebar(Widget):
rl.draw_text_ex(self._font_regular, tr(self._net_type), text_pos, FONT_SIZE, 0, Colors.WHITE)
def _draw_metrics(self, rect: rl.Rectangle):
metrics = [(self._temp_status, 338), (self._panda_status, 496), (self._connect_status, 654)]
metrics = [(self._temp_status, 338), (self._panda_status, 496)]
if not ui_state.dp_dev_disable_connect:
metrics.append((self._connect_status, 654))
for metric, y_offset in metrics:
self._draw_metric(rect, metric, rect.y + y_offset)
+8 -2
View File
@@ -10,6 +10,7 @@ from openpilot.selfdrive.ui.mici.layouts.onboarding import OnboardingWindow
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 dragonpilot.selfdrive.ui.mici.layouts.dashy_qrcode import DashyQRCode
ONROAD_DELAY = 2.5 # seconds
@@ -38,12 +39,16 @@ class MiciMainLayout(Widget):
self._settings_layout = SettingsLayout()
self._onroad_layout = AugmentedRoadView(bookmark_callback=self._on_bookmark_clicked)
# dp dashy
self._dashy_qrcode_layout = DashyQRCode()
# Initialize widget rects
for widget in (self._home_layout, self._settings_layout, self._alerts_layout, self._onroad_layout):
for widget in (self._home_layout, self._settings_layout, self._alerts_layout, self._onroad_layout, self._dashy_qrcode_layout):
# TODO: set parent rect and use it if never passed rect from render (like in Scroller)
widget.set_rect(rl.Rectangle(0, 0, gui_app.width, gui_app.height))
self._scroller = Scroller([
self._dashy_qrcode_layout,
self._alerts_layout,
self._home_layout,
self._onroad_layout,
@@ -85,7 +90,8 @@ class MiciMainLayout(Widget):
if self._alerts_layout.active_alerts() > 0:
self._scroller.scroll_to(self._alerts_layout.rect.x)
else:
self._scroller.scroll_to(self._rect.width)
# dp - home is at index 2 in scroller: [dashy_qrcode, alerts, home, onroad]
self._scroller.scroll_to(gui_app.width * 2)
self._setup = True
# Render
+2 -1
View File
@@ -187,7 +187,8 @@ class HudRenderer(Widget):
self._wheel_alpha_filter.update(255)
self._wheel_y_filter.update(0)
else:
if ui_state.status == UIStatus.DISENGAGED:
# dp - ALKA: show steering wheel when ALKA is active (even when disengaged)
if ui_state.status == UIStatus.DISENGAGED and not ui_state.dp_alka_active:
self._wheel_alpha_filter.update(0)
self._wheel_y_filter.update(wheel_txt.height / 2)
else:
+12 -4
View File
@@ -137,8 +137,8 @@ class ModelRenderer(Widget):
self._update_leads(radar_state, path_x_array)
self._transform_dirty = False
# Draw elements (hide when disengaged)
if ui_state.status != UIStatus.DISENGAGED:
# Draw elements (hide when disengaged, unless ALKA is active)
if ui_state.status != UIStatus.DISENGAGED or ui_state.dp_alka_active:
self._draw_lane_lines()
self._draw_path(sm)
@@ -333,7 +333,11 @@ class ModelRenderer(Widget):
if self._experimental_mode:
# Draw with acceleration coloring
if ui_state.status == UIStatus.DISENGAGED:
draw_polygon(self._rect, self._path.projected_points, rl.Color(0, 0, 0, 90))
if ui_state.dp_alka_active:
# dp - ALKA: winning blue!
draw_polygon(self._rect, self._path.projected_points, rl.Color(40, 117, 165, 90))
else:
draw_polygon(self._rect, self._path.projected_points, rl.Color(0, 0, 0, 90))
elif len(self._exp_gradient.colors) > 1:
draw_polygon(self._rect, self._path.projected_points, gradient=self._exp_gradient)
else:
@@ -350,7 +354,11 @@ class ModelRenderer(Widget):
)
if ui_state.status == UIStatus.DISENGAGED:
draw_polygon(self._rect, self._path.projected_points, rl.Color(0, 0, 0, 90))
if ui_state.dp_alka_active:
# dp - ALKA: winning blue!
draw_polygon(self._rect, self._path.projected_points, rl.Color(40, 117, 165, 90))
else:
draw_polygon(self._rect, self._path.projected_points, rl.Color(0, 0, 0, 90))
else:
draw_polygon(self._rect, self._path.projected_points, gradient=gradient)
+15 -9
View File
@@ -146,11 +146,12 @@ def arc_bar_pts(cx: float, cy: float,
class TorqueBar(Widget):
def __init__(self, demo: bool = False):
def __init__(self, demo: bool = False, scale: float = 1.):
super().__init__()
self._demo = demo
self._torque_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps)
self._torque_line_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
self._scale = scale
def update_filter(self, value: float):
"""Update the torque filter value (for demo mode)."""
@@ -180,8 +181,8 @@ class TorqueBar(Widget):
def _render(self, rect: rl.Rectangle) -> None:
# adjust y pos with torque
torque_line_offset = np.interp(abs(self._torque_filter.x), [0.5, 1], [22, 26])
torque_line_height = np.interp(abs(self._torque_filter.x), [0.5, 1], [14, 56])
torque_line_offset = np.interp(abs(self._torque_filter.x), [0.5, 1], [22 * self._scale, 26 * self._scale])
torque_line_height = np.interp(abs(self._torque_filter.x), [0.5, 1], [14 * self._scale, 56 * self._scale])
# animate alpha and angle span
if not self._demo:
@@ -195,7 +196,7 @@ class TorqueBar(Widget):
torque_line_bg_color = rl.Color(255, 255, 255, int(255 * 0.15 * self._torque_line_alpha_filter.x))
# draw curved line polygon torque bar
torque_line_radius = 1200
torque_line_radius = 1200 * self._scale
top_angle = -90
torque_bg_angle_span = self._torque_line_alpha_filter.x * TORQUE_ANGLE_SPAN
torque_start_angle = top_angle - torque_bg_angle_span / 2
@@ -203,17 +204,22 @@ class TorqueBar(Widget):
# centerline radius & center (you already have these values)
mid_r = torque_line_radius + torque_line_height / 2
cx = rect.x + rect.width / 2 + 8 # offset 8px to right of camera feed
cx = rect.x + rect.width / 2 + (8 * self._scale)
cy = rect.y + rect.height + torque_line_radius - torque_line_offset
# dp - pass cap_radius explicitly so the corners round properly
scaled_cap_radius = 7 * self._scale
# draw bg torque indicator line
bg_pts = arc_bar_pts(cx, cy, mid_r, torque_line_height, torque_start_angle, torque_end_angle)
bg_pts = arc_bar_pts(cx, cy, mid_r, torque_line_height, torque_start_angle, torque_end_angle,
cap_radius=scaled_cap_radius)
draw_polygon(rect, bg_pts, color=torque_line_bg_color)
# draw torque indicator line
a0s = top_angle
a1s = a0s + torque_bg_angle_span / 2 * self._torque_filter.x
sl_pts = arc_bar_pts(cx, cy, mid_r, torque_line_height, a0s, a1s)
sl_pts = arc_bar_pts(cx, cy, mid_r, torque_line_height, a0s, a1s,
cap_radius=scaled_cap_radius)
# draw beautiful gradient from center to 65% of the bg torque bar width
start_grad_pt = cx / rect.width
@@ -251,6 +257,6 @@ class TorqueBar(Widget):
# draw center torque bar dot
if abs(self._torque_filter.x) < 0.5:
dot_y = self._rect.y + self._rect.height - torque_line_offset - torque_line_height / 2
rl.draw_circle(int(cx), int(dot_y), 10 // 2,
dot_y = rect.y + rect.height - torque_line_offset - torque_line_height / 2
rl.draw_circle(int(cx), int(dot_y), int(10 * self._scale) // 2,
rl.Color(182, 182, 182, int(255 * 0.9 * self._torque_line_alpha_filter.x)))
+68 -2
View File
@@ -24,12 +24,19 @@ BORDER_COLORS = {
UIStatus.DISENGAGED: rl.Color(0x12, 0x28, 0x39, 0xFF), # Blue for disengaged state
UIStatus.OVERRIDE: rl.Color(0x89, 0x92, 0x8D, 0xFF), # Gray for override state
UIStatus.ENGAGED: rl.Color(0x16, 0x7F, 0x40, 0xFF), # Green for engaged state
UIStatus.ALKA: rl.Color(0x22, 0xa0, 0xdc, 0xf1), # Blue for ALKA state
}
WIDE_CAM_MAX_SPEED = 10.0 # m/s (22 mph)
ROAD_CAM_MIN_SPEED = 15.0 # m/s (34 mph)
INF_POINT = np.array([1000.0, 0.0, 0.0])
# dp
DP_INDICATOR_BLINK_RATE_FAST = int(gui_app.target_fps * 0.25)
DP_INDICATOR_BLINK_RATE_STD = int(gui_app.target_fps * 0.5)
DP_INDICATOR_COLOR_BSM = rl.Color(255, 255, 0, 255)
DP_INDICATOR_COLOR_BLINKER = rl.Color(0, 255, 0, 255)
class AugmentedRoadView(CameraView):
def __init__(self, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD):
@@ -49,6 +56,14 @@ class AugmentedRoadView(CameraView):
self.alert_renderer = AlertRenderer()
self.driver_state_renderer = DriverStateRenderer()
# DP border indicator
self._dp_indicator_show_left = False
self._dp_indicator_show_right = False
self._dp_indicator_count_left = 0
self._dp_indicator_count_right = 0
self._dp_indicator_color_left = rl.Color(0, 0, 0, 0)
self._dp_indicator_color_right = rl.Color(0, 0, 0, 0)
# debug
self._pm = messaging.PubMaster(['uiDebug'])
@@ -58,6 +73,7 @@ class AugmentedRoadView(CameraView):
if not ui_state.started:
return
self._update_dp_indicator_states(ui_state.sm)
self._switch_stream_if_needed(ui_state.sm)
# Update calibration before rendering
@@ -83,11 +99,17 @@ class AugmentedRoadView(CameraView):
# Render the base camera view
super()._render(rect)
hide_hud = False
if ui_state.dp_ui_hide_hud_speed_ms > 0. and ui_state.sm['carState'].vEgo > ui_state.dp_ui_hide_hud_speed_ms:
hide_hud = True
# Draw all UI overlays
self.model_renderer.render(self._content_rect)
self._hud_renderer.render(self._content_rect)
if not hide_hud:
self._hud_renderer.render(self._content_rect)
self.alert_renderer.render(self._content_rect)
self.driver_state_renderer.render(self._content_rect)
if not hide_hud:
self.driver_state_renderer.render(self._content_rect)
# Custom UI extension point - add custom overlays here
# Use self._content_rect for positioning within camera bounds
@@ -115,10 +137,21 @@ class AugmentedRoadView(CameraView):
rl.draw_rectangle_lines_ex(rect, UI_BORDER_SIZE, rl.BLACK)
border_roundness = 0.12
border_color = BORDER_COLORS.get(ui_state.status, BORDER_COLORS[UIStatus.DISENGAGED])
# dp - ALKA: use ALKA border color when active and disengaged
if ui_state.dp_alka_active and ui_state.status == UIStatus.DISENGAGED:
border_color = BORDER_COLORS[UIStatus.ALKA]
border_rect = rl.Rectangle(rect.x + UI_BORDER_SIZE, rect.y + UI_BORDER_SIZE,
rect.width - 2 * UI_BORDER_SIZE, rect.height - 2 * UI_BORDER_SIZE)
rl.draw_rectangle_rounded_lines_ex(border_rect, border_roundness, 10, UI_BORDER_SIZE, border_color)
# dp - Side indicators
indicator_y = int(rect.y+4*UI_BORDER_SIZE)
indicator_height = int(rect.height-8*UI_BORDER_SIZE)
if self._dp_indicator_show_left:
rl.draw_rectangle(int(rect.x), indicator_y, UI_BORDER_SIZE, indicator_height, self._dp_indicator_color_left)
if self._dp_indicator_show_right:
rl.draw_rectangle(int(rect.x + rect.width-UI_BORDER_SIZE), indicator_y, UI_BORDER_SIZE, indicator_height, self._dp_indicator_color_right)
def _switch_stream_if_needed(self, sm):
if sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams:
v_ego = sm['carState'].vEgo
@@ -217,6 +250,39 @@ class AugmentedRoadView(CameraView):
return self._cached_matrix
def _update_dp_indicator_side_state(self, blinker_state, bsm_state, show_prev, count_prev):
show = show_prev
count = count_prev
color = rl.Color(0, 0, 0, 0)
if not blinker_state and not bsm_state:
show = False
count = 0
else:
count += 1
if bsm_state and blinker_state:
show = not show if count % DP_INDICATOR_BLINK_RATE_FAST == 0 else show
color = DP_INDICATOR_COLOR_BSM
elif blinker_state:
show = not show if count % DP_INDICATOR_BLINK_RATE_STD == 0 else show
color = DP_INDICATOR_COLOR_BLINKER
elif bsm_state:
show = True
color = DP_INDICATOR_COLOR_BSM
else:
show = False
return show, count, color
def _update_dp_indicator_states(self, sm):
cs = sm['carState']
self._dp_indicator_show_left, self._dp_indicator_count_left, self._dp_indicator_color_left = \
self._update_dp_indicator_side_state(cs.leftBlinker, cs.leftBlindspot,
self._dp_indicator_show_left, self._dp_indicator_count_left)
self._dp_indicator_show_right, self._dp_indicator_count_right, self._dp_indicator_color_right = \
self._update_dp_indicator_side_state(cs.rightBlinker, cs.rightBlindspot,
self._dp_indicator_show_right, self._dp_indicator_count_right)
if __name__ == "__main__":
gui_app.init_window("OnRoad Camera View")
+6
View File
@@ -7,6 +7,7 @@ from openpilot.system.ui.lib.application import gui_app, FontWeight
from openpilot.system.ui.lib.multilang import tr
from openpilot.system.ui.lib.text_measure import measure_text_cached
from openpilot.system.ui.widgets import Widget
from openpilot.selfdrive.ui.mici.onroad.torque_bar import TorqueBar
# Constants
SET_SPEED_NA = 255
@@ -72,6 +73,8 @@ class HudRenderer(Widget):
self._exp_button: ExpButton = ExpButton(UI_CONFIG.button_size, UI_CONFIG.wheel_icon_size)
self._torque_bar = TorqueBar(scale=4.0)
def _update_state(self) -> None:
"""Update HUD state based on car state and controls state."""
sm = ui_state.sm
@@ -121,6 +124,9 @@ class HudRenderer(Widget):
button_y = rect.y + UI_CONFIG.border_size
self._exp_button.render(rl.Rectangle(button_x, button_y, UI_CONFIG.button_size, UI_CONFIG.button_size))
if ui_state.sm['controlsState'].lateralControlState.which() != 'angleState':
self._torque_bar.render(rect)
def user_interacting(self) -> bool:
return self._exp_button.is_pressed
+164 -2
View File
@@ -7,7 +7,8 @@ from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.params import Params
from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT
from openpilot.selfdrive.ui.ui_state import ui_state
from openpilot.system.ui.lib.application import gui_app
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.lib.shader_polygon import draw_polygon, Gradient
from openpilot.system.ui.widgets import Widget
@@ -27,6 +28,13 @@ NO_THROTTLE_COLORS = [
rl.Color(242, 242, 242, 0), # HSLF(112/360, 0.0, 0.95, 0.0)
]
# dp
DP_RAINBOW_SCROLL_SPEED_FACTOR = 20.0
DP_RAINBOW_NUM_REPEATS = 3
DP_RAINBOW_ALPHA = 128
DP_RAINBOW_GRADIENT_SAMPLES = 20
DP_RAINBOW_HUE_SECTORS = 6
@dataclass
class ModelPoints:
@@ -39,6 +47,19 @@ class LeadVehicle:
glow: list[float] = field(default_factory=list)
chevron: list[float] = field(default_factory=list)
fill_alpha: int = 0
# dp
v_rel: float = 0.0
d_rel: float = 0.0
x: float = 0.0
y: float = 0.0
sz: float = 0.0
@dataclass
class DpUiLeadMode:
off = 0
lead = 1
radar = 2
all = 3
class ModelRenderer(Widget):
@@ -76,6 +97,10 @@ class ModelRenderer(Widget):
cp = messaging.log_from_bytes(car_params, car.CarParams)
self._longitudinal_control = cp.openpilotLongitudinalControl
# dp
self._dp_ui_rainbow_rotation = 0.0
self._dp_ui_rainbow_gradient = None
def set_transform(self, transform: np.ndarray):
self._car_space_transform = transform.astype(np.float32)
self._transform_dirty = True
@@ -122,6 +147,10 @@ class ModelRenderer(Widget):
self._update_leads(radar_state, path_x_array)
self._transform_dirty = False
# dp - draw live tracks before everything
if ui_state.dp_ui_lead in [DpUiLeadMode.radar, DpUiLeadMode.all] and sm.valid['liveTracks']:
self._draw_live_tracks(sm)
# Draw elements
self._draw_lane_lines()
self._draw_path(sm)
@@ -253,7 +282,7 @@ class ModelRenderer(Widget):
glow = [(x + (sz * 1.35) + g_xo, y + sz + g_yo), (x, y - g_yo), (x - (sz * 1.35) - g_xo, y + sz + g_yo)]
chevron = [(x + (sz * 1.25), y + sz), (x, y), (x - (sz * 1.25), y + sz)]
return LeadVehicle(glow=glow, chevron=chevron, fill_alpha=int(fill_alpha))
return LeadVehicle(glow=glow, chevron=chevron, fill_alpha=int(fill_alpha), d_rel=d_rel, x=x, y=y, sz=sz, v_rel=v_rel)
def _draw_lane_lines(self):
"""Draw lane lines and road edges"""
@@ -278,6 +307,13 @@ class ModelRenderer(Widget):
if not self._path.projected_points.size:
return
if ui_state.dp_ui_rainbow:
v_ego = sm['carState'].vEgo
self._update_rainbow_gradient(v_ego)
if self._dp_ui_rainbow_gradient:
draw_polygon(self._rect, self._path.projected_points, gradient=self._dp_ui_rainbow_gradient)
return
allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control
self._blend_filter.update(int(allow_throttle))
@@ -308,6 +344,24 @@ class ModelRenderer(Widget):
rl.draw_triangle_fan(lead.glow, len(lead.glow), rl.Color(218, 202, 37, 255))
rl.draw_triangle_fan(lead.chevron, len(lead.chevron), rl.Color(201, 34, 49, lead.fill_alpha))
if ui_state.dp_ui_lead in [DpUiLeadMode.lead, DpUiLeadMode.all]:
start_y = lead.y
car_state = ui_state.sm['carState']
# v
v = lead.v_rel + car_state.vEgo
v_str = f"{v * 3.6:.0f} kph" if ui_state.is_metric else f"{v * 2.237:.0f} mph"
# d_rel
dist_str = f"{lead.d_rel:.1f} m" if ui_state.is_metric else f"{lead.d_rel * 3.28084:.1f} ft"
self._dp_paint_centered_lead_text(f"{v_str} | {dist_str}", 40, lead.x, start_y + lead.sz)
# ttc
ttc = (lead.d_rel / car_state.vEgo) if car_state.vEgo > 0 else float("NaN")
if ttc < 5.:
ttc_str = f"{ttc:.1f}s"
self._dp_paint_centered_lead_text(ttc_str, 80, lead.x, start_y + lead.sz + 40)
@staticmethod
def _get_path_length_idx(pos_x_array: np.ndarray, path_distance: float) -> int:
"""Get the index corresponding to the given path distance"""
@@ -433,3 +487,111 @@ class ModelRenderer(Widget):
int(inv_t * start.b + t * end.b),
int(inv_t * start.a + t * end.a)
) for start, end in zip(begin_colors, end_colors, strict=True)]
def _update_rainbow_gradient(self, v_ego):
# Scroll speed
rotation_speed = max(0.01, v_ego) / gui_app.target_fps / DP_RAINBOW_SCROLL_SPEED_FACTOR
self._dp_ui_rainbow_rotation += rotation_speed
if self._dp_ui_rainbow_rotation > 1.0:
self._dp_ui_rainbow_rotation -= 1.0
gradient_stops = np.linspace(0, 1, DP_RAINBOW_GRADIENT_SAMPLES)
hues = (gradient_stops * DP_RAINBOW_NUM_REPEATS + self._dp_ui_rainbow_rotation) % 1.0
# Vectorized hsv_to_rgb
i = np.floor(hues * DP_RAINBOW_HUE_SECTORS).astype(np.uint8)
f = hues * DP_RAINBOW_HUE_SECTORS - i
q = 1 - f
t = f
i %= DP_RAINBOW_HUE_SECTORS
rgb = np.zeros((hues.shape[0], 3))
masks = [i == j for j in range(DP_RAINBOW_HUE_SECTORS)]
rgb[masks[0], 0] = 1
rgb[masks[0], 1] = t[masks[0]]
rgb[masks[1], 0] = q[masks[1]]
rgb[masks[1], 1] = 1
rgb[masks[2], 1] = 1
rgb[masks[2], 2] = t[masks[2]]
rgb[masks[3], 1] = q[masks[3]]
rgb[masks[3], 2] = 1
rgb[masks[4], 0] = t[masks[4]]
rgb[masks[4], 2] = 1
rgb[masks[5], 0] = 1
rgb[masks[5], 2] = q[masks[5]]
rgb_int = (rgb * 255).astype(np.uint8)
colors = [rl.Color(r, g, b, DP_RAINBOW_ALPHA) for r, g, b in rgb_int]
self._dp_ui_rainbow_gradient = Gradient(
start=(0.0, 1.0),
end=(0.0, 0.0),
colors=colors,
stops=gradient_stops.tolist(),
)
def _dp_paint_centered_lead_text(self, text, size, x, y):
font = gui_app.font(FontWeight.NORMAL)
text_width = measure_text_cached(font, text, size).x
text_x = x - text_width / 2
rl.draw_text_ex(font, text, rl.Vector2(text_x, y), size, 0, rl.WHITE)
def _draw_live_tracks(self, sm):
font = gui_app.font(FontWeight.NORMAL)
live_tracks = sm['liveTracks']
font_size = 40
line_height = 40
for point in live_tracks.points:
d_rel = point.dRel
y_rel = point.yRel
v_rel = point.vRel
z_on_path = self._path_offset_z
if d_rel >= 0 and self._path.raw_points.shape[0] > 0:
path_x = self._path.raw_points[:, 0]
path_z = self._path.raw_points[:, 2]
idx = self._get_path_length_idx(path_x, d_rel)
if idx < len(path_z):
z_on_path += path_z[idx]
screen_pos = self._map_to_screen(d_rel, -y_rel, z_on_path)
if screen_pos:
sx, sy = int(screen_pos[0]), int(screen_pos[1])
rl.draw_circle(sx, sy, 10, rl.Color(255, 0, 0, 200))
if ui_state.is_metric:
dist_unit, speed_unit = "m", "m/s"
d_rel_str = f"{d_rel:.2f}"
y_rel_str = f"{y_rel:.2f}"
v_rel_str = f"{v_rel:.2f}"
else:
dist_unit, speed_unit = "ft", "mph"
d_rel_str = f"{d_rel * 3.28084:.2f}"
y_rel_str = f"{y_rel * 3.28084:.2f}"
v_rel_str = f"{v_rel * 2.23694:.2f}"
info_text = (
f"ID: {point.trackId}\n"
f"d: {d_rel_str} {dist_unit}\n"
f"y: {y_rel_str} {dist_unit}\n"
f"dV: {v_rel_str} {speed_unit}"
)
lines = info_text.split('\n')
text_x = sx + 15
text_y = sy - 20
for i, line in enumerate(lines):
rl.draw_text_ex(font, line, rl.Vector2(text_x, text_y + i * line_height), font_size, 0, rl.WHITE)
+12 -2
View File
@@ -10,6 +10,7 @@ from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import Ratekeeper
from openpilot.common.utils import retry
from openpilot.common.swaglog import cloudlog
from openpilot.common.params import Params
from openpilot.system import micd
from openpilot.system.hardware import HARDWARE
@@ -25,7 +26,7 @@ AMBIENT_DB = 30 # DB where MIN_VOLUME is applied
DB_SCALE = 30 # AMBIENT_DB + DB_SCALE is where MAX_VOLUME is applied
VOLUME_BASE = 20
if HARDWARE.get_device_type() == "tizi":
if HARDWARE.get_device_type() in ("tizi", "tici"):
VOLUME_BASE = 10
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
@@ -44,7 +45,7 @@ sound_list: dict[int, tuple[str, int | None, float]] = {
AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME),
AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME),
}
if HARDWARE.get_device_type() == "tizi":
if HARDWARE.get_device_type() in ("tizi", "tici"):
sound_list.update({
AudibleAlert.engage: ("engage_tizi.wav", 1, MAX_VOLUME),
AudibleAlert.disengage: ("disengage_tizi.wav", 1, MAX_VOLUME),
@@ -72,6 +73,11 @@ class Soundd:
self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False)
try:
self._dp_dev_audible_alert_mode = int(Params().get("dp_dev_audible_alert_mode"))
except:
self._dp_dev_audible_alert_mode = 0
def load_sounds(self):
self.loaded_sounds: dict[int, np.ndarray] = {}
@@ -106,6 +112,10 @@ class Soundd:
written_frames += frames_to_write
self.current_sound_frame += frames_to_write
# dp - set vol to 0 instead
if self._dp_dev_audible_alert_mode == 2 or (self._dp_dev_audible_alert_mode == 1 and self.current_alert in [AudibleAlert.engage, AudibleAlert.disengage]):
self.current_volume = 0
return ret * self.current_volume
def callback(self, data_out: np.ndarray, frames: int, time, status) -> None:
+2 -1
View File
@@ -8,6 +8,7 @@ from openpilot.system.ui.lib.application import gui_app
from openpilot.selfdrive.ui.layouts.main import MainLayout
from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout
from openpilot.selfdrive.ui.ui_state import ui_state
from openpilot.common.params import Params
def main():
@@ -15,7 +16,7 @@ def main():
config_realtime_process(0, 51)
gui_app.init_window("UI")
if gui_app.big_ui():
if gui_app.big_ui() and not Params().get_bool("dp_ui_mici"):
main_layout = MainLayout()
else:
main_layout = MiciMainLayout()
+85 -4
View File
@@ -19,6 +19,7 @@ class UIStatus(Enum):
DISENGAGED = "disengaged"
ENGAGED = "engaged"
OVERRIDE = "override"
ALKA = "alka"
class UIState:
@@ -55,6 +56,8 @@ class UIState:
"carControl",
"liveParameters",
"rawAudioData",
"controlsStateExt",
"liveTracks", # dp - for dp_ui_lead
]
)
@@ -85,6 +88,24 @@ class UIState:
self._offroad_transition_callbacks: list[Callable[[], None]] = []
self._engaged_transition_callbacks: list[Callable[[], None]] = []
# dp - ALKA
self.dp_alka_active: bool = False
# dp
self.dp_ui_display_mode = 0
# dp
self.dp_ui_hide_hud_speed_ms: float = float(int(self.params.get("dp_ui_hide_hud_speed_kph") or 0) * 0.278)
# dp
self.dp_ui_rainbow = self.params.get_bool("dp_ui_rainbow")
# dp
self.dp_ui_lead = int(self.params.get("dp_ui_lead") or 0)
# dp
self.dp_dev_disable_connect = self.params.get_bool("dp_dev_disable_connect")
self.update_params()
def add_offroad_transition_callback(self, callback: Callable[[], None]):
@@ -129,7 +150,9 @@ class UIState:
# Handle wide road camera state updates
if self.sm.updated["wideRoadCameraState"]:
cam_state = self.sm["wideRoadCameraState"]
self.light_sensor = max(100.0 - cam_state.exposureValPercent, 0.0)
# rick - for c3: Scale factor based on sensor type
scale = 6.0 if cam_state.sensor == 'ar0231' else 1.0
self.light_sensor = max(100.0 - scale * cam_state.exposureValPercent, 0.0)
elif not self.sm.alive["wideRoadCameraState"] or not self.sm.valid["wideRoadCameraState"]:
self.light_sensor = -1
@@ -142,6 +165,15 @@ class UIState:
self.is_metric = self.params.get_bool("IsMetric")
self.always_on_dm = self.params.get_bool("AlwaysOnDM")
# dp - ALKA
if self.sm.updated["controlsStateExt"]:
self.dp_alka_active = self.sm["controlsStateExt"].alkaActive
# dp
self.dp_ui_display_mode = int(self.params.get("dp_ui_display_mode") or 0)
self.dp_ui_display_mode_cruise_available = False
self.dp_ui_display_mode_cruise_enabled = False
def _update_status(self) -> None:
if self.started and self.sm.updated["selfdriveState"]:
ss = self.sm["selfdriveState"]
@@ -170,6 +202,11 @@ class UIState:
self._started_prev = self.started
# dp
if self.sm.updated["carState"]:
self.dp_ui_display_mode_cruise_available = self.sm["carState"].cruiseState.available
self.dp_ui_display_mode_cruise_enabled = self.sm["carState"].cruiseState.enabled
def update_params(self) -> None:
# For slower operations
# Update longitudinal control state
@@ -211,8 +248,8 @@ class Device:
if self._override_interactive_timeout is not None:
return self._override_interactive_timeout
ignition_timeout = 10 if gui_app.big_ui() else 5
return ignition_timeout if ui_state.ignition else 30
ignition_timeout = 30 if gui_app.big_ui() else 5
return ignition_timeout if ui_state.ignition else 60
def _reset_interactive_timeout(self) -> None:
self._interaction_time = time.monotonic() + self.interactive_timeout
@@ -257,6 +294,48 @@ class Device:
self._brightness_thread.start()
self._last_brightness = brightness
# // Display Mode
# // 0 Std. - Stock behavior.
# // 1 MAIN+ - ACC MAIN on = Display ON
# // 2 OP+ - OP enabled = Display ON
# // 3 MAIN- - ACC MAIN on = Display OFF
# // 4 OP- - OP enabled = Display OFF
def _ignition_state_ovrride(self, ignition):
# 0 stock behaviour or ignition is off
if ui_state.dp_ui_display_mode == 0 or not ignition:
return ignition
# 1 MAIN+ - ACC MAIN on = Display ON
if ui_state.dp_ui_display_mode == 1:
if ui_state.dp_ui_display_mode_cruise_available:
return True
else:
return False
# 2 OP+ - OP enabled = Display ON
if ui_state.dp_ui_display_mode == 2:
if ui_state.dp_ui_display_mode_cruise_enabled:
return True
else:
return False
# 3 MAIN- - ACC MAIN on = Display OFF
if ui_state.dp_ui_display_mode == 3:
if ui_state.dp_ui_display_mode_cruise_available:
return False
else:
return True
# 4 OP- - OP enabled = Display OFF
if ui_state.dp_ui_display_mode == 4:
if ui_state.dp_ui_display_mode_cruise_enabled:
return False
else:
return True
# oops
return ignition
def _update_wakefulness(self):
# Handle interactive timeout
ignition_just_turned_off = not ui_state.ignition and self._ignition
@@ -271,7 +350,9 @@ class Device:
callback()
self._prev_timed_out = interaction_timeout
self._set_awake(ui_state.ignition or not interaction_timeout or PC)
ignition = self._ignition_state_ovrride(ui_state.ignition)
self._set_awake(ignition or not interaction_timeout or PC)
def _set_awake(self, on: bool):
if on != self._awake: