wip
This commit is contained in:
Your Name
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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"]
|
||||
|
||||
@@ -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
|
||||
|
||||
0
selfdrive/clearpilot/manager/api.js
Normal file
0
selfdrive/clearpilot/manager/api.js
Normal file
0
selfdrive/clearpilot/readme.txt
Normal file
0
selfdrive/clearpilot/readme.txt
Normal file
@@ -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()
|
||||
|
||||
@@ -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,
|
||||
},
|
||||
|
||||
@@ -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<int>(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<int>(scene.track_vertices.length() / 2, acceleration_const.size());
|
||||
|
||||
// Copy of the acceleration vector
|
||||
std::vector<float> 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<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
|
||||
// Copy of the acceleration vector
|
||||
std::vector<float> 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<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.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.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);
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user