controlsd: re-apply QoL hooks on top of reverted baseline

Re-adds the non-self-drive controlsd integrations that the UI and
memory-param pipeline need:

- import SpeedState from clearpilot.speed_logic
- self.speed_state, speed_state_frame, was_driving_gear init
- subscribe to gpsLocation (ignored in alive/freq_ok/valid gates — OK if
  the GPS daemon isn't publishing)
- clearpilot_state_control:
  - auto-reset ScreenDisplayMode 3→0 on park→drive transition
  - ~2Hz speed-state update driving the speed-limit sign and cruise
    over/under warning sign via ClearpilotCruiseWarning /
    ClearpilotSpeedLimitDisplay memory params

The debug-button (LFA) ScreenDisplayMode cycling already lived in the
reverted baseline (it was in the first commit), so it's preserved.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>

CLAUDE.md

@@ -15,7 +15,7 @@ ClearPilot is a custom fork of **FrogPilot** (itself a fork of comma.ai's openpi
 - **Native dashcamd**: C++ process capturing raw camera frames via VisionIPC with OMX H.264 hardware encoding
 - **Standstill power saving**: model inference throttled to 1fps and fan quieted when car is stopped
 - **ClearPilot menu**: sidebar settings panel replacing stock home screen (Home, Dashcam, Debug panels)
-- **Status window**: live system stats (temp, fan, storage, RAM, WiFi, VPN, GPS, telemetry status)
+- **Status window**: live system stats (temp, fan, storage, RAM, WiFi, VPN, GPS, telemetry, dashcam status)
 - **Debug button (LFA)**: steering wheel button repurposed for screen toggle and future UI actions
 - **Telemetry system**: diff-based CSV logger via ZMQ IPC, toggleable from Debug panel
 - **Bench mode**: `--bench` flag for onroad UI testing without car connected
@@ -37,6 +37,20 @@ This is self-driving software. All changes must be deliberate and well-understoo
 - **Test everything thoroughly** — Brian must always be in the loop
 - **Do not refactor, clean up, or "improve" code beyond the specific request**
 
+### Logging
+
+**NEVER use `cloudlog`.** It's comma.ai's cloud telemetry pipeline, not ours — writes go to a publisher that's effectively a black hole for us (and the only thing it could do if ever reachable is bother the upstream FrogPilot developer). Our changes must always use **file logging** instead.
+
+Use `print(..., file=sys.stderr, flush=True)`. `manager.py` redirects each managed process's stderr to `/data/log2/current/{process}.log`, so these lines land in the per-process log we already grep. Prefix custom log lines with `CLP ` so they're easy to filter out from upstream noise.
+
+Example:
+```python
+import sys
+print(f"CLP frogpilotPlan valid=False: carState_freq_ok={sm.freq_ok['carState']}", file=sys.stderr, flush=True)
+```
+
+Do not use `cloudlog.warning`, `cloudlog.info`, `cloudlog.error`, `cloudlog.event`, or `cloudlog.exception` in any CLEARPILOT-added code. Existing upstream/FrogPilot `cloudlog` calls can stay untouched.
+
 ### File Ownership
 
 We operate as `root` on this device, but openpilot runs as the `comma` user (uid=1000, gid=1000). After any code changes that touch multiple files or before testing:
@@ -84,6 +98,31 @@ ls /data/log2/current/
 cat /data/log2/current/gpsd.log
 ```
 
+### Changing a Service's Publish Rate
+
+SubMaster's `freq_ok` check requires observed rate to fall within
+`[0.8 × min_freq, 1.2 × max_freq]` of the value declared in
+`cereal/services.py`. Publishing *faster* than declared fails the check
+just as surely as publishing too slowly — it trips `all_freq_ok()` →
+`EventName.commIssue` → "TAKE CONTROL IMMEDIATELY / Communication Issue
+Between Processes".
+
+If you change how often a process publishes, you **must** update the rate
+in `cereal/services.py` to match. Example (real bug we hit):
+
+- Bumped thermald from 2Hz → 4Hz (DT_TRML 0.5→0.25) for faster fan control
+- Bumped gpsd from 1Hz → 2Hz
+- manager publishes `managerState` gated by `sm.update()` returning on
+  `deviceState`, so it picks up thermald's rate automatically
+
+`services.py` still declared `deviceState` and `managerState` as 2Hz
+(ceiling 2.4Hz) and `gpsLocation` as 1Hz (ceiling 1.2Hz), so controlsd
+fired `commIssue` on every cycle the moment any of these arrived at the
+new faster rate.
+
+Fix: update the second tuple element (Hz) in `cereal/services.py` for the
+affected service. Cereal will use the updated rate for all consumers.
+
 ### Adding New Params
 
 The params system uses a C++ whitelist. Adding a new param name in `manager.py` alone will crash with `UnknownKeyName`. You must:
@@ -101,7 +140,7 @@ ClearPilot uses memory params (`/dev/shm/params/d/`) for UI toggles that should
 - **Python access**: Use `Params("/dev/shm/params")`
 - **Defaults**: Set in `manager_init()` via `Params("/dev/shm/params").put(key, value)`
 - **UI toggles**: Use `ToggleControl` with manual `toggleFlipped` lambda that writes via `Params("/dev/shm/params")`. Do NOT use `ParamControl` for memory params — it reads/writes persistent params only
-- **Current memory params**: `TelemetryEnabled` (default "0"), `VpnEnabled` (default "1"), `ModelStandby` (default "0"), `ScreenDisplayMode`
+- **Current memory params**: `TelemetryEnabled` (default "0"), `VpnEnabled` (default "1"), `ModelStandby` (default "0"), `ScreenDisplayMode`, `DashcamState` (default "stopped"), `DashcamFrames` (default "0"), `DashcamShutdown` (default "0")
 - **IMPORTANT — method names differ between C++ and Python**: C++ uses camelCase (`putBool`, `getBool`, `getInt`), Python uses snake_case (`put_bool`, `get_bool`, `get_int`). This is a common source of silent failures — the wrong casing compiles/runs but doesn't work.
 
 ### Building Native (C++) Processes
@@ -195,6 +234,43 @@ The UI process runs a ZMQ REP server at `ipc:///tmp/clearpilot_ui_rpc`. Send `"d
 - **`showDriverView` overriding transitions (fixed)**: was forcing `slayout` to onroad/home every frame at 20Hz, overriding park/drive logic. Fixed to only act when not in started state.
 - **Sidebar appearing during onroad transition (fixed)**: `MainWindow::closeSettings()` was re-enabling the sidebar. Fixed by not calling `closeSettings` during `offroadTransition`.
 
+## Performance Profiling
+
+Use `py-spy` to find CPU hotspots in any Python process. It's installed at `/home/comma/.local/bin/py-spy`. (If missing: `su - comma -c "/usr/local/pyenv/versions/3.11.4/bin/pip install py-spy"`.)
+
+```bash
+# Find the target pid
+ps -eo pid,cmd | grep -E "selfdrive.controls.controlsd" | grep -v grep
+
+# Record 10s of stacks at 200Hz, raw (folded) format
+/home/comma/.local/bin/py-spy record -o /tmp/ctrl.txt --pid <PID> --duration 10 --rate 200 --format raw
+
+# Aggregate: which line of step() is consuming the most samples
+awk -F';' '{
+  for(i=1;i<=NF;i++) if ($i ~ /step \(selfdrive\/controls\/controlsd.py/) step_line=i;
+  if (step_line && step_line < NF) {
+    n=split($NF, parts, " "); count=parts[n];
+    caller = $(step_line+1);
+    sum[caller] += count;
+  }
+  step_line=0;
+} END { for (c in sum) printf "%6d  %s\n", sum[c], c }' /tmp/ctrl.txt | sort -rn | head -15
+
+# Aggregate by a source file — shows hottest lines in that file
+awk -F';' '{
+  for(i=1;i<=NF;i++) if ($i ~ /carstate\.py:/) {
+    match($i, /:[0-9]+/); ln = substr($i, RSTART+1, RLENGTH-1);
+    n=split($NF, parts, " "); count=parts[n];
+    sum[ln] += count;
+  }
+} END { for (l in sum) printf "%5d  line %s\n", sum[l], l }' /tmp/ctrl.txt | sort -rn | head -15
+
+# Quick stack dump (single sample, no recording)
+/home/comma/.local/bin/py-spy dump --pid <PID>
+```
+
+**Known performance trap — hot `Params` writes**: `Params.put()` does `mkstemp` + `fsync` + `flock` + `rename` + `fsync_dir`. At 100Hz even on tmpfs the `flock` contention is ruinous. Cache the last-written value and skip writes when unchanged. Found this pattern in `carstate.py` and `controlsd.py` — controlsd went from 69% → 28% CPU after gating writes.
+
 ## Session Logging
 
 Per-process stderr and an aggregate event log are captured in `/data/log2/current/`.
@@ -245,20 +321,21 @@ A single `session.log` in each session directory records major events:
 - **Segment length**: 3 minutes per file
 - **Save path**: `/data/media/0/videos/YYYYMMDD-HHMMSS/YYYYMMDD-HHMMSS.mp4` (trip directories)
 - **GPS subtitles**: companion `.srt` file per segment with 1Hz entries (speed MPH, lat/lon, UTC timestamp)
-- **Trip lifecycle**: starts recording on launch with 10-min idle timer; car in drive cancels timer; park/off restarts timer; ignition cycle = new trip
+- **Trip lifecycle**: waits in WAITING state until valid system time + GPS fix + car in drive; records until car parked 10 min or ignition off; then returns to WAITING
 - **Graceful shutdown**: thermald sets `DashcamShutdown` param, dashcamd closes segment and acks within 15s
-- **Storage**: ~56 MB per 3-minute segment at 2500 kbps
+- **Storage**: ~56 MB per 3-minute segment at 2500 kbps (verified: actual bitrate ~2570 kbps)
+- **Crash handler**: SIGSEGV/SIGABRT handler writes backtrace to `/tmp/dashcamd_crash.log`
 - **Storage device**: WDC SDINDDH4-128G UFS 2.1 — automotive grade, ~384 TB write endurance, no concern for continuous writes
 
-### Key Differences from Old Screen Recorder
+### OmxEncoder
 
-| | Old (screen recorder) | New (dashcamd) |
+The OMX encoder (`selfdrive/frogpilot/screenrecorder/omx_encoder.cc`) was ported from upstream FrogPilot with the following key properties:
-|---|---|---|
+
-| Source | `QWidget::grab()` screen capture | Raw NV12 from VisionIPC |
+- Each encoder instance calls `OMX_Init()` in constructor and `OMX_Deinit()` in destructor — manages its own OMX lifecycle
-| Resolution | 1440x720 | 1928x1208 |
+- Constructor takes 5 args: `(path, width, height, fps, bitrate)` — no h265/downscale params
-| Works with screen off | No (needs visible widget) | Yes (independent of UI) |
+- `encoder_close()` calls `av_write_trailer` + ffmpeg faststart remux (`-movflags +faststart`)
-| Process type | Part of UI process | Standalone native process |
+- Destructor has null guards and error handling on all OMX state transitions
-| Encoder input | RGBA -> NV12 conversion | NV12 direct (added `encode_frame_nv12`) |
+- ClearPilot addition: `encode_frame_nv12()` for direct NV12 input (dashcamd), alongside original `encode_frame_rgba()` (screen recorder)
 
 ### Key Files
 
@@ -266,21 +343,22 @@ A single `session.log` in each session directory records major events:
 |------|------|
 | `selfdrive/clearpilot/dashcamd.cc` | Main dashcam process — VisionIPC -> OMX encoder |
 | `selfdrive/clearpilot/SConscript` | Build config for dashcamd |
-| `selfdrive/frogpilot/screenrecorder/omx_encoder.cc` | OMX encoder (added `encode_frame_nv12` method) |
+| `selfdrive/frogpilot/screenrecorder/omx_encoder.cc` | OMX encoder (upstream FrogPilot port + `encode_frame_nv12`) |
 | `selfdrive/frogpilot/screenrecorder/omx_encoder.h` | Encoder header |
 | `selfdrive/manager/process_config.py` | dashcamd registered as NativeProcess, camerad always_run, encoderd disabled |
 | `system/loggerd/deleter.py` | Trip-aware storage rotation (oldest trip first, then segments within active trip) |
 
 ### Params
 
-- `DashcamDebug` — when `"1"`, dashcamd runs even without car connected (for bench testing)
+- `DashcamShutdown` (memory param) — set by thermald before power-off, dashcamd acks by clearing it
-- `DashcamShutdown` — set by thermald before power-off, dashcamd acks by clearing it
+- `DashcamState` (memory param) — "stopped", "waiting", or "recording" — published every 5s
+- `DashcamFrames` (memory param) — per-trip encoded frame count, resets each trip — published every 5s
 
 ## Standstill Power Saving
 
 When `carState.standstill` is true:
 
-- **modeld**: skips GPU inference on 19/20 frames (1fps vs 20fps), reports 0 frame drops to avoid triggering `modeldLagging` in controlsd
+- **modeld**: skips GPU inference on 19/20 frames (1fps vs 20fps), reports 0 frame drops to avoid triggering `modeldLagging` in controlsd. Runs full 20fps during calibration (`liveCalibration.calStatus != calibrated`)
 - **dmonitoringmodeld**: same 1fps throttle, added `carState` subscription
 - **Fan controller**: uses offroad clamps (0-30%) instead of onroad (30-100%) at standstill; thermal protection still active via feedforward if temp > 60°C
 
@@ -321,6 +399,12 @@ The Hyundai Tucson's LFA steering wheel button cycles through 5 display modes vi
 
 **Not in drive (parked/off):** any except 3 → 3 (screen off), state 3 → 0 (auto-normal)
 
+**Shift to drive from screen off:** auto-resets to mode 0 (auto-normal) via `controlsd`
+
+**Shift to park from nightrider:** auto-switches to mode 3 (screen off) via `home.cc`
+
+**Tap screen while screen off:** resets to mode 0 (auto-normal) via `window.cc` touch handler
+
 ### Nightrider Mode
 
 - Camera feed suppressed (OpenGL clears to black instead of rendering camera texture)
@@ -357,7 +441,10 @@ Display power is managed by `Device::updateWakefulness()` in `selfdrive/ui/ui.cc
 
 - **Ignition off (offroad)**: screen blanks after `ScreenTimeout` seconds (default 120) of no touch. Tapping wakes it.
 - **Ignition on (onroad)**: screen stays on unconditionally — ignition=true short-circuits the timeout check.
-- **Debug button (LFA)**: cycles through display modes including screen off (state 3). Only state 3 calls `Hardware::set_display_power(false)`.
+- **ScreenDisplayMode 3 override**: `updateWakefulness` checks `ScreenDisplayMode` first — if mode 3, calls `setAwake(false)` unconditionally, preventing ignition-on from overriding screen-off.
+- **Debug button (LFA)**: cycles through display modes including screen off (state 3).
+- **Park transition**: always shows splash screen; if coming from nightrider mode, auto-switches to screen off (mode 3) via `home.cc`.
+- **Touch wake**: tapping screen while in mode 3 resets to mode 0 via `window.cc` event filter.
 
 ## Offroad UI (ClearPilot Menu)
 
@@ -475,7 +562,7 @@ Power On
 
 ### ClearPilot Processes
 
-- `dashcamd` — raw camera dashcam recording (runs onroad or with DashcamDebug flag)
+- `dashcamd` — raw camera dashcam recording (always runs; manages its own trip lifecycle)
 
 ### GPS
 
@@ -500,7 +587,7 @@ Power On
 - **GPS data**: logged directly by telemetryd via cereal `gpsLocation` subscription at 1Hz — group: `gps` (latitude, longitude, speed, altitude, bearing, accuracy)
 - **CSV location**: `/data/log2/current/telemetry.csv` (or session directory)
 - **Onroad overlay**: when telemetry enabled, status bar shows temp, fan %, model FPS, and STANDSTILL indicator
-- **Speed limit**: `speed_limit.calculated` is the final computed speed limit value (in vehicle units, MPH when `is_metric=False`). This is the value that will be used for the future speed limit warning chime feature
+- **Speed limit**: processed by `selfdrive/clearpilot/speed_logic.py` (SpeedState class), converts m/s to display units, writes to memory params. Cruise warning signs appear when cruise set speed exceeds speed limit by threshold (10 mph if limit >= 50, 7 mph if < 50) or is 5+ mph under. Ding sound plays when warning sign appears or speed limit changes while visible (30s cooldown)
 
 ### Key Dependencies
 

cereal/custom.capnp

@@ -12,6 +12,9 @@ struct FrogPilotCarControl @0x81c2f05a394cf4af {
   alwaysOnLateral @0 :Bool;
   speedLimitChanged @1 :Bool;
   trafficModeActive @2 :Bool;
+  # CLEARPILOT: migrated from paramsMemory to avoid file-read syscalls in modeld/UI/carcontroller
+  latRequested @3 :Bool;        # controlsd's request to enable lat before ramp-up delay
+  noLatLaneChange @4 :Bool;     # lat is temporarily suppressed during a lane change
 }
 
 struct FrogPilotCarState @0xaedffd8f31e7b55d {

cereal/services.py

@@ -30,7 +30,7 @@ services: dict[str, tuple] = {
   "temperatureSensor": (True, 2., 200),
   "temperatureSensor2": (True, 2., 200),
   "gpsNMEA": (True, 9.),
-  "deviceState": (True, 2., 1),
+  "deviceState": (True, 5., 1),  # CLEARPILOT: 5Hz — thermald decimates pandaStates (10Hz) every 2 ticks
   "can": (True, 100., 1223),  # decimation gives ~5 msgs in a full segment
   "controlsState": (True, 100., 10),
   "pandaStates": (True, 10., 1),
@@ -50,7 +50,7 @@ services: dict[str, tuple] = {
   "longitudinalPlan": (True, 20., 5),
   "procLog": (True, 0.5, 15),
   "gpsLocationExternal": (True, 10., 10),
-  "gpsLocation": (True, 1., 1),
+  "gpsLocation": (True, 2., 1),  # CLEARPILOT: 2Hz (was 1Hz) — gpsd polls at 2Hz
   "ubloxGnss": (True, 10.),
   "qcomGnss": (True, 2.),
   "gnssMeasurements": (True, 10., 10),
@@ -70,7 +70,7 @@ services: dict[str, tuple] = {
   "wideRoadEncodeIdx": (False, 20., 1),
   "wideRoadCameraState": (True, 20., 20),
   "modelV2": (True, 20., 40),
-  "managerState": (True, 2., 1),
+  "managerState": (True, 5., 1),  # CLEARPILOT: 5Hz — gated by deviceState arrival (see thermald)
   "uploaderState": (True, 0., 1),
   "navInstruction": (True, 1., 10),
   "navRoute": (True, 0.),

common/params.cc

@@ -109,7 +109,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"CurrentBootlog", PERSISTENT},
     {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
-    {"DashcamDebug", PERSISTENT},
+    {"DashcamFrames", PERSISTENT},
+    {"DashcamState", PERSISTENT},
     {"DashcamShutdown", CLEAR_ON_MANAGER_START},
     {"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"DisablePowerDown", PERSISTENT},
@@ -192,12 +193,14 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"RecordFrontLock", PERSISTENT},  // for the internal fleet
     {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"RouteCount", PERSISTENT},
+    {"ShutdownTouchReset", PERSISTENT},
     {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
     {"SshEnabled", PERSISTENT},
     {"TermsVersion", PERSISTENT},
     {"TelemetryEnabled", PERSISTENT},
     {"Timezone", PERSISTENT},
     {"TrainingVersion", PERSISTENT},
+    {"ModelFps", PERSISTENT},
     {"ModelStandby", PERSISTENT},
     {"ModelStandbyTs", PERSISTENT},
     {"UbloxAvailable", PERSISTENT},
@@ -246,7 +249,6 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CarMake", PERSISTENT},
     {"CarModel", PERSISTENT},
 
-    {"CarCruiseDisplayActual", PERSISTENT},
     {"CarSpeedLimit", PERSISTENT},
     
     {"CarSpeedLimitWarning", PERSISTENT},
@@ -288,6 +290,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CEStopLights", PERSISTENT},
     {"CEStopLightsLead", PERSISTENT},
     {"Compass", PERSISTENT},
+    {"ClearpilotShowHealthMetrics", PERSISTENT},
     {"ConditionalExperimental", PERSISTENT},
     {"CrosstrekTorque", PERSISTENT},
     {"CurrentHolidayTheme", PERSISTENT},
@@ -505,8 +508,6 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"WheelIcon", PERSISTENT},
     {"WheelSpeed", PERSISTENT},
 
-    // Clearpilot
-    {"no_lat_lane_change", PERSISTENT},
 };
 
 } // namespace

common/realtime.py

@@ -12,7 +12,7 @@ from openpilot.system.hardware import PC
 # time step for each process
 DT_CTRL = 0.01  # controlsd
 DT_MDL = 0.05  # model
-DT_TRML = 0.5  # thermald and manager
+DT_TRML = 0.25  # thermald and manager — 4 Hz
 DT_DMON = 0.05  # driver monitoring
 
 

selfdrive/car/hyundai/carcontroller.py

@@ -8,7 +8,6 @@ from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican
 from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
 from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR
 from openpilot.selfdrive.car.interfaces import CarControllerBase
-from openpilot.common.params import Params
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
@@ -57,6 +56,7 @@ class CarController(CarControllerBase):
     self.car_fingerprint = CP.carFingerprint
     self.last_button_frame = 0
 
+
   def update(self, CC, CS, now_nanos, frogpilot_variables):
     actuators = CC.actuators
     hud_control = CC.hudControl
@@ -112,7 +112,9 @@ class CarController(CarControllerBase):
       hda2_long = hda2 and self.CP.openpilotLongitudinalControl
 
       # steering control
-      can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_steer))
+      # CLEARPILOT: no_lat_lane_change comes via frogpilot_variables (set by controlsd in-process)
+      no_lat_lane_change = int(getattr(frogpilot_variables, 'no_lat_lane_change', False))
+      can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_steer, no_lat_lane_change))
 
       # prevent LFA from activating on HDA2 by sending "no lane lines detected" to ADAS ECU
       if self.frame % 5 == 0 and hda2:

selfdrive/car/hyundai/carstate.py

@@ -48,6 +48,10 @@ class CarState(CarStateBase):
     self.is_metric = False
     self.buttons_counter = 0
 
+    # CLEARPILOT: cache to avoid per-cycle atomic writes to /dev/shm (eats CPU via fsync/flock)
+    self._prev_car_speed_limit = None
+    self._prev_car_is_metric = None
+
     self.cruise_info = {}
 
     # On some cars, CLU15->CF_Clu_VehicleSpeed can oscillate faster than the dash updates. Sample at 5 Hz
@@ -210,10 +214,14 @@ class CarState(CarStateBase):
       self.lkas_previously_enabled = self.lkas_enabled
       self.lkas_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"]
 
-    # self.params_memory.put_int("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
+    # CLEARPILOT: gate on change — see same fix in update_canfd
-    self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_conv)
+    car_speed_limit = self.calculate_speed_limit(cp, cp_cam) * speed_conv
+    if car_speed_limit != self._prev_car_speed_limit:
+      self.params_memory.put_float("CarSpeedLimit", car_speed_limit)
+      self._prev_car_speed_limit = car_speed_limit
+    if self.is_metric != self._prev_car_is_metric:
       self.params_memory.put("CarIsMetric", "1" if self.is_metric else "0")
-    self.params_memory.put_float("CarCruiseDisplayActual", cp_cruise.vl["SCC11"]["VSetDis"])
+      self._prev_car_is_metric = self.is_metric
 
 
     return ret
@@ -417,11 +425,14 @@ class CarState(CarStateBase):
     #   nonAdaptive = false,
     #   speedCluster = 0 )
 
-    # print("Set limit")
+    # CLEARPILOT: gate on change — these writes run 100Hz, each is an atomic fsync/flock transaction
-    # print(self.calculate_speed_limit(cp, cp_cam))
+    car_speed_limit = self.calculate_speed_limit(cp, cp_cam) * speed_factor
-    # self.params_memory.put_float("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
+    if car_speed_limit != self._prev_car_speed_limit:
-    self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_factor)
+      self.params_memory.put_float("CarSpeedLimit", car_speed_limit)
+      self._prev_car_speed_limit = car_speed_limit
+    if self.is_metric != self._prev_car_is_metric:
       self.params_memory.put("CarIsMetric", "1" if self.is_metric else "0")
+      self._prev_car_is_metric = self.is_metric
 
     # CLEARPILOT: telemetry logging — disabled, re-enable when needed
     # speed_limit_bus = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam

selfdrive/car/hyundai/hyundaicanfd.py

@@ -2,7 +2,6 @@ from openpilot.common.numpy_fast import clip
 from openpilot.selfdrive.car import CanBusBase
 from openpilot.selfdrive.car.hyundai.values import HyundaiFlags
 
-from openpilot.common.params import Params
 
 class CanBus(CanBusBase):
   def __init__(self, CP, hda2=None, fingerprint=None) -> None:
@@ -36,12 +35,9 @@ class CanBus(CanBusBase):
     return self._cam
 
 
-def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer):
+def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer, no_lat_lane_change=0):
-
+  # CLEARPILOT: no_lat_lane_change is passed in by the caller so we can hoist
-  # Im sure there is a better way to do this
+  # the Params read out of the 100Hz hot path (was ~5% of carcontroller time)
-  params_memory = Params("/dev/shm/params")
-  no_lat_lane_change = params_memory.get_int("no_lat_lane_change", 1)
-
   ret = []
 
   values = {
@@ -130,8 +126,6 @@ def create_buttons(packer, CP, CAN, cnt, btn):
 
 def create_buttons_alt(packer, CP, CAN, cnt, btn, template):
   return
-  params_memory = Params("/dev/shm/params")
-  CarCruiseDisplayActual = params_memory.get_float("CarCruiseDisplayActual")
 
   values = {
     "COUNTER": cnt,

selfdrive/clearpilot/dashcamd.cc

@@ -7,49 +7,41 @@
  * Trip directory structure:
  *   /data/media/0/videos/YYYYMMDD-HHMMSS/   (trip directory, named at trip start)
  *     YYYYMMDD-HHMMSS.mp4                    (3-minute segments)
+ *     YYYYMMDD-HHMMSS.srt                    (GPS subtitle sidecar)
  *
  * Trip lifecycle state machine:
  *
- *   On process start (after time-valid wait):
+ *   WAITING:
- *     - Create trip directory, start recording immediately with 10-min idle timer
+ *     - Process starts in this state
- *     - If car is already in drive, timer is cancelled and recording continues
+ *     - Waits for valid system time (year >= 2024) AND car in drive gear
- *     - If car stays parked/off for 10 minutes, trip ends
+ *     - Transitions to RECORDING when both conditions met
  *
- *   IDLE_TIMEOUT → RECORDING:
+ *   RECORDING:
- *     - Car enters drive gear before timer expires → cancel timer, resume recording
+ *     - Actively encoding frames, car is in drive
- *       in the same trip (no new trip directory)
+ *     - Car leaves drive → start 10-min idle timer → IDLE_TIMEOUT
  *
- *   RECORDING → IDLE_TIMEOUT:
+ *   IDLE_TIMEOUT:
- *     - Car leaves drive gear (park, off, neutral) → start 10-minute idle timer,
+ *     - Car left drive, still recording with timer running
- *       continue recording frames during the timeout period
+ *     - Car re-enters drive → cancel timer → RECORDING
- *
+ *     - Timer expires → close trip → WAITING
- *   IDLE_TIMEOUT → TRIP_ENDED:
+ *     - Ignition off → close trip → WAITING
- *     - 10-minute timer expires → close segment, trip is over
- *
- *   TRIP_ENDED → RECORDING (new trip):
- *     - Car enters drive gear → create new trip directory, start recording
- *
- *   Any state → (new trip) on ignition off→on:
- *     - Ignition transitions off→on → close current trip if active, create new trip
- *       directory, start recording with 10-min idle timer. This applies even from
- *       TRIP_ENDED — turning the car on always starts a new trip. If the car is in
- *       park after ignition on, the idle timer runs; entering drive cancels it.
  *
  *   Graceful shutdown (DashcamShutdown param):
  *     - thermald sets DashcamShutdown="1" before device power-off
- *     - dashcamd closes current segment, sets DashcamShutdown="0" (ack), exits
+ *     - dashcamd closes current segment, acks, exits
- *     - thermald waits up to 15s for ack, then proceeds with shutdown
  *
- *   GPS subtitle track:
+ *   Published params (memory, every 5s):
- *     - Each .mp4 segment has a companion .srt subtitle file
+ *     - DashcamState: "waiting" or "recording"
- *     - Updated at most once per second from gpsLocation cereal messages
+ *     - DashcamFrames: per-trip encoded frame count (resets each trip)
- *     - Format: "35 MPH | 44.9216°N 93.3260°W | 2026-04-13 05:19:00 UTC"
- *     - Most players auto-detect .srt files alongside .mp4 files
  */
 
 #include <cstdio>
 #include <ctime>
 #include <string>
+#include <errno.h>
+#include <signal.h>
+#include <sched.h>
+#include <execinfo.h>
 #include <sys/stat.h>
 #include <sys/resource.h>
 #include <unistd.h>
@@ -73,10 +65,9 @@ const double IDLE_TIMEOUT_SECONDS = 600.0;  // 10 minutes
 ExitHandler do_exit;
 
 enum TripState {
-  IDLE,           // no trip active, waiting for drive
+  WAITING,        // no trip, waiting for valid time + drive gear
   RECORDING,      // actively recording, car in drive
-  IDLE_TIMEOUT,   // car parked/off, recording with 10-min timer
+  IDLE_TIMEOUT,   // car left drive, recording with 10-min timer
-  TRIP_ENDED,     // trip closed, waiting for next drive
 };
 
 static std::string make_timestamp() {
@@ -115,21 +106,40 @@ static std::string srt_time(int seconds) {
   return std::string(buf);
 }
 
+static void crash_handler(int sig) {
+  FILE *f = fopen("/tmp/dashcamd_crash.log", "w");
+  if (f) {
+    fprintf(f, "CRASH: signal %d\n", sig);
+    void *bt[30];
+    int n = backtrace(bt, 30);
+    backtrace_symbols_fd(bt, n, fileno(f));
+    fclose(f);
+  }
+  _exit(1);
+}
+
 int main(int argc, char *argv[]) {
+  signal(SIGSEGV, crash_handler);
+  signal(SIGABRT, crash_handler);
   setpriority(PRIO_PROCESS, 0, -10);
 
+  // CLEARPILOT: pin to cores 0-3 (little cluster). Avoids cache/memory-bandwidth
+  // contention with the RT-pinned processes on the big cluster:
+  //   core 4 = controlsd, core 5 = plannerd/radard, core 7 = modeld.
+  // OMX offloads actual H.264 work to hardware, so the main thread just copies
+  // frames and muxes MP4 — fine on the little cluster.
+  cpu_set_t mask;
+  CPU_ZERO(&mask);
+  for (int i = 0; i < 4; i++) CPU_SET(i, &mask);
+  if (sched_setaffinity(0, sizeof(mask), &mask) != 0) {
+    LOGW("dashcamd: sched_setaffinity failed (%d), continuing unpinned", errno);
+  } else {
+    LOGW("dashcamd: pinned to cores 0-3");
+  }
+
   // Ensure base output directory exists
   mkdir(VIDEOS_BASE.c_str(), 0755);
 
-  // Wait for valid system time so trip/segment names have real timestamps
-  LOGW("dashcamd: waiting for system time");
-  while (!do_exit) {
-    if (system_time_valid()) break;
-    usleep(1000000);  // 1 Hz poll
-  }
-  if (do_exit) return 0;
-  LOGW("dashcamd: system time valid");
-
   LOGW("dashcamd: started, connecting to camerad road stream");
   VisionIpcClient vipc("camerad", VISION_STREAM_ROAD, false);
   while (!do_exit && !vipc.connect(false)) {
@@ -156,51 +166,50 @@ int main(int argc, char *argv[]) {
   // Subscribe to carState (gear), deviceState (ignition), gpsLocation (subtitles)
   SubMaster sm({"carState", "deviceState", "gpsLocation"});
   Params params;
+  Params params_memory("/dev/shm/params");
 
   // Trip state
-  TripState state = IDLE;
+  TripState state = WAITING;
   OmxEncoder *encoder = nullptr;
   std::string trip_dir;
-  int frame_count = 0;
+  int frame_count = 0;     // per-segment (for rotation)
+  int trip_frames = 0;     // per-trip (published to params)
   int recv_count = 0;
   uint64_t segment_start_ts = 0;
   double idle_timer_start = 0.0;
 
   // SRT subtitle state
   FILE *srt_file = nullptr;
-  int srt_index = 0;          // subtitle entry counter (1-based)
+  int srt_index = 0;
-  int srt_segment_sec = 0;    // seconds elapsed in current segment
+  int srt_segment_sec = 0;
-  double last_srt_write = 0;  // monotonic time of last SRT write
+  double last_srt_write = 0;
 
-  // Ignition tracking for off→on detection
+  // Ignition tracking
   bool prev_started = false;
   bool started_initialized = false;
 
-  // Param check throttle (don't hit filesystem every frame)
+  // Param publish throttle
   int param_check_counter = 0;
+  double last_param_write = 0;
 
-  // Helper: start a new trip with recording + optional idle timer
+  // Publish initial state
+  params_memory.put("DashcamState", "waiting");
+  params_memory.put("DashcamFrames", "0");
+
+  LOGW("dashcamd: entering main loop in WAITING state");
+
+  // Helper: start a new trip
   auto start_new_trip = [&]() {
-    // Close existing encoder if any
-    if (encoder) {
-      if (state == RECORDING || state == IDLE_TIMEOUT) {
-        encoder->encoder_close();
-      }
-      delete encoder;
-      encoder = nullptr;
-    }
-
     trip_dir = VIDEOS_BASE + "/" + make_timestamp();
     mkdir(trip_dir.c_str(), 0755);
     LOGW("dashcamd: new trip %s", trip_dir.c_str());
 
-    encoder = new OmxEncoder(trip_dir.c_str(), width, height, CAMERA_FPS, BITRATE, false, false);
+    encoder = new OmxEncoder(trip_dir.c_str(), width, height, CAMERA_FPS, BITRATE);
 
     std::string seg_name = make_timestamp();
     LOGW("dashcamd: opening segment %s", seg_name.c_str());
     encoder->encoder_open((seg_name + ".mp4").c_str());
 
-    // Open companion SRT file
     std::string srt_path = trip_dir + "/" + seg_name + ".srt";
     srt_file = fopen(srt_path.c_str(), "w");
     srt_index = 0;
@@ -208,38 +217,38 @@ int main(int argc, char *argv[]) {
     last_srt_write = 0;
 
     frame_count = 0;
+    trip_frames = 0;
     segment_start_ts = nanos_since_boot();
     state = RECORDING;
+
+    params_memory.put("DashcamState", "recording");
+    params_memory.put("DashcamFrames", "0");
   };
 
+  // Helper: close current trip
   auto close_trip = [&]() {
     if (srt_file) { fclose(srt_file); srt_file = nullptr; }
     if (encoder) {
-      if (state == RECORDING || state == IDLE_TIMEOUT) {
       encoder->encoder_close();
       LOGW("dashcamd: segment closed");
-      }
       delete encoder;
       encoder = nullptr;
     }
-    state = TRIP_ENDED;
+    state = WAITING;
     frame_count = 0;
+    trip_frames = 0;
     idle_timer_start = 0.0;
-    LOGW("dashcamd: trip ended");
+    LOGW("dashcamd: trip ended, returning to WAITING");
-  };
 
-  // Start recording immediately — if the car is in drive, great; if parked/off,
+    params_memory.put("DashcamState", "waiting");
-  // the 10-min idle timer will stop the trip if drive is never detected.
+    params_memory.put("DashcamFrames", "0");
-  start_new_trip();
+  };
-  idle_timer_start = nanos_since_boot() / 1e9;
-  state = IDLE_TIMEOUT;
-  LOGW("dashcamd: recording started, waiting for drive (10-min idle timer active)");
 
   while (!do_exit) {
     VisionBuf *buf = vipc.recv();
     if (buf == nullptr) continue;
 
-    // CLEARPILOT: skip frames to match target FPS (SOURCE_FPS -> CAMERA_FPS)
+    // Skip frames to match target FPS (SOURCE_FPS -> CAMERA_FPS)
     recv_count++;
     if (SOURCE_FPS > CAMERA_FPS && (recv_count % (SOURCE_FPS / CAMERA_FPS)) != 0) continue;
 
@@ -257,60 +266,54 @@ int main(int argc, char *argv[]) {
                      gear == cereal::CarState::GearShifter::LOW ||
                      gear == cereal::CarState::GearShifter::MANUMATIC);
 
-    // Detect ignition off→on transition (new ignition cycle = new trip)
+    // Detect ignition off → close any active trip
-    if (started_initialized && !prev_started && started) {
+    if (started_initialized && prev_started && !started) {
-      LOGW("dashcamd: ignition on — new cycle");
+      LOGW("dashcamd: ignition off");
       if (state == RECORDING || state == IDLE_TIMEOUT) {
         close_trip();
       }
-      // Start recording immediately, idle timer until drive is detected
-      start_new_trip();
-      idle_timer_start = now;
-      state = IDLE_TIMEOUT;
     }
     prev_started = started;
     started_initialized = true;
 
-    // Check for graceful shutdown request (every ~1 second = 20 frames)
+    // Check for graceful shutdown request (every ~1 second)
     if (++param_check_counter >= CAMERA_FPS) {
       param_check_counter = 0;
-      if (params.getBool("DashcamShutdown")) {
+      if (params_memory.getBool("DashcamShutdown")) {
-        LOGW("dashcamd: shutdown requested, closing trip");
+        LOGW("dashcamd: shutdown requested");
         if (state == RECORDING || state == IDLE_TIMEOUT) {
           close_trip();
         }
-        params.putBool("DashcamShutdown", false);
+        params_memory.putBool("DashcamShutdown", false);
         LOGW("dashcamd: shutdown ack sent, exiting");
         break;
       }
     }
 
-    // State machine transitions
+    // State machine
     switch (state) {
-      case IDLE:
+      case WAITING: {
-      case TRIP_ENDED:
+        bool has_gps = sm.valid("gpsLocation") && sm["gpsLocation"].getGpsLocation().getHasFix();
-        if (in_drive) {
+        if (in_drive && system_time_valid() && has_gps) {
           start_new_trip();
         }
         break;
+      }
 
       case RECORDING:
         if (!in_drive) {
-          // Car left drive — start idle timeout
           idle_timer_start = now;
           state = IDLE_TIMEOUT;
-          LOGW("dashcamd: car not in drive, starting 10-min idle timer");
+          LOGW("dashcamd: car left drive, starting 10-min idle timer");
         }
         break;
 
       case IDLE_TIMEOUT:
         if (in_drive) {
-          // Back in drive — cancel timer, resume recording same trip
           idle_timer_start = 0.0;
           state = RECORDING;
           LOGW("dashcamd: back in drive, resuming trip");
         } else if ((now - idle_timer_start) >= IDLE_TIMEOUT_SECONDS) {
-          // Timer expired — end trip
           LOGW("dashcamd: idle timeout expired");
           close_trip();
         }
@@ -341,16 +344,28 @@ int main(int argc, char *argv[]) {
 
     uint64_t ts = nanos_since_boot() - segment_start_ts;
 
+    // Validate buffer before encoding
+    if (buf->y == nullptr || buf->uv == nullptr || buf->width == 0 || buf->height == 0) {
+      LOGE("dashcamd: invalid frame buf y=%p uv=%p %zux%zu, skipping", buf->y, buf->uv, buf->width, buf->height);
+      continue;
+    }
+
     // Feed NV12 frame directly to OMX encoder
     encoder->encode_frame_nv12(buf->y, y_stride, buf->uv, uv_stride, width, height, ts);
     frame_count++;
+    trip_frames++;
+
+    // Publish state every 5 seconds
+    if (now - last_param_write >= 5.0) {
+      params_memory.put("DashcamFrames", std::to_string(trip_frames));
+      last_param_write = now;
+    }
 
     // Write GPS subtitle at most once per second
     if (srt_file && (now - last_srt_write) >= 1.0) {
       last_srt_write = now;
       srt_index++;
 
-      // Read GPS data
       double lat = 0, lon = 0, speed_ms = 0;
       bool has_gps = sm.valid("gpsLocation") && sm["gpsLocation"].getGpsLocation().getHasFix();
       if (has_gps) {
@@ -383,13 +398,11 @@ int main(int argc, char *argv[]) {
   }
 
   // Clean exit
-  if (srt_file) { fclose(srt_file); srt_file = nullptr; }
-  if (encoder) {
   if (state == RECORDING || state == IDLE_TIMEOUT) {
-      encoder->encoder_close();
+    close_trip();
-    }
-    delete encoder;
   }
+  params_memory.put("DashcamState", "stopped");
+  params_memory.put("DashcamFrames", "0");
   LOGW("dashcamd: stopped");
 
   return 0;

selfdrive/clearpilot/speed_logic.py

@@ -24,6 +24,21 @@ class SpeedState:
     self.prev_warning = ""
     self.prev_warning_speed_limit = 0
 
+    # Cache last-written param values — each put() is mkstemp+fsync+flock+rename.
+    # Sentinel None so the first call always writes.
+    self._w_has_speed = None
+    self._w_speed_display = None
+    self._w_speed_limit_display = None
+    self._w_speed_unit = None
+    self._w_is_metric = None
+    self._w_cruise_warning = None
+    self._w_cruise_warning_speed = None
+
+  def _put_if_changed(self, key, value, attr):
+    if getattr(self, attr) != value:
+      self.params_memory.put(key, value)
+      setattr(self, attr, value)
+
   def update(self, speed_ms: float, has_speed: bool, speed_limit_ms: float, is_metric: bool,
              cruise_speed_ms: float = 0.0, cruise_active: bool = False, cruise_standstill: bool = False):
     """
@@ -48,12 +63,12 @@ class SpeedState:
 
     self.prev_speed_limit = speed_limit_int
 
-    # Write display-ready values to params_memory
+    # Write display-ready values to params_memory (gated on change)
-    self.params_memory.put("ClearpilotHasSpeed", "1" if has_speed and speed_int > 0 else "0")
+    self._put_if_changed("ClearpilotHasSpeed", "1" if has_speed and speed_int > 0 else "0", "_w_has_speed")
-    self.params_memory.put("ClearpilotSpeedDisplay", str(speed_int) if has_speed and speed_int > 0 else "")
+    self._put_if_changed("ClearpilotSpeedDisplay", str(speed_int) if has_speed and speed_int > 0 else "", "_w_speed_display")
-    self.params_memory.put("ClearpilotSpeedLimitDisplay", str(speed_limit_int) if speed_limit_int > 0 else "0")
+    self._put_if_changed("ClearpilotSpeedLimitDisplay", str(speed_limit_int) if speed_limit_int > 0 else "0", "_w_speed_limit_display")
-    self.params_memory.put("ClearpilotSpeedUnit", unit)
+    self._put_if_changed("ClearpilotSpeedUnit", unit, "_w_speed_unit")
-    self.params_memory.put("ClearpilotIsMetric", "1" if is_metric else "0")
+    self._put_if_changed("ClearpilotIsMetric", "1" if is_metric else "0", "_w_is_metric")
 
     # Cruise warning logic
     warning = ""
@@ -61,7 +76,16 @@ class SpeedState:
     cruise_engaged = cruise_active and not cruise_standstill
 
     if speed_limit_int >= 20 and cruise_engaged and cruise_int > 0:
-      over_threshold = 10 if speed_limit_int >= 50 else 7
+      # Tiers (warning fires at >= limit + threshold):
+      #   limit >= 50:  +9 over ok, warn at +10   (e.g. 60 → warn at 70)
+      #   limit 26-49:  +6 over ok, warn at +7    (e.g. 35 → warn at 42)
+      #   limit <= 25:  +8 over ok, warn at +9    (e.g. 25 → warn at 34, so 33 is ok)
+      if speed_limit_int >= 50:
+        over_threshold = 10
+      elif speed_limit_int <= 25:
+        over_threshold = 9
+      else:
+        over_threshold = 7
       if cruise_int >= speed_limit_int + over_threshold:
         warning = "over"
         warning_speed = str(cruise_int)
@@ -69,8 +93,8 @@ class SpeedState:
         warning = "under"
         warning_speed = str(cruise_int)
 
-    self.params_memory.put("ClearpilotCruiseWarning", warning)
+    self._put_if_changed("ClearpilotCruiseWarning", warning, "_w_cruise_warning")
-    self.params_memory.put("ClearpilotCruiseWarningSpeed", warning_speed)
+    self._put_if_changed("ClearpilotCruiseWarningSpeed", warning_speed, "_w_cruise_warning_speed")
 
     # Ding logic: play when warning sign appears or speed limit changes while visible
     should_ding = False

selfdrive/controls/controlsd.py

@@ -81,9 +81,10 @@ class Controls:
     self.params_memory.put_bool("CPTLkasButtonAction", False)
     self.params_memory.put_int("ScreenDisplayMode", 0)
 
-    # ClearPilot speed processing
+    # CLEARPILOT: speed-limit/cruise-warning state machine + park→drive auto-reset
     self.speed_state = SpeedState()
     self.speed_state_frame = 0
+    self.was_driving_gear = False
 
     self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS
 
@@ -114,8 +115,7 @@ class Controls:
                                    '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_alive=ignore + ['gpsLocation'], ignore_avg_freq=ignore+['radarState', 'testJoystick', 'gpsLocation'], ignore_valid=['testJoystick', 'gpsLocation'],
-                                  ignore_valid=['testJoystick', 'gpsLocation'],
                                   frequency=int(1/DT_CTRL))
 
     self.joystick_mode = self.params.get_bool("JoystickDebugMode")
@@ -204,8 +204,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
@@ -1284,7 +1282,8 @@ class Controls:
 
       self.params_memory.put_int("ScreenDisplayMode", new_mode)
 
-    # ClearPilot speed processing (~2 Hz at 100 Hz loop)
+    # ClearPilot speed processing (~2 Hz at 100 Hz loop) — drives speed-limit sign
+    # and cruise over/under warning sign via memory params read by the UI.
     self.speed_state_frame += 1
     if self.speed_state_frame % 50 == 0:
       gps = self.sm['gpsLocation']
@@ -1297,7 +1296,6 @@ class Controls:
       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)
-
     return CC
 
 def main():

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -58,6 +58,9 @@ class FrogPilotPlanner:
     self.params = Params()
     self.params_memory = Params("/dev/shm/params")
 
+    # CLEARPILOT: track valid transitions so we only log when it flips, not every cycle
+    self._dbg_prev_valid = True
+
     self.cem = ConditionalExperimentalMode()
     self.lead_one = Lead()
     self.mtsc = MapTurnSpeedController()
@@ -242,7 +245,18 @@ class FrogPilotPlanner:
 
   def publish(self, sm, pm):
     frogpilot_plan_send = messaging.new_message('frogpilotPlan')
-    frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
+    valid = sm.all_checks(service_list=['carState', 'controlsState'])
+    # CLEARPILOT: log on transition into invalid — stderr goes to our plannerd.log
+    if valid != self._dbg_prev_valid and not valid:
+      import sys
+      print(
+        "CLP frogpilotPlan valid=False: "
+        f"carState(a={sm.alive['carState']},v={sm.valid['carState']},f={sm.freq_ok['carState']}) "
+        f"controlsState(a={sm.alive['controlsState']},v={sm.valid['controlsState']},f={sm.freq_ok['controlsState']})",
+        file=sys.stderr, flush=True
+      )
+    self._dbg_prev_valid = valid
+    frogpilot_plan_send.valid = valid
     frogpilotPlan = frogpilot_plan_send.frogpilotPlan
 
     frogpilotPlan.accelerationJerk = A_CHANGE_COST * (float(self.jerk) if self.lead_one.status else 1)

selfdrive/frogpilot/screenrecorder/omx_encoder.cc

@@ -35,120 +35,19 @@ int ABGRToNV12(const uint8_t* src_abgr,
   int halfwidth = (width + 1) >> 1;
   void (*ABGRToUVRow)(const uint8_t* src_abgr0, int src_stride_abgr,
                       uint8_t* dst_u, uint8_t* dst_v, int width) =
-      ABGRToUVRow_C;
+      ABGRToUVRow_NEON;
   void (*ABGRToYRow)(const uint8_t* src_abgr, uint8_t* dst_y, int width) =
-      ABGRToYRow_C;
+      ABGRToYRow_NEON;
-  void (*MergeUVRow_)(const uint8_t* src_u, const uint8_t* src_v, uint8_t* dst_uv, int width) = MergeUVRow_C;
+  void (*MergeUVRow_)(const uint8_t* src_u, const uint8_t* src_v, uint8_t* dst_uv, int width) = MergeUVRow_NEON;
   if (!src_abgr || !dst_y || !dst_uv || width <= 0 || height == 0) {
     return -1;
   }
-if (height < 0) {  // Negative height means invert the image.
+  if (height < 0) {
     height = -height;
     src_abgr = src_abgr + (height - 1) * src_stride_abgr;
     src_stride_abgr = -src_stride_abgr;
-}
-#if defined(HAS_ABGRTOYROW_SSSE3) && defined(HAS_ABGRTOUVROW_SSSE3)
-  if (TestCpuFlag(kCpuHasSSSE3)) {
-    ABGRToUVRow = ABGRToUVRow_Any_SSSE3;
-    ABGRToYRow = ABGRToYRow_Any_SSSE3;
-    if (IS_ALIGNED(width, 16)) {
-      ABGRToUVRow = ABGRToUVRow_SSSE3;
-      ABGRToYRow = ABGRToYRow_SSSE3;
   }
-  }
-#endif
-#if defined(HAS_ABGRTOYROW_AVX2) && defined(HAS_ABGRTOUVROW_AVX2)
-  if (TestCpuFlag(kCpuHasAVX2)) {
-    ABGRToUVRow = ABGRToUVRow_Any_AVX2;
-    ABGRToYRow = ABGRToYRow_Any_AVX2;
-    if (IS_ALIGNED(width, 32)) {
-      ABGRToUVRow = ABGRToUVRow_AVX2;
-      ABGRToYRow = ABGRToYRow_AVX2;
-    }
-  }
-#endif
-#if defined(HAS_ABGRTOYROW_NEON)
-  if (TestCpuFlag(kCpuHasNEON)) {
-    ABGRToYRow = ABGRToYRow_Any_NEON;
-    if (IS_ALIGNED(width, 8)) {
-      ABGRToYRow = ABGRToYRow_NEON;
-    }
-  }
-#endif
-#if defined(HAS_ABGRTOUVROW_NEON)
-  if (TestCpuFlag(kCpuHasNEON)) {
-    ABGRToUVRow = ABGRToUVRow_Any_NEON;
-    if (IS_ALIGNED(width, 16)) {
-      ABGRToUVRow = ABGRToUVRow_NEON;
-    }
-  }
-#endif
-#if defined(HAS_ABGRTOYROW_MMI) && defined(HAS_ABGRTOUVROW_MMI)
-  if (TestCpuFlag(kCpuHasMMI)) {
-    ABGRToYRow = ABGRToYRow_Any_MMI;
-    ABGRToUVRow = ABGRToUVRow_Any_MMI;
-    if (IS_ALIGNED(width, 8)) {
-      ABGRToYRow = ABGRToYRow_MMI;
-    }
-    if (IS_ALIGNED(width, 16)) {
-      ABGRToUVRow = ABGRToUVRow_MMI;
-    }
-  }
-#endif
-#if defined(HAS_ABGRTOYROW_MSA) && defined(HAS_ABGRTOUVROW_MSA)
-  if (TestCpuFlag(kCpuHasMSA)) {
-    ABGRToYRow = ABGRToYRow_Any_MSA;
-    ABGRToUVRow = ABGRToUVRow_Any_MSA;
-    if (IS_ALIGNED(width, 16)) {
-      ABGRToYRow = ABGRToYRow_MSA;
-    }
-    if (IS_ALIGNED(width, 32)) {
-      ABGRToUVRow = ABGRToUVRow_MSA;
-    }
-  }
-#endif
-#if defined(HAS_MERGEUVROW_SSE2)
-  if (TestCpuFlag(kCpuHasSSE2)) {
-    MergeUVRow_ = MergeUVRow_Any_SSE2;
-    if (IS_ALIGNED(halfwidth, 16)) {
-      MergeUVRow_ = MergeUVRow_SSE2;
-    }
-  }
-#endif
-#if defined(HAS_MERGEUVROW_AVX2)
-  if (TestCpuFlag(kCpuHasAVX2)) {
-    MergeUVRow_ = MergeUVRow_Any_AVX2;
-    if (IS_ALIGNED(halfwidth, 32)) {
-      MergeUVRow_ = MergeUVRow_AVX2;
-    }
-  }
-#endif
-#if defined(HAS_MERGEUVROW_NEON)
-  if (TestCpuFlag(kCpuHasNEON)) {
-    MergeUVRow_ = MergeUVRow_Any_NEON;
-    if (IS_ALIGNED(halfwidth, 16)) {
-      MergeUVRow_ = MergeUVRow_NEON;
-    }
-  }
-#endif
-#if defined(HAS_MERGEUVROW_MMI)
-  if (TestCpuFlag(kCpuHasMMI)) {
-    MergeUVRow_ = MergeUVRow_Any_MMI;
-    if (IS_ALIGNED(halfwidth, 8)) {
-      MergeUVRow_ = MergeUVRow_MMI;
-    }
-  }
-#endif
-#if defined(HAS_MERGEUVROW_MSA)
-  if (TestCpuFlag(kCpuHasMSA)) {
-    MergeUVRow_ = MergeUVRow_Any_MSA;
-    if (IS_ALIGNED(halfwidth, 16)) {
-      MergeUVRow_ = MergeUVRow_MSA;
-    }
-  }
-#endif
   {
-    // Allocate a rows of uv.
     align_buffer_64(row_u, ((halfwidth + 31) & ~31) * 2);
     uint8_t* row_v = row_u + ((halfwidth + 31) & ~31);
 
@@ -182,9 +81,9 @@ extern ExitHandler do_exit;
 // ***** OMX callback functions *****
 
 void OmxEncoder::wait_for_state(OMX_STATETYPE state_) {
-  std::unique_lock lk(this->state_lock);
+  std::unique_lock lk(state_lock);
-  while (this->state != state_) {
+  while (state != state_) {
-    this->state_cv.wait(lk);
+    state_cv.wait(lk);
   }
 }
 
@@ -236,166 +135,106 @@ static const char* omx_color_fomat_name(uint32_t format) __attribute__((unused))
 static const char* omx_color_fomat_name(uint32_t format) {
   switch (format) {
   case OMX_COLOR_FormatUnused: return "OMX_COLOR_FormatUnused";
-  case OMX_COLOR_FormatMonochrome: return "OMX_COLOR_FormatMonochrome";
-  case OMX_COLOR_Format8bitRGB332: return "OMX_COLOR_Format8bitRGB332";
-  case OMX_COLOR_Format12bitRGB444: return "OMX_COLOR_Format12bitRGB444";
-  case OMX_COLOR_Format16bitARGB4444: return "OMX_COLOR_Format16bitARGB4444";
-  case OMX_COLOR_Format16bitARGB1555: return "OMX_COLOR_Format16bitARGB1555";
-  case OMX_COLOR_Format16bitRGB565: return "OMX_COLOR_Format16bitRGB565";
-  case OMX_COLOR_Format16bitBGR565: return "OMX_COLOR_Format16bitBGR565";
-  case OMX_COLOR_Format18bitRGB666: return "OMX_COLOR_Format18bitRGB666";
-  case OMX_COLOR_Format18bitARGB1665: return "OMX_COLOR_Format18bitARGB1665";
-  case OMX_COLOR_Format19bitARGB1666: return "OMX_COLOR_Format19bitARGB1666";
-  case OMX_COLOR_Format24bitRGB888: return "OMX_COLOR_Format24bitRGB888";
-  case OMX_COLOR_Format24bitBGR888: return "OMX_COLOR_Format24bitBGR888";
-  case OMX_COLOR_Format24bitARGB1887: return "OMX_COLOR_Format24bitARGB1887";
-  case OMX_COLOR_Format25bitARGB1888: return "OMX_COLOR_Format25bitARGB1888";
-  case OMX_COLOR_Format32bitBGRA8888: return "OMX_COLOR_Format32bitBGRA8888";
-  case OMX_COLOR_Format32bitARGB8888: return "OMX_COLOR_Format32bitARGB8888";
-  case OMX_COLOR_FormatYUV411Planar: return "OMX_COLOR_FormatYUV411Planar";
-  case OMX_COLOR_FormatYUV411PackedPlanar: return "OMX_COLOR_FormatYUV411PackedPlanar";
-  case OMX_COLOR_FormatYUV420Planar: return "OMX_COLOR_FormatYUV420Planar";
-  case OMX_COLOR_FormatYUV420PackedPlanar: return "OMX_COLOR_FormatYUV420PackedPlanar";
   case OMX_COLOR_FormatYUV420SemiPlanar: return "OMX_COLOR_FormatYUV420SemiPlanar";
-  case OMX_COLOR_FormatYUV422Planar: return "OMX_COLOR_FormatYUV422Planar";
-  case OMX_COLOR_FormatYUV422PackedPlanar: return "OMX_COLOR_FormatYUV422PackedPlanar";
-  case OMX_COLOR_FormatYUV422SemiPlanar: return "OMX_COLOR_FormatYUV422SemiPlanar";
-  case OMX_COLOR_FormatYCbYCr: return "OMX_COLOR_FormatYCbYCr";
-  case OMX_COLOR_FormatYCrYCb: return "OMX_COLOR_FormatYCrYCb";
-  case OMX_COLOR_FormatCbYCrY: return "OMX_COLOR_FormatCbYCrY";
-  case OMX_COLOR_FormatCrYCbY: return "OMX_COLOR_FormatCrYCbY";
-  case OMX_COLOR_FormatYUV444Interleaved: return "OMX_COLOR_FormatYUV444Interleaved";
-  case OMX_COLOR_FormatRawBayer8bit: return "OMX_COLOR_FormatRawBayer8bit";
-  case OMX_COLOR_FormatRawBayer10bit: return "OMX_COLOR_FormatRawBayer10bit";
-  case OMX_COLOR_FormatRawBayer8bitcompressed: return "OMX_COLOR_FormatRawBayer8bitcompressed";
-  case OMX_COLOR_FormatL2: return "OMX_COLOR_FormatL2";
-  case OMX_COLOR_FormatL4: return "OMX_COLOR_FormatL4";
-  case OMX_COLOR_FormatL8: return "OMX_COLOR_FormatL8";
-  case OMX_COLOR_FormatL16: return "OMX_COLOR_FormatL16";
-  case OMX_COLOR_FormatL24: return "OMX_COLOR_FormatL24";
-  case OMX_COLOR_FormatL32: return "OMX_COLOR_FormatL32";
-  case OMX_COLOR_FormatYUV420PackedSemiPlanar: return "OMX_COLOR_FormatYUV420PackedSemiPlanar";
-  case OMX_COLOR_FormatYUV422PackedSemiPlanar: return "OMX_COLOR_FormatYUV422PackedSemiPlanar";
-  case OMX_COLOR_Format18BitBGR666: return "OMX_COLOR_Format18BitBGR666";
-  case OMX_COLOR_Format24BitARGB6666: return "OMX_COLOR_Format24BitARGB6666";
-  case OMX_COLOR_Format24BitABGR6666: return "OMX_COLOR_Format24BitABGR6666";
-
-  case OMX_COLOR_FormatAndroidOpaque: return "OMX_COLOR_FormatAndroidOpaque";
-  case OMX_TI_COLOR_FormatYUV420PackedSemiPlanar: return "OMX_TI_COLOR_FormatYUV420PackedSemiPlanar";
-  case OMX_QCOM_COLOR_FormatYVU420SemiPlanar: return "OMX_QCOM_COLOR_FormatYVU420SemiPlanar";
-  case OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka: return "OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka";
-  case OMX_SEC_COLOR_FormatNV12Tiled: return "OMX_SEC_COLOR_FormatNV12Tiled";
   case OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar32m: return "OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar32m";
-
-  case QOMX_COLOR_FormatYVU420PackedSemiPlanar32m4ka: return "QOMX_COLOR_FormatYVU420PackedSemiPlanar32m4ka";
-  case QOMX_COLOR_FormatYUV420PackedSemiPlanar16m2ka: return "QOMX_COLOR_FormatYUV420PackedSemiPlanar16m2ka";
-  case QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mMultiView: return "QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mMultiView";
-  case QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mCompressed: return "QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mCompressed";
   case QOMX_COLOR_Format32bitRGBA8888: return "QOMX_COLOR_Format32bitRGBA8888";
-  case QOMX_COLOR_Format32bitRGBA8888Compressed: return "QOMX_COLOR_Format32bitRGBA8888Compressed";
+  default: return "unkn";
-
-  default:
-    return "unkn";
   }
 }
 
 
 // ***** encoder functions *****
 
-OmxEncoder::OmxEncoder(const char* path, int width, int height, int fps, int bitrate, bool h265, bool downscale) {
+OmxEncoder::OmxEncoder(const char* path, int width, int height, int fps, int bitrate) {
   this->path = path;
   this->width = width;
   this->height = height;
   this->fps = fps;
-  this->remuxing = !h265;
 
-  this->downscale = downscale;
+  OMX_ERRORTYPE err = OMX_Init();
-  if (this->downscale) {
+  if (err != OMX_ErrorNone) {
-    this->y_ptr2 = (uint8_t *)malloc(this->width*this->height);
+    LOGE("OMX_Init failed: %x", err);
-    this->u_ptr2 = (uint8_t *)malloc(this->width*this->height/4);
+    return;
-    this->v_ptr2 = (uint8_t *)malloc(this->width*this->height/4);
   }
 
-  auto component = (OMX_STRING)(h265 ? "OMX.qcom.video.encoder.hevc" : "OMX.qcom.video.encoder.avc");
+  OMX_STRING component = (OMX_STRING)("OMX.qcom.video.encoder.avc");
-  int err = OMX_GetHandle(&this->handle, component, this, &omx_callbacks);
+  err = OMX_GetHandle(&handle, component, this, &omx_callbacks);
   if (err != OMX_ErrorNone) {
-    LOGE("error getting codec: %x", err);
+    LOGE("Error getting codec: %x", err);
+    OMX_Deinit();
+    return;
   }
 
   // setup input port
-
   OMX_PARAM_PORTDEFINITIONTYPE in_port = {0};
   in_port.nSize = sizeof(in_port);
   in_port.nPortIndex = (OMX_U32) PORT_INDEX_IN;
-  OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port));
+  OMX_CHECK(OMX_GetParameter(handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port));
 
-  in_port.format.video.nFrameWidth = this->width;
+  in_port.format.video.nFrameWidth = width;
-  in_port.format.video.nFrameHeight = this->height;
+  in_port.format.video.nFrameHeight = height;
-  in_port.format.video.nStride = VENUS_Y_STRIDE(COLOR_FMT_NV12, this->width);
+  in_port.format.video.nStride = VENUS_Y_STRIDE(COLOR_FMT_NV12, width);
-  in_port.format.video.nSliceHeight = this->height;
+  in_port.format.video.nSliceHeight = height;
-  in_port.nBufferSize = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, this->width, this->height);
+  in_port.nBufferSize = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, width, height);
-  in_port.format.video.xFramerate = (this->fps * 65536);
+  in_port.format.video.xFramerate = (fps * 65536);
   in_port.format.video.eCompressionFormat = OMX_VIDEO_CodingUnused;
   in_port.format.video.eColorFormat = (OMX_COLOR_FORMATTYPE)QOMX_COLOR_FORMATYUV420PackedSemiPlanar32m;
 
-  OMX_CHECK(OMX_SetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port));
+  OMX_CHECK(OMX_SetParameter(handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port));
-  OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port));
+  OMX_CHECK(OMX_GetParameter(handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port));
-  this->in_buf_headers.resize(in_port.nBufferCountActual);
+  in_buf_headers.resize(in_port.nBufferCountActual);
 
   // setup output port
-
+  OMX_PARAM_PORTDEFINITIONTYPE out_port;
-  OMX_PARAM_PORTDEFINITIONTYPE out_port = {0};
+  memset(&out_port, 0, sizeof(OMX_PARAM_PORTDEFINITIONTYPE));
   out_port.nSize = sizeof(out_port);
+  out_port.nVersion.s.nVersionMajor = 1;
+  out_port.nVersion.s.nVersionMinor = 0;
+  out_port.nVersion.s.nRevision = 0;
+  out_port.nVersion.s.nStep = 0;
   out_port.nPortIndex = (OMX_U32) PORT_INDEX_OUT;
-  OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR)&out_port));
+
-  out_port.format.video.nFrameWidth = this->width;
+  OMX_ERRORTYPE error = OMX_GetParameter(handle, OMX_IndexParamPortDefinition, (OMX_PTR)&out_port);
-  out_port.format.video.nFrameHeight = this->height;
+  if (error != OMX_ErrorNone) {
+    LOGE("Error getting output port parameters: 0x%08x", error);
+    return;
+  }
+
+  out_port.format.video.nFrameWidth = width;
+  out_port.format.video.nFrameHeight = height;
   out_port.format.video.xFramerate = 0;
   out_port.format.video.nBitrate = bitrate;
-  if (h265) {
-    out_port.format.video.eCompressionFormat = OMX_VIDEO_CodingHEVC;
-  } else {
   out_port.format.video.eCompressionFormat = OMX_VIDEO_CodingAVC;
-  }
   out_port.format.video.eColorFormat = OMX_COLOR_FormatUnused;
 
-  OMX_CHECK(OMX_SetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &out_port));
+  error = OMX_SetParameter(handle, OMX_IndexParamPortDefinition, (OMX_PTR) &out_port);
+  if (error != OMX_ErrorNone) {
+    LOGE("Error setting output port parameters: 0x%08x", error);
+    return;
+  }
 
-  OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &out_port));
+  error = OMX_GetParameter(handle, OMX_IndexParamPortDefinition, (OMX_PTR) &out_port);
-  this->out_buf_headers.resize(out_port.nBufferCountActual);
+  if (error != OMX_ErrorNone) {
+    LOGE("Error getting updated output port parameters: 0x%08x", error);
+    return;
+  }
+
+  out_buf_headers.resize(out_port.nBufferCountActual);
 
   OMX_VIDEO_PARAM_BITRATETYPE bitrate_type = {0};
   bitrate_type.nSize = sizeof(bitrate_type);
   bitrate_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT;
-  OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamVideoBitrate, (OMX_PTR) &bitrate_type));
+  OMX_CHECK(OMX_GetParameter(handle, OMX_IndexParamVideoBitrate, (OMX_PTR) &bitrate_type));
   bitrate_type.eControlRate = OMX_Video_ControlRateVariable;
   bitrate_type.nTargetBitrate = bitrate;
 
-  OMX_CHECK(OMX_SetParameter(this->handle, OMX_IndexParamVideoBitrate, (OMX_PTR) &bitrate_type));
+  OMX_CHECK(OMX_SetParameter(handle, OMX_IndexParamVideoBitrate, (OMX_PTR) &bitrate_type));
 
-  if (h265) {
-    // setup HEVC
-  #ifndef QCOM2
-    OMX_VIDEO_PARAM_HEVCTYPE hevc_type = {0};
-    OMX_INDEXTYPE index_type = (OMX_INDEXTYPE) OMX_IndexParamVideoHevc;
-  #else
-    OMX_VIDEO_PARAM_PROFILELEVELTYPE hevc_type = {0};
-    OMX_INDEXTYPE index_type = OMX_IndexParamVideoProfileLevelCurrent;
-  #endif
-    hevc_type.nSize = sizeof(hevc_type);
-    hevc_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT;
-    OMX_CHECK(OMX_GetParameter(this->handle, index_type, (OMX_PTR) &hevc_type));
-
-    hevc_type.eProfile = OMX_VIDEO_HEVCProfileMain;
-    hevc_type.eLevel = OMX_VIDEO_HEVCHighTierLevel5;
-
-    OMX_CHECK(OMX_SetParameter(this->handle, index_type, (OMX_PTR) &hevc_type));
-  } else {
   // setup h264
-    OMX_VIDEO_PARAM_AVCTYPE avc = { 0 };
+  OMX_VIDEO_PARAM_AVCTYPE avc = {0};
   avc.nSize = sizeof(avc);
   avc.nPortIndex = (OMX_U32) PORT_INDEX_OUT;
-    OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamVideoAvc, &avc));
+  OMX_CHECK(OMX_GetParameter(handle, OMX_IndexParamVideoAvc, &avc));
 
   avc.nBFrames = 0;
   avc.nPFrames = 15;
@@ -412,70 +251,64 @@ OmxEncoder::OmxEncoder(const char* path, int width, int height, int fps, int bit
   avc.bWeightedPPrediction = OMX_TRUE;
   avc.bconstIpred = OMX_TRUE;
 
-    OMX_CHECK(OMX_SetParameter(this->handle, OMX_IndexParamVideoAvc, &avc));
+  OMX_CHECK(OMX_SetParameter(handle, OMX_IndexParamVideoAvc, &avc));
+
+  OMX_CHECK(OMX_SendCommand(handle, OMX_CommandStateSet, OMX_StateIdle, NULL));
+
+  for (OMX_BUFFERHEADERTYPE* &buf : in_buf_headers) {
+    OMX_CHECK(OMX_AllocateBuffer(handle, &buf, PORT_INDEX_IN, this, in_port.nBufferSize));
   }
 
-  OMX_CHECK(OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateIdle, NULL));
+  for (OMX_BUFFERHEADERTYPE* &buf : out_buf_headers) {
-
+    OMX_CHECK(OMX_AllocateBuffer(handle, &buf, PORT_INDEX_OUT, this, out_port.nBufferSize));
-  for (auto &buf : this->in_buf_headers) {
-    OMX_CHECK(OMX_AllocateBuffer(this->handle, &buf, PORT_INDEX_IN, this,
-                             in_port.nBufferSize));
-  }
-
-  for (auto &buf : this->out_buf_headers) {
-    OMX_CHECK(OMX_AllocateBuffer(this->handle, &buf, PORT_INDEX_OUT, this,
-                             out_port.nBufferSize));
   }
 
   wait_for_state(OMX_StateIdle);
 
-  OMX_CHECK(OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateExecuting, NULL));
+  OMX_CHECK(OMX_SendCommand(handle, OMX_CommandStateSet, OMX_StateExecuting, NULL));
 
   wait_for_state(OMX_StateExecuting);
 
   // give omx all the output buffers
-  for (auto &buf : this->out_buf_headers) {
+  for (OMX_BUFFERHEADERTYPE* &buf : out_buf_headers) {
-    OMX_CHECK(OMX_FillThisBuffer(this->handle, buf));
+    OMX_CHECK(OMX_FillThisBuffer(handle, buf));
   }
 
   // fill the input free queue
-  for (auto &buf : this->in_buf_headers) {
+  for (OMX_BUFFERHEADERTYPE* &buf : in_buf_headers) {
-    this->free_in.push(buf);
+    free_in.push(buf);
   }
 }
 
-void OmxEncoder::handle_out_buf(OmxEncoder *e, OMX_BUFFERHEADERTYPE *out_buf) {
+void OmxEncoder::handle_out_buf(OmxEncoder *encoder, OMX_BUFFERHEADERTYPE *out_buf) {
   int err;
   uint8_t *buf_data = out_buf->pBuffer + out_buf->nOffset;
 
   if (out_buf->nFlags & OMX_BUFFERFLAG_CODECCONFIG) {
-    if (e->codec_config_len < out_buf->nFilledLen) {
+    if (encoder->codec_config_len < out_buf->nFilledLen) {
-      e->codec_config = (uint8_t *)realloc(e->codec_config, out_buf->nFilledLen);
+      encoder->codec_config = (uint8_t *)realloc(encoder->codec_config, out_buf->nFilledLen);
     }
-    e->codec_config_len = out_buf->nFilledLen;
+    encoder->codec_config_len = out_buf->nFilledLen;
-    memcpy(e->codec_config, buf_data, out_buf->nFilledLen);
+    memcpy(encoder->codec_config, buf_data, out_buf->nFilledLen);
 #ifdef QCOM2
     out_buf->nTimeStamp = 0;
 #endif
   }
 
-  if (e->of) {
+  if (encoder->of) {
-    fwrite(buf_data, out_buf->nFilledLen, 1, e->of);
+    fwrite(buf_data, out_buf->nFilledLen, 1, encoder->of);
   }
 
-  if (e->remuxing) {
+  if (!encoder->wrote_codec_config && encoder->codec_config_len > 0) {
-    if (!e->wrote_codec_config && e->codec_config_len > 0) {
     // extradata will be freed by av_free() in avcodec_free_context()
-      e->codec_ctx->extradata = (uint8_t*)av_mallocz(e->codec_config_len + AV_INPUT_BUFFER_PADDING_SIZE);
+    encoder->out_stream->codecpar->extradata = (uint8_t*)av_mallocz(encoder->codec_config_len + AV_INPUT_BUFFER_PADDING_SIZE);
-      e->codec_ctx->extradata_size = e->codec_config_len;
+    encoder->out_stream->codecpar->extradata_size = encoder->codec_config_len;
-      memcpy(e->codec_ctx->extradata, e->codec_config, e->codec_config_len);
+    memcpy(encoder->out_stream->codecpar->extradata, encoder->codec_config, encoder->codec_config_len);
 
-      err = avcodec_parameters_from_context(e->out_stream->codecpar, e->codec_ctx);
+    err = avformat_write_header(encoder->ofmt_ctx, NULL);
-      assert(err >= 0);
-      err = avformat_write_header(e->ofmt_ctx, NULL);
     assert(err >= 0);
 
-      e->wrote_codec_config = true;
+    encoder->wrote_codec_config = true;
   }
 
   if (out_buf->nTimeStamp > 0) {
@@ -488,18 +321,17 @@ void OmxEncoder::handle_out_buf(OmxEncoder *e, OMX_BUFFERHEADERTYPE *out_buf) {
     pkt.size = out_buf->nFilledLen;
 
     enum AVRounding rnd = static_cast<enum AVRounding>(AV_ROUND_NEAR_INF|AV_ROUND_PASS_MINMAX);
-      pkt.pts = pkt.dts = av_rescale_q_rnd(out_buf->nTimeStamp, in_timebase, e->ofmt_ctx->streams[0]->time_base, rnd);
+    pkt.pts = pkt.dts = av_rescale_q_rnd(out_buf->nTimeStamp, in_timebase, encoder->out_stream->time_base, rnd);
-      pkt.duration = av_rescale_q(50*1000, in_timebase, e->ofmt_ctx->streams[0]->time_base);
+    pkt.duration = av_rescale_q(1, AVRational{1, encoder->fps}, encoder->out_stream->time_base);
 
     if (out_buf->nFlags & OMX_BUFFERFLAG_SYNCFRAME) {
       pkt.flags |= AV_PKT_FLAG_KEY;
     }
 
-      err = av_write_frame(e->ofmt_ctx, &pkt);
+    err = av_write_frame(encoder->ofmt_ctx, &pkt);
     if (err < 0) { LOGW("ts encoder write issue"); }
 
-      av_free_packet(&pkt);
+    av_packet_unref(&pkt);
-    }
   }
 
   // give omx back the buffer
@@ -508,59 +340,53 @@ void OmxEncoder::handle_out_buf(OmxEncoder *e, OMX_BUFFERHEADERTYPE *out_buf) {
     out_buf->nTimeStamp = 0;
   }
 #endif
-  OMX_CHECK(OMX_FillThisBuffer(e->handle, out_buf));
+  OMX_CHECK(OMX_FillThisBuffer(encoder->handle, out_buf));
 }
 
 int OmxEncoder::encode_frame_rgba(const uint8_t *ptr, int in_width, int in_height, uint64_t ts) {
-  int err;
+  if (!is_open) {
-  if (!this->is_open) {
     return -1;
   }
 
-  // this sometimes freezes... put it outside the encoder lock so we can still trigger rotates...
-  // THIS IS A REALLY BAD IDEA, but apparently the race has to happen 30 times to trigger this
   OMX_BUFFERHEADERTYPE* in_buf = nullptr;
-  while (!this->free_in.try_pop(in_buf, 20)) {
+  while (!free_in.try_pop(in_buf, 20)) {
     if (do_exit) {
       return -1;
     }
   }
 
-  int ret = this->counter;
+  int ret = counter;
 
   uint8_t *in_buf_ptr = in_buf->pBuffer;
 
   uint8_t *in_y_ptr = in_buf_ptr;
-  int in_y_stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, this->width);
+  int in_y_stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, width);
-  int in_uv_stride = VENUS_UV_STRIDE(COLOR_FMT_NV12, this->width);
+  int in_uv_stride = VENUS_UV_STRIDE(COLOR_FMT_NV12, width);
-  uint8_t *in_uv_ptr = in_buf_ptr + (in_y_stride * VENUS_Y_SCANLINES(COLOR_FMT_NV12, this->height));
+  uint8_t *in_uv_ptr = in_buf_ptr + (in_y_stride * VENUS_Y_SCANLINES(COLOR_FMT_NV12, height));
 
-  err = ABGRToNV12(ptr, this->width*4,
+  int err = ABGRToNV12(ptr, width * 4, in_y_ptr, in_y_stride, in_uv_ptr, in_uv_stride, width, height);
-                   in_y_ptr, in_y_stride,
-                   in_uv_ptr, in_uv_stride,
-                   this->width, this->height);
   assert(err == 0);
 
-  in_buf->nFilledLen = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, this->width, this->height);
+  in_buf->nFilledLen = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, width, height);
   in_buf->nFlags = OMX_BUFFERFLAG_ENDOFFRAME;
   in_buf->nOffset = 0;
-  in_buf->nTimeStamp = ts/1000LL;  // OMX_TICKS, in microseconds
+  in_buf->nTimeStamp = ts / 1000LL;  // OMX_TICKS, in microseconds
-  this->last_t = in_buf->nTimeStamp;
+  last_t = in_buf->nTimeStamp;
 
-  OMX_CHECK(OMX_EmptyThisBuffer(this->handle, in_buf));
+  OMX_CHECK(OMX_EmptyThisBuffer(handle, in_buf));
 
   // pump output
   while (true) {
     OMX_BUFFERHEADERTYPE *out_buf;
-    if (!this->done_out.try_pop(out_buf)) {
+    if (!done_out.try_pop(out_buf)) {
       break;
     }
     handle_out_buf(this, out_buf);
   }
 
-  this->dirty = true;
+  dirty = true;
 
-  this->counter++;
+  counter++;
 
   return ret;
 }
@@ -568,24 +394,24 @@ int OmxEncoder::encode_frame_rgba(const uint8_t *ptr, int in_width, int in_heigh
 // CLEARPILOT: encode raw NV12 frames directly (no RGBA conversion needed)
 int OmxEncoder::encode_frame_nv12(const uint8_t *y_ptr, int y_stride, const uint8_t *uv_ptr, int uv_stride,
                                    int in_width, int in_height, uint64_t ts) {
-  if (!this->is_open) {
+  if (!is_open) {
     return -1;
   }
 
   OMX_BUFFERHEADERTYPE* in_buf = nullptr;
-  while (!this->free_in.try_pop(in_buf, 20)) {
+  while (!free_in.try_pop(in_buf, 20)) {
     if (do_exit) {
       return -1;
     }
   }
 
-  int ret = this->counter;
+  int ret = counter;
 
   uint8_t *in_buf_ptr = in_buf->pBuffer;
-  int venus_y_stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, this->width);
+  int venus_y_stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, width);
-  int venus_uv_stride = VENUS_UV_STRIDE(COLOR_FMT_NV12, this->width);
+  int venus_uv_stride = VENUS_UV_STRIDE(COLOR_FMT_NV12, width);
   uint8_t *dst_y = in_buf_ptr;
-  uint8_t *dst_uv = in_buf_ptr + (venus_y_stride * VENUS_Y_SCANLINES(COLOR_FMT_NV12, this->height));
+  uint8_t *dst_uv = in_buf_ptr + (venus_y_stride * VENUS_Y_SCANLINES(COLOR_FMT_NV12, height));
 
   // Copy Y plane row by row (source stride may differ from VENUS stride)
   for (int row = 0; row < in_height; row++) {
@@ -597,99 +423,103 @@ int OmxEncoder::encode_frame_nv12(const uint8_t *y_ptr, int y_stride, const uint
     memcpy(dst_uv + row * venus_uv_stride, uv_ptr + row * uv_stride, in_width);
   }
 
-  in_buf->nFilledLen = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, this->width, this->height);
+  in_buf->nFilledLen = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, width, height);
   in_buf->nFlags = OMX_BUFFERFLAG_ENDOFFRAME;
   in_buf->nOffset = 0;
   in_buf->nTimeStamp = ts / 1000LL;
-  this->last_t = in_buf->nTimeStamp;
+  last_t = in_buf->nTimeStamp;
 
-  OMX_CHECK(OMX_EmptyThisBuffer(this->handle, in_buf));
+  OMX_CHECK(OMX_EmptyThisBuffer(handle, in_buf));
 
   while (true) {
     OMX_BUFFERHEADERTYPE *out_buf;
-    if (!this->done_out.try_pop(out_buf)) {
+    if (!done_out.try_pop(out_buf)) {
       break;
     }
     handle_out_buf(this, out_buf);
   }
 
-  this->dirty = true;
+  dirty = true;
-  this->counter++;
+  counter++;
 
   return ret;
 }
 
 void OmxEncoder::encoder_open(const char* filename) {
-  int err;
+  if (!filename || strlen(filename) == 0) {
+    return;
+  }
+
+  if (strlen(filename) + path.size() + 2 > sizeof(vid_path)) {
+    return;
+  }
 
   struct stat st = {0};
-  if (stat(this->path.c_str(), &st) == -1) {
+  if (stat(path.c_str(), &st) == -1) {
-    mkdir(this->path.c_str(), 0755);
+    if (mkdir(path.c_str(), 0755) == -1) {
+      return;
+    }
   }
 
-  snprintf(this->vid_path, sizeof(this->vid_path), "%s/%s", this->path.c_str(), filename);
+  snprintf(vid_path, sizeof(vid_path), "%s/%s", path.c_str(), filename);
-  printf("encoder_open %s remuxing:%d\n", this->vid_path, this->remuxing);
 
-  if (this->remuxing) {
+  if (avformat_alloc_output_context2(&ofmt_ctx, NULL, NULL, vid_path) < 0 || !ofmt_ctx) {
-    avformat_alloc_output_context2(&this->ofmt_ctx, NULL, NULL, this->vid_path);
+    return;
-    assert(this->ofmt_ctx);
-
-    this->out_stream = avformat_new_stream(this->ofmt_ctx, NULL);
-    assert(this->out_stream);
-
-    // set codec correctly
-    av_register_all();
-
-    AVCodec *codec = NULL;
-    codec = avcodec_find_encoder(AV_CODEC_ID_H264);
-    assert(codec);
-
-    this->codec_ctx = avcodec_alloc_context3(codec);
-    assert(this->codec_ctx);
-    this->codec_ctx->width = this->width;
-    this->codec_ctx->height = this->height;
-    this->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P;
-    this->codec_ctx->time_base = (AVRational){ 1, this->fps };
-
-    err = avio_open(&this->ofmt_ctx->pb, this->vid_path, AVIO_FLAG_WRITE);
-    assert(err >= 0);
-
-    this->wrote_codec_config = false;
-  } else {
-    this->of = fopen(this->vid_path, "wb");
-    assert(this->of);
-#ifndef QCOM2
-    if (this->codec_config_len > 0) {
-      fwrite(this->codec_config, this->codec_config_len, 1, this->of);
-    }
-#endif
   }
 
-  // create camera lock file
+  out_stream = avformat_new_stream(ofmt_ctx, NULL);
-  snprintf(this->lock_path, sizeof(this->lock_path), "%s/%s.lock", this->path.c_str(), filename);
+  if (!out_stream) {
-  int lock_fd = HANDLE_EINTR(open(this->lock_path, O_RDWR | O_CREAT, 0664));
+    avformat_free_context(ofmt_ctx);
-  assert(lock_fd >= 0);
+    ofmt_ctx = nullptr;
+    return;
+  }
+
+  out_stream->time_base = AVRational{1, fps};
+
+  out_stream->codecpar->codec_id = AV_CODEC_ID_H264;
+  out_stream->codecpar->codec_type = AVMEDIA_TYPE_VIDEO;
+  out_stream->codecpar->width = width;
+  out_stream->codecpar->height = height;
+
+  int err = avio_open(&ofmt_ctx->pb, vid_path, AVIO_FLAG_WRITE);
+  if (err < 0) {
+    avformat_free_context(ofmt_ctx);
+    ofmt_ctx = nullptr;
+    return;
+  }
+
+  wrote_codec_config = false;
+
+  snprintf(lock_path, sizeof(lock_path), "%s/%s.lock", path.c_str(), filename);
+  int lock_fd = HANDLE_EINTR(open(lock_path, O_RDWR | O_CREAT, 0664));
+  if (lock_fd < 0) {
+    avio_closep(&ofmt_ctx->pb);
+    avformat_free_context(ofmt_ctx);
+    ofmt_ctx = nullptr;
+    return;
+  }
   close(lock_fd);
 
-  this->is_open = true;
+  is_open = true;
-  this->counter = 0;
+  counter = 0;
 }
 
 void OmxEncoder::encoder_close() {
-  if (this->is_open) {
+  if (!is_open) return;
-    if (this->dirty) {
-      // drain output only if there could be frames in the encoder
 
-      OMX_BUFFERHEADERTYPE* in_buf = this->free_in.pop();
+  if (dirty) {
+    OMX_BUFFERHEADERTYPE* in_buf = free_in.pop();
+    if (in_buf) {
       in_buf->nFilledLen = 0;
       in_buf->nOffset = 0;
       in_buf->nFlags = OMX_BUFFERFLAG_EOS;
-      in_buf->nTimeStamp = this->last_t + 1000000LL/this->fps;
+      in_buf->nTimeStamp = last_t + 1000000LL / fps;
 
-      OMX_CHECK(OMX_EmptyThisBuffer(this->handle, in_buf));
+      OMX_CHECK(OMX_EmptyThisBuffer(handle, in_buf));
 
       while (true) {
-        OMX_BUFFERHEADERTYPE *out_buf = this->done_out.pop();
+        OMX_BUFFERHEADERTYPE *out_buf = done_out.pop();
+        if (!out_buf) break;
 
         handle_out_buf(this, out_buf);
 
@@ -697,55 +527,112 @@ void OmxEncoder::encoder_close() {
           break;
         }
       }
-      this->dirty = false;
+    }
+    dirty = false;
   }
 
-    if (this->remuxing) {
+  if (out_stream) {
-      av_write_trailer(this->ofmt_ctx);
+    out_stream->nb_frames = counter;
-      avcodec_free_context(&this->codec_ctx);
+    out_stream->duration = av_rescale_q(counter, AVRational{1, fps}, out_stream->time_base);
-      avio_closep(&this->ofmt_ctx->pb);
+  }
-      avformat_free_context(this->ofmt_ctx);
+
-    } else {
+  if (ofmt_ctx) {
-      fclose(this->of);
+    av_write_trailer(ofmt_ctx);
-      this->of = nullptr;
+    ofmt_ctx->duration = out_stream ? out_stream->duration : 0;
+    avio_closep(&ofmt_ctx->pb);
+    avformat_free_context(ofmt_ctx);
+    ofmt_ctx = nullptr;
+    out_stream = nullptr;
+  }
+
+  if (lock_path[0] != '\0') {
+    unlink(lock_path);
+  }
+
+  is_open = false;
+
+  // Remux with faststart for streaming/seeking support
+  if (strlen(vid_path) > 0) {
+    char fixed_path[1024];
+    snprintf(fixed_path, sizeof(fixed_path), "%s.fixed.mp4", vid_path);
+
+    char cmd[2048];
+    snprintf(cmd, sizeof(cmd), "ffmpeg -y -i \"%s\" -c copy -movflags +faststart \"%s\" && mv \"%s\" \"%s\"",
+             vid_path, fixed_path, fixed_path, vid_path);
+
+    int ret = system(cmd);
+    if (ret != 0) {
+      LOGW("ffmpeg faststart remux failed with exit code %d", ret);
     }
-    unlink(this->lock_path);
   }
-  this->is_open = false;
 }
 
 OmxEncoder::~OmxEncoder() {
-  assert(!this->is_open);
+  if (is_open) {
-
+    LOGE("OmxEncoder closed with is_open=true, calling encoder_close()");
-  OMX_CHECK(OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateIdle, NULL));
+    encoder_close();
-
-  wait_for_state(OMX_StateIdle);
-
-  OMX_CHECK(OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateLoaded, NULL));
-
-  for (auto &buf : this->in_buf_headers) {
-    OMX_CHECK(OMX_FreeBuffer(this->handle, PORT_INDEX_IN, buf));
   }
 
-  for (auto &buf : this->out_buf_headers) {
+  if (!handle) {
-    OMX_CHECK(OMX_FreeBuffer(this->handle, PORT_INDEX_OUT, buf));
+    LOGE("OMX handle is null in destructor, skipping teardown.");
+    return;
+  }
+
+  OMX_ERRORTYPE err;
+
+  err = OMX_SendCommand(handle, OMX_CommandStateSet, OMX_StateIdle, NULL);
+  if (err != OMX_ErrorNone) {
+    LOGE("Failed to set OMX state to Idle: %x", err);
+  } else {
+    wait_for_state(OMX_StateIdle);
+  }
+
+  err = OMX_SendCommand(handle, OMX_CommandStateSet, OMX_StateLoaded, NULL);
+  if (err != OMX_ErrorNone) {
+    LOGE("Failed to set OMX state to Loaded: %x", err);
+  }
+
+  for (OMX_BUFFERHEADERTYPE *buf : in_buf_headers) {
+    if (buf) {
+      err = OMX_FreeBuffer(handle, PORT_INDEX_IN, buf);
+      if (err != OMX_ErrorNone) {
+        LOGE("Failed to free input buffer: %x", err);
+      }
+    }
+  }
+
+  for (OMX_BUFFERHEADERTYPE *buf : out_buf_headers) {
+    if (buf) {
+      err = OMX_FreeBuffer(handle, PORT_INDEX_OUT, buf);
+      if (err != OMX_ErrorNone) {
+        LOGE("Failed to free output buffer: %x", err);
+      }
+    }
   }
 
   wait_for_state(OMX_StateLoaded);
 
-  OMX_CHECK(OMX_FreeHandle(this->handle));
+  err = OMX_FreeHandle(handle);
+  if (err != OMX_ErrorNone) {
+    LOGE("Failed to free OMX handle: %x", err);
+  }
+
+  handle = nullptr;
+
+  err = OMX_Deinit();
+  if (err != OMX_ErrorNone) {
+    LOGE("OMX_Deinit failed: %x", err);
+  }
 
   OMX_BUFFERHEADERTYPE *out_buf;
-  while (this->free_in.try_pop(out_buf));
+  while (free_in.try_pop(out_buf));
-  while (this->done_out.try_pop(out_buf));
+  while (done_out.try_pop(out_buf));
 
-  if (this->codec_config) {
+  if (codec_config) {
-    free(this->codec_config);
+    free(codec_config);
+    codec_config = nullptr;
   }
 
-  if (this->downscale) {
+  in_buf_headers.clear();
-    free(this->y_ptr2);
+  out_buf_headers.clear();
-    free(this->u_ptr2);
-    free(this->v_ptr2);
-  }
 }

selfdrive/frogpilot/screenrecorder/omx_encoder.h

@@ -12,10 +12,10 @@ extern "C" {
 
 #include "common/queue.h"
 
-// OmxEncoder, lossey codec using hardware hevc
+// OmxEncoder, lossey codec using hardware H.264
 class OmxEncoder {
 public:
-  OmxEncoder(const char* path, int width, int height, int fps, int bitrate, bool h265, bool downscale);
+  OmxEncoder(const char* path, int width, int height, int fps, int bitrate);
   ~OmxEncoder();
 
   int encode_frame_rgba(const uint8_t *ptr, int in_width, int in_height, uint64_t ts);
@@ -44,31 +44,26 @@ private:
   int counter = 0;
 
   std::string path;
-  FILE *of;
+  FILE *of = nullptr;
 
-  size_t codec_config_len;
+  size_t codec_config_len = 0;
-  uint8_t *codec_config = NULL;
+  uint8_t *codec_config = nullptr;
-  bool wrote_codec_config;
+  bool wrote_codec_config = false;
 
   std::mutex state_lock;
   std::condition_variable state_cv;
   OMX_STATETYPE state = OMX_StateLoaded;
 
-  OMX_HANDLETYPE handle;
+  OMX_HANDLETYPE handle = nullptr;
 
   std::vector<OMX_BUFFERHEADERTYPE *> in_buf_headers;
   std::vector<OMX_BUFFERHEADERTYPE *> out_buf_headers;
 
-  uint64_t last_t;
+  uint64_t last_t = 0;
 
   SafeQueue<OMX_BUFFERHEADERTYPE *> free_in;
   SafeQueue<OMX_BUFFERHEADERTYPE *> done_out;
 
-  AVFormatContext *ofmt_ctx;
+  AVFormatContext *ofmt_ctx = nullptr;
-  AVCodecContext *codec_ctx;
+  AVStream *out_stream = nullptr;
-  AVStream *out_stream;
-  bool remuxing;
-
-  bool downscale;
-  uint8_t *y_ptr2, *u_ptr2, *v_ptr2;
 };

selfdrive/frogpilot/screenrecorder/screenrecorder.cc

@@ -27,7 +27,7 @@ ScreenRecorder::ScreenRecorder(QWidget *parent) : QPushButton(parent), image_que
 
 void ScreenRecorder::initializeEncoder() {
   const std::string path = "/data/media/0/videos";
-  encoder = std::make_unique<OmxEncoder>(path.c_str(), recording_width, recording_height, 60, 2 * 1024 * 1024, false, false);
+  encoder = std::make_unique<OmxEncoder>(path.c_str(), recording_width, recording_height, 60, 2 * 1024 * 1024);
 }
 
 ScreenRecorder::~ScreenRecorder() {

selfdrive/manager/manager.py

@@ -87,7 +87,12 @@ def manager_init(frogpilot_functions) -> None:
   params_memory = Params("/dev/shm/params")
   params_memory.put("TelemetryEnabled", "0")
   params_memory.put("VpnEnabled", "1")
+  params_memory.put("DashcamFrames", "0")
+  params_memory.put("DashcamState", "stopped")
+  params_memory.put("DashcamShutdown", "0")
+  params_memory.put("ModelFps", "20")
   params_memory.put("ModelStandby", "0")
+  params_memory.put("ShutdownTouchReset", "0")
   params_memory.put("ModelStandbyTs", "0")
   params_memory.put("CarIsMetric", "0")
   params_memory.put("ClearpilotSpeedDisplay", "")
@@ -173,7 +178,6 @@ def manager_init(frogpilot_functions) -> None:
     ("DisableOpenpilotLongitudinal", "0"),
     ("DisableVTSCSmoothing", "0"),
     ("DisengageVolume", "100"),
-    ("DashcamDebug", "1"),
     ("TelemetryEnabled", "0"),
     ("DragonPilotTune", "0"),
     ("DriverCamera", "0"),

selfdrive/modeld/modeld.py

@@ -248,8 +248,7 @@ def main(demo=False):
     lat_active = sm['carControl'].latActive
     lane_changing = params_memory.get_bool("no_lat_lane_change")
     standstill = sm['carState'].standstill
-    calibrating = sm['liveCalibration'].calStatus != log.LiveCalibrationData.Status.calibrated
+    full_rate = lat_active or lane_changing
-    full_rate = lat_active or lane_changing or calibrating
 
     # Standby transitions (standstill only)
     should_standby = standstill and not full_rate

selfdrive/monitoring/dmonitoringd.py

@@ -21,6 +21,7 @@ def dmonitoringd_thread():
 
   v_cruise_last = 0
   driver_engaged = False
+  dbg_prev_valid = True  # CLEARPILOT: track valid transitions
 
   # 10Hz <- dmonitoringmodeld
   while True:
@@ -43,7 +44,18 @@ def dmonitoringd_thread():
     # Get data from dmonitoringmodeld
     events = Events()
 
-    if sm.all_checks() and len(sm['liveCalibration'].rpyCalib):
+    # CLEARPILOT: narrow update_states gate. The original sm.all_checks() also
+    # required modelV2 fresh (stops at standstill in two-state modeld) and
+    # liveCalibration.valid (calibrationd cascades its own freq_ok to valid, which
+    # flaps). Both made DM freeze pose → face_detected stuck False → awareness
+    # decayed to 0 within 6s of engagement. Narrow the gate to the subs
+    # update_states actually reads, and only to alive+valid (skip freq_ok and
+    # skip liveCalibration.valid). rpyCalib presence is sufficient to know
+    # calibration has produced output.
+    if (sm.alive['driverStateV2'] and sm.valid['driverStateV2'] and
+        sm.alive['carState'] and sm.valid['carState'] and
+        sm.alive['controlsState'] and sm.valid['controlsState'] and
+        sm.alive['liveCalibration'] and len(sm['liveCalibration'].rpyCalib) > 0):
       driver_status.update_states(sm['driverStateV2'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled)
 
     # Block engaging after max number of distrations
@@ -54,8 +66,16 @@ def dmonitoringd_thread():
     # Update events from driver state
     driver_status.update_events(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill)
 
+    # CLEARPILOT: log on transition to invalid so we can see which sub caused the cascade
+    dm_valid = sm.all_checks()
+    if dm_valid != dbg_prev_valid and not dm_valid:
+      import sys
+      bad = [s for s in sm.alive if not (sm.alive[s] and sm.valid[s] and sm.freq_ok.get(s, True))]
+      details = [f"{s}(a={sm.alive[s]},v={sm.valid[s]},f={sm.freq_ok[s]})" for s in bad]
+      print(f"CLP driverMonitoringState valid=False: {' '.join(details)}", file=sys.stderr, flush=True)
+    dbg_prev_valid = dm_valid
     # build driverMonitoringState packet
-    dat = messaging.new_message('driverMonitoringState', valid=sm.all_checks())
+    dat = messaging.new_message('driverMonitoringState', valid=dm_valid)
     dat.driverMonitoringState = {
       "events": events.to_msg(),
       "faceDetected": driver_status.face_detected,

selfdrive/thermald/fan_controller.py

@@ -1,5 +1,4 @@
 #!/usr/bin/env python3
-import os
 from abc import ABC, abstractmethod
 
 from openpilot.common.realtime import DT_TRML
@@ -7,11 +6,11 @@ from openpilot.common.numpy_fast import interp
 from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.controls.lib.pid import PIDController
 
-BENCH_MODE = os.environ.get("BENCH_MODE") == "1"
 
 class BaseFanController(ABC):
   @abstractmethod
-  def update(self, cur_temp: float, ignition: bool, standstill: bool = False) -> int:
+  def update(self, cur_temp: float, ignition: bool, standstill: bool = False,
+             is_parked: bool = True, cruise_engaged: bool = False) -> int:
     pass
 
 
@@ -23,25 +22,27 @@ class TiciFanController(BaseFanController):
     self.last_ignition = False
     self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, rate=(1 / DT_TRML))
 
-  def update(self, cur_temp: float, ignition: bool, standstill: bool = False) -> int:
+  def update(self, cur_temp: float, ignition: bool, standstill: bool = False,
-    # CLEARPILOT: bench mode always uses normal onroad fan range (30-100%)
+             is_parked: bool = True, cruise_engaged: bool = False) -> int:
-    if BENCH_MODE and ignition:
+    # CLEARPILOT fan range rules:
+    #   parked                                              → 0-100% (full, no floor)
+    #   in drive + cruise engaged (any speed, inc standstill) → 30-100%
+    #   in drive + cruise off + standstill                  → 10-100%
+    #   in drive + cruise off + moving                      → 30-100%
+    # In the PID output, neg_limit is how negative it can go (= max fan as %),
+    # pos_limit is how positive (= negative of min fan %).
+    if is_parked:
+      self.controller.neg_limit = -100
+      self.controller.pos_limit = 0
+    elif cruise_engaged:
       self.controller.neg_limit = -100
       self.controller.pos_limit = -30
-    # CLEARPILOT: at standstill below 74°C, clamp to 0-30% (quiet)
+    elif standstill:
-    # at standstill above 74°C, allow full 0-100% range
-    elif ignition and standstill and cur_temp < 74:
-      self.controller.neg_limit = -30
-      self.controller.pos_limit = 0
-    elif ignition and standstill:
       self.controller.neg_limit = -100
-      self.controller.pos_limit = 0
+      self.controller.pos_limit = -10
-    elif ignition:
-      self.controller.neg_limit = -100
-      self.controller.pos_limit = -15
     else:
-      self.controller.neg_limit = -30
+      self.controller.neg_limit = -100
-      self.controller.pos_limit = 0
+      self.controller.pos_limit = -30
 
     if ignition != self.last_ignition:
       self.controller.reset()

selfdrive/thermald/power_monitoring.py

@@ -36,12 +36,9 @@ class PowerMonitoring:
     # Reset capacity if it's low
     self.car_battery_capacity_uWh = max((CAR_BATTERY_CAPACITY_uWh / 10), int(car_battery_capacity_uWh))
 
-    # FrogPilot variables
+    # CLEARPILOT: hardcoded 30 minute shutdown timer (not user-configurable)
-    device_management = self.params.get_bool("DeviceManagement")
+    self.device_shutdown_time = 1800
-    device_shutdown_setting = self.params.get_int("DeviceShutdown") if device_management else 33
+    self.low_voltage_shutdown = VBATT_PAUSE_CHARGING
-    # If the toggle is set for < 1 hour, configure by 15 minute increments
-    self.device_shutdown_time = (device_shutdown_setting - 3) * 3600 if device_shutdown_setting >= 4 else device_shutdown_setting * (60 * 15)
-    self.low_voltage_shutdown = self.params.get_float("LowVoltageShutdown") if device_management else VBATT_PAUSE_CHARGING
 
   # Calculation tick
   def calculate(self, voltage: int | None, ignition: bool):
@@ -125,7 +122,13 @@ class PowerMonitoring:
                             offroad_time > VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S)
     should_shutdown |= offroad_time > self.device_shutdown_time
     should_shutdown |= low_voltage_shutdown
-    should_shutdown |= (self.car_battery_capacity_uWh <= 0)
+    # CLEARPILOT: removed `car_battery_capacity_uWh <= 0` trigger. That's a virtual
+    # capacity counter floor-limited to 3e6 µWh on boot which drains in ~12 min at
+    # typical device power. With a short drive that doesn't fully recharge (charges
+    # at 45W, cap 30e6 µWh = 36 min to full), a quick store stop could trip shutdown
+    # well before the intended 30-min idle timer. The real protection we want here
+    # is the car battery voltage check (kept above) — the virtual counter is now
+    # retained only for telemetry.
     should_shutdown &= not ignition
     should_shutdown &= (not self.params.get_bool("DisablePowerDown"))
     should_shutdown &= in_car

selfdrive/thermald/thermald.py

@@ -10,7 +10,7 @@ from pathlib import Path
 import psutil
 
 import cereal.messaging as messaging
-from cereal import log
+from cereal import car, log
 from cereal.services import SERVICE_LIST
 from openpilot.common.dict_helpers import strip_deprecated_keys
 from openpilot.common.filter_simple import FirstOrderFilter
@@ -197,7 +197,9 @@ def thermald_thread(end_event, hw_queue) -> None:
   engaged_prev = False
 
   params = Params()
+  params_memory = Params("/dev/shm/params")
   power_monitor = PowerMonitoring()
+  last_touch_reset = "0"  # CLEARPILOT: track last seen touch reset value
 
   HARDWARE.initialize_hardware()
   thermal_config = HARDWARE.get_thermal_config()
@@ -290,8 +292,18 @@ def thermald_thread(end_event, hw_queue) -> None:
     msg.deviceState.maxTempC = all_comp_temp
 
     if fan_controller is not None:
-      standstill = sm['carState'].standstill if sm.seen['carState'] else False
+      # CLEARPILOT: fan rules based on gear (parked vs drive) and cruise-engaged state
-      msg.deviceState.fanSpeedPercentDesired = fan_controller.update(all_comp_temp, onroad_conditions["ignition"], standstill)
+      if sm.seen['carState']:
+        cs = sm['carState']
+        standstill = cs.standstill
+        is_parked = cs.gearShifter == car.CarState.GearShifter.park
+      else:
+        standstill = False
+        is_parked = True  # default safe: assume parked, no fan floor
+      cruise_engaged = sm['controlsState'].enabled if sm.seen['controlsState'] else False
+      msg.deviceState.fanSpeedPercentDesired = fan_controller.update(
+        all_comp_temp, onroad_conditions["ignition"], standstill,
+        is_parked=is_parked, cruise_engaged=cruise_engaged)
 
     is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (time.monotonic() - off_ts > 60 * 5))
     if is_offroad_for_5_min and offroad_comp_temp > OFFROAD_DANGER_TEMP:
@@ -409,14 +421,21 @@ def thermald_thread(end_event, hw_queue) -> None:
     statlog.sample("som_power_draw", som_power_draw)
     msg.deviceState.somPowerDrawW = som_power_draw
 
+    # CLEARPILOT: screen tap resets shutdown timer (off_ts) while offroad
+    touch_reset = params_memory.get("ShutdownTouchReset")
+    if touch_reset is not None and touch_reset != last_touch_reset and off_ts is not None:
+      off_ts = time.monotonic()
+      cloudlog.info("shutdown timer reset by screen touch")
+    last_touch_reset = touch_reset
+
     # Check if we need to shut down
     if power_monitor.should_shutdown(onroad_conditions["ignition"], in_car, off_ts, started_seen):
       cloudlog.warning(f"shutting device down, offroad since {off_ts}")
       # CLEARPILOT: signal dashcamd to close recording gracefully before power-off
-      params.put_bool("DashcamShutdown", True)
+      params_memory.put_bool("DashcamShutdown", True)
       deadline = time.monotonic() + 15.0
       while time.monotonic() < deadline:
-        if not params.getBool("DashcamShutdown"):
+        if not params_memory.get_bool("DashcamShutdown"):
           cloudlog.info("dashcamd shutdown ack received")
           break
         time.sleep(0.5)

selfdrive/ui/qt/home.cc

@@ -85,15 +85,16 @@ void HomeWindow::updateState(const UIState &s) {
     showDriverView(s.scene.driver_camera_timer >= 10, true);
 
     // CLEARPILOT: show splash screen when onroad but in park
-    // In nightrider mode (states 1,4), stay on onroad view in park — only offroad transition shows splash
     bool parked = s.scene.parked;
     int screenMode = paramsMemory.getInt("ScreenDisplayMode");
     bool nightrider = (screenMode == 1 || screenMode == 4);
 
     if (parked && !was_parked_onroad) {
-      if (!nightrider) {
       LOGW("CLP UI: park transition -> showing splash");
       slayout->setCurrentWidget(ready);
+      // If we were in nightrider mode, switch to screen off
+      if (nightrider) {
+        paramsMemory.putInt("ScreenDisplayMode", 3);
       }
     } else if (!parked && was_parked_onroad) {
       LOGW("CLP UI: drive transition -> showing onroad");
@@ -121,9 +122,11 @@ void HomeWindow::offroadTransition(bool offroad) {
     slayout->setCurrentWidget(ready);
   } else {
     // CLEARPILOT: start onroad in splash — updateState will switch to
-    // camera view once the car shifts out of park
+    // camera view once the car shifts out of park. Reset has_driven so
+    // fresh ignition shows the READY text (not the post-drive textless splash).
     LOGW("CLP UI: onroad transition -> showing splash (parked)");
     was_parked_onroad = true;
+    ready->has_driven = false;
     slayout->setCurrentWidget(ready);
   }
 }
@@ -178,13 +181,6 @@ static const char *clpSidebarBtnStyle = R"(
 
 // clpActionBtnStyle removed — no longer used
 
-// Shutdown timer: param value -> display label
-static QString shutdownLabel(int val) {
-  if (val == 0) return "5 mins";
-  if (val <= 3) return QString::number(val * 15) + " mins";
-  int hours = val - 3;
-  return QString::number(hours) + (hours == 1 ? " hour" : " hours");
-}
 
 ClearPilotPanel::ClearPilotPanel(QWidget* parent) : QFrame(parent) {
   // Sidebar
@@ -255,27 +251,6 @@ ClearPilotPanel::ClearPilotPanel(QWidget* parent) : QFrame(parent) {
   });
   general_panel->addItem(resetCalibBtn);
 
-  // Shutdown Timer
-  int cur_shutdown = Params().getInt("DeviceShutdown");
-  auto shutdownBtn = new ButtonControl("Shutdown Timer", shutdownLabel(cur_shutdown),
-    "How long the device stays on after the car is turned off.");
-  connect(shutdownBtn, &ButtonControl::clicked, [=]() {
-    QStringList options;
-    for (int i = 0; i <= 33; i++) {
-      options << shutdownLabel(i);
-    }
-    int current = Params().getInt("DeviceShutdown");
-    QString sel = MultiOptionDialog::getSelection("Shutdown Timer", options, shutdownLabel(current), this);
-    if (!sel.isEmpty()) {
-      int idx = options.indexOf(sel);
-      if (idx >= 0) {
-        Params().putInt("DeviceShutdown", idx);
-        shutdownBtn->setValue(shutdownLabel(idx));
-      }
-    }
-  });
-  general_panel->addItem(shutdownBtn);
-
   // Power buttons
   QHBoxLayout *power_layout = new QHBoxLayout();
   power_layout->setSpacing(30);
@@ -365,6 +340,15 @@ ClearPilotPanel::ClearPilotPanel(QWidget* parent) : QFrame(parent) {
   });
   debug_panel->addItem(telemetry_toggle);
 
+  auto *health_metrics_toggle = new ToggleControl("System Health Overlay",
+    "Show controls lag, model frame drops, temperature, CPU, and memory usage "
+    "in the lower-right of the onroad UI. For diagnosing slowdown issues.", "",
+    Params().getBool("ClearpilotShowHealthMetrics"), this);
+  QObject::connect(health_metrics_toggle, &ToggleControl::toggleFlipped, [](bool on) {
+    Params().putBool("ClearpilotShowHealthMetrics", on);
+  });
+  debug_panel->addItem(health_metrics_toggle);
+
   auto *vpn_toggle = new ToggleControl("VPN",
     "Connect to vpn.hanson.xyz for remote SSH access. "
     "Disabling kills the active tunnel and stops reconnection attempts.", "",

selfdrive/ui/qt/onroad.cc

@@ -79,7 +79,8 @@ void OnroadWindow::updateState(const UIState &s) {
   nvg->update();  // CLEARPILOT: force repaint every frame for HUD elements
 
   QColor bgColor = bg_colors[s.status];
-  if (paramsMemory.getBool("no_lat_lane_change") == 1) {
+  // CLEARPILOT: read from frogpilotCarControl cereal message (was paramsMemory)
+  if ((*s.sm)["frogpilotCarControl"].getFrogpilotCarControl().getNoLatLaneChange()) {
     bgColor = bg_colors[STATUS_DISENGAGED];
   }
 
@@ -468,6 +469,9 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
   drawSpeedLimitSign(p);
   drawCruiseWarningSign(p);
 
+  // CLEARPILOT: system health metrics in lower-right (debug overlay)
+  drawHealthMetrics(p);
+
   // Draw FrogPilot widgets
   paintFrogPilotWidgets(p);
 }
@@ -603,9 +607,9 @@ void AnnotatedCameraWidget::drawSpeedLimitSign(QPainter &p) {
     p.setFont(InterFont(30, QFont::Bold));
     p.drawText(innerRect.adjusted(0, 48, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "LIMIT");
 
-    // Speed limit number
+    // Speed limit number — shifted down ~10% of innerRect height via extra top inset
     p.setFont(InterFont(90, QFont::Bold));
-    p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpSpeedLimitDisplay);
+    p.drawText(innerRect.adjusted(0, 86, 0, 0), Qt::AlignCenter, clpSpeedLimitDisplay);
   } else {
     // Normal: white background, black border and text
     QColor borderColor(0, 0, 0);
@@ -631,9 +635,9 @@ void AnnotatedCameraWidget::drawSpeedLimitSign(QPainter &p) {
     p.setFont(InterFont(30, QFont::Bold));
     p.drawText(innerRect.adjusted(0, 48, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "LIMIT");
 
-    // Speed limit number
+    // Speed limit number — shifted down ~10% of innerRect height via extra top inset
     p.setFont(InterFont(90, QFont::Bold));
-    p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpSpeedLimitDisplay);
+    p.drawText(innerRect.adjusted(0, 86, 0, 0), Qt::AlignCenter, clpSpeedLimitDisplay);
   }
 
   p.restore();
@@ -687,7 +691,7 @@ void AnnotatedCameraWidget::drawCruiseWarningSign(QPainter &p) {
 
     // Cruise speed number
     p.setFont(InterFont(90, QFont::Bold));
-    p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpCruiseWarningSpeed);
+    p.drawText(innerRect.adjusted(0, 86, 0, 0), Qt::AlignCenter, clpCruiseWarningSpeed);
   } else {
     // Normal: colored background with white border/text
     QColor bgColor = isOver ? QColor(200, 30, 30, 240) : QColor(40, 160, 60, 240);
@@ -713,7 +717,7 @@ void AnnotatedCameraWidget::drawCruiseWarningSign(QPainter &p) {
 
     // Cruise speed number
     p.setFont(InterFont(90, QFont::Bold));
-    p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpCruiseWarningSpeed);
+    p.drawText(innerRect.adjusted(0, 86, 0, 0), Qt::AlignCenter, clpCruiseWarningSpeed);
   }
 
   p.restore();
@@ -727,6 +731,87 @@ void AnnotatedCameraWidget::drawText(QPainter &p, int x, int y, const QString &t
   p.drawText(real_rect.x(), real_rect.bottom(), text);
 }
 
+// CLEARPILOT: System health overlay — shows metrics that indicate the system
+// is overburdened or behind. Toggled via ClearpilotShowHealthMetrics param.
+// Metrics (top→bottom): FPS, DROP, TEMP, CPU, MEM, FAN
+//   FPS: modeld framerate — 20 normally, 0 in park. Read from ModelFps memory
+//        param which modeld writes only on transition.
+//   DROP (%): modelV2 frameDropPerc — modeld losing frames; controlsd errors >20%
+//   TEMP (°C): deviceState.maxTempC — thermal throttling starts ~75, serious >88
+//   CPU (%): max core from deviceState.cpuUsagePercent
+//   MEM (%): deviceState.memoryUsagePercent
+//   FAN (%): actual fan duty from peripheralState RPM (scaled to 6500 RPM = 100%)
+// Each value color-codes green/yellow/red by severity.
+void AnnotatedCameraWidget::drawHealthMetrics(QPainter &p) {
+  static bool enabled = Params().getBool("ClearpilotShowHealthMetrics");
+  static int check_counter = 0;
+  // re-check the param every ~2s without a toggle signal path
+  if (++check_counter >= 40) {
+    check_counter = 0;
+    enabled = Params().getBool("ClearpilotShowHealthMetrics");
+  }
+  if (!enabled) return;
+
+  SubMaster &sm = *(uiState()->sm);
+  auto ds = sm["deviceState"].getDeviceState();
+  auto mv = sm["modelV2"].getModelV2();
+  auto ps = sm["peripheralState"].getPeripheralState();
+
+  int model_fps = paramsMemory.getInt("ModelFps");
+  float drop_pct = mv.getFrameDropPerc();
+  float temp_c = ds.getMaxTempC();
+  int mem_pct = ds.getMemoryUsagePercent();
+  int cpu_pct = 0;
+  for (auto v : ds.getCpuUsagePercent()) cpu_pct = std::max(cpu_pct, (int)v);
+  // Actual fan (not commanded): scale RPM to 0-100 using 6500 RPM as full scale
+  int fan_pct = std::min(100, (int)(ps.getFanSpeedRpm() * 100 / 6500));
+
+  auto color_for = [](float v, float warn, float crit) {
+    if (v >= crit) return QColor(0xff, 0x50, 0x50);   // red
+    if (v >= warn) return QColor(0xff, 0xd0, 0x40);   // yellow
+    return QColor(0xff, 0xff, 0xff);                   // white (ok)
+  };
+
+  struct Row { QString label; QString value; QColor color; };
+  Row rows[] = {
+    {"FPS",  QString::number(model_fps),    QColor(0xff, 0xff, 0xff)},
+    {"DROP", QString::number((int)drop_pct),color_for(drop_pct, 5.f,  15.f)},
+    {"TEMP", QString::number((int)temp_c),  color_for(temp_c,  75.f,  88.f)},
+    {"CPU",  QString::number(cpu_pct),      color_for((float)cpu_pct, 75.f, 90.f)},
+    {"MEM",  QString::number(mem_pct),      color_for((float)mem_pct, 70.f, 85.f)},
+    {"FAN",  QString::number(fan_pct),      QColor(0xff, 0xff, 0xff)},
+  };
+
+  p.save();
+  p.setFont(InterFont(90, QFont::Bold));
+  QFontMetrics fm = p.fontMetrics();
+  int row_h = fm.height();              // natural line height at 90pt bold
+  int gap = 40;                         // requested 40px between values
+  int margin = 30;                      // requested 30px margin
+  int panel_w = 360;                    // fixed width — fits "TEMP 99"
+  int n = sizeof(rows) / sizeof(rows[0]);
+  int panel_h = n * row_h + (n - 1) * gap + 2 * margin;
+  int x = width() - panel_w - margin;
+  int y = height() - panel_h - margin;
+
+  // black background
+  p.setPen(Qt::NoPen);
+  p.setBrush(QColor(0, 0, 0, 200));
+  p.drawRoundedRect(QRect(x, y, panel_w, panel_h), 20, 20);
+
+  // rows
+  int text_y = y + margin + fm.ascent();
+  for (int i = 0; i < n; i++) {
+    p.setPen(rows[i].color);
+    // label left (shifted -50px per user request), value right
+    p.drawText(x + margin - 50, text_y, rows[i].label);
+    QRect vrect = fm.boundingRect(rows[i].value);
+    p.drawText(x + panel_w - margin - vrect.width(), text_y, rows[i].value);
+    text_y += row_h + gap;
+  }
+  p.restore();
+}
+
 void AnnotatedCameraWidget::initializeGL() {
   CameraWidget::initializeGL();
   qInfo() << "OpenGL version:" << QString((const char*)glGetString(GL_VERSION));
@@ -774,12 +859,14 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
 
   // CLEARPILOT: nightrider lines are 1px wider (3 instead of 2)
   int outlineWidth = outlineOnly ? 3 : 2;
+  // CLEARPILOT: lane lines 5% thinner than the generic outline (QPen accepts float width)
+  float laneLineWidth = outlineOnly ? (float)outlineWidth * 0.95f : (float)outlineWidth;
 
   // lanelines
   for (int i = 0; i < std::size(scene.lane_line_vertices); ++i) {
     QColor lineColor = QColor::fromRgbF(1.0, 1.0, 1.0, std::clamp<float>(scene.lane_line_probs[i], 0.0, 0.7));
     if (outlineOnly) {
-      painter.setPen(QPen(lineColor, outlineWidth));
+      painter.setPen(QPen(lineColor, laneLineWidth));
       painter.setBrush(Qt::NoBrush);
     } else {
       painter.setPen(Qt::NoPen);
@@ -806,7 +893,8 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
   // paint center lane path
   // QColor bg_colors[CHANGE_LANE_PATH_COLOR];
 
-  bool is_no_lat_lane_change = paramsMemory.getBool("no_lat_lane_change");
+  // CLEARPILOT: read from frogpilotCarControl cereal message (was paramsMemory)
+  bool is_no_lat_lane_change = sm["frogpilotCarControl"].getFrogpilotCarControl().getNoLatLaneChange();
 
   QColor center_lane_color;
   
@@ -867,8 +955,11 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
     }
 
     if (outlineOnly) {
-      painter.setPen(QPen(QColor(center_lane_color.red(), center_lane_color.green(),
+      // CLEARPILOT: in nightrider, the tire path outline is light blue at 3px.
-                                 center_lane_color.blue(), 180), outlineWidth));
+      // Uses a fixed light-blue instead of center_lane_color (which is status-tinted) so
+      // the path reads as a neutral guide, not as engagement/status feedback.
+      QColor lightBlue(153, 204, 255, 220);  // #99CCFF light blue, mostly opaque
+      painter.setPen(QPen(lightBlue, 3));
       painter.setBrush(Qt::NoBrush);
     } else {
       painter.setPen(Qt::NoPen);

selfdrive/ui/qt/onroad.h

@@ -53,6 +53,7 @@ private:
   void drawSpeedWidget(QPainter &p, int x, int y, const QString &title, const QString &speedLimitStr, QColor colorSpeed, int width = 176);
   void drawSpeedLimitSign(QPainter &p);
   void drawCruiseWarningSign(QPainter &p);
+  void drawHealthMetrics(QPainter &p);
 
   QVBoxLayout *main_layout;
   QPixmap dm_img;

selfdrive/ui/qt/window.cc

@@ -171,6 +171,16 @@ bool MainWindow::eventFilter(QObject *obj, QEvent *event) {
     case QEvent::TouchEnd:
     case QEvent::MouseButtonPress:
     case QEvent::MouseMove: {
+      // CLEARPILOT: tap while screen-off (mode 3) -> wake to auto-normal (mode 0)
+      Params pmem{"/dev/shm/params"};
+      if (!device()->isAwake()) {
+        if (pmem.getInt("ScreenDisplayMode") == 3) {
+          pmem.putInt("ScreenDisplayMode", 0);
+        }
+      }
+      // CLEARPILOT: reset shutdown timer on any screen touch
+      static int touch_counter = 0;
+      pmem.put("ShutdownTouchReset", std::to_string(++touch_counter));
       // ignore events when device is awakened by resetInteractiveTimeout
       ignore = !device()->isAwake();
       device()->resetInteractiveTimeout(uiState()->scene.screen_timeout, uiState()->scene.screen_timeout_onroad);
@@ -256,6 +266,11 @@ static StatusWindow::StatusData collectStatus() {
   // Telemetry
   d.telemetry = readFile("/data/params/d/TelemetryEnabled");
 
+  // Dashcam
+  d.dashcam_state = readFile("/dev/shm/params/d/DashcamState");
+  if (d.dashcam_state.isEmpty()) d.dashcam_state = "stopped";
+  d.dashcam_frames = readFile("/dev/shm/params/d/DashcamFrames");
+
   // Panda: checked on UI thread in applyResults() via scene.pandaType
 
   return d;
@@ -298,6 +313,7 @@ StatusWindow::StatusWindow(QWidget *parent) : QFrame(parent) {
   vpn_label = makeRow("VPN");
   gps_label = makeRow("GPS");
   telemetry_label = makeRow("Telemetry");
+  dashcam_label = makeRow("Dashcam");
 
   layout->addStretch();
 
@@ -369,6 +385,19 @@ void StatusWindow::applyResults() {
     telemetry_label->setText("Disabled");
     telemetry_label->setStyleSheet("color: grey; font-size: 38px;");
   }
+
+  if (d.dashcam_state == "recording") {
+    QString text = "Recording";
+    if (!d.dashcam_frames.isEmpty() && d.dashcam_frames != "0") text += " (" + d.dashcam_frames + " frames)";
+    dashcam_label->setText(text);
+    dashcam_label->setStyleSheet("color: #17c44d; font-size: 38px;");
+  } else if (d.dashcam_state == "waiting") {
+    dashcam_label->setText("Waiting");
+    dashcam_label->setStyleSheet("color: #ffaa00; font-size: 38px;");
+  } else {
+    dashcam_label->setText("Stopped");
+    dashcam_label->setStyleSheet("color: #ff4444; font-size: 38px;");
+  }
 }
 
 void StatusWindow::mousePressEvent(QMouseEvent *e) {

selfdrive/ui/qt/window.h

@@ -20,6 +20,7 @@ public:
   struct StatusData {
     QString time, storage, ram, load, temp, fan, ip, wifi;
     QString vpn_status, vpn_ip, gps, telemetry;
+    QString dashcam_state, dashcam_frames;
     float temp_c = 0;
   };
 
@@ -49,6 +50,7 @@ private:
   QLabel *gps_label;
   QLabel *time_label;
   QLabel *telemetry_label;
+  QLabel *dashcam_label;
   QLabel *panda_label;
 };
 

selfdrive/ui/ui.cc

@@ -118,8 +118,10 @@ void update_model(UIState *s,
   }
 
   // update path
+  // CLEARPILOT: read from frogpilotCarControl cereal message (was paramsMemory)
+  bool no_lat_lane_change = (*s->sm)["frogpilotCarControl"].getFrogpilotCarControl().getNoLatLaneChange();
   float path;
-  if (paramsMemory.getBool("no_lat_lane_change")) {
+  if (no_lat_lane_change) {
     path = (float)LANE_CHANGE_NO_LAT_PATH_WIDTH / 20;  // Release: better calc for EU users
   } else {
     path = (float)CENTER_LANE_WIDTH / 20; // Release: better calc for EU users
@@ -439,7 +441,7 @@ void UIState::updateStatus() {
 UIState::UIState(QObject *parent) : QObject(parent) {
   sm = std::make_unique<SubMaster, const std::initializer_list<const char *>>({
     "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
-    "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2",
+    "pandaStates", "peripheralState", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2",
     "wideRoadCameraState", "managerState", "uiPlan", "liveTorqueParameters",
     "frogpilotCarControl", "frogpilotDeviceState", "frogpilotPlan",
     "gpsLocation",
@@ -556,7 +558,10 @@ void Device::updateWakefulness(const UIState &s) {
   }
 
   if (ignition_state_changed) {
-    if (ignition_on && s.scene.screen_brightness_onroad == 0 && !s.scene.standby_mode) {
+    if (!ignition_on) {
+      // CLEARPILOT: ignition on→off blanks the screen immediately (tap still wakes).
+      resetInteractiveTimeout(0, 0);
+    } else if (s.scene.screen_brightness_onroad == 0 && !s.scene.standby_mode) {
       resetInteractiveTimeout(0, 0);
     } else {
       resetInteractiveTimeout(s.scene.screen_timeout, s.scene.screen_timeout_onroad);

system/clearpilot/gpsd.py

@@ -20,17 +20,16 @@ from openpilot.common.time import system_time_valid
 from openpilot.system.hardware.tici.pins import GPIO
 
 
-def is_daylight(lat: float, lon: float, utc_dt: datetime.datetime) -> bool:
+def _sunrise_sunset_min(lat: float, lon: float, utc_dt: datetime.datetime):
-  """Return True if it's between sunrise and sunset at the given location.
+  """Compute (sunrise_min, sunset_min) in UTC minutes since midnight of utc_dt's day.
-  Uses NOAA simplified solar position equations. Pure math, no external libs."""
+  Values can be negative or >1440 for western/eastern longitudes. Returns
+  (None, None) for polar night, ('always', 'always') for midnight sun."""
   n = utc_dt.timetuple().tm_yday
   gamma = 2 * math.pi / 365 * (n - 1 + (utc_dt.hour - 12) / 24)
-  # Equation of time (minutes)
   eqtime = 229.18 * (0.000075 + 0.001868 * math.cos(gamma)
            - 0.032077 * math.sin(gamma)
            - 0.014615 * math.cos(2 * gamma)
            - 0.040849 * math.sin(2 * gamma))
-  # Solar declination (radians)
   decl = (0.006918 - 0.399912 * math.cos(gamma)
           + 0.070257 * math.sin(gamma)
           - 0.006758 * math.cos(2 * gamma)
@@ -42,14 +41,41 @@ def is_daylight(lat: float, lon: float, utc_dt: datetime.datetime) -> bool:
   cos_ha = (math.cos(zenith) / (math.cos(lat_rad) * math.cos(decl))
             - math.tan(lat_rad) * math.tan(decl))
   if cos_ha < -1:
-    return True   # midnight sun
+    return ('always', 'always')   # midnight sun
   if cos_ha > 1:
-    return False  # polar night
+    return (None, None)           # polar night
   ha = math.degrees(math.acos(cos_ha))
   sunrise_min = 720 - 4 * (lon + ha) - eqtime
   sunset_min = 720 - 4 * (lon - ha) - eqtime
+  return (sunrise_min, sunset_min)
+
+
+def is_daylight(lat: float, lon: float, utc_dt: datetime.datetime) -> bool:
+  """Return True if the sun is currently above the horizon at (lat, lon).
+
+  Handles west-of-Greenwich correctly: at UTC midnight it may still be
+  evening local time, and the relevant sunset is the PREVIOUS UTC day's
+  value (which is >1440 min if we re-ref to that day, i.e. it's past
+  midnight UTC). Symmetric case for east-of-Greenwich at the other end.
+
+  Strategy: compute sunrise/sunset for yesterday, today, and tomorrow (each
+  relative to its own UTC midnight), shift them all onto today's clock
+  (yesterday = -1440, tomorrow = +1440), and check if now_min falls inside
+  any of the three [sunrise, sunset] intervals.
+  """
   now_min = utc_dt.hour * 60 + utc_dt.minute + utc_dt.second / 60
-  return sunrise_min <= now_min <= sunset_min
+  for day_offset in (-1, 0, 1):
+    d = utc_dt + datetime.timedelta(days=day_offset)
+    sr, ss = _sunrise_sunset_min(lat, lon, d)
+    if sr == 'always':
+      return True
+    if sr is None:
+      continue  # polar night this day
+    sr += day_offset * 1440
+    ss += day_offset * 1440
+    if sr <= now_min <= ss:
+      return True
+  return False
 
 
 def at_cmd(cmd: str) -> str:
@@ -159,6 +185,7 @@ def main():
   params_memory = Params("/dev/shm/params")
   last_daylight_check = 0.0
   daylight_computed = False
+  prev_daylight = None  # CLEARPILOT: gate IsDaylight write on change
   print("gpsd: entering main loop", file=sys.stderr, flush=True)
 
   while True:
@@ -205,7 +232,10 @@ def main():
           last_daylight_check = now_mono
           utc_now = datetime.datetime.utcfromtimestamp(fix["timestamp_ms"] / 1000)
           daylight = is_daylight(fix["latitude"], fix["longitude"], utc_now)
+          # CLEARPILOT: gate on change — daylight flips twice a day, don't rewrite every 30s
+          if daylight != prev_daylight:
             params_memory.put_bool("IsDaylight", daylight)
+            prev_daylight = daylight
 
           if not daylight_computed:
             daylight_computed = True
@@ -221,7 +251,7 @@ def main():
             params_memory.put_int("ScreenDisplayMode", 0)
             cloudlog.warning("gpsd: auto-switch to normal (sunrise)")
 
-    time.sleep(1.0)  # 1 Hz polling
+    time.sleep(0.5)  # 2 Hz polling
 
 
 if __name__ == "__main__":