restore driving logic to pre-variable-fps baseline
Wholesale revert of driving-relevant files to the snapshot in /projects/openpilot/archive/clearpilot (HEAD 980f0aa). Goal: get known-good driving behavior back, then re-introduce optimizations slowly to track down a "feels like the wheel pulls right" regression. Files restored from baseline: - selfdrive/controls/controlsd.py - selfdrive/controls/lib/events.py - selfdrive/controls/lib/longitudinal_planner.py - selfdrive/modeld/modeld.py - selfdrive/modeld/dmonitoringmodeld.py - selfdrive/locationd/calibrationd.py - selfdrive/locationd/paramsd.py - selfdrive/locationd/torqued.py - selfdrive/car/interfaces.py - selfdrive/car/hyundai/carstate.py (CAN-FD telemetry preserved as a commented block for future re-enable) - selfdrive/monitoring/dmonitoringd.py - selfdrive/frogpilot/controls/frogpilot_planner.py - common/realtime.py Intentionally NOT restored (kept as current): - selfdrive/thermald/* (fan/power tuning kept) - selfdrive/car/hyundai/carcontroller.py + hyundaicanfd.py (perf-only hoist of no_lat_lane_change Params read; behavior-equivalent) - cereal/services.py, cereal/custom.capnp (additive only) - selfdrive/manager/*, common/params.cc (heavy ClearPilot infrastructure: bench mode, log dir, dashcamd, gpsd, params) - All selfdrive/ui/, selfdrive/clearpilot/, system/clearpilot/ UI features will be re-wired in a follow-up commit. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
+83
-235
@@ -37,8 +37,6 @@ from openpilot.system.version import get_short_branch
|
||||
from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CRUISING_SPEED, PROBABILITY, MovingAverageCalculator
|
||||
|
||||
from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS
|
||||
from openpilot.selfdrive.clearpilot.telemetry import tlog
|
||||
from openpilot.selfdrive.clearpilot.speed_logic import SpeedState
|
||||
from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
|
||||
|
||||
SOFT_DISABLE_TIME = 3 # seconds
|
||||
@@ -49,7 +47,7 @@ CAMERA_OFFSET = 0.04
|
||||
REPLAY = "REPLAY" in os.environ
|
||||
SIMULATION = "SIMULATION" in os.environ
|
||||
TESTING_CLOSET = "TESTING_CLOSET" in os.environ
|
||||
IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd", "telemetryd", "dashcamd"}
|
||||
IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"}
|
||||
|
||||
ThermalStatus = log.DeviceState.ThermalStatus
|
||||
State = log.ControlsState.OpenpilotState
|
||||
@@ -79,11 +77,7 @@ class Controls:
|
||||
self.params_storage = Params("/persist/params")
|
||||
|
||||
self.params_memory.put_bool("CPTLkasButtonAction", False)
|
||||
self.params_memory.put_int("ScreenDisplayMode", 0)
|
||||
|
||||
# ClearPilot speed processing
|
||||
self.speed_state = SpeedState()
|
||||
self.speed_state_frame = 0
|
||||
self.params_memory.put_bool("ScreenDisplayMode", 0)
|
||||
|
||||
self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS
|
||||
|
||||
@@ -113,9 +107,8 @@ class Controls:
|
||||
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
|
||||
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
|
||||
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
|
||||
'testJoystick', 'frogpilotPlan', 'gpsLocation'] + self.camera_packets + self.sensor_packets,
|
||||
ignore_alive=ignore+['gpsLocation'], ignore_avg_freq=ignore+['radarState', 'testJoystick', 'gpsLocation'],
|
||||
ignore_valid=['testJoystick', 'gpsLocation'],
|
||||
'testJoystick', 'frogpilotPlan'] + self.camera_packets + self.sensor_packets,
|
||||
ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ],
|
||||
frequency=int(1/DT_CTRL))
|
||||
|
||||
self.joystick_mode = self.params.get_bool("JoystickDebugMode")
|
||||
@@ -170,26 +163,6 @@ class Controls:
|
||||
self.current_alert_types = [ET.PERMANENT]
|
||||
self.logged_comm_issue = None
|
||||
self.not_running_prev = None
|
||||
self.lat_requested_ts = 0 # CLEARPILOT: timestamp when lat was first requested (ramp-up delay + comm issue suppression)
|
||||
self.prev_lat_requested = False
|
||||
# CLEARPILOT: gate frogpilotCarControl send on change (3 static bools, was publishing 100Hz)
|
||||
self._prev_fpcc = (None, None, None, None, None)
|
||||
self._last_fpcc_send_frame = 0
|
||||
# CLEARPILOT: hysteresis counters for transient-filter on cascade alerts. A blip
|
||||
# under 50ms (5 cycles at 100Hz) is typical of MSGQ conflate + slow-polling-consumer
|
||||
# freq-measurement artifacts — not a real comm issue. Persistent failure still
|
||||
# alerts after 50ms, well inside a driver's reaction window.
|
||||
self._hyst_commissue = 0
|
||||
self._hyst_locationd_tmp = 0
|
||||
self._hyst_paramsd_tmp = 0
|
||||
self._hyst_posenet = 0
|
||||
self.HYST_CYCLES = 5
|
||||
# CLEARPILOT: parked-cycle skip — in park+ignition-on, run the full step() only
|
||||
# every 10th cycle. CAN parse + CAN TX still happen every cycle; outer logic
|
||||
# (events, state machine, PID/MPC, publishing) runs at 10Hz. Cached message
|
||||
# bytes are re-published on skipped cycles so downstream freq_ok stays OK.
|
||||
self._cached_controlsState_bytes = None
|
||||
self._cached_carControl_bytes = None
|
||||
self.steer_limited = False
|
||||
self.desired_curvature = 0.0
|
||||
self.experimental_mode = False
|
||||
@@ -224,8 +197,6 @@ class Controls:
|
||||
self.always_on_lateral_main = self.always_on_lateral and self.params.get_bool("AlwaysOnLateralMain")
|
||||
|
||||
self.drive_added = False
|
||||
self.driving_gear = False
|
||||
self.was_driving_gear = False
|
||||
self.fcw_random_event_triggered = False
|
||||
self.holiday_theme_alerted = False
|
||||
self.onroad_distance_pressed = False
|
||||
@@ -264,51 +235,27 @@ class Controls:
|
||||
CS = self.data_sample()
|
||||
cloudlog.timestamp("Data sampled")
|
||||
|
||||
# CLEARPILOT: handle debug-button press + display-mode transitions on every
|
||||
# cycle — button edge events only live in the cycle's CS.buttonEvents and
|
||||
# would otherwise be dropped on skipped cycles.
|
||||
self.handle_screen_mode(CS)
|
||||
self.update_events(CS)
|
||||
self.update_frogpilot_events(CS)
|
||||
self.update_clearpilot_events(CS)
|
||||
|
||||
# CLEARPILOT: in park, only run the full step() every 10th cycle. data_sample
|
||||
# above still runs (CAN parse, button detection). Below, card.controls_update
|
||||
# still runs (CAN TX heartbeat, counters increment). The skipped outer logic
|
||||
# (events, state machine, PID/MPC, publishing) causes at most ~100ms lag on
|
||||
# button→state transitions, which is fine in park. Cached message bytes are
|
||||
# re-sent so downstream consumers see steady 100Hz.
|
||||
parked = CS.gearShifter == car.CarState.GearShifter.park
|
||||
full_cycle = (not parked) or (self.sm.frame % 10 == 0) or (self._cached_controlsState_bytes is None)
|
||||
cloudlog.timestamp("Events updated")
|
||||
|
||||
if full_cycle:
|
||||
self.update_events(CS)
|
||||
self.update_frogpilot_events(CS)
|
||||
self.update_clearpilot_events(CS)
|
||||
if not self.CP.passive and self.initialized:
|
||||
# Update control state
|
||||
self.state_transition(CS)
|
||||
|
||||
cloudlog.timestamp("Events updated")
|
||||
# Compute actuators (runs PID loops and lateral MPC)
|
||||
CC, lac_log = self.state_control(CS)
|
||||
CC = self.clearpilot_state_control(CC, CS)
|
||||
|
||||
if not self.CP.passive and self.initialized:
|
||||
# Update control state
|
||||
self.state_transition(CS)
|
||||
# Publish data
|
||||
self.publish_logs(CS, start_time, CC, lac_log)
|
||||
|
||||
# Compute actuators (runs PID loops and lateral MPC)
|
||||
CC, lac_log = self.state_control(CS)
|
||||
CC = self.clearpilot_state_control(CC, CS)
|
||||
|
||||
# Publish data (also sends CAN TX via card.controls_update inside)
|
||||
self.publish_logs(CS, start_time, CC, lac_log)
|
||||
|
||||
self.CS_prev = CS
|
||||
|
||||
# Update FrogPilot variables
|
||||
self.update_frogpilot_variables(CS)
|
||||
else:
|
||||
# CAN TX heartbeat: keep counters incrementing and CAN frames flowing to the car
|
||||
if not self.CP.passive and self.initialized:
|
||||
self.card.controls_update(self.CC, self.frogpilot_variables)
|
||||
# Re-publish cached messages so downstream freq_ok checks don't trip
|
||||
self.pm.send('controlsState', self._cached_controlsState_bytes)
|
||||
self.pm.send('carControl', self._cached_carControl_bytes)
|
||||
self.CS_prev = CS
|
||||
self.CS_prev = CS
|
||||
|
||||
# Update FrogPilot variables
|
||||
self.update_frogpilot_variables(CS)
|
||||
|
||||
def data_sample(self):
|
||||
"""Receive data from sockets and update carState"""
|
||||
@@ -494,24 +441,6 @@ class Controls:
|
||||
# All events here should at least have NO_ENTRY and SOFT_DISABLE.
|
||||
num_events = len(self.events)
|
||||
|
||||
# CLEARPILOT: compute model standby suppression early — used by multiple checks below
|
||||
try:
|
||||
standby_ts = float(self.params_memory.get("ModelStandbyTs") or "0")
|
||||
except (ValueError, TypeError):
|
||||
standby_ts = 0
|
||||
now = time.monotonic()
|
||||
model_suppress = (now - standby_ts) < 2.0
|
||||
|
||||
# CLEARPILOT: suppress commIssue/location/params errors for 2s after lat was requested
|
||||
# Model FPS jumps from 4 to 20 on engage, causing transient freq check failures
|
||||
# lat_requested_ts is set in state_control on the False->True transition of LatRequested
|
||||
# CLEARPILOT: 2s window — long enough to cover the modeld rate transition burst
|
||||
# without hiding real comms issues. Extending this further risks masking a genuine
|
||||
# fault that demands immediate driver takeover.
|
||||
lat_engage_suppress = (now - self.lat_requested_ts) < 2.0
|
||||
|
||||
# CLEARPILOT: expose suppress flags + standby_ts for the debug dumper in step()
|
||||
|
||||
not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning}
|
||||
if self.sm.recv_frame['managerState'] and (not_running - IGNORE_PROCESSES):
|
||||
self.events.add(EventName.processNotRunning)
|
||||
@@ -524,17 +453,10 @@ class Controls:
|
||||
self.events.add(EventName.cameraMalfunction)
|
||||
elif not self.sm.all_freq_ok(self.camera_packets):
|
||||
self.events.add(EventName.cameraFrameRate)
|
||||
# CLEARPILOT: alert only when avg cycle time exceeds 20ms (≈50Hz effective).
|
||||
# Stock rk.lagging fires at 11.1ms (90% of 10ms) which the Hyundai CAN load
|
||||
# routinely crosses while driving — that's normal, not a fault. 50Hz control is
|
||||
# still plenty responsive. `rk.lagging` is still used defensively elsewhere
|
||||
# (lines ~479, 492) to skip secondary checks when slightly overloaded.
|
||||
avg_dt = sum(self.rk._dts) / len(self.rk._dts)
|
||||
alert_lagging = avg_dt > 0.020
|
||||
if not REPLAY and alert_lagging and not model_suppress and not lat_engage_suppress:
|
||||
if not REPLAY and self.rk.lagging:
|
||||
self.events.add(EventName.controlsdLagging)
|
||||
if not self.radarless_model:
|
||||
if not model_suppress and (len(self.sm['radarState'].radarErrors) or (not self.rk.lagging and not self.sm.all_checks(['radarState']))):
|
||||
if len(self.sm['radarState'].radarErrors) or (not self.rk.lagging and not self.sm.all_checks(['radarState'])):
|
||||
self.events.add(EventName.radarFault)
|
||||
if not self.sm.valid['pandaStates']:
|
||||
self.events.add(EventName.usbError)
|
||||
@@ -546,60 +468,34 @@ class Controls:
|
||||
# generic catch-all. ideally, a more specific event should be added above instead
|
||||
has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE))
|
||||
no_system_errors = (not has_disable_events) or (len(self.events) == num_events)
|
||||
# CLEARPILOT: commIssue fires ONLY when messages actually aren't flowing. If all
|
||||
# subscribers are alive and CAN isn't timing out, comms are fine — any `valid=False`
|
||||
# is an upstream self-declaration that cascades from the MSGQ conflate +
|
||||
# slow-polling-consumer freq_ok measurement artifact. The car drives correctly
|
||||
# during these cascades (content is still usable), so we stop raising the banner.
|
||||
# Genuine comms failures — missing messages or CAN RX timeout — still fire.
|
||||
comms_really_broken = (not self.sm.all_alive()) or self.card.can_rcv_timeout
|
||||
commissue_cond = comms_really_broken and no_system_errors and not model_suppress and not lat_engage_suppress
|
||||
if commissue_cond:
|
||||
self._hyst_commissue += 1
|
||||
else:
|
||||
self._hyst_commissue = 0
|
||||
if self._hyst_commissue >= self.HYST_CYCLES:
|
||||
self.events.add(EventName.commIssue)
|
||||
if commissue_cond:
|
||||
# Log ONCE per entry into comms-really-broken state — no hyst counter so it doesn't
|
||||
# spam every cycle (hyst changes every cycle and would make logs dict always differ).
|
||||
if (not self.sm.all_checks() or self.card.can_rcv_timeout) and no_system_errors:
|
||||
if not self.sm.all_alive():
|
||||
self.events.add(EventName.commIssue)
|
||||
elif not self.sm.all_freq_ok():
|
||||
self.events.add(EventName.commIssueAvgFreq)
|
||||
else: # invalid or can_rcv_timeout.
|
||||
self.events.add(EventName.commIssue)
|
||||
|
||||
logs = {
|
||||
'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],
|
||||
'can_rcv_timeout': self.card.can_rcv_timeout,
|
||||
}
|
||||
if logs != self.logged_comm_issue:
|
||||
import sys, json
|
||||
print("CLP commIssue " + json.dumps(logs), file=sys.stderr, flush=True)
|
||||
cloudlog.event("commIssue", error=True, **logs)
|
||||
self.logged_comm_issue = logs
|
||||
else:
|
||||
self.logged_comm_issue = None
|
||||
|
||||
if not (self.CP.notCar and self.joystick_mode):
|
||||
# CLEARPILOT: suppress locationd/paramsd "temporary error" when we're in the
|
||||
# freq-only cascade (same root as the commIssue suppression above). These events
|
||||
# cascade from upstream freq_ok artifacts — locationd's filterInitialized latches
|
||||
# False if calibrationd tips invalid due to freq, which cascades here.
|
||||
# Real failures still fire commIssue above via alive/valid/can_rcv_timeout gate.
|
||||
freq_only_cascade = (not self.sm.all_checks()) and not comms_really_broken
|
||||
|
||||
# deviceFalling is real-physics (shock sensor), no hysteresis, no cascade suppression.
|
||||
if not self.sm['liveLocationKalman'].posenetOK:
|
||||
self.events.add(EventName.posenetInvalid)
|
||||
if not self.sm['liveLocationKalman'].deviceStable:
|
||||
self.events.add(EventName.deviceFalling)
|
||||
|
||||
posenet_bad = not self.sm['liveLocationKalman'].posenetOK and not model_suppress and not lat_engage_suppress and not freq_only_cascade
|
||||
self._hyst_posenet = self._hyst_posenet + 1 if posenet_bad else 0
|
||||
if self._hyst_posenet >= self.HYST_CYCLES:
|
||||
self.events.add(EventName.posenetInvalid)
|
||||
|
||||
locd_bad = not self.sm['liveLocationKalman'].inputsOK and not model_suppress and not lat_engage_suppress and not freq_only_cascade
|
||||
self._hyst_locationd_tmp = self._hyst_locationd_tmp + 1 if locd_bad else 0
|
||||
if self._hyst_locationd_tmp >= self.HYST_CYCLES:
|
||||
if not self.sm['liveLocationKalman'].inputsOK:
|
||||
self.events.add(EventName.locationdTemporaryError)
|
||||
|
||||
paramsd_bad = not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY) and not model_suppress and not lat_engage_suppress and not freq_only_cascade
|
||||
self._hyst_paramsd_tmp = self._hyst_paramsd_tmp + 1 if paramsd_bad else 0
|
||||
if self._hyst_paramsd_tmp >= self.HYST_CYCLES:
|
||||
if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY):
|
||||
self.events.add(EventName.paramsdTemporaryError)
|
||||
|
||||
# conservative HW alert. if the data or frequency are off, locationd will throw an error
|
||||
@@ -613,8 +509,16 @@ class Controls:
|
||||
if self.cruise_mismatch_counter > int(6. / DT_CTRL):
|
||||
self.events.add(EventName.cruiseMismatch)
|
||||
|
||||
# CLEARPILOT: FCW disabled — car's own radar AEB works better and triggers reliably.
|
||||
# The comma FCW was producing false positives and adds nothing over the stock system.
|
||||
# Check for FCW
|
||||
stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25
|
||||
model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking
|
||||
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
|
||||
|
||||
for m in messaging.drain_sock(self.log_sock, wait_for_one=False):
|
||||
try:
|
||||
@@ -637,7 +541,7 @@ class Controls:
|
||||
self.distance_traveled = 0
|
||||
self.distance_traveled += CS.vEgo * DT_CTRL
|
||||
|
||||
if self.sm['modelV2'].frameDropPerc > 20 and not model_suppress:
|
||||
if self.sm['modelV2'].frameDropPerc > 20:
|
||||
self.events.add(EventName.modeldLagging)
|
||||
|
||||
|
||||
@@ -724,14 +628,6 @@ class Controls:
|
||||
# Check if openpilot is engaged and actuators are enabled
|
||||
self.enabled = self.state in ENABLED_STATES
|
||||
self.active = self.state in ACTIVE_STATES
|
||||
|
||||
# CLEARPILOT: engagement telemetry disabled — was running at 100Hz, causing CPU load
|
||||
# tlog("engage", {
|
||||
# "state": self.state.name if hasattr(self.state, 'name') else str(self.state),
|
||||
# "enabled": self.enabled, "active": self.active,
|
||||
# "cruise_enabled": CS.cruiseState.enabled, "cruise_available": CS.cruiseState.available,
|
||||
# "brakePressed": CS.brakePressed,
|
||||
# })
|
||||
if self.active:
|
||||
self.current_alert_types.append(ET.WARNING)
|
||||
|
||||
@@ -762,19 +658,8 @@ class Controls:
|
||||
|
||||
# Check which actuators can be enabled
|
||||
standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
|
||||
# CLEARPILOT: lat_requested ignores momentary MDPS faults so modeld doesn't drop rate during
|
||||
# brief steerFaultTemporary blips (stale predictions on recovery caused a fault feedback loop).
|
||||
# lat_engaged gates the actual steering command and still respects the fault.
|
||||
lat_requested = (self.active or self.FPCC.alwaysOnLateral) and self.speed_check and \
|
||||
(not standstill or self.joystick_mode)
|
||||
lat_engaged = lat_requested and not CS.steerFaultTemporary and not CS.steerFaultPermanent
|
||||
|
||||
now_ts = time.monotonic()
|
||||
if lat_requested and not self.prev_lat_requested:
|
||||
self.lat_requested_ts = now_ts
|
||||
self.prev_lat_requested = lat_requested
|
||||
self.FPCC.latRequested = lat_requested
|
||||
CC.latActive = lat_engaged and (now_ts - self.lat_requested_ts) >= 0.25
|
||||
CC.latActive = (self.active or self.FPCC.alwaysOnLateral) and self.speed_check and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
|
||||
(not standstill or self.joystick_mode)
|
||||
CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
|
||||
|
||||
actuators = CC.actuators
|
||||
@@ -789,13 +674,11 @@ class Controls:
|
||||
CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left
|
||||
CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right
|
||||
|
||||
no_lat_lane_change = model_v2.meta.laneChangeState == LaneChangeState.laneChangeStarting and clearpilot_disable_lat_on_lane_change
|
||||
if no_lat_lane_change:
|
||||
if model_v2.meta.laneChangeState == LaneChangeState.laneChangeStarting and clearpilot_disable_lat_on_lane_change:
|
||||
CC.latActive = False
|
||||
# CLEARPILOT: noLatLaneChange now lives on frogpilotCarControl (see update_frogpilot_variables).
|
||||
# Also mirrored to frogpilot_variables so in-process carcontroller reads it without IPC.
|
||||
self.FPCC.noLatLaneChange = no_lat_lane_change
|
||||
self.frogpilot_variables.no_lat_lane_change = no_lat_lane_change
|
||||
self.params_memory.put_bool("no_lat_lane_change", True)
|
||||
else:
|
||||
self.params_memory.put_bool("no_lat_lane_change", False)
|
||||
|
||||
if CS.leftBlinker or CS.rightBlinker:
|
||||
self.last_blinker_frame = self.sm.frame
|
||||
@@ -1031,8 +914,6 @@ class Controls:
|
||||
controlsState.lateralControlState.torqueState = lac_log
|
||||
|
||||
self.pm.send('controlsState', dat)
|
||||
# CLEARPILOT: cache for re-publication on parked-skip cycles
|
||||
self._cached_controlsState_bytes = dat.to_bytes()
|
||||
|
||||
# onroadEvents - logged every second or on change
|
||||
if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev):
|
||||
@@ -1047,7 +928,6 @@ class Controls:
|
||||
cc_send.valid = CS.canValid
|
||||
cc_send.carControl = CC
|
||||
self.pm.send('carControl', cc_send)
|
||||
self._cached_carControl_bytes = cc_send.to_bytes()
|
||||
|
||||
# copy CarControl to pass to CarInterface on the next iteration
|
||||
self.CC = CC
|
||||
@@ -1078,11 +958,6 @@ class Controls:
|
||||
while True:
|
||||
self.step()
|
||||
self.rk.monitor_time()
|
||||
# CLEARPILOT: accumulated debt from startup/blocking calls never recovers.
|
||||
# Reset the rate keeper when catastrophically behind — prevents a one-time
|
||||
# ~8s fingerprinting stall from poisoning the lag metric for the whole session.
|
||||
if self.rk.remaining < -1.0:
|
||||
self.rk._next_frame_time = time.monotonic() + self.rk._interval
|
||||
except SystemExit:
|
||||
e.set()
|
||||
t.join()
|
||||
@@ -1153,11 +1028,13 @@ class Controls:
|
||||
elif self.sm['modelV2'].meta.turnDirection == Desire.turnRight:
|
||||
self.events.add(EventName.turningRight)
|
||||
|
||||
# CLEARPILOT: check crash file once per second, not every cycle (stat syscall at 100Hz hurts)
|
||||
if self.sm.frame % 100 == 0:
|
||||
self._crashed_file_exists = os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt'))
|
||||
if getattr(self, '_crashed_file_exists', False) 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)
|
||||
|
||||
# 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:
|
||||
@@ -1280,18 +1157,10 @@ class Controls:
|
||||
|
||||
self.FPCC.trafficModeActive = self.frogpilot_variables.traffic_mode and self.params_memory.get_bool("TrafficModeActive")
|
||||
|
||||
# CLEARPILOT: send only when any field changes, with 1Hz keepalive
|
||||
# Saves ~7ms/sec of capnp+zmq work (was sending 100Hz despite static contents)
|
||||
# latRequested and noLatLaneChange are edge-triggered too (rare flips)
|
||||
fpcc_tuple = (self.FPCC.alwaysOnLateral, self.FPCC.speedLimitChanged, self.FPCC.trafficModeActive,
|
||||
self.FPCC.latRequested, self.FPCC.noLatLaneChange)
|
||||
if fpcc_tuple != self._prev_fpcc or (self.sm.frame - self._last_fpcc_send_frame) >= 100:
|
||||
fpcc_send = messaging.new_message('frogpilotCarControl')
|
||||
fpcc_send.valid = CS.canValid
|
||||
fpcc_send.frogpilotCarControl = self.FPCC
|
||||
self.pm.send('frogpilotCarControl', fpcc_send)
|
||||
self._prev_fpcc = fpcc_tuple
|
||||
self._last_fpcc_send_frame = self.sm.frame
|
||||
fpcc_send = messaging.new_message('frogpilotCarControl')
|
||||
fpcc_send.valid = CS.canValid
|
||||
fpcc_send.frogpilotCarControl = self.FPCC
|
||||
self.pm.send('frogpilotCarControl', fpcc_send)
|
||||
|
||||
if self.params_memory.get_bool("FrogPilotTogglesUpdated"):
|
||||
self.update_frogpilot_params()
|
||||
@@ -1364,53 +1233,32 @@ class Controls:
|
||||
self.frogpilot_variables.use_ev_tables = self.params.get_bool("EVTable")
|
||||
|
||||
def update_clearpilot_events(self, CS):
|
||||
if (len(CS.buttonEvents) > 0):
|
||||
if (len(CS.buttonEvents) > 0):
|
||||
print (CS.buttonEvents)
|
||||
if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
|
||||
self.events.add(EventName.clpDebug)
|
||||
|
||||
def handle_screen_mode(self, CS):
|
||||
"""CLEARPILOT: tracks driving_gear, auto-resets display, and cycles
|
||||
ScreenDisplayMode on debug-button presses. Must run every cycle so button
|
||||
edge events aren't lost during parked-skip mode (edges happen in carstate
|
||||
edge detection and only appear in that one cycle's CS.buttonEvents)."""
|
||||
self.driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown)
|
||||
|
||||
# auto-reset display when shifting into drive from screen-off
|
||||
if self.driving_gear and not self.was_driving_gear:
|
||||
if self.params_memory.get_int("ScreenDisplayMode") == 3:
|
||||
self.params_memory.put_int("ScreenDisplayMode", 0)
|
||||
self.was_driving_gear = self.driving_gear
|
||||
|
||||
if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
|
||||
current = self.params_memory.get_int("ScreenDisplayMode")
|
||||
|
||||
if self.driving_gear:
|
||||
# Onroad: 0→4, 1→2, 2→3, 3→4, 4→2 (never back to auto via button)
|
||||
transitions = {0: 4, 1: 2, 2: 3, 3: 4, 4: 2}
|
||||
new_mode = transitions.get(current, 0)
|
||||
else:
|
||||
# Not in drive: any except 3 → 3, state 3 → 0
|
||||
new_mode = 0 if current == 3 else 3
|
||||
|
||||
self.params_memory.put_int("ScreenDisplayMode", new_mode)
|
||||
# Uncomment to alert when lkas button pressed
|
||||
# if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
|
||||
# self.events.add(EventName.clpDebug)
|
||||
|
||||
def clearpilot_state_control(self, CC, CS):
|
||||
if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
|
||||
|
||||
# Uncomment for debug testing.
|
||||
# if self.params_memory.get_bool("CPTLkasButtonAction"):
|
||||
# self.params_memory.put_bool("CPTLkasButtonAction", False)
|
||||
# else:
|
||||
# self.params_memory.put_bool("CPTLkasButtonAction", True)
|
||||
|
||||
# Rotate display mode. These are mostly used in the frontend ui app.
|
||||
max_display_mode = 2
|
||||
current_display_mode = self.params_memory.get_int("ScreenDisplayMode")
|
||||
current_display_mode = current_display_mode + 1
|
||||
if current_display_mode > max_display_mode:
|
||||
current_display_mode = 0
|
||||
self.params_memory.put_int("ScreenDisplayMode", current_display_mode)
|
||||
|
||||
# ClearPilot speed processing (~2 Hz at 100 Hz loop)
|
||||
self.speed_state_frame += 1
|
||||
if self.speed_state_frame % 50 == 0:
|
||||
gps = self.sm['gpsLocation']
|
||||
has_gps = self.sm.valid['gpsLocation'] and gps.hasFix
|
||||
speed_ms = gps.speed if has_gps else 0.0
|
||||
speed_limit_ms = self.params_memory.get_float("CarSpeedLimit")
|
||||
is_metric = (self.params_memory.get("CarIsMetric", encoding="utf-8") or "0") == "1"
|
||||
cruise_speed_ms = CS.cruiseState.speed
|
||||
cruise_active = CS.cruiseState.enabled
|
||||
cruise_standstill = CS.cruiseState.standstill
|
||||
self.speed_state.update(speed_ms, has_gps, speed_limit_ms, is_metric,
|
||||
cruise_speed_ms, cruise_active, cruise_standstill)
|
||||
|
||||
# Leftovers
|
||||
# self.params_memory.put_int("SpeedLimitLatDesired", CC.actuators.speed * CV.MS_TO_MPH )
|
||||
return CC
|
||||
|
||||
def main():
|
||||
|
||||
Reference in New Issue
Block a user