wip
This commit is contained in:
Your Name
@@ -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,
|
||||
},
|
||||
|
||||
Reference in New Issue
Block a user