mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-08-22 00:33:48 +08:00
feat: Squash all min-features into full
This commit is contained in:
@@ -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
|
||||
|
||||
@@ -101,6 +102,9 @@ class Car:
|
||||
with car.CarParams.from_bytes(cached_params_raw) as _cached_params:
|
||||
cached_params = _cached_params
|
||||
|
||||
if self.params.get_bool("dp_lat_alka"):
|
||||
dp_params |= structs.DPFlags.LateralALKA
|
||||
|
||||
self.CI = get_car(*self.can_callbacks, obd_callback(self.params), alpha_long_allowed, is_release, num_pandas, dp_params, cached_params)
|
||||
self.RI = interfaces[self.CI.CP.carFingerprint].RadarInterface(self.CI.CP)
|
||||
self.CP = self.CI.CP
|
||||
@@ -111,7 +115,14 @@ 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
|
||||
if dp_params & structs.DPFlags.LateralALKA:
|
||||
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
|
||||
|
||||
@@ -38,7 +38,7 @@ class Controls:
|
||||
self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
|
||||
'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
|
||||
'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
|
||||
self.pm = messaging.PubMaster(['carControl', 'controlsState'])
|
||||
self.pm = messaging.PubMaster(['carControl', 'controlsState', 'dpControlsState'])
|
||||
|
||||
self.steer_limited_by_controls = False
|
||||
self.curvature = 0.0
|
||||
@@ -57,6 +57,9 @@ class Controls:
|
||||
elif self.CP.lateralTuning.which() == 'torque':
|
||||
self.LaC = LatControlTorque(self.CP, self.CI)
|
||||
|
||||
self.alka_enabled = self.params.get_bool("dp_lat_alka")
|
||||
self.alka_active = False
|
||||
|
||||
def update(self):
|
||||
self.sm.update(15)
|
||||
if self.sm.updated["liveCalibration"]:
|
||||
@@ -92,7 +95,9 @@ 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 \
|
||||
self.alka_active = self.alka_enabled and CS.cruiseState.available and not standstill and CS.gearShifter != car.CarState.GearShifter.reverse
|
||||
lat_active = self.sm['selfdriveState'].active or self.alka_active
|
||||
CC.latActive = lat_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
|
||||
|
||||
@@ -175,6 +180,13 @@ class Controls:
|
||||
# TODO: both controlsState and carControl valids should be set by
|
||||
# sm.all_checks(), but this creates a circular dependency
|
||||
|
||||
# dpControlsState
|
||||
dat = messaging.new_message('dpControlsState')
|
||||
dat.valid = True
|
||||
ncs = dat.dpControlsState
|
||||
ncs.alkaActive = self.alka_active
|
||||
self.pm.send('dpControlsState', dat)
|
||||
|
||||
# controlsState
|
||||
dat = messaging.new_message('controlsState')
|
||||
dat.valid = CS.canValid
|
||||
|
||||
@@ -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,20 +40,26 @@ 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.
|
||||
|
||||
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
|
||||
|
||||
# LaneChangeState.preLaneChange
|
||||
elif self.lane_change_state == LaneChangeState.preLaneChange:
|
||||
@@ -64,8 +71,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,8 @@ 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
|
||||
|
||||
LON_MPC_STEP = 0.2 # first step is 0.2s
|
||||
A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
|
||||
@@ -27,6 +29,9 @@ _A_TOTAL_MAX_V = [1.7, 3.2]
|
||||
_A_TOTAL_MAX_BP = [20., 40.]
|
||||
|
||||
class DPFlags:
|
||||
ACM = 1
|
||||
ACM_DOWNHILL = 2 ** 1
|
||||
AEM = 2 ** 2
|
||||
pass
|
||||
|
||||
def get_max_accel(v_ego):
|
||||
@@ -70,6 +75,8 @@ 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()
|
||||
|
||||
@staticmethod
|
||||
def parse_model(model_msg):
|
||||
@@ -92,7 +99,39 @@ class LongitudinalPlanner:
|
||||
return x, v, a, j, throttle_prob
|
||||
|
||||
def update(self, sm, dp_flags = 0):
|
||||
mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
|
||||
v_ego = sm['carState'].vEgo
|
||||
|
||||
# --- Calculate current cycle variables needed by AEM ---
|
||||
x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'])
|
||||
# Don't clip at low speeds since throttle_prob doesn't account for creep
|
||||
self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
|
||||
|
||||
# --- AEM Logic: Determine MPC mode ---
|
||||
if sm['selfdriveState'].experimentalMode:
|
||||
mode = 'blended'
|
||||
else:
|
||||
mode = 'acc'
|
||||
|
||||
if (dp_flags & DPFlags.AEM) and not self.aem.enabled:
|
||||
self.aem.enabled = True
|
||||
|
||||
if self.aem.enabled:
|
||||
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
|
||||
model_path_plan_for_aem = {'x': x, 'v': v, 'a': a, 'j': j}
|
||||
|
||||
current_cycle_mode = self.aem.get_mode(
|
||||
v_ego_raw=v_ego,
|
||||
lead_one_data_raw=sm['radarState'].leadOne,
|
||||
steering_angle_deg_raw=steer_angle_without_offset,
|
||||
standstill_raw=sm['carState'].standstill,
|
||||
long_personality=self.aem.personality,
|
||||
allow_throttle_planner=self.allow_throttle,
|
||||
model_path_plan_raw=model_path_plan_for_aem,
|
||||
a_target_from_prev_cycle=self.output_a_target,
|
||||
model_predicts_stop_prev=self.output_should_stop,
|
||||
fcw_active_prev=self.fcw,
|
||||
)
|
||||
mode = current_cycle_mode
|
||||
|
||||
if len(sm['carControl'].orientationNED) == 3:
|
||||
accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
|
||||
@@ -112,6 +151,20 @@ class LongitudinalPlanner:
|
||||
# PCM cruise speed may be updated a few cycles later, check if initialized
|
||||
reset_state = reset_state or not v_cruise_initialized
|
||||
|
||||
# Update ACM status
|
||||
if not sm['selfdriveState'].experimentalMode:
|
||||
if not self.acm.enabled and dp_flags & DPFlags.ACM:
|
||||
self.acm.enabled = True
|
||||
self.acm.downhill_only = bool(dp_flags & DPFlags.ACM_DOWNHILL)
|
||||
else:
|
||||
self.acm.enabled = False
|
||||
|
||||
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)
|
||||
|
||||
if self.acm.just_disabled:
|
||||
reset_state = True
|
||||
|
||||
# No change cost when user is controlling the speed, or when standstill
|
||||
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
|
||||
|
||||
@@ -129,9 +182,10 @@ class LongitudinalPlanner:
|
||||
|
||||
# Prevent divergence, smooth in current v_ego
|
||||
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
|
||||
x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'])
|
||||
# Don't clip at low speeds since throttle_prob doesn't account for creep
|
||||
self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
|
||||
# AEM - move to top so it can access them
|
||||
# x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'])
|
||||
# # Don't clip at low speeds since throttle_prob doesn't account for creep
|
||||
# self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
|
||||
|
||||
if not self.allow_throttle:
|
||||
clipped_accel_coast = max(accel_coast, accel_clip[0])
|
||||
@@ -141,14 +195,18 @@ class LongitudinalPlanner:
|
||||
if force_slow_decel:
|
||||
v_cruise = 0.0
|
||||
|
||||
self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
|
||||
self.aem.set_personality(v_ego, sm['selfdriveState'].personality)
|
||||
self.mpc.set_weights(prev_accel_constraint, personality=self.aem.personality)
|
||||
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
|
||||
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality)
|
||||
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=self.aem.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)
|
||||
self.j_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution)
|
||||
|
||||
# Apply ACM post-processing to the acceleration trajectory if active
|
||||
self.a_desired_trajectory = self.acm.update_a_desired_trajectory(self.a_desired_trajectory)
|
||||
|
||||
# TODO counter is only needed because radar is glitchy, remove once radar is gone
|
||||
self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill
|
||||
if self.fcw:
|
||||
@@ -172,6 +230,9 @@ class LongitudinalPlanner:
|
||||
output_a_target = min(output_a_target_mpc, output_a_target_e2e)
|
||||
self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
|
||||
|
||||
# Apply ACM to the final output acceleration target as well
|
||||
output_a_target = self.acm.update_output_a_target(output_a_target)
|
||||
|
||||
for idx in range(2):
|
||||
accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)
|
||||
self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1])
|
||||
|
||||
@@ -23,7 +23,12 @@ def main():
|
||||
poll='modelV2')
|
||||
|
||||
dp_flags = 0
|
||||
|
||||
if params.get_bool("dp_lon_acm"):
|
||||
dp_flags |= DPFlags.ACM
|
||||
if params.get_bool("dp_lon_acm_downhill"):
|
||||
dp_flags |= DPFlags.ACM_DOWNHILL
|
||||
if params.get_bool("dp_lon_aem"):
|
||||
dp_flags |= DPFlags.AEM
|
||||
while True:
|
||||
sm.update()
|
||||
if sm.updated['modelV2']:
|
||||
|
||||
@@ -30,6 +30,7 @@ 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
|
||||
|
||||
|
||||
PROCESS_NAME = "selfdrive.modeld.modeld"
|
||||
@@ -220,7 +221,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()
|
||||
@@ -251,7 +252,10 @@ 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)
|
||||
RED = RoadEdgeDetector(params.get_bool("dp_lat_road_edge_detection"))
|
||||
|
||||
while True:
|
||||
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
|
||||
@@ -337,6 +341,7 @@ 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)
|
||||
prev_action = action
|
||||
@@ -348,7 +353,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
|
||||
@@ -358,6 +366,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
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,142 @@
|
||||
#!/usr/bin/env python3
|
||||
import time
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import config_realtime_process, Ratekeeper, DT_DMON
|
||||
from cereal import log
|
||||
|
||||
EventName = log.OnroadEvent.EventName
|
||||
|
||||
class SimpleDriverMonitoring:
|
||||
def __init__(self):
|
||||
# Timing configuration (in seconds)
|
||||
self.FIRST_WARNING_TIME = 45.0
|
||||
self.SECOND_WARNING_TIME = 60.0
|
||||
self.THIRD_WARNING_TIME = 75.0
|
||||
|
||||
# State variables
|
||||
self.awareness = 1.0 # Full awareness
|
||||
self.current_events = []
|
||||
# self.last_interaction_time = 0
|
||||
self.hands_on_steering = False
|
||||
|
||||
# Warning thresholds (normalized to 0-1 scale)
|
||||
self.threshold_prompt = self.FIRST_WARNING_TIME / self.THIRD_WARNING_TIME # ~0.643 for first warning
|
||||
self.threshold_critical = self.SECOND_WARNING_TIME / self.THIRD_WARNING_TIME # ~0.857 for second warning
|
||||
|
||||
# Step change (how much awareness decreases per step)
|
||||
self.step_change = DT_DMON / self.THIRD_WARNING_TIME
|
||||
|
||||
params = Params()
|
||||
self.is_rhd = params.get_bool("dp_device_is_rhd")
|
||||
self.monitoring_disabled = params.get_bool("dp_device_monitoring_disabled")
|
||||
|
||||
def update_events(self, reset_condition, op_engaged):
|
||||
self.current_events = []
|
||||
|
||||
if self.monitoring_disabled:
|
||||
return
|
||||
|
||||
# If not engaged, reset awareness and return
|
||||
if not op_engaged:
|
||||
self.awareness = 1.0
|
||||
return
|
||||
|
||||
# Reset awareness on any reset condition (standstill, any input)
|
||||
if reset_condition:
|
||||
self.awareness = 1.0
|
||||
return
|
||||
|
||||
# Only decrease awareness if we're not detecting hands on steering
|
||||
self.awareness = max(self.awareness - self.step_change, 0.0)
|
||||
|
||||
# Determine alert level based on awareness
|
||||
if self.awareness <= 0.0:
|
||||
# Third warning (red alert) at 70 seconds
|
||||
self.current_events.append(EventName.driverUnresponsive)
|
||||
elif self.awareness <= (1.0 - self.threshold_critical):
|
||||
# Second warning (orange alert) at 60 seconds
|
||||
self.current_events.append(EventName.promptDriverUnresponsive)
|
||||
elif self.awareness <= (1.0 - self.threshold_prompt):
|
||||
# First warning (green alert) at 45 seconds
|
||||
self.current_events.append(EventName.preDriverUnresponsive)
|
||||
|
||||
def get_state_packet(self, valid=True):
|
||||
# Create driver monitoring state message
|
||||
dat = messaging.new_message('driverMonitoringState', valid=valid)
|
||||
events = []
|
||||
|
||||
for event_name in self.current_events:
|
||||
event = log.OnroadEvent.new_message()
|
||||
event.name = event_name
|
||||
events.append(event)
|
||||
|
||||
dat.driverMonitoringState = {
|
||||
"events": events,
|
||||
"faceDetected": False, # Not using face detection
|
||||
"isDistracted": self.awareness <= (1.0 - self.threshold_prompt),
|
||||
"distractedType": 0, # Not using distraction types
|
||||
"awarenessStatus": 1.0, #self.awareness, (always 1.0 so no decel)
|
||||
"posePitchOffset": 0.0,
|
||||
"posePitchValidCount": 0,
|
||||
"poseYawOffset": 0.0,
|
||||
"poseYawValidCount": 0,
|
||||
"stepChange": self.step_change,
|
||||
"awarenessActive": self.awareness,
|
||||
"awarenessPassive": self.awareness,
|
||||
"isLowStd": True,
|
||||
"hiStdCount": 0,
|
||||
"isActiveMode": True,
|
||||
"isRHD": self.is_rhd,
|
||||
}
|
||||
return dat
|
||||
|
||||
def dmonitoringd_thread():
|
||||
# Configure process priority
|
||||
config_realtime_process([0, 1, 2, 3], 5)
|
||||
|
||||
# Initialize parameters and messaging
|
||||
pm = messaging.PubMaster(['driverMonitoringState'])
|
||||
sm = messaging.SubMaster(['carState', 'selfdriveState'])
|
||||
|
||||
# Initialize driver monitoring system
|
||||
DM = SimpleDriverMonitoring()
|
||||
|
||||
# Create ratekeeper for 20Hz operation
|
||||
rk = Ratekeeper(20, None)
|
||||
|
||||
# Main loop running at 20Hz
|
||||
while True:
|
||||
sm.update()
|
||||
|
||||
# Check if steering is touched (only monitoring steering for hands-on)
|
||||
|
||||
# Reset conditions: not engaged, standstill, or any input
|
||||
reset_condition = (
|
||||
sm['carState'].standstill or
|
||||
sm['carState'].steeringPressed or
|
||||
sm['carState'].gasPressed or
|
||||
sm['carState'].brakePressed or
|
||||
sm['carState'].leftBlinker or
|
||||
sm['carState'].rightBlinker
|
||||
)
|
||||
|
||||
# Process driver monitoring - monitoring only steering for hands-on
|
||||
# but resetting on any input
|
||||
DM.update_events(
|
||||
reset_condition=reset_condition,
|
||||
op_engaged=sm['selfdriveState'].enabled
|
||||
)
|
||||
|
||||
# Publish driver monitoring state
|
||||
dat = DM.get_state_packet()
|
||||
pm.send('driverMonitoringState', dat)
|
||||
|
||||
# Maintain 20Hz
|
||||
rk.keep_time()
|
||||
|
||||
def main():
|
||||
dmonitoringd_thread()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -372,6 +372,7 @@ void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control)
|
||||
static uint16_t prev_fan_speed = 999;
|
||||
static int ir_pwr = 0;
|
||||
static int prev_ir_pwr = 999;
|
||||
const bool lite = getenv("LITE");
|
||||
|
||||
static FirstOrderFilter integ_lines_filter(0, 30.0, 0.05);
|
||||
|
||||
@@ -386,7 +387,7 @@ void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control)
|
||||
}
|
||||
}
|
||||
|
||||
if (sm.updated("driverCameraState")) {
|
||||
if (!lite && sm.updated("driverCameraState")) {
|
||||
auto event = sm["driverCameraState"];
|
||||
int cur_integ_lines = event.getDriverCameraState().getIntegLines();
|
||||
|
||||
@@ -403,14 +404,14 @@ void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control)
|
||||
}
|
||||
|
||||
// Disable IR on input timeout
|
||||
if (nanos_since_boot() - last_driver_camera_t > 1e9) {
|
||||
if (!lite && nanos_since_boot() - last_driver_camera_t > 1e9) {
|
||||
ir_pwr = 0;
|
||||
}
|
||||
|
||||
if (ir_pwr != prev_ir_pwr || sm.frame % 100 == 0) {
|
||||
int16_t ir_panda = util::map_val(ir_pwr, 0, 100, 0, MAX_IR_PANDA_VAL);
|
||||
int16_t ir_panda = util::map_val(ir_pwr, 0, 100, 0, MAX_IR_PANDA_VAL);
|
||||
panda->set_ir_pwr(ir_panda);
|
||||
Hardware::set_ir_power(ir_pwr);
|
||||
Hardware::set_ir_power(ir_pwr);
|
||||
prev_ir_pwr = ir_pwr;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -23,6 +23,7 @@ from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroa
|
||||
|
||||
from openpilot.system.hardware import HARDWARE
|
||||
from openpilot.system.version import get_build_metadata
|
||||
from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
|
||||
REPLAY = "REPLAY" in os.environ
|
||||
SIMULATION = "SIMULATION" in os.environ
|
||||
@@ -58,6 +59,8 @@ class SelfdriveD:
|
||||
|
||||
self.car_events = CarSpecificEvents(self.CP)
|
||||
|
||||
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 +77,18 @@ 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']
|
||||
if REPLAY:
|
||||
# no vipc in replay will make them ignored anyways
|
||||
ignore += ['roadCameraState', 'wideRoadCameraState']
|
||||
if os.getenv("DISABLE_DRIVER") or os.getenv("LITE"):
|
||||
ignore += ['driverCameraState']
|
||||
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,7 +124,7 @@ 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)
|
||||
|
||||
# some comma three with NVMe experience NVMe dropouts mid-drive that
|
||||
@@ -261,8 +266,8 @@ 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):
|
||||
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.events.add(EventName.laneChangeBlocked)
|
||||
else:
|
||||
if direction == LaneChangeDirection.left:
|
||||
|
||||
@@ -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,7 @@ 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:
|
||||
if active or self.alka:
|
||||
self.current_alert_types.append(ET.WARNING)
|
||||
return enabled, active
|
||||
|
||||
|
||||
@@ -26,6 +26,7 @@ Export('widgets')
|
||||
qt_libs = [widgets, qt_util] + base_libs
|
||||
|
||||
qt_src = ["main.cc", "ui.cc", "qt/sidebar.cc", "qt/body.cc",
|
||||
"qt/offroad/model_selector.cc",
|
||||
"qt/window.cc", "qt/home.cc", "qt/offroad/settings.cc",
|
||||
"qt/offroad/software_settings.cc", "qt/offroad/developer_panel.cc", "qt/offroad/onboarding.cc", "qt/offroad/dp_panel.cc",
|
||||
"qt/offroad/driverview.cc", "qt/offroad/experimental_mode.cc", "qt/offroad/firehose.cc",
|
||||
|
||||
@@ -106,7 +106,19 @@ void DPPanel::add_lateral_toggles() {
|
||||
QString::fromUtf8("🐉 ") + tr("Lateral Ctrl"),
|
||||
"",
|
||||
},
|
||||
{
|
||||
"dp_lat_alka",
|
||||
tr("Always-on Lane Keeping Assist (ALKA)"),
|
||||
"",
|
||||
},
|
||||
{
|
||||
"dp_lat_road_edge_detection",
|
||||
tr("Road Edge Detection (RED)"),
|
||||
tr("Block lane change assist when the system detects the road edge.\nNOTE: This will show 'Car Detected in Blindspot' warning.")
|
||||
},
|
||||
};
|
||||
auto lca_speed_toggle = new ParamSpinBoxControl("dp_lat_lca_speed", tr("LCA Speed:"), tr("Off = Disable LCA\n1 mph ≈ 1.2 km/h"), "", 0, 100, 5, tr(" mph"), tr("Off"));
|
||||
lca_sec_toggle = new ParamDoubleSpinBoxControl("dp_lat_lca_auto_sec", QString::fromUtf8(" ") + tr("Auto Lane Change after:"), tr("Off = Disable Auto Lane Change."), "", 0, 5.0, 0.5, tr(" sec"), tr("Off"));
|
||||
|
||||
QWidget *label = nullptr;
|
||||
bool has_toggle = false;
|
||||
@@ -115,6 +127,9 @@ void DPPanel::add_lateral_toggles() {
|
||||
if (param.isEmpty()) {
|
||||
label = new LabelControl(title, "");
|
||||
addItem(label);
|
||||
addItem(lca_speed_toggle);
|
||||
addItem(lca_sec_toggle);
|
||||
has_toggle = true;
|
||||
continue;
|
||||
}
|
||||
|
||||
@@ -139,6 +154,26 @@ void DPPanel::add_longitudinal_toggles() {
|
||||
QString::fromUtf8("🐉 ") + tr("Longitudinal Ctrl"),
|
||||
"",
|
||||
},
|
||||
{
|
||||
"dp_lon_ext_radar",
|
||||
tr("Use External Radar"),
|
||||
tr("See https://github.com/eFiniLan/openpilot-ext-radar-addon for more information."),
|
||||
},
|
||||
{
|
||||
"dp_lon_acm",
|
||||
QString::fromUtf8("🚧 ") + tr("Enable Adaptive Coasting Mode (ACM)"),
|
||||
tr("Adaptive Coasting Mode (ACM) reduces braking to allow smoother coasting when appropriate.\nDOES NOT WORK with Experimental Mode enabled."),
|
||||
},
|
||||
{
|
||||
"dp_lon_acm_downhill",
|
||||
QString::fromUtf8(" ") + tr("Downhill Only"),
|
||||
tr("Limited to downhill driving."),
|
||||
},
|
||||
{
|
||||
"dp_lon_aem",
|
||||
QString::fromUtf8("🚧 ") + tr("Adaptive Experimental Mode (AEM)"),
|
||||
tr("Adaptive mode switcher between ACC and Blended based on driving context."),
|
||||
},
|
||||
};
|
||||
|
||||
QWidget *label = nullptr;
|
||||
@@ -150,6 +185,15 @@ void DPPanel::add_longitudinal_toggles() {
|
||||
addItem(label);
|
||||
continue;
|
||||
}
|
||||
if (param == "dp_lon_ext_radar" && !vehicle_has_radar_unavailable) {
|
||||
continue;
|
||||
}
|
||||
if ((param == "dp_lon_acm" || param == "dp_lon_acm_downhill") && !vehicle_has_long_ctrl) {
|
||||
continue;
|
||||
}
|
||||
if (param == "dp_lon_aem" && !vehicle_has_long_ctrl) {
|
||||
continue;
|
||||
}
|
||||
|
||||
has_toggle = true;
|
||||
auto toggle = new ParamControl(param, title, desc, "", this);
|
||||
@@ -172,7 +216,23 @@ void DPPanel::add_ui_toggles() {
|
||||
QString::fromUtf8("🐉 ") + tr("UI"),
|
||||
"",
|
||||
},
|
||||
{
|
||||
"dp_ui_radar_tracks",
|
||||
tr("Display Radar Tracks"),
|
||||
"",
|
||||
},
|
||||
{
|
||||
"dp_ui_rainbow",
|
||||
tr("Rainbow Driving Path like Tesla"),
|
||||
tr("Why not?"),
|
||||
},
|
||||
};
|
||||
std::vector<QString> display_off_mode_texts{tr("Std."), tr("MAIN+"), tr("OP+"), tr("MAIN-"), tr("OP-")};
|
||||
ButtonParamControl* display_off_mode_setting = new ButtonParamControl("dp_ui_display_mode", tr("Display Mode"),
|
||||
tr("Std. - Stock behavior.\nMAIN+ - ACC MAIN on = Display ON.\nOP+ - OP enabled = Display ON.\nMAIN- - ACC MAIN on = Display OFF\nOP- - OP enabled = Display OFF."),
|
||||
"",
|
||||
display_off_mode_texts, 200);
|
||||
auto hide_hud = new ParamSpinBoxControl("dp_ui_hide_hud_speed_kph", tr("Hide HUD When Moves above:"), tr("To prevent screen burn-in, hide Speed, MAX Speed, and Steering/DM Icons when the car moves.\nOff = Stock Behavior\n1 km/h ≈ 0.6 mph"), "", 0, 120, 5, tr(" km/h"), tr("Off"));
|
||||
|
||||
QWidget *label = nullptr;
|
||||
bool has_toggle = false;
|
||||
@@ -181,6 +241,13 @@ void DPPanel::add_ui_toggles() {
|
||||
if (param.isEmpty()) {
|
||||
label = new LabelControl(title, "");
|
||||
addItem(label);
|
||||
addItem(display_off_mode_setting);
|
||||
has_toggle = true;
|
||||
addItem(hide_hud);
|
||||
has_toggle = true;
|
||||
continue;
|
||||
}
|
||||
if (param == "dp_ui_radar_tracks" && !vehicle_has_long_ctrl) {
|
||||
continue;
|
||||
}
|
||||
|
||||
@@ -205,15 +272,56 @@ void DPPanel::add_device_toggles() {
|
||||
QString::fromUtf8("🐉 ") + tr("Device"),
|
||||
"",
|
||||
},
|
||||
{
|
||||
"dp_device_is_rhd",
|
||||
tr("Enable Right-Hand Drive Mode"),
|
||||
tr("Allow openpilot to obey right-hand traffic conventions on right driver seat."),
|
||||
},
|
||||
{
|
||||
"dp_device_monitoring_disabled",
|
||||
tr("Disable Driver Monitoring"),
|
||||
"",
|
||||
},
|
||||
{
|
||||
"dp_device_beep",
|
||||
tr("Enable Beep (Warning)"),
|
||||
"",
|
||||
}
|
||||
};
|
||||
std::vector<QString> audible_alert_mode_texts{tr("Std."), tr("Warning"), tr("Off")};
|
||||
ButtonParamControl* audible_alert_mode_setting = new ButtonParamControl("dp_device_audible_alert_mode", tr("Audible Alert Mode"),
|
||||
tr("Warning - Only emits sound when there is a warning.\nOff - Does not emit any sound at all."),
|
||||
"",
|
||||
audible_alert_mode_texts);
|
||||
|
||||
auto auto_shutdown_toggle = new ParamSpinBoxControl("dp_device_auto_shutdown_in", tr("Auto Shutdown In:"), tr("0 mins = Immediately"), "", -5, 300, 5, tr(" mins"), tr("Off"));
|
||||
|
||||
std::vector<QString> dashy_mode_texts{tr("Off"), tr("Lite"), tr("Full")};
|
||||
ButtonParamControl* dashy_mode_settings = new ButtonParamControl("dp_dev_dashy", tr("dashy"),
|
||||
tr("dashy - dragonpilot's all-in-one system hub for you.\n\nVisit http://<device_ip>:5088 to access.\n\nOff - Turn off dashy completely.\nLite: File Manager only.\nFull: File Manager + Live Stream."),
|
||||
"",
|
||||
dashy_mode_texts);
|
||||
|
||||
|
||||
auto delay_loggerd_toggle = new ParamSpinBoxControl("dp_dev_delay_loggerd", tr("Delay Starting Loggerd for:"), tr("Delays the startup of loggerd and its related processes when the device goes on-road.\nThis prevents the initial moments of a drive from being recorded, protecting location privacy at the start of a trip."), "", 0, 300, 5, tr(" secs"), tr("Off"));
|
||||
|
||||
QWidget *label = nullptr;
|
||||
bool has_toggle = false;
|
||||
|
||||
const bool lite = getenv("LITE");
|
||||
for (auto &[param, title, desc] : toggle_defs) {
|
||||
if (param.isEmpty()) {
|
||||
label = new LabelControl(title, "");
|
||||
addItem(label);
|
||||
addItem(auto_shutdown_toggle);
|
||||
has_toggle = true;
|
||||
addItem(dashy_mode_settings);
|
||||
has_toggle = true;
|
||||
addItem(delay_loggerd_toggle);
|
||||
has_toggle = true;
|
||||
continue;
|
||||
}
|
||||
if ((param == "dp_device_is_rhd" || param == "dp_device_monitoring_disabled" || param == "dp_device_beep") && !lite) {
|
||||
continue;
|
||||
}
|
||||
|
||||
@@ -224,6 +332,10 @@ void DPPanel::add_device_toggles() {
|
||||
addItem(toggle);
|
||||
toggles[param.toStdString()] = toggle;
|
||||
}
|
||||
if (!getenv("DISABLE_DRIVER")) { // lite check
|
||||
addItem(audible_alert_mode_setting);
|
||||
has_toggle = true;
|
||||
}
|
||||
|
||||
// If no toggles were added, hide the label
|
||||
if (!has_toggle && label) {
|
||||
@@ -281,12 +393,19 @@ void DPPanel::showEvent(QShowEvent *event) {
|
||||
|
||||
void DPPanel::updateStates() {
|
||||
// do fs_watch here
|
||||
fs_watch->addParam("dp_lat_lca_speed");
|
||||
fs_watch->addParam("dp_lon_ext_radar");
|
||||
fs_watch->addParam("dp_lon_acm");
|
||||
|
||||
if (!isVisible()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// do state change logic here
|
||||
lca_sec_toggle->setVisible(std::atoi(params.get("dp_lat_lca_speed").c_str()) > 0);
|
||||
if (vehicle_has_long_ctrl) {
|
||||
toggles["dp_lon_acm_downhill"]->setVisible(params.getBool("dp_lon_acm"));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -28,4 +28,6 @@ private:
|
||||
void add_device_toggles();
|
||||
void updateStates();
|
||||
void showEvent(QShowEvent *event) override;
|
||||
|
||||
ParamDoubleSpinBoxControl* lca_sec_toggle;
|
||||
};
|
||||
|
||||
@@ -0,0 +1,230 @@
|
||||
/*
|
||||
Copyright (c) 2025 Rick Lan
|
||||
|
||||
This software is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License (CC BY-NC-SA 4.0).
|
||||
You are free to share and adapt this work for non-commercial purposes, provided you give appropriate credit and distribute any modifications under the same license.
|
||||
|
||||
To view a copy of this license, visit:
|
||||
http://creativecommons.org/licenses/by-nc-sa/4.0/
|
||||
|
||||
---
|
||||
|
||||
**Commercial Licensing:**
|
||||
Use of this software for commercial purposes is strictly prohibited without a separate, paid license.
|
||||
To purchase a commercial license, please contact ricklan@gmail.com.
|
||||
*/
|
||||
|
||||
#include "selfdrive/ui/qt/offroad/model_selector.h"
|
||||
|
||||
// Define style constants to improve maintainability
|
||||
namespace {
|
||||
const QString SELECTOR_BTN_STYLE = "background-color: #00309a; font-size: 48px;";
|
||||
const QString MODEL_LIST_STYLE = "font-size: 64px;";
|
||||
const QString SCROLLBAR_STYLE = "width: 96px;";
|
||||
const QString GROUP_HEADER_BG_COLOR = "#c8c8c8"; // Light gray
|
||||
const QString GROUP_HEADER_TEXT_COLOR = "#000000"; // Black
|
||||
|
||||
// Role for storing the actual model name without indentation
|
||||
const int ModelNameRole = Qt::UserRole;
|
||||
}
|
||||
|
||||
ModelSelector::ModelSelector(QWidget *parent) : QWidget(parent) {
|
||||
setupUI();
|
||||
setupModelListPanel();
|
||||
connectSignals();
|
||||
}
|
||||
|
||||
QWidget* ModelSelector::setupUI() {
|
||||
QVBoxLayout* main_layout = new QVBoxLayout(this);
|
||||
main_layout->addSpacing(10);
|
||||
|
||||
// Selector button
|
||||
QWidget* model_selector_btn_widget = new QWidget;
|
||||
QHBoxLayout* model_selector_btn_layout = new QHBoxLayout();
|
||||
QLabel* vehicle_model_label = new QLabel(tr("Vehicle Model:"));
|
||||
vehicle_model_label->setStyleSheet("margin-right: 2px; font-size: 48px;");
|
||||
model_selector_btn_layout->addWidget(vehicle_model_label);
|
||||
|
||||
QString model_selected = QString::fromUtf8(Params().get("dp_device_model_selected").c_str());
|
||||
model_selector_btn = new QPushButton(model_selected.isEmpty() ? tr("[AUTO DETECT]") : model_selected);
|
||||
model_selector_btn->setObjectName("ModelSelectorBtn");
|
||||
model_selector_btn->setStyleSheet(SELECTOR_BTN_STYLE);
|
||||
model_selector_btn_layout->addWidget(model_selector_btn);
|
||||
model_selector_btn_layout->setAlignment(Qt::AlignCenter);
|
||||
model_selector_btn_layout->setStretch(1, 1);
|
||||
|
||||
model_selector_btn_widget->setLayout(model_selector_btn_layout);
|
||||
main_layout->addWidget(model_selector_btn_widget);
|
||||
main_layout->addSpacing(10);
|
||||
main_layout->addStretch(); // Add stretch to push everything to the top
|
||||
|
||||
setLayout(main_layout);
|
||||
return model_selector_btn_widget;
|
||||
}
|
||||
|
||||
void ModelSelector::setupModelListPanel() {
|
||||
// Create model list panel
|
||||
model_list_panel = new QWidget();
|
||||
QVBoxLayout* model_list_layout = new QVBoxLayout(model_list_panel);
|
||||
model_list_layout->setContentsMargins(50, 25, 50, 25);
|
||||
|
||||
model_list = new QListWidget(model_list_panel);
|
||||
|
||||
// Set styles using the constants
|
||||
QString listStyle = QString("QListWidget { %1 } QScrollBar:vertical { %2 }")
|
||||
.arg(MODEL_LIST_STYLE)
|
||||
.arg(SCROLLBAR_STYLE);
|
||||
model_list->setStyleSheet(listStyle);
|
||||
|
||||
model_list->setFixedHeight(750);
|
||||
model_list_layout->addWidget(model_list);
|
||||
|
||||
model_list_frame = new ScrollView(model_list_panel, nullptr);
|
||||
}
|
||||
|
||||
void ModelSelector::loadModelList() {
|
||||
if (model_list->count() > 0) {
|
||||
// If list is already populated, just update the selection
|
||||
updateCurrentSelection();
|
||||
return;
|
||||
}
|
||||
|
||||
// Add auto-detect option
|
||||
QListWidgetItem* autoDetectItem = new QListWidgetItem(tr("[AUTO DETECT]"));
|
||||
autoDetectItem->setData(ModelNameRole, tr("[AUTO DETECT]"));
|
||||
model_list->addItem(autoDetectItem);
|
||||
|
||||
Params params;
|
||||
QString model_list_str = QString::fromStdString(params.get("dp_device_model_list"));
|
||||
|
||||
QJsonDocument document = QJsonDocument::fromJson(model_list_str.toUtf8());
|
||||
if (document.isArray()) {
|
||||
QJsonArray models = document.array();
|
||||
|
||||
for (const auto& groupValue : models) {
|
||||
QJsonObject group = groupValue.toObject();
|
||||
QString groupName = group["group"].toString();
|
||||
|
||||
// Add group header item
|
||||
QListWidgetItem* groupHeader = new QListWidgetItem(groupName);
|
||||
groupHeader->setFlags(Qt::NoItemFlags); // Make non-selectable
|
||||
groupHeader->setBackground(QColor(GROUP_HEADER_BG_COLOR));
|
||||
groupHeader->setForeground(QColor(GROUP_HEADER_TEXT_COLOR));
|
||||
groupHeader->setTextAlignment(Qt::AlignCenter);
|
||||
model_list->addItem(groupHeader);
|
||||
|
||||
// Add models in this group
|
||||
QJsonArray groupModels = group["models"].toArray();
|
||||
for (const auto& model : groupModels) {
|
||||
QString modelName = model.toString();
|
||||
// Create item with visual indentation
|
||||
QListWidgetItem* modelItem = new QListWidgetItem(" " + modelName);
|
||||
// Store actual model name without indentation in user role
|
||||
modelItem->setData(ModelNameRole, modelName);
|
||||
model_list->addItem(modelItem);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Set the current selection after loading
|
||||
updateCurrentSelection();
|
||||
}
|
||||
|
||||
// New helper method to update the selection
|
||||
void ModelSelector::updateCurrentSelection() {
|
||||
// Get the currently selected model from params
|
||||
Params params;
|
||||
QString currentModel = QString::fromStdString(params.get("dp_device_model_selected"));
|
||||
|
||||
// If empty, select the AUTO DETECT option
|
||||
if (currentModel.isEmpty()) {
|
||||
model_list->setCurrentRow(0); // AUTO DETECT is the first item
|
||||
return;
|
||||
}
|
||||
|
||||
// Otherwise, find and select the matching model
|
||||
for (int i = 0; i < model_list->count(); i++) {
|
||||
QListWidgetItem* item = model_list->item(i);
|
||||
// Only check selectable items (not group headers)
|
||||
if (item->flags() & Qt::ItemIsSelectable) {
|
||||
QString modelName = item->data(ModelNameRole).toString();
|
||||
if (modelName == currentModel) {
|
||||
model_list->setCurrentItem(item);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ModelSelector::clearModelList() {
|
||||
model_list->clear();
|
||||
}
|
||||
|
||||
void ModelSelector::updateButtonText(const QString& text) {
|
||||
model_selector_btn->setText(text);
|
||||
}
|
||||
|
||||
QWidget* ModelSelector::getModelListPanel() {
|
||||
return model_list_frame;
|
||||
}
|
||||
|
||||
void ModelSelector::setPanelWidget(QStackedWidget* panel) {
|
||||
panel_widget = panel;
|
||||
if (panel_widget && model_list_frame->parent() != panel_widget) {
|
||||
model_list_frame->setParent(panel_widget);
|
||||
panel_widget->addWidget(model_list_frame);
|
||||
}
|
||||
}
|
||||
|
||||
void ModelSelector::setNavButtonGroup(QButtonGroup* buttons) {
|
||||
nav_btns = buttons;
|
||||
}
|
||||
|
||||
void ModelSelector::connectSignals() {
|
||||
connect(model_selector_btn, &QPushButton::clicked, [this]() {
|
||||
if (panel_widget) {
|
||||
// Load the model list when needed
|
||||
loadModelList();
|
||||
panel_widget->setCurrentWidget(model_list_frame);
|
||||
}
|
||||
emit buttonClicked();
|
||||
});
|
||||
|
||||
connect(model_list, &QListWidget::itemClicked, [this](QListWidgetItem* item) {
|
||||
// Only process clicks on selectable items (not group headers)
|
||||
if (item->flags() & Qt::ItemIsSelectable) {
|
||||
// Get model name from the data role rather than trimming text
|
||||
QString model_name = item->data(ModelNameRole).toString();
|
||||
QString param_value = (model_name == tr("[AUTO DETECT]")) ? QString() : model_name;
|
||||
|
||||
// Update param and button text
|
||||
Params().put("dp_device_model_selected", param_value.toStdString());
|
||||
updateButtonText(param_value.isEmpty() ? tr("[AUTO DETECT]") : model_name);
|
||||
|
||||
// Emit signal that model was selected
|
||||
emit modelSelected(model_name);
|
||||
|
||||
// Go back to the previous panel
|
||||
if (nav_btns && nav_btns->checkedButton()) {
|
||||
nav_btns->checkedButton()->click();
|
||||
} else if (nav_btns && nav_btns->buttons().size() > 0) {
|
||||
// Default to first panel if none selected
|
||||
nav_btns->buttons().first()->click();
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
if (model_list_frame) {
|
||||
model_list_frame->installEventFilter(this);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool ModelSelector::eventFilter(QObject *obj, QEvent *event) {
|
||||
if (obj == model_list_frame) {
|
||||
if (event->type() == QEvent::Hide) {
|
||||
clearModelList();
|
||||
}
|
||||
}
|
||||
return QWidget::eventFilter(obj, event);
|
||||
}
|
||||
@@ -0,0 +1,73 @@
|
||||
/*
|
||||
Copyright (c) 2025 Rick Lan
|
||||
|
||||
This software is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License (CC BY-NC-SA 4.0).
|
||||
You are free to share and adapt this work for non-commercial purposes, provided you give appropriate credit and distribute any modifications under the same license.
|
||||
|
||||
To view a copy of this license, visit:
|
||||
http://creativecommons.org/licenses/by-nc-sa/4.0/
|
||||
|
||||
---
|
||||
|
||||
**Commercial Licensing:**
|
||||
Use of this software for commercial purposes is strictly prohibited without a separate, paid license.
|
||||
To purchase a commercial license, please contact ricklan@gmail.com.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <QWidget>
|
||||
#include <QPushButton>
|
||||
#include <QListWidget>
|
||||
#include <QVBoxLayout>
|
||||
#include <QHBoxLayout>
|
||||
#include <QLabel>
|
||||
#include <QJsonDocument>
|
||||
#include <QJsonArray>
|
||||
#include <QJsonObject>
|
||||
#include <QListWidgetItem>
|
||||
#include <QDebug>
|
||||
#include <QStackedWidget>
|
||||
#include <QButtonGroup> // Add this include for QButtonGroup
|
||||
#include <QEvent>
|
||||
#include "common/params.h"
|
||||
#include "selfdrive/ui/qt/widgets/scrollview.h"
|
||||
|
||||
class ModelSelector : public QWidget {
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
explicit ModelSelector(QWidget *parent = nullptr);
|
||||
|
||||
// Get the model list panel widget
|
||||
QWidget* getModelListPanel();
|
||||
|
||||
// Set the panel widget to switch to when selecting models
|
||||
void setPanelWidget(QStackedWidget* panel);
|
||||
|
||||
// Set the button group to return to after model selection
|
||||
void setNavButtonGroup(QButtonGroup* nav_btns);
|
||||
|
||||
signals:
|
||||
void buttonClicked();
|
||||
void modelSelected(const QString& model_name);
|
||||
|
||||
private:
|
||||
QPushButton* model_selector_btn;
|
||||
QListWidget* model_list;
|
||||
QWidget* model_list_panel;
|
||||
ScrollView* model_list_frame;
|
||||
QStackedWidget* panel_widget = nullptr;
|
||||
QButtonGroup* nav_btns = nullptr;
|
||||
|
||||
QWidget* setupUI();
|
||||
void setupModelListPanel();
|
||||
void loadModelList();
|
||||
void updateCurrentSelection();
|
||||
void clearModelList();
|
||||
void connectSignals();
|
||||
void updateButtonText(const QString& text);
|
||||
|
||||
protected:
|
||||
bool eventFilter(QObject *obj, QEvent *event) override;
|
||||
};
|
||||
@@ -16,6 +16,7 @@
|
||||
#include "selfdrive/ui/qt/offroad/developer_panel.h"
|
||||
#include "selfdrive/ui/qt/offroad/firehose.h"
|
||||
#include "selfdrive/ui/qt/offroad/dp_panel.h"
|
||||
#include "selfdrive/ui/qt/offroad/model_selector.h"
|
||||
|
||||
TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
|
||||
// param, title, desc, icon, restart needed
|
||||
@@ -103,8 +104,11 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
|
||||
|
||||
// set up uiState update for personality setting
|
||||
QObject::connect(uiState(), &UIState::uiUpdate, this, &TogglesPanel::updateState);
|
||||
|
||||
const bool lite = getenv("LITE");
|
||||
for (auto &[param, title, desc, icon, needs_restart] : toggle_defs) {
|
||||
if ((param == "AlwaysOnDM" || param == "RecordFront" || param == "RecordAudio" || param == "RecordAudioFeedback") && lite) {
|
||||
continue;
|
||||
}
|
||||
auto toggle = new ParamControl(param, title, desc, icon, this);
|
||||
|
||||
bool locked = params.getBool((param + "Lock").toStdString());
|
||||
@@ -223,6 +227,7 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) {
|
||||
addItem(new LabelControl(tr("Dongle ID"), getDongleId().value_or(tr("N/A"))));
|
||||
addItem(new LabelControl(tr("Serial"), params.get("HardwareSerial").c_str()));
|
||||
|
||||
const bool lite = getenv("LITE");
|
||||
pair_device = new ButtonControl(tr("Pair Device"), tr("PAIR"),
|
||||
tr("Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer."));
|
||||
connect(pair_device, &ButtonControl::clicked, [=]() {
|
||||
@@ -232,12 +237,12 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) {
|
||||
addItem(pair_device);
|
||||
|
||||
// offroad-only buttons
|
||||
|
||||
if (!lite) {
|
||||
auto dcamBtn = new ButtonControl(tr("Driver Camera"), tr("PREVIEW"),
|
||||
tr("Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off)"));
|
||||
connect(dcamBtn, &ButtonControl::clicked, [=]() { emit showDriverView(); });
|
||||
addItem(dcamBtn);
|
||||
|
||||
}
|
||||
resetCalibBtn = new ButtonControl(tr("Reset Calibration"), tr("RESET"), "");
|
||||
connect(resetCalibBtn, &ButtonControl::showDescriptionEvent, this, &DevicePanel::updateCalibDescription);
|
||||
connect(resetCalibBtn, &ButtonControl::clicked, [&]() {
|
||||
@@ -535,7 +540,26 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) {
|
||||
|
||||
sidebar_widget->setFixedWidth(500);
|
||||
main_layout->addWidget(sidebar_widget);
|
||||
main_layout->addWidget(panel_widget);
|
||||
|
||||
// Create right column with model selector on top and panel_widget below
|
||||
QWidget* right_column = new QWidget(this);
|
||||
QVBoxLayout* right_layout = new QVBoxLayout(right_column);
|
||||
right_layout->setContentsMargins(0, 0, 0, 0);
|
||||
right_layout->setSpacing(20); // Space between model selector and panel
|
||||
|
||||
// Create the ModelSelector button at the top of right column
|
||||
ModelSelector* model_selector = new ModelSelector(this);
|
||||
right_layout->addWidget(model_selector);
|
||||
|
||||
// Set up panel widget and nav button references
|
||||
model_selector->setPanelWidget(panel_widget);
|
||||
model_selector->setNavButtonGroup(nav_btns);
|
||||
|
||||
// Add panel_widget below the model selector
|
||||
right_layout->addWidget(panel_widget, 1); // Give panel_widget stretch priority
|
||||
|
||||
// Add right column to main layout
|
||||
main_layout->addWidget(right_column);
|
||||
|
||||
setStyleSheet(R"(
|
||||
* {
|
||||
|
||||
@@ -95,6 +95,7 @@ private:
|
||||
QLabel *onroadLbl;
|
||||
LabelControl *versionLbl;
|
||||
ButtonControl *installBtn;
|
||||
ButtonControl *onOffRoadBtn;
|
||||
ButtonControl *downloadBtn;
|
||||
ButtonControl *targetBranchBtn;
|
||||
|
||||
|
||||
@@ -29,6 +29,16 @@ SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) {
|
||||
versionLbl = new LabelControl(tr("Current Version"), "");
|
||||
addItem(versionLbl);
|
||||
|
||||
// on/off road mode switch
|
||||
onOffRoadBtn = new ButtonControl(tr("Onroad/Offroad Mode"), tr("Go Offroad"));
|
||||
connect(onOffRoadBtn, &ButtonControl::clicked, [&]() {
|
||||
if (ConfirmationDialog::confirm(tr("Are you sure you want to switch mode?"), tr("CONFIRM"), this)) {
|
||||
bool val = params.getBool("dp_device_go_off_road");
|
||||
params.putBool("dp_device_go_off_road", !val);
|
||||
}
|
||||
});
|
||||
addItem(onOffRoadBtn);
|
||||
|
||||
// download update btn
|
||||
downloadBtn = new ButtonControl(tr("Download"), tr("CHECK"));
|
||||
connect(downloadBtn, &ButtonControl::clicked, [=]() {
|
||||
@@ -111,6 +121,7 @@ void SoftwarePanel::updateLabels() {
|
||||
fs_watch->addParam("UpdateFailedCount");
|
||||
fs_watch->addParam("UpdaterState");
|
||||
fs_watch->addParam("UpdateAvailable");
|
||||
fs_watch->addParam("dp_device_go_off_road");
|
||||
|
||||
if (!isVisible()) {
|
||||
return;
|
||||
@@ -120,6 +131,13 @@ void SoftwarePanel::updateLabels() {
|
||||
onroadLbl->setVisible(is_onroad);
|
||||
downloadBtn->setVisible(!is_onroad);
|
||||
|
||||
// on/off road text change
|
||||
if (params.getBool("dp_device_go_off_road")) {
|
||||
onOffRoadBtn->setText(tr("Go Onroad"));
|
||||
} else {
|
||||
onOffRoadBtn->setText(tr("Go Offroad"));
|
||||
}
|
||||
|
||||
// download update
|
||||
QString updater_state = QString::fromStdString(params.get("UpdaterState"));
|
||||
bool failed = std::atoi(params.get("UpdateFailedCount").c_str()) > 0;
|
||||
|
||||
@@ -24,7 +24,9 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget *par
|
||||
void AnnotatedCameraWidget::updateState(const UIState &s) {
|
||||
// update engageability/experimental mode button
|
||||
experimental_btn->updateState(s);
|
||||
dmon.updateState(s);
|
||||
if (!s.scene.lite) {
|
||||
dmon.updateState(s);
|
||||
}
|
||||
}
|
||||
|
||||
void AnnotatedCameraWidget::initializeGL() {
|
||||
@@ -130,9 +132,17 @@ void AnnotatedCameraWidget::paintGL() {
|
||||
painter.setPen(Qt::NoPen);
|
||||
|
||||
model.draw(painter, rect());
|
||||
dmon.draw(painter, rect());
|
||||
hud.updateState(*s);
|
||||
hud.draw(painter, rect());
|
||||
bool hide_hud = s->scene.dp_ui_hide_hud_speed_kph > 0 && sm["carState"].getCarState().getVEgo() > s->scene.dp_ui_hide_hud_speed_kph * 0.278;
|
||||
if (!hide_hud) {
|
||||
if (!s->scene.lite) {
|
||||
dmon.draw(painter, rect());
|
||||
}
|
||||
hud.updateState(*s);
|
||||
hud.draw(painter, rect());
|
||||
experimental_btn->setVisible(true);
|
||||
} else {
|
||||
experimental_btn->setVisible(false);
|
||||
}
|
||||
|
||||
double cur_draw_t = millis_since_boot();
|
||||
double dt = cur_draw_t - prev_draw_t;
|
||||
|
||||
@@ -48,6 +48,11 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) {
|
||||
}
|
||||
}
|
||||
|
||||
if (s->scene.dp_ui_radar_tracks) {
|
||||
const auto &live_tracks = sm["liveTracks"].getLiveTracks();
|
||||
drawLiveTracks(painter, live_tracks, model, surface_rect);
|
||||
}
|
||||
|
||||
painter.restore();
|
||||
}
|
||||
|
||||
@@ -107,7 +112,39 @@ void ModelRenderer::drawLaneLines(QPainter &painter) {
|
||||
|
||||
void ModelRenderer::drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height) {
|
||||
QLinearGradient bg(0, height, 0, 0);
|
||||
if (experimental_mode) {
|
||||
|
||||
auto *s = uiState();
|
||||
if (s->scene.dp_ui_rainbow) {
|
||||
constexpr int NUM_COLORS = 25;
|
||||
constexpr int ALPHA = 128;
|
||||
|
||||
float v_ego = (*uiState()->sm)["carState"].getCarState().getVEgo();
|
||||
|
||||
if (!dp_rainbow_init) {
|
||||
dp_rainbow_color_list.reserve(NUM_COLORS);
|
||||
for (int i = 0; i < NUM_COLORS; ++i) {
|
||||
qreal t = static_cast<qreal>(i) / (NUM_COLORS - 1);
|
||||
dp_rainbow_color_list.append(QColor::fromHsvF(t, 1.0, 1.0, ALPHA / 255.0));
|
||||
}
|
||||
dp_rainbow_init = true;
|
||||
}
|
||||
bg.setSpread(QGradient::RepeatSpread);
|
||||
// bigger = faster, however it is still limited to the global UI_FREQ (refresh rate)
|
||||
// only way to make it move faster is to reduce NUM_COLORS, but that will also reduce the color smoothness.
|
||||
qreal rotation_speed = std::max(0.01f, v_ego) / UI_FREQ;
|
||||
dp_rainbow_rotation -= rotation_speed;
|
||||
|
||||
if (dp_rainbow_rotation < 0.0) {
|
||||
dp_rainbow_rotation += 1.0;
|
||||
dp_rainbow_color_list.append(dp_rainbow_color_list.takeFirst());
|
||||
}
|
||||
// fill color
|
||||
const qreal step = 1.0 / (NUM_COLORS - 1);
|
||||
for (int i = 0; i < NUM_COLORS; ++i) {
|
||||
bg.setColorAt(i * step, dp_rainbow_color_list.at(i));
|
||||
}
|
||||
|
||||
} else if (experimental_mode) {
|
||||
// The first half of track_vertices are the points for the right side of the path
|
||||
const auto &acceleration = model.getAcceleration().getX();
|
||||
const int max_len = std::min<int>(track_vertices.length() / 2, acceleration.size());
|
||||
@@ -186,6 +223,56 @@ QColor ModelRenderer::blendColors(const QColor &start, const QColor &end, float
|
||||
(1 - t) * start.alphaF() + t * end.alphaF());
|
||||
}
|
||||
|
||||
void ModelRenderer::drawLiveTracks(QPainter &painter,
|
||||
const cereal::RadarData::Reader &live_tracks,
|
||||
const cereal::ModelDataV2::Reader &model_data,
|
||||
const QRect &surface_rect) {
|
||||
|
||||
// Get the model's predicted path for Z-coordinate calculation
|
||||
const auto& model_path_position = model_data.getPosition();
|
||||
|
||||
// Set text properties
|
||||
painter.setPen(Qt::white);
|
||||
painter.setFont(QFont("Inter", 24, QFont::Bold));
|
||||
|
||||
// Iterate through each radar point from live_tracks
|
||||
for (const auto& point : live_tracks.getPoints()) {
|
||||
float dRel = point.getDRel();
|
||||
float yRel = point.getYRel();
|
||||
float yvRel = point.getYvRel();
|
||||
float vRel = point.getVRel();
|
||||
|
||||
// Calculate Z-coordinate using the model's path
|
||||
float z_on_path = path_offset_z; // Default base offset
|
||||
|
||||
// Ensure dRel is non-negative for indexing
|
||||
if (dRel >= 0) {
|
||||
z_on_path += model_path_position.getZ()[get_path_length_idx(model_path_position, dRel)];
|
||||
}
|
||||
|
||||
QPointF screen_pos;
|
||||
// mapToScreen projects a point from car space to screen space
|
||||
if (mapToScreen(dRel, -yRel, z_on_path, &screen_pos)) { // yRel is negated as in update_leads
|
||||
// Basic drawing: Draw a small circle for the point
|
||||
painter.setBrush(QColor(255, 0, 0, 200)); // Cyan color for live tracks
|
||||
painter.drawEllipse(screen_pos, 10, 10); // Draw a small circle of radius 5
|
||||
|
||||
// Prepare text to display
|
||||
QString infoText = QString("ID: %1\nd: %2 m\ny: %3 m\ndV: %4 m/s\nyV: %5 m/s")
|
||||
.arg(point.getTrackId())
|
||||
.arg(dRel, 0, 'f', 2)
|
||||
.arg(yRel, 0, 'f', 2)
|
||||
.arg(vRel, 0, 'f', 2)
|
||||
.arg(yvRel, 0, 'f', 2);
|
||||
|
||||
// Draw text near the point
|
||||
// Adjust text position for better visibility (e.g., slightly offset from the point)
|
||||
QRectF textRect(screen_pos.x() + 10, screen_pos.y() - 20, 250, 250); // Adjust size as needed
|
||||
painter.drawText(textRect, Qt::AlignLeft, infoText);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data,
|
||||
const QPointF &vd, const QRect &surface_rect) {
|
||||
const float speedBuff = 10.;
|
||||
|
||||
@@ -15,6 +15,7 @@ private:
|
||||
bool mapToScreen(float in_x, float in_y, float in_z, QPointF *out);
|
||||
void mapLineToPolygon(const cereal::XYZTData::Reader &line, float y_off, float z_off,
|
||||
QPolygonF *pvd, int max_idx, bool allow_invert = true);
|
||||
void drawLiveTracks(QPainter &painter, const cereal::RadarData::Reader &live_tracks, const cereal::ModelDataV2::Reader &model_data, const QRect &surface_rect);
|
||||
void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd, const QRect &surface_rect);
|
||||
void update_leads(const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line);
|
||||
void update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead);
|
||||
@@ -36,4 +37,7 @@ private:
|
||||
QPointF lead_vertices[2] = {};
|
||||
Eigen::Matrix3f car_space_transform = Eigen::Matrix3f::Zero();
|
||||
QRectF clip_region;
|
||||
QVector<QColor> dp_rainbow_color_list;
|
||||
qreal dp_rainbow_rotation = 0;
|
||||
bool dp_rainbow_init = false;
|
||||
};
|
||||
|
||||
@@ -39,16 +39,48 @@ OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) {
|
||||
QObject::connect(uiState(), &UIState::offroadTransition, this, &OnroadWindow::offroadTransition);
|
||||
}
|
||||
|
||||
void OnroadWindow::updateDpIndicatorSideState(bool blinker_state, bool bsm_state, bool &show, bool &show_prev, int &count, QColor &color) {
|
||||
if (!blinker_state && !bsm_state) {
|
||||
show = false;
|
||||
count = 0;
|
||||
} else {
|
||||
count += 1;
|
||||
}
|
||||
if (bsm_state && blinker_state) {
|
||||
show = count % DP_INDICATOR_BLINK_RATE_FAST == 0? !show : show;
|
||||
color = DP_INDICATOR_COLOR_BSM;
|
||||
} else if (blinker_state) {
|
||||
show = count % DP_INDICATOR_BLINK_RATE_STD == 0? !show : show;
|
||||
color = DP_INDICATOR_COLOR_BLINKER;
|
||||
} else if (bsm_state) {
|
||||
show = true;
|
||||
color = DP_INDICATOR_COLOR_BSM;
|
||||
} else {
|
||||
show = false;
|
||||
}
|
||||
}
|
||||
|
||||
void OnroadWindow::updateDpIndicatorStates(const UIState &s) {
|
||||
const auto cs = (*s.sm)["carState"].getCarState();
|
||||
updateDpIndicatorSideState(cs.getLeftBlinker(), cs.getLeftBlindspot(), dp_indicator_show_left, dp_indicator_show_left_prev, dp_indicator_count_left, dp_indicator_color_left);
|
||||
updateDpIndicatorSideState(cs.getRightBlinker(), cs.getRightBlindspot(), dp_indicator_show_right, dp_indicator_show_right_prev, dp_indicator_count_right, dp_indicator_color_right);
|
||||
}
|
||||
|
||||
void OnroadWindow::updateState(const UIState &s) {
|
||||
if (!s.scene.started) {
|
||||
return;
|
||||
}
|
||||
|
||||
dp_indicator_show_left_prev = dp_indicator_show_left;
|
||||
dp_indicator_show_right_prev = dp_indicator_show_right;
|
||||
updateDpIndicatorStates(s);
|
||||
bool indicator_states_changed = dp_indicator_show_left != dp_indicator_show_left_prev || dp_indicator_show_right != dp_indicator_show_right_prev;
|
||||
|
||||
alerts->updateState(s);
|
||||
nvg->updateState(s);
|
||||
|
||||
QColor bgColor = bg_colors[s.status];
|
||||
if (bg != bgColor) {
|
||||
QColor bgColor = bg_colors[s.scene.alka_active && s.status == STATUS_DISENGAGED? STATUS_ALKA : s.status];
|
||||
if (bg != bgColor || indicator_states_changed) {
|
||||
// repaint border
|
||||
bg = bgColor;
|
||||
update();
|
||||
@@ -61,5 +93,7 @@ void OnroadWindow::offroadTransition(bool offroad) {
|
||||
|
||||
void OnroadWindow::paintEvent(QPaintEvent *event) {
|
||||
QPainter p(this);
|
||||
p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255));
|
||||
p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 180));
|
||||
if (dp_indicator_show_left) p.fillRect(QRect(0, 0, width() * 0.2, height()), dp_indicator_color_left);
|
||||
if (dp_indicator_show_right) p.fillRect(QRect(width() * 0.8, 0, width() * 0.2, height()), dp_indicator_color_right);
|
||||
}
|
||||
|
||||
@@ -1,11 +1,18 @@
|
||||
#pragma once
|
||||
|
||||
#include <QColor>
|
||||
|
||||
#include "selfdrive/ui/qt/onroad/alerts.h"
|
||||
#include "selfdrive/ui/qt/onroad/annotated_camera.h"
|
||||
|
||||
class OnroadWindow : public QWidget {
|
||||
Q_OBJECT
|
||||
|
||||
const int DP_INDICATOR_BLINK_RATE_STD = 8;
|
||||
const int DP_INDICATOR_BLINK_RATE_FAST = 4;
|
||||
const QColor DP_INDICATOR_COLOR_BLINKER = QColor(0, 0xff, 0, 255);
|
||||
const QColor DP_INDICATOR_COLOR_BSM = QColor(0xff, 0xff, 0, 255);
|
||||
|
||||
public:
|
||||
OnroadWindow(QWidget* parent = 0);
|
||||
|
||||
@@ -16,6 +23,19 @@ private:
|
||||
QColor bg = bg_colors[STATUS_DISENGAGED];
|
||||
QHBoxLayout* split;
|
||||
|
||||
void updateDpIndicatorSideState(bool blinker_state, bool bsm_state, bool &show, bool &show_prev, int &count, QColor &color);
|
||||
void updateDpIndicatorStates(const UIState &s);
|
||||
// left
|
||||
int dp_indicator_count_left = 0;
|
||||
QColor dp_indicator_color_left = DP_INDICATOR_COLOR_BLINKER;
|
||||
bool dp_indicator_show_left = false;
|
||||
bool dp_indicator_show_left_prev = false;
|
||||
// right
|
||||
int dp_indicator_count_right = 0;
|
||||
QColor dp_indicator_color_right = DP_INDICATOR_COLOR_BLINKER;
|
||||
bool dp_indicator_show_right = false;
|
||||
bool dp_indicator_show_right_prev = false;
|
||||
|
||||
private slots:
|
||||
void offroadTransition(bool offroad);
|
||||
void updateState(const UIState &s);
|
||||
|
||||
@@ -27,7 +27,8 @@ QString getVersion() {
|
||||
}
|
||||
|
||||
QString getBrand() {
|
||||
return QObject::tr("dragonpilot");
|
||||
const bool lite = getenv("LITE");
|
||||
return QObject::tr("dragonpilot") + (lite ? QString::fromStdString(" - Lite") : QString(""));
|
||||
}
|
||||
|
||||
QString getUserAgent() {
|
||||
|
||||
@@ -10,6 +10,7 @@ from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.realtime import Ratekeeper
|
||||
from openpilot.common.retry import retry
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.params import Params
|
||||
|
||||
from openpilot.system import micd
|
||||
|
||||
@@ -62,6 +63,11 @@ class Soundd:
|
||||
|
||||
self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False)
|
||||
|
||||
try:
|
||||
self._dp_device_audible_alert_mode = int(Params().get("dp_device_audible_alert_mode"))
|
||||
except:
|
||||
self._dp_device_audible_alert_mode = 0
|
||||
|
||||
def load_sounds(self):
|
||||
self.loaded_sounds: dict[int, np.ndarray] = {}
|
||||
|
||||
@@ -96,6 +102,10 @@ class Soundd:
|
||||
written_frames += frames_to_write
|
||||
self.current_sound_frame += frames_to_write
|
||||
|
||||
# dp - set vol to 0 instead
|
||||
if self._dp_device_audible_alert_mode == 2 or (self._dp_device_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:
|
||||
|
||||
+65
-1
@@ -60,6 +60,7 @@ static void update_state(UIState *s) {
|
||||
scene.light_sensor = -1;
|
||||
}
|
||||
scene.started = sm["deviceState"].getDeviceState().getStarted() && scene.ignition;
|
||||
scene.alka_active = sm["dpControlsState"].getDpControlsState().getAlkaActive();
|
||||
|
||||
auto params = Params();
|
||||
scene.recording_audio = params.getBool("RecordAudio") && scene.started;
|
||||
@@ -68,6 +69,11 @@ static void update_state(UIState *s) {
|
||||
void ui_update_params(UIState *s) {
|
||||
auto params = Params();
|
||||
s->scene.is_metric = params.getBool("IsMetric");
|
||||
s->scene.lite = getenv("LITE");
|
||||
s->scene.display_mode = std::atoi(params.get("dp_ui_display_mode").c_str());
|
||||
s->scene.dp_ui_hide_hud_speed_kph = std::atoi(params.get("dp_ui_hide_hud_speed_kph").c_str());
|
||||
s->scene.dp_ui_rainbow = params.getBool("dp_ui_rainbow");
|
||||
s->scene.dp_ui_radar_tracks = params.getBool("dp_ui_radar_tracks");
|
||||
}
|
||||
|
||||
void UIState::updateStatus() {
|
||||
@@ -102,6 +108,8 @@ UIState::UIState(QObject *parent) : QObject(parent) {
|
||||
"modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
|
||||
"pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2",
|
||||
"wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
|
||||
"dpControlsState",
|
||||
"liveTracks",
|
||||
});
|
||||
prime_state = new PrimeState(this);
|
||||
language = QString::fromStdString(Params().get("LanguageSetting"));
|
||||
@@ -180,6 +188,62 @@ void Device::updateBrightness(const UIState &s) {
|
||||
}
|
||||
}
|
||||
|
||||
// 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
|
||||
bool Device::applyDisplayMode(const UIState &s, int timeout) {
|
||||
// standard
|
||||
if (s.scene.display_mode == 0 || !s.scene.ignition) {
|
||||
return (s.scene.ignition || timeout > 0);
|
||||
}
|
||||
|
||||
bool cruise_available = false;
|
||||
bool cruise_enabled = false;
|
||||
|
||||
auto &sm = *(s.sm);
|
||||
if (sm.updated("carState")) {
|
||||
auto cs = sm["carState"].getCarState().getCruiseState();
|
||||
cruise_available = cs.getAvailable();
|
||||
cruise_enabled = cs.getEnabled();
|
||||
}
|
||||
|
||||
if (sm["selfdriveState"].getSelfdriveState().getAlertSize() != cereal::SelfdriveState::AlertSize::NONE) {
|
||||
resetInteractiveTimeout(5);
|
||||
return true;
|
||||
}
|
||||
|
||||
// 1 MAIN+ - ACC MAIN on = Display ON
|
||||
if (s.scene.display_mode == 1 && cruise_available) {
|
||||
return s.scene.ignition;
|
||||
}
|
||||
|
||||
// 2 OP+ - OP enabled = Display ON
|
||||
if (s.scene.display_mode == 2 && cruise_enabled) {
|
||||
return s.scene.ignition;
|
||||
}
|
||||
|
||||
// 3 MAIN- - ACC MAIN on = Display OFF
|
||||
if (s.scene.display_mode == 3 && cruise_available) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 4 OP- - OP enabled = Display OFF
|
||||
if (s.scene.display_mode == 4 && cruise_enabled) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (s.scene.display_mode >= 3) {
|
||||
// 3,4
|
||||
return s.scene.ignition;
|
||||
} else {
|
||||
// 1,2
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void Device::updateWakefulness(const UIState &s) {
|
||||
bool ignition_just_turned_off = !s.scene.ignition && ignition_on;
|
||||
ignition_on = s.scene.ignition;
|
||||
@@ -190,7 +254,7 @@ void Device::updateWakefulness(const UIState &s) {
|
||||
emit interactiveTimeout();
|
||||
}
|
||||
|
||||
setAwake(s.scene.ignition || interactive_timeout > 0);
|
||||
setAwake(applyDisplayMode(s, interactive_timeout));
|
||||
}
|
||||
|
||||
UIState *uiState() {
|
||||
|
||||
@@ -42,12 +42,14 @@ typedef enum UIStatus {
|
||||
STATUS_DISENGAGED,
|
||||
STATUS_OVERRIDE,
|
||||
STATUS_ENGAGED,
|
||||
STATUS_ALKA,
|
||||
} UIStatus;
|
||||
|
||||
const QColor bg_colors [] = {
|
||||
[STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8),
|
||||
[STATUS_OVERRIDE] = QColor(0x91, 0x9b, 0x95, 0xf1),
|
||||
[STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1),
|
||||
[STATUS_ALKA] = QColor(0x22, 0xa0, 0xdc, 0xf1),
|
||||
};
|
||||
|
||||
typedef struct UIScene {
|
||||
@@ -60,6 +62,12 @@ typedef struct UIScene {
|
||||
float light_sensor = -1;
|
||||
bool started, ignition, is_metric, recording_audio;
|
||||
uint64_t started_frame;
|
||||
bool lite = false;
|
||||
bool alka_active = false;
|
||||
int display_mode = 0;
|
||||
int dp_ui_hide_hud_speed_kph = 0;
|
||||
bool dp_ui_rainbow = false;
|
||||
bool dp_ui_radar_tracks = false;
|
||||
} UIScene;
|
||||
|
||||
class UIState : public QObject {
|
||||
@@ -115,6 +123,7 @@ private:
|
||||
FirstOrderFilter brightness_filter;
|
||||
QFuture<void> brightness_future;
|
||||
|
||||
bool applyDisplayMode(const UIState &s, int timeout);
|
||||
void updateBrightness(const UIState &s);
|
||||
void updateWakefulness(const UIState &s);
|
||||
void setAwake(bool on);
|
||||
|
||||
Reference in New Issue
Block a user