brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
Your Name
2024-05-11 15:27:31 -05:00
parent bc2a4e2266
commit 4373a0198f
11 changed files with 303 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
clearpilot_devqueue_amain.txt
View File

@@ -1,3 +1,32 @@
- Swap dark blue always on color for gray
- totally remove path when not engaged
- hello world bootstrap dashboard
- button stuff:
- read paddle left (for full nudgeless lane change + blinker, reset to drive)
- read paddle right ("")
- write pause, res, accel, mode, drive
OP -> Oscar
- oscar - global clearpilot state var
- oscar.cs - clearpilot car state (populated by op variables for cp.planner)
- oscar.ce - clearpilot car event queue (array that can be pushed to from car)
- oscar.ceh - event queue history (for debug)
- oscar.clog - error and console.log style one off messages to transmit to logs or watching consoles
Oscar -> OP
- oscar.ps - clearpilot planner state (data from cp)
- oscar.pe - clearpilot planner event (one time events from planner to be processed by openpilot)
- oscar.peh - planner event history (for debug)
- oscar.plog - retransmits clog as well as additional data for UI based log consoles
- oscar - clearpilot's planner - nodejs process that synchronizes state between all processes and makes decisions
- settings menu
- - branch selector - release, stage, dev. Stage is my personal release branch. Should always be able to easilly switch to stage while im in a development state.
-------
- make functions - make functions
--- get_curvature --- get_curvature
--- get_wheel_angle --- get_wheel_angle

12
selfdrive/car/card.py
View File

@@ -18,6 +18,18 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase
REPLAY = "REPLAY" in os.environ REPLAY = "REPLAY" in os.environ
# Notes:
# This is a generic class for interfacing between controlsd and specific implementations for a car.
# Variables:
# CI - carInterfaceBase
# CP - carParams
# CS - car state, should be consistent between every type of car
# Methods:
# state_update - calls CI.update, gets a new CS. Read only, this is regenerated every time called.
# state_publish - reads CS, CP, and carOutput.actuatorsOutput and sends via self.pm messaging
# controls_update - reads CI.apply, gets canbus commands, publishes via self.pm, something else executes canbus commands
class CarD: class CarD:
CI: CarInterfaceBase CI: CarInterfaceBase

2
selfdrive/car/hyundai/carstate.py
View File

@@ -311,8 +311,6 @@ class CarState(CarStateBase):
self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0 self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0
self.lkas_previously_enabled = self.lkas_enabled self.lkas_previously_enabled = self.lkas_enabled
# CLEARPILOT fix
lkas_enabled = False lkas_enabled = False
try: try:
lkas_enabled = cp.vl[self.cruise_btns_msg_canfd]["LFA_BTN"] lkas_enabled = cp.vl[self.cruise_btns_msg_canfd]["LFA_BTN"]

14
selfdrive/car/interfaces.py
View File

@@ -22,6 +22,20 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS, V_
from openpilot.selfdrive.controls.lib.events import Events from openpilot.selfdrive.controls.lib.events import Events
from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
# Notes:
# CarInterfaceBase
# Has multiple functions. Haven't figured them all out yet.
# It turns car state into events.
# It passes calls to update() to the specific cars interface.py (get car state)
# It passes calls to apply() to the specific cars interface.py (apply car state, gen canbus commands)
# CarStateBase
# carstate is variable (not event) based tracking of the current properties of the car
# such as which buttons are pressed or if blinkers are enabled. It uses previous frame
# to detect transitions (previous lkas button vs lkas button, if not equal, button just pressed)
# - init - defines default values for carstate.
ButtonType = car.CarState.ButtonEvent.Type ButtonType = car.CarState.ButtonEvent.Type
GearShifter = car.CarState.GearShifter GearShifter = car.CarState.GearShifter
EventName = car.CarEvent.EventName EventName = car.CarEvent.EventName

0
selfdrive/clearpilot/api/api.js → selfdrive/clearpilot/__init__.py
View File

0
selfdrive/clearpilot/manager/api.js Normal file
View File

0
selfdrive/clearpilot/readme.txt Normal file
View File

363
selfdrive/controls/controlsd.py
View File

@@ -223,6 +223,84 @@ class Controls:
if any(ps.controlsAllowed for ps in self.sm['pandaStates']): if any(ps.controlsAllowed for ps in self.sm['pandaStates']):
self.state = State.enabled self.state = State.enabled
def step(self):
start_time = time.monotonic()
# Sample data from sockets and get a carState
CS = self.data_sample()
cloudlog.timestamp("Data sampled")
self.update_events(CS)
self.update_frogpilot_events(CS)
self.update_clearpilot_events(CS)
cloudlog.timestamp("Events updated")
if not self.CP.passive and self.initialized:
# Update control state
self.state_transition(CS)
# Compute actuators (runs PID loops and lateral MPC)
CC, lac_log = self.state_control(CS)
CC = self.clearpilot_state_control(CC, CS)
# Publish data
self.publish_logs(CS, start_time, CC, lac_log)
self.CS_prev = CS
# Update FrogPilot variables
self.update_frogpilot_variables(CS)
def data_sample(self):
"""Receive data from sockets and update carState"""
CS = self.card.state_update(self.frogpilot_variables)
self.sm.update(0)
if not self.initialized:
all_valid = CS.canValid and self.sm.all_checks()
timed_out = self.sm.frame * DT_CTRL > 6.
if all_valid or timed_out or (SIMULATION and not REPLAY):
available_streams = VisionIpcClient.available_streams("camerad", block=False)
if VisionStreamType.VISION_STREAM_ROAD not in available_streams:
self.sm.ignore_alive.append('roadCameraState')
if VisionStreamType.VISION_STREAM_WIDE_ROAD not in available_streams:
self.sm.ignore_alive.append('wideRoadCameraState')
if not self.CP.passive:
self.card.initialize()
self.initialized = True
self.set_initial_state()
self.params.put_bool_nonblocking("ControlsReady", True)
cloudlog.event(
"controlsd.initialized",
dt=self.sm.frame*DT_CTRL,
timeout=timed_out,
canValid=CS.canValid,
invalid=[s for s, valid in self.sm.valid.items() if not valid],
not_alive=[s for s, alive in self.sm.alive.items() if not alive],
not_freq_ok=[s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok],
error=True,
)
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not self.enabled:
self.mismatch_counter = 0
# All pandas not in silent mode must have controlsAllowed when openpilot is enabled
if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates']
if ps.safetyModel not in IGNORED_SAFETY_MODES):
self.mismatch_counter += 1
return CS
def update_events(self, CS): def update_events(self, CS):
"""Compute onroadEvents from carState""" """Compute onroadEvents from carState"""
@@ -431,10 +509,10 @@ class Controls:
planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
if planner_fcw or model_fcw: if planner_fcw or model_fcw:
self.events.add(EventName.fcw) self.events.add(EventName.fcw)
self.fcw_random_event_triggered = True # self.fcw_random_event_triggered = True
elif self.fcw_random_event_triggered and self.random_events: # elif self.fcw_random_event_triggered and self.random_events:
self.events.add(EventName.yourFrogTriedToKillMe) # self.events.add(EventName.yourFrogTriedToKillMe)
self.fcw_random_event_triggered = False # self.fcw_random_event_triggered = False
for m in messaging.drain_sock(self.log_sock, wait_for_one=False): for m in messaging.drain_sock(self.log_sock, wait_for_one=False):
try: try:
@@ -448,6 +526,7 @@ class Controls:
pass pass
# TODO: fix simulator # TODO: fix simulator
# CLEARPILOT fix me:
if not SIMULATION or REPLAY: if not SIMULATION or REPLAY:
# Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
if not self.sm['liveLocationKalman'].gpsOK and self.sm['liveLocationKalman'].inputsOK and (self.distance_traveled > 1000): if not self.sm['liveLocationKalman'].gpsOK and self.sm['liveLocationKalman'].inputsOK and (self.distance_traveled > 1000):
@@ -459,56 +538,6 @@ class Controls:
if self.sm['modelV2'].frameDropPerc > 20: if self.sm['modelV2'].frameDropPerc > 20:
self.events.add(EventName.modeldLagging) self.events.add(EventName.modeldLagging)
self.update_frogpilot_events(CS)
def data_sample(self):
"""Receive data from sockets and update carState"""
CS = self.card.state_update(self.frogpilot_variables)
self.sm.update(0)
if not self.initialized:
all_valid = CS.canValid and self.sm.all_checks()
timed_out = self.sm.frame * DT_CTRL > 6.
if all_valid or timed_out or (SIMULATION and not REPLAY):
available_streams = VisionIpcClient.available_streams("camerad", block=False)
if VisionStreamType.VISION_STREAM_ROAD not in available_streams:
self.sm.ignore_alive.append('roadCameraState')
if VisionStreamType.VISION_STREAM_WIDE_ROAD not in available_streams:
self.sm.ignore_alive.append('wideRoadCameraState')
if not self.CP.passive:
self.card.initialize()
self.initialized = True
self.set_initial_state()
self.params.put_bool_nonblocking("ControlsReady", True)
cloudlog.event(
"controlsd.initialized",
dt=self.sm.frame*DT_CTRL,
timeout=timed_out,
canValid=CS.canValid,
invalid=[s for s, valid in self.sm.valid.items() if not valid],
not_alive=[s for s, alive in self.sm.alive.items() if not alive],
not_freq_ok=[s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok],
error=True,
)
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not self.enabled:
self.mismatch_counter = 0
# All pandas not in silent mode must have controlsAllowed when openpilot is enabled
if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates']
if ps.safetyModel not in IGNORED_SAFETY_MODES):
self.mismatch_counter += 1
return CS
def state_transition(self, CS): def state_transition(self, CS):
"""Compute conditional state transitions and execute actions on state transitions""" """Compute conditional state transitions and execute actions on state transitions"""
@@ -705,12 +734,12 @@ class Controls:
good_speed = CS.vEgo > 5 good_speed = CS.vEgo > 5
max_torque = abs(actuators.steer) > 0.99 max_torque = abs(actuators.steer) > 0.99
if undershooting and turning and good_speed and max_torque and not self.random_event_triggered: if undershooting and turning and good_speed and max_torque and not self.random_event_triggered:
if self.sm.frame % 10000 == 0 and self.random_events: lac_log.active and self.events.add(EventName.goatSteerSaturated if self.goat_scream else EventName.steerSaturated)
lac_log.active and self.events.add(EventName.firefoxSteerSaturated) # if self.sm.frame % 10000 == 0 and self.random_events:
self.params_memory.put_int("CurrentRandomEvent", 1) # lac_log.active and self.events.add(EventName.firefoxSteerSaturated)
self.random_event_triggered = True # self.params_memory.put_int("CurrentRandomEvent", 1)
else: # self.random_event_triggered = True
lac_log.active and self.events.add(EventName.goatSteerSaturated if self.goat_scream else EventName.steerSaturated) # else:
elif lac_log.saturated: elif lac_log.saturated:
# TODO probably should not use dpath_points but curvature # TODO probably should not use dpath_points but curvature
dpath_points = model_v2.position.y dpath_points = model_v2.position.y
@@ -896,30 +925,6 @@ class Controls:
# copy CarControl to pass to CarInterface on the next iteration # copy CarControl to pass to CarInterface on the next iteration
self.CC = CC self.CC = CC
def step(self):
start_time = time.monotonic()
# Sample data from sockets and get a carState
CS = self.data_sample()
cloudlog.timestamp("Data sampled")
self.update_events(CS)
cloudlog.timestamp("Events updated")
if not self.CP.passive and self.initialized:
# Update control state
self.state_transition(CS)
# Compute actuators (runs PID loops and lateral MPC)
CC, lac_log = self.state_control(CS)
# Publish data
self.publish_logs(CS, start_time, CC, lac_log)
self.CS_prev = CS
# Update FrogPilot variables
self.update_frogpilot_variables(CS)
def read_personality_param(self): def read_personality_param(self):
try: try:
@@ -954,31 +959,31 @@ class Controls:
self.events.add(EventName.blockUser) self.events.add(EventName.blockUser)
return return
if self.random_events: # if self.random_events:
acceleration = CS.aEgo # acceleration = CS.aEgo
if not CS.gasPressed: # if not CS.gasPressed:
self.max_acceleration = max(acceleration, self.max_acceleration) # self.max_acceleration = max(acceleration, self.max_acceleration)
else: # else:
self.max_acceleration = 0 # self.max_acceleration = 0
if 3.5 > self.max_acceleration >= 3.0 and acceleration < 1.5: # if 3.5 > self.max_acceleration >= 3.0 and acceleration < 1.5:
self.events.add(EventName.accel30) # self.events.add(EventName.accel30)
self.params_memory.put_int("CurrentRandomEvent", 2) # self.params_memory.put_int("CurrentRandomEvent", 2)
self.random_event_triggered = True # self.random_event_triggered = True
self.max_acceleration = 0 # self.max_acceleration = 0
elif 4.0 > self.max_acceleration >= 3.5 and acceleration < 1.5: # elif 4.0 > self.max_acceleration >= 3.5 and acceleration < 1.5:
self.events.add(EventName.accel35) # self.events.add(EventName.accel35)
self.params_memory.put_int("CurrentRandomEvent", 3) # self.params_memory.put_int("CurrentRandomEvent", 3)
self.random_event_triggered = True # self.random_event_triggered = True
self.max_acceleration = 0 # self.max_acceleration = 0
elif self.max_acceleration >= 4.0 and acceleration < 1.5: # elif self.max_acceleration >= 4.0 and acceleration < 1.5:
self.events.add(EventName.accel40) # self.events.add(EventName.accel40)
self.params_memory.put_int("CurrentRandomEvent", 4) # self.params_memory.put_int("CurrentRandomEvent", 4)
self.random_event_triggered = True # self.random_event_triggered = True
self.max_acceleration = 0 # self.max_acceleration = 0
if self.green_light_alert: if self.green_light_alert:
green_light = not self.sm['frogpilotPlan'].redLight green_light = not self.sm['frogpilotPlan'].redLight
@@ -991,9 +996,9 @@ class Controls:
if self.green_light_mac.get_moving_average() >= PROBABILITY: if self.green_light_mac.get_moving_average() >= PROBABILITY:
self.events.add(EventName.greenLight) self.events.add(EventName.greenLight)
if self.sm.frame >= 1000 and self.holiday_themes and self.params_memory.get_int("CurrentHolidayTheme") != 0 and not self.holiday_theme_alerted: # if self.sm.frame >= 1000 and self.holiday_themes and self.params_memory.get_int("CurrentHolidayTheme") != 0 and not self.holiday_theme_alerted:
self.events.add(EventName.holidayActive) # self.events.add(EventName.holidayActive)
self.holiday_theme_alerted = True # self.holiday_theme_alerted = True
if self.lead_departing_alert and self.sm.frame % 50 == 0: if self.lead_departing_alert and self.sm.frame % 50 == 0:
lead = self.sm['radarState'].leadOne lead = self.sm['radarState'].leadOne
@@ -1018,72 +1023,73 @@ class Controls:
if os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')) and self.crashed_timer < 10: if os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')) and self.crashed_timer < 10:
self.events.add(EventName.openpilotCrashed) self.events.add(EventName.openpilotCrashed)
if self.random_events and not self.openpilot_crashed_triggered: # if self.random_events and not self.openpilot_crashed_triggered:
self.events.add(EventName.openpilotCrashedRandomEvents) # self.events.add(EventName.openpilotCrashedRandomEvents)
self.openpilot_crashed_triggered = True # self.openpilot_crashed_triggered = True
self.crashed_timer += DT_CTRL self.crashed_timer += DT_CTRL
if self.speed_limit_alert or self.speed_limit_confirmation: # if self.speed_limit_alert or self.speed_limit_confirmation:
current_speed_limit = self.sm['frogpilotPlan'].slcSpeedLimit # current_speed_limit = self.sm['frogpilotPlan'].slcSpeedLimit
desired_speed_limit = self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit # desired_speed_limit = self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit
speed_limit_changed = desired_speed_limit != self.previous_speed_limit and abs(current_speed_limit - desired_speed_limit) > 1 # speed_limit_changed = desired_speed_limit != self.previous_speed_limit and abs(current_speed_limit - desired_speed_limit) > 1
speed_limit_changed_lower = speed_limit_changed and self.previous_speed_limit > desired_speed_limit # speed_limit_changed_lower = speed_limit_changed and self.previous_speed_limit > desired_speed_limit
speed_limit_changed_higher = speed_limit_changed and self.previous_speed_limit < desired_speed_limit # speed_limit_changed_higher = speed_limit_changed and self.previous_speed_limit < desired_speed_limit
self.previous_speed_limit = desired_speed_limit # self.previous_speed_limit = desired_speed_limit
if self.CP.pcmCruise and self.FPCC.speedLimitChanged: # if self.CP.pcmCruise and self.FPCC.speedLimitChanged:
if any(be.type == ButtonType.accelCruise for be in CS.buttonEvents): # if any(be.type == ButtonType.accelCruise for be in CS.buttonEvents):
self.params_memory.put_bool("SLCConfirmed", True) # self.params_memory.put_bool("SLCConfirmed", True)
self.params_memory.put_bool("SLCConfirmedPressed", True) # self.params_memory.put_bool("SLCConfirmedPressed", True)
elif any(be.type == ButtonType.decelCruise for be in CS.buttonEvents): # elif any(be.type == ButtonType.decelCruise for be in CS.buttonEvents):
self.params_memory.put_bool("SLCConfirmed", False) # self.params_memory.put_bool("SLCConfirmed", False)
self.params_memory.put_bool("SLCConfirmedPressed", True) # self.params_memory.put_bool("SLCConfirmedPressed", True)
if speed_limit_changed_lower: # if speed_limit_changed_lower:
if self.speed_limit_confirmation_lower: # if self.speed_limit_confirmation_lower:
self.FPCC.speedLimitChanged = True # self.FPCC.speedLimitChanged = True
else: # else:
self.params_memory.put_bool("SLCConfirmed", True) # self.params_memory.put_bool("SLCConfirmed", True)
elif speed_limit_changed_higher: # elif speed_limit_changed_higher:
if self.speed_limit_confirmation_higher: # if self.speed_limit_confirmation_higher:
self.FPCC.speedLimitChanged = True # self.FPCC.speedLimitChanged = True
else: # else:
self.params_memory.put_bool("SLCConfirmed", True) # self.params_memory.put_bool("SLCConfirmed", True)
if self.params_memory.get_bool("SLCConfirmedPressed") or not self.speed_limit_confirmation: # if self.params_memory.get_bool("SLCConfirmedPressed") or not self.speed_limit_confirmation:
self.FPCC.speedLimitChanged = False # self.FPCC.speedLimitChanged = False
self.params_memory.put_bool("SLCConfirmedPressed", False) # self.params_memory.put_bool("SLCConfirmedPressed", False)
if (speed_limit_changed_lower or speed_limit_changed_higher) and self.speed_limit_alert: # if (speed_limit_changed_lower or speed_limit_changed_higher) and self.speed_limit_alert:
self.events.add(EventName.speedLimitChanged) # self.events.add(EventName.speedLimitChanged)
if self.FPCC.speedLimitChanged: # if self.FPCC.speedLimitChanged:
self.speed_limit_timer += DT_CTRL # self.speed_limit_timer += DT_CTRL
if self.speed_limit_timer >= 10: # if self.speed_limit_timer >= 10:
self.FPCC.speedLimitChanged = False # self.FPCC.speedLimitChanged = False
self.speed_limit_timer = 0 # self.speed_limit_timer = 0
else: # else:
self.speed_limit_timer = 0 # self.speed_limit_timer = 0
else: # else:
self.FPCC.speedLimitChanged = False # self.FPCC.speedLimitChanged = False
# Todo: just output this to a debug log / console
if self.sm.frame == 550 and self.CP.lateralTuning.which() == 'torque' and self.CI.use_nnff: if self.sm.frame == 550 and self.CP.lateralTuning.which() == 'torque' and self.CI.use_nnff:
self.events.add(EventName.torqueNNLoad) self.events.add(EventName.torqueNNLoad)
if self.random_events: # if self.random_events:
conversion = 1 if self.is_metric else CV.KPH_TO_MPH # conversion = 1 if self.is_metric else CV.KPH_TO_MPH
v_cruise = max(self.v_cruise_helper.v_cruise_cluster_kph, self.v_cruise_helper.v_cruise_kph) * conversion # v_cruise = max(self.v_cruise_helper.v_cruise_cluster_kph, self.v_cruise_helper.v_cruise_kph) * conversion
if 70 > v_cruise >= 69: # if 70 > v_cruise >= 69:
if not self.vCruise69_alert_played: # if not self.vCruise69_alert_played:
self.events.add(EventName.vCruise69) # self.events.add(EventName.vCruise69)
self.vCruise69_alert_played = True # self.vCruise69_alert_played = True
else: # else:
self.vCruise69_alert_played = False # self.vCruise69_alert_played = False
def update_frogpilot_variables(self, CS): def update_frogpilot_variables(self, CS):
self.driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown) self.driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown)
@@ -1117,23 +1123,23 @@ class Controls:
self.params_storage.put_int_nonblocking("FrogPilotDrives", current_total_drives + 1) self.params_storage.put_int_nonblocking("FrogPilotDrives", current_total_drives + 1)
self.drive_added = True self.drive_added = True
if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents) and self.experimental_mode_via_lkas: # if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents) and self.experimental_mode_via_lkas:
if self.frogpilot_variables.conditional_experimental_mode: # if self.frogpilot_variables.conditional_experimental_mode:
conditional_status = self.params_memory.get_int("CEStatus") # conditional_status = self.params_memory.get_int("CEStatus")
override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 3 if conditional_status >= 7 else 4 # override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 3 if conditional_status >= 7 else 4
self.params_memory.put_int("CEStatus", override_value) # self.params_memory.put_int("CEStatus", override_value)
else: # else:
self.params.put_bool_nonblocking("ExperimentalMode", not self.experimental_mode) # self.params.put_bool_nonblocking("ExperimentalMode", not self.experimental_mode)
self.previously_enabled |= (self.enabled or self.FPCC.alwaysOnLateral) and CS.vEgo > CRUISING_SPEED self.previously_enabled |= (self.enabled or self.FPCC.alwaysOnLateral) and CS.vEgo > CRUISING_SPEED
self.previously_enabled &= self.driving_gear self.previously_enabled &= self.driving_gear
if self.random_event_triggered: # if self.random_event_triggered:
self.random_event_timer += DT_CTRL # self.random_event_timer += DT_CTRL
if self.random_event_timer >= 4: # if self.random_event_timer >= 4:
self.random_event_triggered = False # self.random_event_triggered = False
self.random_event_timer = 0 # self.random_event_timer = 0
self.params_memory.remove("CurrentRandomEvent") # self.params_memory.remove("CurrentRandomEvent")
signal_check = CS.vEgo >= self.pause_lateral_below_speed or not (CS.leftBlinker or CS.rightBlinker) or CS.standstill signal_check = CS.vEgo >= self.pause_lateral_below_speed or not (CS.leftBlinker or CS.rightBlinker) or CS.standstill
self.speed_check = CS.vEgo >= self.pause_lateral_below_speed or CS.standstill or signal_check and self.pause_lateral_below_signal self.speed_check = CS.vEgo >= self.pause_lateral_below_speed or CS.standstill or signal_check and self.pause_lateral_below_signal
@@ -1161,9 +1167,9 @@ class Controls:
custom_theme = self.params.get_bool("CustomTheme") custom_theme = self.params.get_bool("CustomTheme")
custom_sounds = self.params.get_int("CustomSounds") if custom_theme else 0 custom_sounds = self.params.get_int("CustomSounds") if custom_theme else 0
frog_sounds = custom_sounds == 1 frog_sounds = custom_sounds == 1
self.goat_scream = frog_sounds and self.params.get_bool("GoatScream") self.goat_scream = False
self.holiday_themes = custom_theme and self.params.get_bool("HolidayThemes") self.holiday_themes = False
self.random_events = custom_theme and self.params.get_bool("RandomEvents") self.random_events = False
device_management = self.params.get_bool("DeviceManagement") device_management = self.params.get_bool("DeviceManagement")
self.increase_thermal_limits = device_management and self.params.get_bool("IncreaseThermalLimits") self.increase_thermal_limits = device_management and self.params.get_bool("IncreaseThermalLimits")
@@ -1211,6 +1217,15 @@ class Controls:
self.frogpilot_variables.use_ev_tables = self.params.get_bool("EVTable") self.frogpilot_variables.use_ev_tables = self.params.get_bool("EVTable")
def update_clearpilot_events(self, CS):
if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
self.events.add(EventName.clpDebug)
# clearpilot_notice(CP_NOTICE_DEBUG, "LKAS Debug Action Invoked")
foo = "bar"
def clearpilot_state_control(self, CC, CS):
foo = "bar"
def main(): def main():
config_realtime_process(4, Priority.CTRL_HIGH) config_realtime_process(4, Priority.CTRL_HIGH)
controls = Controls() controls = Controls()

14
selfdrive/controls/lib/events.py
View File

@@ -262,6 +262,16 @@ def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messag
AlertStatus.normal, AlertSize.mid, AlertStatus.normal, AlertSize.mid,
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2)
def clp_debug_notice(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
if not hasattr(clp_debug_notice, "counter"):
clp_debug_notice.counter = 0
clp_debug_notice.counter += 1
return Alert(
f"Clearpilot Debug Function Executed ({clp_debug_notice.counter})",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 3.0)
def no_gps_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: def no_gps_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
# Clearpilot todo: # Clearpilot todo:
@@ -768,6 +778,10 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
ET.SOFT_DISABLE: soft_disable_alert("Sensor Data Invalid"), ET.SOFT_DISABLE: soft_disable_alert("Sensor Data Invalid"),
}, },
EventName.clpDebug: {
ET.PERMANENT: clp_debug_notice,
},
EventName.noGps: { EventName.noGps: {
ET.PERMANENT: no_gps_alert, ET.PERMANENT: no_gps_alert,
}, },

80
selfdrive/ui/qt/onroad.cc
View File

@@ -853,51 +853,53 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
QColor center_lane_color = bg_colors[CENTER_LANE_COLOR]; QColor center_lane_color = bg_colors[CENTER_LANE_COLOR];
QLinearGradient path_gradient(0, height(), 0, 0); QLinearGradient path_gradient(0, height(), 0, 0);
if (experimentalMode || scene.acceleration_path) { if (edgeColor != bg_colors[STATUS_DISENGAGED]) {
// The first half of track_vertices are the points for the right side of the path if (experimentalMode || scene.acceleration_path) {
// and the indices match the positions of accel from uiPlan // The first half of track_vertices are the points for the right side of the path
const auto &acceleration_const = sm["uiPlan"].getUiPlan().getAccel(); // and the indices match the positions of accel from uiPlan
const int max_len = std::min<int>(scene.track_vertices.length() / 2, acceleration_const.size()); const auto &acceleration_const = sm["uiPlan"].getUiPlan().getAccel();
const int max_len = std::min<int>(scene.track_vertices.length() / 2, acceleration_const.size());
// Copy of the acceleration vector // Copy of the acceleration vector
std::vector<float> acceleration; std::vector<float> acceleration;
for (int i = 0; i < acceleration_const.size(); i++) { for (int i = 0; i < acceleration_const.size(); i++) {
acceleration.push_back(acceleration_const[i]); acceleration.push_back(acceleration_const[i]);
}
try {
for (int i = 0; i < max_len; ++i) {
// Rewrote to generate color based off of bg_colors[CENTER_LANE_COLOR] constant
if (scene.track_vertices[i].y() < 0 || scene.track_vertices[i].y() > height()) continue;
float lin_grad_point = (height() - scene.track_vertices[i].y()) / height();
double _h, _s, _l; // Use double for compatibility with QColor::getHslF()
center_lane_color.getHslF(&_h, &_s, &_l);
// Calculate saturation and lightness based on acceleration
float adjusted_saturation = std::min(std::abs(acceleration[i] * 1.5f), 1.f);
float adjusted_lightness = util::map_val(adjusted_saturation, 0.f, 1.f, static_cast<float>(_l), 0.95f); // Using base lightness as a starting point
// Calculate dynamic alpha based on lin_grad_point
float dynamic_alpha = util::map_val(lin_grad_point, 0.75f / 2.f, 0.75f, CENTER_LANE_ALPHA, 0.f);
QColor final_color = QColor::fromHslF(static_cast<float>(_h / 360.f), adjusted_saturation, adjusted_lightness, dynamic_alpha);
path_gradient.setColorAt(lin_grad_point, final_color);
i += (i + 2) < max_len ? 1 : 0; // Skipping a point to optimize rendering
} }
} catch (const std::exception& e) {
// Default shading if for some reason the above code fails try {
path_gradient = QLinearGradient(0, height(), 0, 0); for (int i = 0; i < max_len; ++i) {
// Rewrote to generate color based off of bg_colors[CENTER_LANE_COLOR] constant
if (scene.track_vertices[i].y() < 0 || scene.track_vertices[i].y() > height()) continue;
float lin_grad_point = (height() - scene.track_vertices[i].y()) / height();
double _h, _s, _l; // Use double for compatibility with QColor::getHslF()
center_lane_color.getHslF(&_h, &_s, &_l);
// Calculate saturation and lightness based on acceleration
float adjusted_saturation = std::min(std::abs(acceleration[i] * 1.5f), 1.f);
float adjusted_lightness = util::map_val(adjusted_saturation, 0.f, 1.f, static_cast<float>(_l), 0.95f); // Using base lightness as a starting point
// Calculate dynamic alpha based on lin_grad_point
float dynamic_alpha = util::map_val(lin_grad_point, 0.75f / 2.f, 0.75f, CENTER_LANE_ALPHA, 0.f);
QColor final_color = QColor::fromHslF(static_cast<float>(_h / 360.f), adjusted_saturation, adjusted_lightness, dynamic_alpha);
path_gradient.setColorAt(lin_grad_point, final_color);
i += (i + 2) < max_len ? 1 : 0; // Skipping a point to optimize rendering
}
} catch (const std::exception& e) {
// Default shading if for some reason the above code fails
path_gradient = QLinearGradient(0, height(), 0, 0);
path_gradient.setColorAt(0.0, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.5)));
path_gradient.setColorAt(0.5, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.4)));
path_gradient.setColorAt(1.0, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.0)));
}
} else {
path_gradient.setColorAt(0.0, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.5))); path_gradient.setColorAt(0.0, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.5)));
path_gradient.setColorAt(0.5, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.4))); path_gradient.setColorAt(0.5, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.4)));
path_gradient.setColorAt(1.0, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.0))); path_gradient.setColorAt(1.0, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.0)));
} }
} else {
path_gradient.setColorAt(0.0, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.5)));
path_gradient.setColorAt(0.5, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.4)));
path_gradient.setColorAt(1.0, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.0)));
} }
painter.setBrush(path_gradient); painter.setBrush(path_gradient);

7
selfdrive/ui/ui.h
View File

@@ -42,9 +42,10 @@ const int OTHER_LANE_WIDTH = 16;
// Clearpilot custom colors // Clearpilot custom colors
const QColor bg_colors [] = { const QColor bg_colors [] = {
[STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8), [STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8),
[STATUS_OVERRIDE] = QColor(94, 112, 255, 0xd1), // When you nudge the steering wheel while engaged [STATUS_OVERRIDE] = QColor(94, 112, 255, 0xd1),
[STATUS_ENGAGED] = QColor(94, 112, 255, 0xd1), // Bright Blue [STATUS_ENGAGED] = QColor(76, 97, 255, 0xd1),
[STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(0, 0, 255, 0xd1), // True Blue [STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(177, 183, 224, 0xd1),
// [STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(184, 193, 255, 0xd1),
[STATUS_TRAFFIC_MODE_ACTIVE] = QColor(0xc9, 0x22, 0x31, 0xd1), // ? unused? [STATUS_TRAFFIC_MODE_ACTIVE] = QColor(0xc9, 0x22, 0x31, 0xd1), // ? unused?
[STATUS_EXPERIMENTAL_ACTIVE] = QColor(201, 41, 204, 0xd1), // Magenta [STATUS_EXPERIMENTAL_ACTIVE] = QColor(201, 41, 204, 0xd1), // Magenta
[CENTER_LANE_COLOR] = QColor(150, 150, 150, 0xd1), // Gray [CENTER_LANE_COLOR] = QColor(150, 150, 150, 0xd1), // Gray