feat: Squash all min-features into full

This commit is contained in:
Rick Lan
2025-09-05 11:07:32 +08:00
parent 028b6e5664
commit 5e0f34905d
75 changed files with 4041 additions and 83 deletions
+11
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
@@ -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
+14 -2
View File
@@ -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
+20 -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,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
+67 -6
View File
@@ -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])
+6 -1
View File
@@ -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']:
+12 -3
View File
@@ -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
+142
View File
@@ -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()
+5 -4
View File
@@ -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;
}
}
+10 -5
View File
@@ -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:
+3 -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,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
+1
View File
@@ -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",
+119
View File
@@ -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"));
}
}
+2
View File
@@ -28,4 +28,6 @@ private:
void add_device_toggles();
void updateStates();
void showEvent(QShowEvent *event) override;
ParamDoubleSpinBoxControl* lca_sec_toggle;
};
+230
View File
@@ -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);
}
+73
View File
@@ -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;
};
+28 -4
View File
@@ -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"(
* {
+1
View File
@@ -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;
+14 -4
View File
@@ -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;
+88 -1
View File
@@ -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.;
+4
View File
@@ -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;
};
+37 -3
View File
@@ -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);
}
+20
View File
@@ -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);
+2 -1
View File
@@ -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
View File
@@ -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
View File
@@ -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() {
+9
View File
@@ -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);