diff --git a/clearpilot_devqueue_amain.txt b/clearpilot_devqueue_amain.txt index 90f7c32..da52dcc 100644 --- a/clearpilot_devqueue_amain.txt +++ b/clearpilot_devqueue_amain.txt @@ -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 --- get_curvature --- get_wheel_angle diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 82ae811..ee339d5 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -18,6 +18,18 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase 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: CI: CarInterfaceBase diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 9131e1b..c34f074 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -311,8 +311,6 @@ class CarState(CarStateBase): self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0 self.lkas_previously_enabled = self.lkas_enabled - - # CLEARPILOT fix lkas_enabled = False try: lkas_enabled = cp.vl[self.cruise_btns_msg_canfd]["LFA_BTN"] diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index dfb0b38..ec80622 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -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.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 GearShifter = car.CarState.GearShifter EventName = car.CarEvent.EventName diff --git a/selfdrive/clearpilot/api/api.js b/selfdrive/clearpilot/__init__.py similarity index 100% rename from selfdrive/clearpilot/api/api.js rename to selfdrive/clearpilot/__init__.py diff --git a/selfdrive/clearpilot/manager/api.js b/selfdrive/clearpilot/manager/api.js new file mode 100644 index 0000000..e69de29 diff --git a/selfdrive/clearpilot/readme.txt b/selfdrive/clearpilot/readme.txt new file mode 100644 index 0000000..e69de29 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2249eed..4809368 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -223,6 +223,84 @@ class Controls: if any(ps.controlsAllowed for ps in self.sm['pandaStates']): 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): """Compute onroadEvents from carState""" @@ -431,10 +509,10 @@ class Controls: planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled if planner_fcw or model_fcw: self.events.add(EventName.fcw) - self.fcw_random_event_triggered = True - elif self.fcw_random_event_triggered and self.random_events: - self.events.add(EventName.yourFrogTriedToKillMe) - self.fcw_random_event_triggered = False + # self.fcw_random_event_triggered = True + # elif self.fcw_random_event_triggered and self.random_events: + # self.events.add(EventName.yourFrogTriedToKillMe) + # self.fcw_random_event_triggered = False for m in messaging.drain_sock(self.log_sock, wait_for_one=False): try: @@ -448,6 +526,7 @@ class Controls: pass # TODO: fix simulator + # CLEARPILOT fix me: if not SIMULATION or REPLAY: # 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): @@ -459,56 +538,6 @@ class Controls: if self.sm['modelV2'].frameDropPerc > 20: 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): """Compute conditional state transitions and execute actions on state transitions""" @@ -705,12 +734,12 @@ class Controls: good_speed = CS.vEgo > 5 max_torque = abs(actuators.steer) > 0.99 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.firefoxSteerSaturated) - self.params_memory.put_int("CurrentRandomEvent", 1) - self.random_event_triggered = True - else: - lac_log.active and self.events.add(EventName.goatSteerSaturated if self.goat_scream else EventName.steerSaturated) + lac_log.active and self.events.add(EventName.goatSteerSaturated if self.goat_scream else EventName.steerSaturated) + # if self.sm.frame % 10000 == 0 and self.random_events: + # lac_log.active and self.events.add(EventName.firefoxSteerSaturated) + # self.params_memory.put_int("CurrentRandomEvent", 1) + # self.random_event_triggered = True + # else: elif lac_log.saturated: # TODO probably should not use dpath_points but curvature dpath_points = model_v2.position.y @@ -896,30 +925,6 @@ class Controls: # copy CarControl to pass to CarInterface on the next iteration 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): try: @@ -954,31 +959,31 @@ class Controls: self.events.add(EventName.blockUser) return - if self.random_events: - acceleration = CS.aEgo + # if self.random_events: + # acceleration = CS.aEgo - if not CS.gasPressed: - self.max_acceleration = max(acceleration, self.max_acceleration) - else: - self.max_acceleration = 0 + # if not CS.gasPressed: + # self.max_acceleration = max(acceleration, self.max_acceleration) + # else: + # self.max_acceleration = 0 - if 3.5 > self.max_acceleration >= 3.0 and acceleration < 1.5: - self.events.add(EventName.accel30) - self.params_memory.put_int("CurrentRandomEvent", 2) - self.random_event_triggered = True - self.max_acceleration = 0 + # if 3.5 > self.max_acceleration >= 3.0 and acceleration < 1.5: + # self.events.add(EventName.accel30) + # self.params_memory.put_int("CurrentRandomEvent", 2) + # self.random_event_triggered = True + # self.max_acceleration = 0 - elif 4.0 > self.max_acceleration >= 3.5 and acceleration < 1.5: - self.events.add(EventName.accel35) - self.params_memory.put_int("CurrentRandomEvent", 3) - self.random_event_triggered = True - self.max_acceleration = 0 + # elif 4.0 > self.max_acceleration >= 3.5 and acceleration < 1.5: + # self.events.add(EventName.accel35) + # self.params_memory.put_int("CurrentRandomEvent", 3) + # self.random_event_triggered = True + # self.max_acceleration = 0 - elif self.max_acceleration >= 4.0 and acceleration < 1.5: - self.events.add(EventName.accel40) - self.params_memory.put_int("CurrentRandomEvent", 4) - self.random_event_triggered = True - self.max_acceleration = 0 + # elif self.max_acceleration >= 4.0 and acceleration < 1.5: + # self.events.add(EventName.accel40) + # self.params_memory.put_int("CurrentRandomEvent", 4) + # self.random_event_triggered = True + # self.max_acceleration = 0 if self.green_light_alert: green_light = not self.sm['frogpilotPlan'].redLight @@ -991,9 +996,9 @@ class Controls: if self.green_light_mac.get_moving_average() >= PROBABILITY: 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: - self.events.add(EventName.holidayActive) - self.holiday_theme_alerted = True + # 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.holiday_theme_alerted = True if self.lead_departing_alert and self.sm.frame % 50 == 0: 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: self.events.add(EventName.openpilotCrashed) - if self.random_events and not self.openpilot_crashed_triggered: - self.events.add(EventName.openpilotCrashedRandomEvents) - self.openpilot_crashed_triggered = True + # if self.random_events and not self.openpilot_crashed_triggered: + # self.events.add(EventName.openpilotCrashedRandomEvents) + # self.openpilot_crashed_triggered = True self.crashed_timer += DT_CTRL - if self.speed_limit_alert or self.speed_limit_confirmation: - current_speed_limit = self.sm['frogpilotPlan'].slcSpeedLimit - desired_speed_limit = self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit + # if self.speed_limit_alert or self.speed_limit_confirmation: + # current_speed_limit = self.sm['frogpilotPlan'].slcSpeedLimit + # 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_higher = 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 - self.previous_speed_limit = desired_speed_limit + # self.previous_speed_limit = desired_speed_limit - if self.CP.pcmCruise and self.FPCC.speedLimitChanged: - if any(be.type == ButtonType.accelCruise for be in CS.buttonEvents): - self.params_memory.put_bool("SLCConfirmed", True) - self.params_memory.put_bool("SLCConfirmedPressed", True) - elif any(be.type == ButtonType.decelCruise for be in CS.buttonEvents): - self.params_memory.put_bool("SLCConfirmed", False) - self.params_memory.put_bool("SLCConfirmedPressed", True) + # if self.CP.pcmCruise and self.FPCC.speedLimitChanged: + # if any(be.type == ButtonType.accelCruise for be in CS.buttonEvents): + # self.params_memory.put_bool("SLCConfirmed", True) + # self.params_memory.put_bool("SLCConfirmedPressed", True) + # elif any(be.type == ButtonType.decelCruise for be in CS.buttonEvents): + # self.params_memory.put_bool("SLCConfirmed", False) + # self.params_memory.put_bool("SLCConfirmedPressed", True) - if speed_limit_changed_lower: - if self.speed_limit_confirmation_lower: - self.FPCC.speedLimitChanged = True - else: - self.params_memory.put_bool("SLCConfirmed", True) - elif speed_limit_changed_higher: - if self.speed_limit_confirmation_higher: - self.FPCC.speedLimitChanged = True - else: - self.params_memory.put_bool("SLCConfirmed", True) + # if speed_limit_changed_lower: + # if self.speed_limit_confirmation_lower: + # self.FPCC.speedLimitChanged = True + # else: + # self.params_memory.put_bool("SLCConfirmed", True) + # elif speed_limit_changed_higher: + # if self.speed_limit_confirmation_higher: + # self.FPCC.speedLimitChanged = True + # else: + # self.params_memory.put_bool("SLCConfirmed", True) - if self.params_memory.get_bool("SLCConfirmedPressed") or not self.speed_limit_confirmation: - self.FPCC.speedLimitChanged = False - self.params_memory.put_bool("SLCConfirmedPressed", False) + # if self.params_memory.get_bool("SLCConfirmedPressed") or not self.speed_limit_confirmation: + # self.FPCC.speedLimitChanged = False + # self.params_memory.put_bool("SLCConfirmedPressed", False) - if (speed_limit_changed_lower or speed_limit_changed_higher) and self.speed_limit_alert: - self.events.add(EventName.speedLimitChanged) + # if (speed_limit_changed_lower or speed_limit_changed_higher) and self.speed_limit_alert: + # self.events.add(EventName.speedLimitChanged) - if self.FPCC.speedLimitChanged: - self.speed_limit_timer += DT_CTRL - if self.speed_limit_timer >= 10: - self.FPCC.speedLimitChanged = False - self.speed_limit_timer = 0 - else: - self.speed_limit_timer = 0 - else: - self.FPCC.speedLimitChanged = False + # if self.FPCC.speedLimitChanged: + # self.speed_limit_timer += DT_CTRL + # if self.speed_limit_timer >= 10: + # self.FPCC.speedLimitChanged = False + # self.speed_limit_timer = 0 + # else: + # self.speed_limit_timer = 0 + # else: + # 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: self.events.add(EventName.torqueNNLoad) - if self.random_events: - 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 + # if self.random_events: + # 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 - if 70 > v_cruise >= 69: - if not self.vCruise69_alert_played: - self.events.add(EventName.vCruise69) - self.vCruise69_alert_played = True - else: - self.vCruise69_alert_played = False + # if 70 > v_cruise >= 69: + # if not self.vCruise69_alert_played: + # self.events.add(EventName.vCruise69) + # self.vCruise69_alert_played = True + # else: + # self.vCruise69_alert_played = False def update_frogpilot_variables(self, CS): 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.drive_added = True - 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: - 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 - self.params_memory.put_int("CEStatus", override_value) - else: - self.params.put_bool_nonblocking("ExperimentalMode", not self.experimental_mode) + # 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: + # 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 + # self.params_memory.put_int("CEStatus", override_value) + # else: + # 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.driving_gear - if self.random_event_triggered: - self.random_event_timer += DT_CTRL - if self.random_event_timer >= 4: - self.random_event_triggered = False - self.random_event_timer = 0 - self.params_memory.remove("CurrentRandomEvent") + # if self.random_event_triggered: + # self.random_event_timer += DT_CTRL + # if self.random_event_timer >= 4: + # self.random_event_triggered = False + # self.random_event_timer = 0 + # self.params_memory.remove("CurrentRandomEvent") 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 @@ -1161,9 +1167,9 @@ class Controls: custom_theme = self.params.get_bool("CustomTheme") custom_sounds = self.params.get_int("CustomSounds") if custom_theme else 0 frog_sounds = custom_sounds == 1 - self.goat_scream = frog_sounds and self.params.get_bool("GoatScream") - self.holiday_themes = custom_theme and self.params.get_bool("HolidayThemes") - self.random_events = custom_theme and self.params.get_bool("RandomEvents") + self.goat_scream = False + self.holiday_themes = False + self.random_events = False device_management = self.params.get_bool("DeviceManagement") 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") + 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(): config_realtime_process(4, Priority.CTRL_HIGH) controls = Controls() diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 2aa5ef3..6d23406 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -262,6 +262,16 @@ def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messag AlertStatus.normal, AlertSize.mid, 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: # Clearpilot todo: @@ -768,6 +778,10 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { ET.SOFT_DISABLE: soft_disable_alert("Sensor Data Invalid"), }, + EventName.clpDebug: { + ET.PERMANENT: clp_debug_notice, + }, + EventName.noGps: { ET.PERMANENT: no_gps_alert, }, diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index a38d6a9..bd1141c 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -853,51 +853,53 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { QColor center_lane_color = bg_colors[CENTER_LANE_COLOR]; QLinearGradient path_gradient(0, height(), 0, 0); - if (experimentalMode || scene.acceleration_path) { - // The first half of track_vertices are the points for the right side of the path - // and the indices match the positions of accel from uiPlan - const auto &acceleration_const = sm["uiPlan"].getUiPlan().getAccel(); - const int max_len = std::min(scene.track_vertices.length() / 2, acceleration_const.size()); + if (edgeColor != bg_colors[STATUS_DISENGAGED]) { + if (experimentalMode || scene.acceleration_path) { + // The first half of track_vertices are the points for the right side of the path + // and the indices match the positions of accel from uiPlan + const auto &acceleration_const = sm["uiPlan"].getUiPlan().getAccel(); + const int max_len = std::min(scene.track_vertices.length() / 2, acceleration_const.size()); - // Copy of the acceleration vector - std::vector acceleration; - for (int i = 0; i < acceleration_const.size(); 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(_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(_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 + // Copy of the acceleration vector + std::vector acceleration; + for (int i = 0; i < acceleration_const.size(); i++) { + acceleration.push_back(acceleration_const[i]); } - } catch (const std::exception& e) { - // Default shading if for some reason the above code fails - path_gradient = QLinearGradient(0, height(), 0, 0); + + 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(_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(_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(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(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(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(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(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(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(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(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(CENTER_LANE_ALPHA * 255 * 0.0))); } painter.setBrush(path_gradient); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 97564e5..9e86b68 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -42,9 +42,10 @@ const int OTHER_LANE_WIDTH = 16; // Clearpilot custom colors const QColor bg_colors [] = { [STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8), - [STATUS_OVERRIDE] = QColor(94, 112, 255, 0xd1), // When you nudge the steering wheel while engaged - [STATUS_ENGAGED] = QColor(94, 112, 255, 0xd1), // Bright Blue - [STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(0, 0, 255, 0xd1), // True Blue + [STATUS_OVERRIDE] = QColor(94, 112, 255, 0xd1), + [STATUS_ENGAGED] = QColor(76, 97, 255, 0xd1), + [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_EXPERIMENTAL_ACTIVE] = QColor(201, 41, 204, 0xd1), // Magenta [CENTER_LANE_COLOR] = QColor(150, 150, 150, 0xd1), // Gray