Compare commits
16 Commits
7a0854387e
...
fps20
| Author | SHA1 | Date | |
|---|---|---|---|
| 2ce6e8fe0c | |||
| 3bd0c942e8 | |||
| 12da9acfdd | |||
| 62a403d0f1 | |||
| 7ee923b0e6 | |||
| 6a79996a14 | |||
| 426382960a | |||
| 54a2a3afc5 | |||
| 4c8ef93b2b | |||
| ba4176ffd0 | |||
| e86eafde15 | |||
| cf2b3fc637 | |||
| 5b67d4798b | |||
| d64a0f6420 | |||
| 4dae5804ab | |||
| 25b4672fda |
@@ -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`, `DashcamState` (default "stopped"), `DashcamFrames` (default "0")
|
||||
- **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
|
||||
@@ -311,8 +350,7 @@ The OMX encoder (`selfdrive/frogpilot/screenrecorder/omx_encoder.cc`) was ported
|
||||
|
||||
### Params
|
||||
|
||||
- `DashcamDebug` — when `"1"`, dashcamd runs even without car connected (for bench testing)
|
||||
- `DashcamShutdown` — set by thermald before power-off, dashcamd acks by clearing it
|
||||
- `DashcamShutdown` (memory param) — 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
|
||||
|
||||
@@ -524,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
|
||||
|
||||
|
||||
@@ -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 {
|
||||
|
||||
+3
-3
@@ -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.),
|
||||
|
||||
@@ -109,7 +109,6 @@ 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},
|
||||
@@ -201,7 +200,6 @@ std::unordered_map<std::string, uint32_t> keys = {
|
||||
{"TelemetryEnabled", PERSISTENT},
|
||||
{"Timezone", PERSISTENT},
|
||||
{"TrainingVersion", PERSISTENT},
|
||||
{"LatRequested", PERSISTENT},
|
||||
{"ModelFps", PERSISTENT},
|
||||
{"ModelStandby", PERSISTENT},
|
||||
{"ModelStandbyTs", PERSISTENT},
|
||||
@@ -510,8 +508,6 @@ std::unordered_map<std::string, uint32_t> keys = {
|
||||
{"WheelIcon", PERSISTENT},
|
||||
{"WheelSpeed", PERSISTENT},
|
||||
|
||||
// Clearpilot
|
||||
{"no_lat_lane_change", PERSISTENT},
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
+1
-1
@@ -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
|
||||
|
||||
|
||||
|
||||
@@ -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,9 +56,6 @@ class CarController(CarControllerBase):
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.last_button_frame = 0
|
||||
|
||||
# CLEARPILOT: cache Params instance — create_steering_messages was building
|
||||
# a new one every 10ms (100Hz), causing needless allocation + file opens
|
||||
self.params_memory = Params("/dev/shm/params")
|
||||
|
||||
def update(self, CC, CS, now_nanos, frogpilot_variables):
|
||||
actuators = CC.actuators
|
||||
@@ -116,7 +112,8 @@ class CarController(CarControllerBase):
|
||||
hda2_long = hda2 and self.CP.openpilotLongitudinalControl
|
||||
|
||||
# steering control
|
||||
no_lat_lane_change = self.params_memory.get_int("no_lat_lane_change", 1)
|
||||
# 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
|
||||
|
||||
Binary file not shown.
@@ -279,12 +279,12 @@ int main(int argc, char *argv[]) {
|
||||
// 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");
|
||||
if (state == RECORDING || state == IDLE_TIMEOUT) {
|
||||
close_trip();
|
||||
}
|
||||
params.putBool("DashcamShutdown", false);
|
||||
params_memory.putBool("DashcamShutdown", false);
|
||||
LOGW("dashcamd: shutdown ack sent, exiting");
|
||||
break;
|
||||
}
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 50 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 50 KiB |
@@ -81,9 +81,11 @@ 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.driving_gear = False
|
||||
|
||||
self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS
|
||||
|
||||
@@ -114,8 +116,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_valid=['testJoystick', 'gpsLocation'],
|
||||
ignore_alive=ignore + ['gpsLocation'], ignore_avg_freq=ignore+['radarState', 'testJoystick', 'gpsLocation'], ignore_valid=['testJoystick', 'gpsLocation'],
|
||||
frequency=int(1/DT_CTRL))
|
||||
|
||||
self.joystick_mode = self.params.get_bool("JoystickDebugMode")
|
||||
@@ -170,12 +171,6 @@ class Controls:
|
||||
self.current_alert_types = [ET.PERMANENT]
|
||||
self.logged_comm_issue = None
|
||||
self.not_running_prev = None
|
||||
self.lat_requested_ts = 0 # CLEARPILOT: timestamp when lat was first requested (ramp-up delay + comm issue suppression)
|
||||
self.prev_lat_requested = False
|
||||
self.prev_no_lat_lane_change = None # CLEARPILOT: gate no_lat_lane_change write on change
|
||||
# CLEARPILOT: gate frogpilotCarControl send on change (3 static bools, was publishing 100Hz)
|
||||
self._prev_fpcc = (None, None, None)
|
||||
self._last_fpcc_send_frame = 0
|
||||
self.steer_limited = False
|
||||
self.desired_curvature = 0.0
|
||||
self.experimental_mode = False
|
||||
@@ -210,8 +205,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
|
||||
@@ -461,13 +454,7 @@ class Controls:
|
||||
standby_ts = float(self.params_memory.get("ModelStandbyTs") or "0")
|
||||
except (ValueError, TypeError):
|
||||
standby_ts = 0
|
||||
now = time.monotonic()
|
||||
model_suppress = (now - standby_ts) < 2.0
|
||||
|
||||
# CLEARPILOT: suppress commIssue/location/params errors for 2s after lat was requested
|
||||
# Model FPS jumps from 4 to 20 on engage, causing transient freq check failures
|
||||
# lat_requested_ts is set in state_control on the False->True transition of LatRequested
|
||||
lat_engage_suppress = (now - self.lat_requested_ts) < 2.0
|
||||
model_suppress = (time.monotonic() - standby_ts) < 2.0
|
||||
|
||||
not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning}
|
||||
if self.sm.recv_frame['managerState'] and (not_running - IGNORE_PROCESSES):
|
||||
@@ -481,10 +468,7 @@ class Controls:
|
||||
self.events.add(EventName.cameraMalfunction)
|
||||
elif not self.sm.all_freq_ok(self.camera_packets):
|
||||
self.events.add(EventName.cameraFrameRate)
|
||||
# CLEARPILOT: also gate on model_suppress + lat_engage_suppress — the fps transition
|
||||
# on engage briefly pushes avg cycle time over the 11.1ms (90% of 10ms) threshold
|
||||
# while downstream services (plannerd, locationd) drain the sudden 5x message rate
|
||||
if not REPLAY and self.rk.lagging and not model_suppress and not lat_engage_suppress:
|
||||
if not REPLAY and self.rk.lagging:
|
||||
import sys
|
||||
print(f"CLP controlsdLagging: remaining={self.rk.remaining:.4f} standstill={CS.standstill} vEgo={CS.vEgo:.2f}", file=sys.stderr)
|
||||
self.events.add(EventName.controlsdLagging)
|
||||
@@ -501,7 +485,7 @@ class Controls:
|
||||
# generic catch-all. ideally, a more specific event should be added above instead
|
||||
has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE))
|
||||
no_system_errors = (not has_disable_events) or (len(self.events) == num_events)
|
||||
if (not self.sm.all_checks() or self.card.can_rcv_timeout) and no_system_errors and not model_suppress and not lat_engage_suppress:
|
||||
if (not self.sm.all_checks() or self.card.can_rcv_timeout) and no_system_errors and not model_suppress:
|
||||
if not self.sm.all_alive():
|
||||
self.events.add(EventName.commIssue)
|
||||
elif not self.sm.all_freq_ok():
|
||||
@@ -522,15 +506,13 @@ class Controls:
|
||||
self.logged_comm_issue = None
|
||||
|
||||
if not (self.CP.notCar and self.joystick_mode):
|
||||
# CLEARPILOT: also suppress on lat engage — modeld 4fps→20fps transition causes
|
||||
# transient inputsOK/valid drops in locationd/paramsd downstream
|
||||
if not self.sm['liveLocationKalman'].posenetOK and not model_suppress and not lat_engage_suppress:
|
||||
if not self.sm['liveLocationKalman'].posenetOK and not model_suppress:
|
||||
self.events.add(EventName.posenetInvalid)
|
||||
if not self.sm['liveLocationKalman'].deviceStable:
|
||||
self.events.add(EventName.deviceFalling)
|
||||
if not self.sm['liveLocationKalman'].inputsOK and not model_suppress and not lat_engage_suppress:
|
||||
if not self.sm['liveLocationKalman'].inputsOK and not model_suppress:
|
||||
self.events.add(EventName.locationdTemporaryError)
|
||||
if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY) and not model_suppress and not lat_engage_suppress:
|
||||
if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY) and not model_suppress:
|
||||
self.events.add(EventName.paramsdTemporaryError)
|
||||
|
||||
# conservative HW alert. if the data or frequency are off, locationd will throw an error
|
||||
@@ -544,8 +526,16 @@ class Controls:
|
||||
if self.cruise_mismatch_counter > int(6. / DT_CTRL):
|
||||
self.events.add(EventName.cruiseMismatch)
|
||||
|
||||
# CLEARPILOT: FCW disabled — car's own radar AEB works better and triggers reliably.
|
||||
# The comma FCW was producing false positives and adds nothing over the stock system.
|
||||
# Check for FCW
|
||||
stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25
|
||||
model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking
|
||||
planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
|
||||
if planner_fcw or model_fcw:
|
||||
self.events.add(EventName.fcw)
|
||||
# self.fcw_random_event_triggered = True
|
||||
# elif self.fcw_random_event_triggered and self.random_events:
|
||||
# self.events.add(EventName.yourFrogTriedToKillMe)
|
||||
# self.fcw_random_event_triggered = False
|
||||
|
||||
for m in messaging.drain_sock(self.log_sock, wait_for_one=False):
|
||||
try:
|
||||
@@ -693,18 +683,8 @@ class Controls:
|
||||
|
||||
# Check which actuators can be enabled
|
||||
standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
|
||||
lat_requested = (self.active or self.FPCC.alwaysOnLateral) and self.speed_check and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
|
||||
(not standstill or self.joystick_mode)
|
||||
|
||||
# CLEARPILOT: tell modeld early so it can ramp to 20fps, then delay latActive by
|
||||
# 250ms (5 frames at 20fps) so downstream services stabilize before steering engages.
|
||||
now_ts = time.monotonic()
|
||||
if lat_requested != self.prev_lat_requested:
|
||||
self.params_memory.put_bool("LatRequested", lat_requested)
|
||||
if lat_requested:
|
||||
self.lat_requested_ts = now_ts
|
||||
self.prev_lat_requested = lat_requested
|
||||
CC.latActive = lat_requested and (now_ts - self.lat_requested_ts) >= 0.25
|
||||
CC.latActive = (self.active or self.FPCC.alwaysOnLateral) and self.speed_check and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
|
||||
(not standstill or self.joystick_mode)
|
||||
CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
|
||||
|
||||
actuators = CC.actuators
|
||||
@@ -719,13 +699,11 @@ class Controls:
|
||||
CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left
|
||||
CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right
|
||||
|
||||
no_lat_lane_change = model_v2.meta.laneChangeState == LaneChangeState.laneChangeStarting and clearpilot_disable_lat_on_lane_change
|
||||
if no_lat_lane_change:
|
||||
if model_v2.meta.laneChangeState == LaneChangeState.laneChangeStarting and clearpilot_disable_lat_on_lane_change:
|
||||
CC.latActive = False
|
||||
# CLEARPILOT: only write on change — per-cycle atomic writes were eating ~15% CPU
|
||||
if no_lat_lane_change != self.prev_no_lat_lane_change:
|
||||
self.params_memory.put_bool("no_lat_lane_change", no_lat_lane_change)
|
||||
self.prev_no_lat_lane_change = no_lat_lane_change
|
||||
self.frogpilot_variables.no_lat_lane_change = True
|
||||
else:
|
||||
self.frogpilot_variables.no_lat_lane_change = False
|
||||
|
||||
if CS.leftBlinker or CS.rightBlinker:
|
||||
self.last_blinker_frame = self.sm.frame
|
||||
@@ -1005,11 +983,6 @@ class Controls:
|
||||
while True:
|
||||
self.step()
|
||||
self.rk.monitor_time()
|
||||
# CLEARPILOT: accumulated debt from startup/blocking calls never recovers.
|
||||
# Reset the rate keeper when catastrophically behind — prevents a one-time
|
||||
# ~8s fingerprinting stall from poisoning the lag metric for the whole session.
|
||||
if self.rk.remaining < -1.0:
|
||||
self.rk._next_frame_time = time.monotonic() + self.rk._interval
|
||||
except SystemExit:
|
||||
e.set()
|
||||
t.join()
|
||||
@@ -1080,11 +1053,13 @@ class Controls:
|
||||
elif self.sm['modelV2'].meta.turnDirection == Desire.turnRight:
|
||||
self.events.add(EventName.turningRight)
|
||||
|
||||
# CLEARPILOT: check crash file once per second, not every cycle (stat syscall at 100Hz hurts)
|
||||
if self.sm.frame % 100 == 0:
|
||||
self._crashed_file_exists = os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt'))
|
||||
if getattr(self, '_crashed_file_exists', False) and self.crashed_timer < 10:
|
||||
if os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')) and self.crashed_timer < 10:
|
||||
self.events.add(EventName.openpilotCrashed)
|
||||
|
||||
# if self.random_events and not self.openpilot_crashed_triggered:
|
||||
# self.events.add(EventName.openpilotCrashedRandomEvents)
|
||||
# self.openpilot_crashed_triggered = True
|
||||
|
||||
self.crashed_timer += DT_CTRL
|
||||
|
||||
# if self.speed_limit_alert or self.speed_limit_confirmation:
|
||||
@@ -1207,16 +1182,10 @@ class Controls:
|
||||
|
||||
self.FPCC.trafficModeActive = self.frogpilot_variables.traffic_mode and self.params_memory.get_bool("TrafficModeActive")
|
||||
|
||||
# CLEARPILOT: send only when one of the 3 fields changes, with 1Hz keepalive
|
||||
# Saves ~7ms/sec of capnp+zmq work (was sending 100Hz despite static contents)
|
||||
fpcc_tuple = (self.FPCC.alwaysOnLateral, self.FPCC.speedLimitChanged, self.FPCC.trafficModeActive)
|
||||
if fpcc_tuple != self._prev_fpcc or (self.sm.frame - self._last_fpcc_send_frame) >= 100:
|
||||
fpcc_send = messaging.new_message('frogpilotCarControl')
|
||||
fpcc_send.valid = CS.canValid
|
||||
fpcc_send.frogpilotCarControl = self.FPCC
|
||||
self.pm.send('frogpilotCarControl', fpcc_send)
|
||||
self._prev_fpcc = fpcc_tuple
|
||||
self._last_fpcc_send_frame = self.sm.frame
|
||||
fpcc_send = messaging.new_message('frogpilotCarControl')
|
||||
fpcc_send.valid = CS.canValid
|
||||
fpcc_send.frogpilotCarControl = self.FPCC
|
||||
self.pm.send('frogpilotCarControl', fpcc_send)
|
||||
|
||||
if self.params_memory.get_bool("FrogPilotTogglesUpdated"):
|
||||
self.update_frogpilot_params()
|
||||
@@ -1314,7 +1283,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']
|
||||
@@ -1327,7 +1297,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():
|
||||
|
||||
@@ -143,9 +143,6 @@ class LongitudinalPlanner:
|
||||
self.params = Params()
|
||||
self.params_memory = Params("/dev/shm/params")
|
||||
|
||||
# CLEARPILOT: track model FPS for dynamic dt adjustment
|
||||
self.model_fps = 20
|
||||
|
||||
self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS
|
||||
|
||||
self.release = get_short_branch() == "clearpilot"
|
||||
@@ -176,19 +173,6 @@ class LongitudinalPlanner:
|
||||
return x, v, a, j
|
||||
|
||||
def update(self, sm):
|
||||
# CLEARPILOT: read actual model FPS and adjust dt accordingly
|
||||
model_fps_raw = self.params_memory.get("ModelFps")
|
||||
if model_fps_raw is not None:
|
||||
try:
|
||||
fps = int(model_fps_raw)
|
||||
if fps > 0 and fps != self.model_fps:
|
||||
self.model_fps = fps
|
||||
self.dt = 1.0 / fps
|
||||
self.v_desired_filter.dt = self.dt
|
||||
self.v_desired_filter.update_alpha(2.0) # rc=2.0, same as __init__
|
||||
except (ValueError, ZeroDivisionError):
|
||||
pass
|
||||
|
||||
self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
|
||||
|
||||
v_ego = sm['carState'].vEgo
|
||||
@@ -255,10 +239,7 @@ class LongitudinalPlanner:
|
||||
self.j_desired_trajectory = np.interp(ModelConstants.T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
|
||||
|
||||
# TODO counter is only needed because radar is glitchy, remove once radar is gone
|
||||
# CLEARPILOT: scale crash_cnt threshold by FPS — at 20fps need 3 frames (0.15s),
|
||||
# at 4fps need 1 frame (0.25s already exceeds 0.15s window)
|
||||
fcw_threshold = max(0, int(0.15 * self.model_fps) - 1) # 20fps->2, 4fps->0
|
||||
self.fcw = self.mpc.crash_cnt > fcw_threshold and not sm['carState'].standstill
|
||||
self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill
|
||||
if self.fcw:
|
||||
cloudlog.info("FCW triggered")
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -89,7 +89,7 @@ def manager_init(frogpilot_functions) -> None:
|
||||
params_memory.put("VpnEnabled", "1")
|
||||
params_memory.put("DashcamFrames", "0")
|
||||
params_memory.put("DashcamState", "stopped")
|
||||
params_memory.put("LatRequested", "0")
|
||||
params_memory.put("DashcamShutdown", "0")
|
||||
params_memory.put("ModelFps", "20")
|
||||
params_memory.put("ModelStandby", "0")
|
||||
params_memory.put("ShutdownTouchReset", "0")
|
||||
@@ -178,7 +178,6 @@ def manager_init(frogpilot_functions) -> None:
|
||||
("DisableOpenpilotLongitudinal", "0"),
|
||||
("DisableVTSCSmoothing", "0"),
|
||||
("DisengageVolume", "100"),
|
||||
("DashcamDebug", "1"),
|
||||
("TelemetryEnabled", "0"),
|
||||
("DragonPilotTune", "0"),
|
||||
("DriverCamera", "0"),
|
||||
|
||||
@@ -183,9 +183,6 @@ def main(demo=False):
|
||||
model_transform_main = np.zeros((3, 3), dtype=np.float32)
|
||||
model_transform_extra = np.zeros((3, 3), dtype=np.float32)
|
||||
live_calib_seen = False
|
||||
model_standby = False
|
||||
last_standby_ts_write = 0
|
||||
params_memory = Params("/dev/shm/params")
|
||||
nav_features = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32)
|
||||
nav_instructions = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32)
|
||||
buf_main, buf_extra = None, None
|
||||
@@ -241,59 +238,10 @@ def main(demo=False):
|
||||
|
||||
sm.update(0)
|
||||
|
||||
# CLEARPILOT: power saving — three modes based on driving state
|
||||
# Full 20fps: lat requested or lane changing
|
||||
# Reduced 4fps: not lat requested, not standstill (driving without cruise)
|
||||
# Standby 0fps: standstill
|
||||
# Uses LatRequested (not carControl.latActive) so modeld ramps to 20fps BEFORE
|
||||
# controlsd actually engages steering — gives downstream services time to stabilize.
|
||||
lat_active = params_memory.get_bool("LatRequested")
|
||||
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 or calibrating
|
||||
|
||||
# Standby transitions (standstill only)
|
||||
should_standby = standstill and not full_rate
|
||||
if should_standby and not model_standby:
|
||||
params_memory.put_bool("ModelStandby", True)
|
||||
model_standby = True
|
||||
cloudlog.warning("modeld: standby ON (standstill)")
|
||||
elif not should_standby and model_standby:
|
||||
params_memory.put_bool("ModelStandby", False)
|
||||
model_standby = False
|
||||
run_count = 0
|
||||
frame_dropped_filter.x = 0.
|
||||
cloudlog.warning("modeld: standby OFF")
|
||||
if model_standby:
|
||||
now = _time.monotonic()
|
||||
if now - last_standby_ts_write > 1.0:
|
||||
params_memory.put("ModelStandbyTs", str(now))
|
||||
last_standby_ts_write = now
|
||||
last_vipc_frame_id = meta_main.frame_id
|
||||
continue
|
||||
|
||||
# Reduced framerate: daylight 10fps (skip 1/2), night 4fps (skip 4/5)
|
||||
# Daytime runs twice as fast — better model responsiveness when lighting is good
|
||||
# and the neural net has more signal. Night stays conservative for power.
|
||||
# Write standby timestamp so controlsd suppresses transient errors
|
||||
if not full_rate:
|
||||
is_daylight = params_memory.get_bool("IsDaylight")
|
||||
skip_interval = 2 if is_daylight else 5
|
||||
target_fps = b"10" if is_daylight else b"4"
|
||||
if params_memory.get("ModelFps") != target_fps:
|
||||
params_memory.put("ModelFps", target_fps.decode())
|
||||
now = _time.monotonic()
|
||||
if now - last_standby_ts_write > 1.0:
|
||||
params_memory.put("ModelStandbyTs", str(now))
|
||||
last_standby_ts_write = now
|
||||
if run_count % skip_interval != 0:
|
||||
last_vipc_frame_id = meta_main.frame_id
|
||||
run_count += 1
|
||||
continue
|
||||
else:
|
||||
if params_memory.get("ModelFps") != b"20":
|
||||
params_memory.put("ModelFps", "20")
|
||||
# CLEARPILOT: constant 20fps. Variable-rate + standby logic removed — the
|
||||
# variable-rate path caused freq_ok cascades in downstream consumers
|
||||
# (calibrationd/locationd/paramsd). Running at the camera's native rate is
|
||||
# simpler and keeps the full-stack localization chain happy.
|
||||
|
||||
desire = DH.desire
|
||||
is_rhd = sm["driverMonitoringState"].isRHD
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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,29 +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:
|
||||
# CLEARPILOT: bench mode always uses normal onroad fan range (30-100%)
|
||||
if BENCH_MODE and ignition:
|
||||
def update(self, cur_temp: float, ignition: bool, standstill: bool = False,
|
||||
is_parked: bool = True, cruise_engaged: bool = False) -> int:
|
||||
# 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)
|
||||
# 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:
|
||||
elif standstill:
|
||||
self.controller.neg_limit = -100
|
||||
self.controller.pos_limit = 0
|
||||
elif ignition:
|
||||
self.controller.neg_limit = -100
|
||||
self.controller.pos_limit = -15
|
||||
# CLEARPILOT: offroad but overheating (startup cooling) — full fan range
|
||||
elif cur_temp >= 75:
|
||||
self.controller.neg_limit = -100
|
||||
self.controller.pos_limit = 0
|
||||
self.controller.pos_limit = -10
|
||||
else:
|
||||
self.controller.neg_limit = -30
|
||||
self.controller.pos_limit = 0
|
||||
self.controller.neg_limit = -100
|
||||
self.controller.pos_limit = -30
|
||||
|
||||
if ignition != self.last_ignition:
|
||||
self.controller.reset()
|
||||
|
||||
@@ -36,8 +36,8 @@ class PowerMonitoring:
|
||||
# Reset capacity if it's low
|
||||
self.car_battery_capacity_uWh = max((CAR_BATTERY_CAPACITY_uWh / 10), int(car_battery_capacity_uWh))
|
||||
|
||||
# CLEARPILOT: hardcoded 10 minute shutdown timer (not user-configurable)
|
||||
self.device_shutdown_time = 600
|
||||
# CLEARPILOT: hardcoded 30 minute shutdown timer (not user-configurable)
|
||||
self.device_shutdown_time = 1800
|
||||
self.low_voltage_shutdown = VBATT_PAUSE_CHARGING
|
||||
|
||||
# Calculation tick
|
||||
@@ -122,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
|
||||
|
||||
@@ -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
|
||||
@@ -292,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
|
||||
msg.deviceState.fanSpeedPercentDesired = fan_controller.update(all_comp_temp, onroad_conditions["ignition"], standstill)
|
||||
# CLEARPILOT: fan rules based on gear (parked vs drive) and cruise-engaged state
|
||||
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:
|
||||
@@ -422,10 +432,10 @@ def thermald_thread(end_event, hw_queue) -> None:
|
||||
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.get_bool("DashcamShutdown"):
|
||||
if not params_memory.get_bool("DashcamShutdown"):
|
||||
cloudlog.info("dashcamd shutdown ack received")
|
||||
break
|
||||
time.sleep(0.5)
|
||||
|
||||
@@ -122,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);
|
||||
}
|
||||
}
|
||||
|
||||
+29
-18
@@ -79,9 +79,10 @@ 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];
|
||||
}
|
||||
}
|
||||
|
||||
if (bg != bgColor) {
|
||||
bg = bgColor;
|
||||
@@ -606,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);
|
||||
@@ -634,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();
|
||||
@@ -690,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);
|
||||
@@ -716,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();
|
||||
@@ -732,12 +733,14 @@ void AnnotatedCameraWidget::drawText(QPainter &p, int x, int y, const QString &t
|
||||
|
||||
// CLEARPILOT: System health overlay — shows metrics that indicate the system
|
||||
// is overburdened or behind. Toggled via ClearpilotShowHealthMetrics param.
|
||||
// Metrics (top→bottom): LAG, DROP, TEMP, CPU, MEM
|
||||
// LAG (ms): controlsd cumLagMs — most direct "is the loop keeping up" indicator
|
||||
// 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");
|
||||
@@ -750,16 +753,18 @@ void AnnotatedCameraWidget::drawHealthMetrics(QPainter &p) {
|
||||
if (!enabled) return;
|
||||
|
||||
SubMaster &sm = *(uiState()->sm);
|
||||
auto cs = sm["controlsState"].getControlsState();
|
||||
auto ds = sm["deviceState"].getDeviceState();
|
||||
auto mv = sm["modelV2"].getModelV2();
|
||||
auto ps = sm["peripheralState"].getPeripheralState();
|
||||
|
||||
float lag_ms = cs.getCumLagMs();
|
||||
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
|
||||
@@ -769,11 +774,12 @@ void AnnotatedCameraWidget::drawHealthMetrics(QPainter &p) {
|
||||
|
||||
struct Row { QString label; QString value; QColor color; };
|
||||
Row rows[] = {
|
||||
{"LAG", QString::number((int)lag_ms), color_for(lag_ms, 50.f, 200.f)},
|
||||
{"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();
|
||||
@@ -853,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);
|
||||
@@ -885,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;
|
||||
|
||||
@@ -946,9 +955,11 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
|
||||
}
|
||||
|
||||
if (outlineOnly) {
|
||||
// CLEARPILOT: center path (tire track) is 2x wider than other lines in nightrider
|
||||
painter.setPen(QPen(QColor(center_lane_color.red(), center_lane_color.green(),
|
||||
center_lane_color.blue(), 180), outlineWidth * 2));
|
||||
// CLEARPILOT: in nightrider, the tire path outline is light blue at 3px.
|
||||
// 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);
|
||||
|
||||
Binary file not shown.
+8
-3
@@ -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);
|
||||
|
||||
@@ -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:
|
||||
"""Return True if it's between sunrise and sunset at the given location.
|
||||
Uses NOAA simplified solar position equations. Pure math, no external libs."""
|
||||
def _sunrise_sunset_min(lat: float, lon: float, utc_dt: datetime.datetime):
|
||||
"""Compute (sunrise_min, sunset_min) in UTC minutes since midnight of utc_dt's day.
|
||||
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:
|
||||
|
||||
Reference in New Issue
Block a user