Compare commits
13 Commits
f46339c949
...
speed-limi
| Author | SHA1 | Date | |
|---|---|---|---|
| 5e7c8bed52 | |||
| 6cf21a3698 | |||
| adcffad276 | |||
| 6e7117b177 | |||
| 8ccdb47c88 | |||
| b84c268b6e | |||
| ffa9da2f97 | |||
| 5e7911e599 | |||
| 6fcd4b37ba | |||
| 8d5903b945 | |||
| cea8926604 | |||
| e98ae2f9d1 | |||
| 531b3edcd2 |
2
.gitignore
vendored
2
.gitignore
vendored
@@ -2,6 +2,8 @@ prebuilt
|
||||
system/clearpilot/dev/on_start_brian.sh
|
||||
system/clearpilot/dev/id_rsa
|
||||
system/clearpilot/dev/id_rsa.pub
|
||||
system/clearpilot/dev/id_ed25519
|
||||
system/clearpilot/dev/id_ed25519.pub
|
||||
venv/
|
||||
.venv/
|
||||
.ci_cache
|
||||
|
||||
@@ -47,7 +47,7 @@ chown -R comma:comma /data/openpilot
|
||||
|
||||
### Git
|
||||
|
||||
- Remote: `git@git.internal.hanson.xyz:brianhansonxyz/comma.git`
|
||||
- Remote: `git@git.hanson.xyz:brianhansonxyz/clearpilot.git`
|
||||
- Branch: `clearpilot`
|
||||
- Large model files are tracked in git (intentional — this is a backup)
|
||||
|
||||
|
||||
@@ -217,6 +217,7 @@ std::unordered_map<std::string, uint32_t> keys = {
|
||||
|
||||
// FrogPilot parameters
|
||||
{"AccelerationPath", PERSISTENT},
|
||||
{"BenchCruiseActive", CLEAR_ON_MANAGER_START},
|
||||
{"BenchCruiseSpeed", CLEAR_ON_MANAGER_START},
|
||||
{"ClpUiState", CLEAR_ON_MANAGER_START},
|
||||
{"BenchEngaged", CLEAR_ON_MANAGER_START},
|
||||
@@ -250,6 +251,16 @@ std::unordered_map<std::string, uint32_t> keys = {
|
||||
|
||||
{"CarSpeedLimitWarning", PERSISTENT},
|
||||
{"CarSpeedLimitLiteral", PERSISTENT},
|
||||
{"CarIsMetric", PERSISTENT},
|
||||
|
||||
{"ClearpilotSpeedDisplay", PERSISTENT},
|
||||
{"ClearpilotSpeedLimitDisplay", PERSISTENT},
|
||||
{"ClearpilotHasSpeed", PERSISTENT},
|
||||
{"ClearpilotIsMetric", PERSISTENT},
|
||||
{"ClearpilotSpeedUnit", PERSISTENT},
|
||||
{"ClearpilotCruiseWarning", PERSISTENT},
|
||||
{"ClearpilotCruiseWarningSpeed", PERSISTENT},
|
||||
{"ClearpilotPlayDing", PERSISTENT},
|
||||
|
||||
// {"SpeedLimitLatDesired", PERSISTENT},
|
||||
// {"SpeedLimitVTSC", PERSISTENT},
|
||||
|
||||
@@ -32,7 +32,9 @@ def main():
|
||||
continue
|
||||
|
||||
cloudlog.info("starting athena daemon")
|
||||
proc = Process(name='athenad', target=launcher, args=('selfdrive.athena.athenad', 'athenad'))
|
||||
from openpilot.selfdrive.manager.process import _log_dir
|
||||
log_path = _log_dir + "/athenad.log"
|
||||
proc = Process(name='athenad', target=launcher, args=('selfdrive.athena.athenad', 'athenad', log_path))
|
||||
proc.start()
|
||||
proc.join()
|
||||
cloudlog.event("athenad exited", exitcode=proc.exitcode)
|
||||
|
||||
@@ -212,6 +212,7 @@ class CarState(CarStateBase):
|
||||
|
||||
# self.params_memory.put_int("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
|
||||
self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_conv)
|
||||
self.params_memory.put("CarIsMetric", "1" if self.is_metric else "0")
|
||||
self.params_memory.put_float("CarCruiseDisplayActual", cp_cruise.vl["SCC11"]["VSetDis"])
|
||||
|
||||
|
||||
@@ -420,6 +421,7 @@ class CarState(CarStateBase):
|
||||
# print(self.calculate_speed_limit(cp, cp_cam))
|
||||
# self.params_memory.put_float("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
|
||||
self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_factor)
|
||||
self.params_memory.put("CarIsMetric", "1" if self.is_metric else "0")
|
||||
|
||||
# CLEARPILOT: telemetry logging — disabled, re-enable when needed
|
||||
# speed_limit_bus = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam
|
||||
|
||||
@@ -7,7 +7,9 @@ Usage:
|
||||
python3 -m selfdrive.clearpilot.bench_cmd speed 20
|
||||
python3 -m selfdrive.clearpilot.bench_cmd speedlimit 45
|
||||
python3 -m selfdrive.clearpilot.bench_cmd cruise 55
|
||||
python3 -m selfdrive.clearpilot.bench_cmd cruiseactive 0|1|2 (0=disabled, 1=active, 2=paused)
|
||||
python3 -m selfdrive.clearpilot.bench_cmd engaged 1
|
||||
python3 -m selfdrive.clearpilot.bench_cmd ding (trigger speed limit ding sound)
|
||||
python3 -m selfdrive.clearpilot.bench_cmd debugbutton (simulate LKAS debug button press)
|
||||
python3 -m selfdrive.clearpilot.bench_cmd dump
|
||||
python3 -m selfdrive.clearpilot.bench_cmd wait_ready
|
||||
@@ -89,6 +91,7 @@ def main():
|
||||
"speed": "BenchSpeed",
|
||||
"speedlimit": "BenchSpeedLimit",
|
||||
"cruise": "BenchCruiseSpeed",
|
||||
"cruiseactive": "BenchCruiseActive",
|
||||
"engaged": "BenchEngaged",
|
||||
}
|
||||
|
||||
@@ -106,6 +109,10 @@ def main():
|
||||
elif cmd == "wait_ready":
|
||||
wait_ready()
|
||||
|
||||
elif cmd == "ding":
|
||||
params.put("ClearpilotPlayDing", "1")
|
||||
print("Ding triggered")
|
||||
|
||||
elif cmd == "debugbutton":
|
||||
# Simulate LKAS debug button — same state machine as controlsd.clearpilot_state_control()
|
||||
current = params.get_int("ScreenDisplayMode")
|
||||
|
||||
@@ -8,6 +8,7 @@ configurable vehicle state. Control values via params in /dev/shm/params:
|
||||
BenchSpeed - vehicle speed in mph (default: 0)
|
||||
BenchSpeedLimit - speed limit in mph (default: 0, 0=hidden)
|
||||
BenchCruiseSpeed - cruise set speed in mph (default: 0, 0=not set)
|
||||
BenchCruiseActive - 0=disabled, 1=active, 2=paused/standstill (default: 0)
|
||||
BenchGear - P, D, R, N (default: P)
|
||||
BenchEngaged - 0 or 1, cruise engaged (default: 0)
|
||||
|
||||
@@ -21,8 +22,8 @@ import time
|
||||
import cereal.messaging as messaging
|
||||
from cereal import log, car
|
||||
from openpilot.common.params import Params
|
||||
|
||||
MS_PER_MPH = 0.44704
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.selfdrive.clearpilot.speed_logic import SpeedState
|
||||
|
||||
|
||||
def main():
|
||||
@@ -33,9 +34,13 @@ def main():
|
||||
params_mem.put("BenchSpeed", "0")
|
||||
params_mem.put("BenchSpeedLimit", "0")
|
||||
params_mem.put("BenchCruiseSpeed", "0")
|
||||
params_mem.put("BenchCruiseActive", "0")
|
||||
params_mem.put("BenchGear", "P")
|
||||
params_mem.put("BenchEngaged", "0")
|
||||
|
||||
# ClearPilot speed processing
|
||||
speed_state = SpeedState()
|
||||
|
||||
# thermald handles deviceState (reads our fake pandaStates for ignition)
|
||||
pm = messaging.PubMaster([
|
||||
"pandaStates", "carState", "controlsState",
|
||||
@@ -61,11 +66,25 @@ def main():
|
||||
speed_limit_mph = float((params_mem.get("BenchSpeedLimit", encoding="utf-8") or "0").strip())
|
||||
cruise_mph = float((params_mem.get("BenchCruiseSpeed", encoding="utf-8") or "0").strip())
|
||||
gear_str = (params_mem.get("BenchGear", encoding="utf-8") or "P").strip().upper()
|
||||
cruise_active_str = (params_mem.get("BenchCruiseActive", encoding="utf-8") or "0").strip()
|
||||
engaged = (params_mem.get("BenchEngaged", encoding="utf-8") or "0").strip() == "1"
|
||||
|
||||
speed_ms = speed_mph * MS_PER_MPH
|
||||
speed_ms = speed_mph * CV.MPH_TO_MS
|
||||
speed_limit_ms = speed_limit_mph * CV.MPH_TO_MS
|
||||
cruise_ms = cruise_mph * CV.MPH_TO_MS
|
||||
gear = gear_map.get(gear_str, car.CarState.GearShifter.park)
|
||||
|
||||
# Cruise state: 0=disabled, 1=active, 2=paused
|
||||
cruise_active = cruise_active_str == "1"
|
||||
cruise_standstill = cruise_active_str == "2"
|
||||
|
||||
# ClearPilot speed processing (~2 Hz at 10 Hz loop)
|
||||
if frame % 5 == 0:
|
||||
has_speed = speed_mph > 0
|
||||
speed_state.update(speed_ms, has_speed, speed_limit_ms, is_metric=False,
|
||||
cruise_speed_ms=cruise_ms, cruise_active=cruise_active or cruise_standstill,
|
||||
cruise_standstill=cruise_standstill)
|
||||
|
||||
# pandaStates — 10 Hz (thermald reads ignition from this)
|
||||
if frame % 1 == 0:
|
||||
dat = messaging.new_message("pandaStates", 1)
|
||||
@@ -82,7 +101,7 @@ def main():
|
||||
cs.standstill = speed_ms < 0.1
|
||||
cs.cruiseState.available = True
|
||||
cs.cruiseState.enabled = engaged
|
||||
cs.cruiseState.speed = cruise_mph * MS_PER_MPH if cruise_mph > 0 else 0
|
||||
cs.cruiseState.speed = cruise_mph * CV.MPH_TO_MS if cruise_mph > 0 else 0
|
||||
pm.send("carState", dat)
|
||||
|
||||
# controlsState — 10 Hz
|
||||
|
||||
Binary file not shown.
BIN
selfdrive/clearpilot/sounds/ding.wav
Normal file
BIN
selfdrive/clearpilot/sounds/ding.wav
Normal file
Binary file not shown.
90
selfdrive/clearpilot/speed_logic.py
Normal file
90
selfdrive/clearpilot/speed_logic.py
Normal file
@@ -0,0 +1,90 @@
|
||||
"""
|
||||
ClearPilot speed processing module.
|
||||
|
||||
Shared logic for converting raw speed and speed limit data into display-ready
|
||||
values. Called from controlsd (live mode) and bench_onroad (bench mode).
|
||||
|
||||
Reads raw inputs, converts to display units (mph or kph based on car's CAN
|
||||
unit setting), detects speed limit changes, and writes results to params_memory
|
||||
for the onroad UI to read.
|
||||
"""
|
||||
import math
|
||||
import time
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
|
||||
|
||||
class SpeedState:
|
||||
def __init__(self):
|
||||
self.params_memory = Params("/dev/shm/params")
|
||||
self.prev_speed_limit = 0
|
||||
|
||||
# Ding state tracking
|
||||
self.last_ding_time = 0.0
|
||||
self.prev_warning = ""
|
||||
self.prev_warning_speed_limit = 0
|
||||
|
||||
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):
|
||||
"""
|
||||
Convert raw m/s values to display-ready strings and write to params_memory.
|
||||
"""
|
||||
now = time.monotonic()
|
||||
|
||||
if is_metric:
|
||||
speed_display = speed_ms * CV.MS_TO_KPH
|
||||
speed_limit_display = speed_limit_ms * CV.MS_TO_KPH
|
||||
cruise_display = cruise_speed_ms * CV.MS_TO_KPH
|
||||
unit = "km/h"
|
||||
else:
|
||||
speed_display = speed_ms * CV.MS_TO_MPH
|
||||
speed_limit_display = speed_limit_ms * CV.MS_TO_MPH
|
||||
cruise_display = cruise_speed_ms * CV.MS_TO_MPH
|
||||
unit = "mph"
|
||||
|
||||
speed_int = int(math.floor(speed_display))
|
||||
speed_limit_int = int(round(speed_limit_display))
|
||||
cruise_int = int(round(cruise_display))
|
||||
|
||||
self.prev_speed_limit = speed_limit_int
|
||||
|
||||
# Write display-ready values to params_memory
|
||||
self.params_memory.put("ClearpilotHasSpeed", "1" if has_speed and speed_int > 0 else "0")
|
||||
self.params_memory.put("ClearpilotSpeedDisplay", str(speed_int) if has_speed and speed_int > 0 else "")
|
||||
self.params_memory.put("ClearpilotSpeedLimitDisplay", str(speed_limit_int) if speed_limit_int > 0 else "0")
|
||||
self.params_memory.put("ClearpilotSpeedUnit", unit)
|
||||
self.params_memory.put("ClearpilotIsMetric", "1" if is_metric else "0")
|
||||
|
||||
# Cruise warning logic
|
||||
warning = ""
|
||||
warning_speed = ""
|
||||
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
|
||||
if cruise_int >= speed_limit_int + over_threshold:
|
||||
warning = "over"
|
||||
warning_speed = str(cruise_int)
|
||||
elif cruise_int <= speed_limit_int - 5:
|
||||
warning = "under"
|
||||
warning_speed = str(cruise_int)
|
||||
|
||||
self.params_memory.put("ClearpilotCruiseWarning", warning)
|
||||
self.params_memory.put("ClearpilotCruiseWarningSpeed", warning_speed)
|
||||
|
||||
# Ding logic: play when warning sign appears or speed limit changes while visible
|
||||
should_ding = False
|
||||
if warning:
|
||||
if not self.prev_warning:
|
||||
# Warning sign just appeared
|
||||
should_ding = True
|
||||
elif speed_limit_int != self.prev_warning_speed_limit:
|
||||
# Speed limit changed while warning sign is visible
|
||||
should_ding = True
|
||||
|
||||
if should_ding and now - self.last_ding_time >= 30:
|
||||
self.params_memory.put("ClearpilotPlayDing", "1")
|
||||
self.last_ding_time = now
|
||||
|
||||
self.prev_warning = warning
|
||||
self.prev_warning_speed_limit = speed_limit_int if warning else 0
|
||||
@@ -38,6 +38,7 @@ from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CRUIS
|
||||
|
||||
from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS
|
||||
from openpilot.selfdrive.clearpilot.telemetry import tlog
|
||||
from openpilot.selfdrive.clearpilot.speed_logic import SpeedState
|
||||
from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
|
||||
|
||||
SOFT_DISABLE_TIME = 3 # seconds
|
||||
@@ -80,6 +81,10 @@ class Controls:
|
||||
self.params_memory.put_bool("CPTLkasButtonAction", False)
|
||||
self.params_memory.put_int("ScreenDisplayMode", 0)
|
||||
|
||||
# ClearPilot speed processing
|
||||
self.speed_state = SpeedState()
|
||||
self.speed_state_frame = 0
|
||||
|
||||
self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS
|
||||
|
||||
with car.CarParams.from_bytes(self.params.get("CarParams", block=True)) as msg:
|
||||
@@ -108,8 +113,9 @@ class Controls:
|
||||
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
|
||||
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
|
||||
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
|
||||
'testJoystick', 'frogpilotPlan'] + self.camera_packets + self.sensor_packets,
|
||||
ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ],
|
||||
'testJoystick', 'frogpilotPlan', 'gpsLocation'] + self.camera_packets + self.sensor_packets,
|
||||
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")
|
||||
@@ -198,6 +204,8 @@ 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
|
||||
@@ -1257,6 +1265,12 @@ class Controls:
|
||||
self.events.add(EventName.clpDebug)
|
||||
|
||||
def clearpilot_state_control(self, CC, CS):
|
||||
# CLEARPILOT: auto-reset display when shifting into drive from screen-off
|
||||
if self.driving_gear and not self.was_driving_gear:
|
||||
if self.params_memory.get_int("ScreenDisplayMode") == 3:
|
||||
self.params_memory.put_int("ScreenDisplayMode", 0)
|
||||
self.was_driving_gear = self.driving_gear
|
||||
|
||||
if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
|
||||
current = self.params_memory.get_int("ScreenDisplayMode")
|
||||
|
||||
@@ -1269,6 +1283,21 @@ class Controls:
|
||||
new_mode = 0 if current == 3 else 3
|
||||
|
||||
self.params_memory.put_int("ScreenDisplayMode", new_mode)
|
||||
|
||||
# ClearPilot speed processing (~2 Hz at 100 Hz loop)
|
||||
self.speed_state_frame += 1
|
||||
if self.speed_state_frame % 50 == 0:
|
||||
gps = self.sm['gpsLocation']
|
||||
has_gps = self.sm.valid['gpsLocation'] and gps.hasFix
|
||||
speed_ms = gps.speed if has_gps else 0.0
|
||||
speed_limit_ms = self.params_memory.get_float("CarSpeedLimit")
|
||||
is_metric = (self.params_memory.get("CarIsMetric", encoding="utf-8") or "0") == "1"
|
||||
cruise_speed_ms = CS.cruiseState.speed
|
||||
cruise_active = CS.cruiseState.enabled
|
||||
cruise_standstill = CS.cruiseState.standstill
|
||||
self.speed_state.update(speed_ms, has_gps, speed_limit_ms, is_metric,
|
||||
cruise_speed_ms, cruise_active, cruise_standstill)
|
||||
|
||||
return CC
|
||||
|
||||
def main():
|
||||
|
||||
@@ -89,6 +89,15 @@ def manager_init(frogpilot_functions) -> None:
|
||||
params_memory.put("VpnEnabled", "1")
|
||||
params_memory.put("ModelStandby", "0")
|
||||
params_memory.put("ModelStandbyTs", "0")
|
||||
params_memory.put("CarIsMetric", "0")
|
||||
params_memory.put("ClearpilotSpeedDisplay", "")
|
||||
params_memory.put("ClearpilotSpeedLimitDisplay", "0")
|
||||
params_memory.put("ClearpilotHasSpeed", "0")
|
||||
params_memory.put("ClearpilotIsMetric", "0")
|
||||
params_memory.put("ClearpilotSpeedUnit", "mph")
|
||||
params_memory.put("ClearpilotCruiseWarning", "")
|
||||
params_memory.put("ClearpilotCruiseWarningSpeed", "")
|
||||
params_memory.put("ClearpilotPlayDing", "0")
|
||||
params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION)
|
||||
params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION)
|
||||
if is_release_branch():
|
||||
|
||||
@@ -248,7 +248,8 @@ def main(demo=False):
|
||||
lat_active = sm['carControl'].latActive
|
||||
lane_changing = params_memory.get_bool("no_lat_lane_change")
|
||||
standstill = sm['carState'].standstill
|
||||
full_rate = lat_active or lane_changing
|
||||
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
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
from abc import ABC, abstractmethod
|
||||
|
||||
from openpilot.common.realtime import DT_TRML
|
||||
@@ -6,6 +7,8 @@ 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:
|
||||
@@ -21,10 +24,14 @@ class TiciFanController(BaseFanController):
|
||||
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: at standstill below 74°C, clamp to 0-10% (near silent)
|
||||
# CLEARPILOT: bench mode always uses normal onroad fan range (30-100%)
|
||||
if BENCH_MODE and ignition:
|
||||
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
|
||||
if ignition and standstill and cur_temp < 74:
|
||||
self.controller.neg_limit = -10
|
||||
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
|
||||
|
||||
@@ -355,14 +355,15 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
|
||||
setSpeed *= KM_TO_MILE;
|
||||
}
|
||||
|
||||
// CLEARPILOT: use GPS speed if available, hide speed display if no GPS fix
|
||||
has_gps_speed = sm.valid("gpsLocation") && sm["gpsLocation"].getGpsLocation().getHasFix();
|
||||
if (has_gps_speed) {
|
||||
float gps_speed_ms = sm["gpsLocation"].getGpsLocation().getSpeed();
|
||||
speed = std::max<float>(0.0, gps_speed_ms);
|
||||
speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH;
|
||||
} else {
|
||||
speed = 0.0;
|
||||
// CLEARPILOT: read speed and speed limit from params (written by speed_logic.py ~2Hz)
|
||||
clpParamFrame++;
|
||||
if (clpParamFrame % 10 == 0) { // ~2Hz at 20Hz UI update rate
|
||||
clpHasSpeed = paramsMemory.get("ClearpilotHasSpeed") == "1";
|
||||
clpSpeedDisplay = QString::fromStdString(paramsMemory.get("ClearpilotSpeedDisplay"));
|
||||
clpSpeedLimitDisplay = QString::fromStdString(paramsMemory.get("ClearpilotSpeedLimitDisplay"));
|
||||
clpSpeedUnit = QString::fromStdString(paramsMemory.get("ClearpilotSpeedUnit"));
|
||||
clpCruiseWarning = QString::fromStdString(paramsMemory.get("ClearpilotCruiseWarning"));
|
||||
clpCruiseWarningSpeed = QString::fromStdString(paramsMemory.get("ClearpilotCruiseWarningSpeed"));
|
||||
}
|
||||
|
||||
// auto speed_limit_sign = nav_instruction.getSpeedLimitSign();
|
||||
@@ -431,7 +432,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
|
||||
|
||||
// QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–";
|
||||
// QString speedLimitOffsetStr = slcSpeedLimitOffset == 0 ? "–" : QString::number(slcSpeedLimitOffset, 'f', 0).prepend(slcSpeedLimitOffset > 0 ? "+" : "");
|
||||
QString speedStr = QString::number(std::nearbyint(speed));
|
||||
// QString speedStr = QString::number(std::nearbyint(speed));
|
||||
QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–";
|
||||
|
||||
p.restore();
|
||||
@@ -455,14 +456,18 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
|
||||
// Todo: lead speed
|
||||
// Todo: Experimental speed
|
||||
|
||||
// CLEARPILOT: show GPS speed, hide when no fix
|
||||
if (has_gps_speed && !scene.hide_speed) {
|
||||
// CLEARPILOT: show speed from speed_logic params, hide when no speed or speed=0
|
||||
if (clpHasSpeed && !clpSpeedDisplay.isEmpty() && !scene.hide_speed) {
|
||||
p.setFont(InterFont(140, QFont::Bold));
|
||||
drawText(p, rect().center().x(), 210, speedStr);
|
||||
drawText(p, rect().center().x(), 210, clpSpeedDisplay);
|
||||
p.setFont(InterFont(50));
|
||||
drawText(p, rect().center().x(), 290, speedUnit, 200);
|
||||
drawText(p, rect().center().x(), 290, clpSpeedUnit, 200);
|
||||
}
|
||||
|
||||
// CLEARPILOT: speed limit sign in lower-left, cruise warning above it
|
||||
drawSpeedLimitSign(p);
|
||||
drawCruiseWarningSign(p);
|
||||
|
||||
// Draw FrogPilot widgets
|
||||
paintFrogPilotWidgets(p);
|
||||
}
|
||||
@@ -556,6 +561,164 @@ void AnnotatedCameraWidget::drawSpeedWidget(QPainter &p, int x, int y, const QSt
|
||||
}
|
||||
|
||||
|
||||
void AnnotatedCameraWidget::drawSpeedLimitSign(QPainter &p) {
|
||||
// Hide when no speed limit or speed limit is 0
|
||||
if (clpSpeedLimitDisplay.isEmpty() || clpSpeedLimitDisplay == "0") return;
|
||||
|
||||
p.save();
|
||||
|
||||
const int signW = 189;
|
||||
const int signH = 239;
|
||||
const int margin = 20;
|
||||
const int borderW = 6;
|
||||
const int innerBorderW = 4;
|
||||
const int innerMargin = 10;
|
||||
const int cornerR = 15;
|
||||
|
||||
// Position: 20px from lower-left corner
|
||||
QRect signRect(margin, height() - signH - margin, signW, signH);
|
||||
|
||||
if (nightriderMode) {
|
||||
// Nightrider: black background, light gray-blue border and text
|
||||
QColor borderColor(160, 180, 210);
|
||||
QColor textColor(160, 180, 210);
|
||||
|
||||
// Outer border
|
||||
p.setPen(QPen(borderColor, borderW));
|
||||
p.setBrush(QColor(0, 0, 0, 220));
|
||||
p.drawRoundedRect(signRect, cornerR, cornerR);
|
||||
|
||||
// Inner border
|
||||
QRect innerRect = signRect.adjusted(innerMargin, innerMargin, -innerMargin, -innerMargin);
|
||||
p.setPen(QPen(borderColor, innerBorderW));
|
||||
p.setBrush(Qt::NoBrush);
|
||||
p.drawRoundedRect(innerRect, cornerR - 4, cornerR - 4);
|
||||
|
||||
// "SPEED" text
|
||||
p.setPen(textColor);
|
||||
p.setFont(InterFont(30, QFont::Bold));
|
||||
p.drawText(innerRect.adjusted(0, 15, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SPEED");
|
||||
|
||||
// "LIMIT" text
|
||||
p.setFont(InterFont(30, QFont::Bold));
|
||||
p.drawText(innerRect.adjusted(0, 48, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "LIMIT");
|
||||
|
||||
// Speed limit number
|
||||
p.setFont(InterFont(90, QFont::Bold));
|
||||
p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpSpeedLimitDisplay);
|
||||
} else {
|
||||
// Normal: white background, black border and text
|
||||
QColor borderColor(0, 0, 0);
|
||||
QColor textColor(0, 0, 0);
|
||||
|
||||
// Outer border
|
||||
p.setPen(QPen(borderColor, borderW));
|
||||
p.setBrush(QColor(255, 255, 255, 240));
|
||||
p.drawRoundedRect(signRect, cornerR, cornerR);
|
||||
|
||||
// Inner border
|
||||
QRect innerRect = signRect.adjusted(innerMargin, innerMargin, -innerMargin, -innerMargin);
|
||||
p.setPen(QPen(borderColor, innerBorderW));
|
||||
p.setBrush(Qt::NoBrush);
|
||||
p.drawRoundedRect(innerRect, cornerR - 4, cornerR - 4);
|
||||
|
||||
// "SPEED" text
|
||||
p.setPen(textColor);
|
||||
p.setFont(InterFont(30, QFont::Bold));
|
||||
p.drawText(innerRect.adjusted(0, 15, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SPEED");
|
||||
|
||||
// "LIMIT" text
|
||||
p.setFont(InterFont(30, QFont::Bold));
|
||||
p.drawText(innerRect.adjusted(0, 48, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "LIMIT");
|
||||
|
||||
// Speed limit number
|
||||
p.setFont(InterFont(90, QFont::Bold));
|
||||
p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpSpeedLimitDisplay);
|
||||
}
|
||||
|
||||
p.restore();
|
||||
}
|
||||
|
||||
void AnnotatedCameraWidget::drawCruiseWarningSign(QPainter &p) {
|
||||
// Only show when there's an active warning and the speed limit sign is visible
|
||||
if (clpCruiseWarning.isEmpty() || clpCruiseWarningSpeed.isEmpty()) return;
|
||||
if (clpSpeedLimitDisplay.isEmpty() || clpSpeedLimitDisplay == "0") return;
|
||||
|
||||
bool isOver = (clpCruiseWarning == "over");
|
||||
if (!isOver && clpCruiseWarning != "under") return;
|
||||
|
||||
p.save();
|
||||
|
||||
// Same dimensions as speed limit sign
|
||||
const int signW = 189;
|
||||
const int signH = 239;
|
||||
const int margin = 20;
|
||||
const int borderW = 6;
|
||||
const int innerBorderW = 4;
|
||||
const int innerMargin = 10;
|
||||
const int cornerR = 15;
|
||||
const int gap = 20;
|
||||
|
||||
// Position: directly above the speed limit sign
|
||||
int speedLimitY = height() - signH - margin;
|
||||
QRect signRect(margin, speedLimitY - signH - gap, signW, signH);
|
||||
|
||||
if (nightriderMode) {
|
||||
// Nightrider: black background with colored border/text
|
||||
QColor accentColor = isOver ? QColor(220, 50, 50) : QColor(50, 180, 80);
|
||||
|
||||
p.setPen(QPen(accentColor, borderW));
|
||||
p.setBrush(QColor(0, 0, 0, 220));
|
||||
p.drawRoundedRect(signRect, cornerR, cornerR);
|
||||
|
||||
QRect innerRect = signRect.adjusted(innerMargin, innerMargin, -innerMargin, -innerMargin);
|
||||
p.setPen(QPen(accentColor, innerBorderW));
|
||||
p.setBrush(Qt::NoBrush);
|
||||
p.drawRoundedRect(innerRect, cornerR - 4, cornerR - 4);
|
||||
|
||||
// "CRUISE" text
|
||||
p.setPen(accentColor);
|
||||
p.setFont(InterFont(26, QFont::Bold));
|
||||
p.drawText(innerRect.adjusted(0, 15, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "CRUISE");
|
||||
|
||||
// "SET" text
|
||||
p.setFont(InterFont(26, QFont::Bold));
|
||||
p.drawText(innerRect.adjusted(0, 45, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SET");
|
||||
|
||||
// Cruise speed number
|
||||
p.setFont(InterFont(90, QFont::Bold));
|
||||
p.drawText(innerRect.adjusted(0, 42, 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);
|
||||
QColor fgColor(255, 255, 255);
|
||||
|
||||
p.setPen(QPen(fgColor, borderW));
|
||||
p.setBrush(bgColor);
|
||||
p.drawRoundedRect(signRect, cornerR, cornerR);
|
||||
|
||||
QRect innerRect = signRect.adjusted(innerMargin, innerMargin, -innerMargin, -innerMargin);
|
||||
p.setPen(QPen(fgColor, innerBorderW));
|
||||
p.setBrush(Qt::NoBrush);
|
||||
p.drawRoundedRect(innerRect, cornerR - 4, cornerR - 4);
|
||||
|
||||
// "CRUISE" text
|
||||
p.setPen(fgColor);
|
||||
p.setFont(InterFont(26, QFont::Bold));
|
||||
p.drawText(innerRect.adjusted(0, 15, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "CRUISE");
|
||||
|
||||
// "SET" text
|
||||
p.setFont(InterFont(26, QFont::Bold));
|
||||
p.drawText(innerRect.adjusted(0, 45, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SET");
|
||||
|
||||
// Cruise speed number
|
||||
p.setFont(InterFont(90, QFont::Bold));
|
||||
p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpCruiseWarningSpeed);
|
||||
}
|
||||
|
||||
p.restore();
|
||||
}
|
||||
|
||||
void AnnotatedCameraWidget::drawText(QPainter &p, int x, int y, const QString &text, int alpha) {
|
||||
QRect real_rect = p.fontMetrics().boundingRect(text);
|
||||
real_rect.moveCenter({x, y - real_rect.height() / 2});
|
||||
|
||||
@@ -51,6 +51,8 @@ public:
|
||||
private:
|
||||
void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255);
|
||||
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);
|
||||
|
||||
QVBoxLayout *main_layout;
|
||||
QPixmap dm_img;
|
||||
@@ -59,6 +61,16 @@ private:
|
||||
bool nightriderMode = false;
|
||||
int displayMode = 0;
|
||||
QString speedUnit;
|
||||
|
||||
// ClearPilot speed state (from params_memory, updated ~2Hz)
|
||||
bool clpHasSpeed = false;
|
||||
QString clpSpeedDisplay;
|
||||
QString clpSpeedLimitDisplay;
|
||||
QString clpSpeedUnit;
|
||||
QString clpCruiseWarning;
|
||||
QString clpCruiseWarningSpeed;
|
||||
int clpParamFrame = 0;
|
||||
|
||||
float setSpeed;
|
||||
float speedLimit;
|
||||
bool is_cruise_set = false;
|
||||
|
||||
Binary file not shown.
@@ -93,6 +93,27 @@ class Soundd:
|
||||
|
||||
self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False)
|
||||
|
||||
# ClearPilot ding (plays independently of alerts)
|
||||
self.ding_sound = None
|
||||
self.ding_frame = 0
|
||||
self.ding_playing = False
|
||||
self.ding_check_counter = 0
|
||||
self._load_ding()
|
||||
|
||||
def _load_ding(self):
|
||||
ding_path = BASEDIR + "/selfdrive/clearpilot/sounds/ding.wav"
|
||||
try:
|
||||
wavefile = wave.open(ding_path, 'r')
|
||||
assert wavefile.getnchannels() == 1
|
||||
assert wavefile.getsampwidth() == 2
|
||||
assert wavefile.getframerate() == SAMPLE_RATE
|
||||
length = wavefile.getnframes()
|
||||
self.ding_sound = np.frombuffer(wavefile.readframes(length), dtype=np.int16).astype(np.float32) / (2**16/2)
|
||||
cloudlog.info(f"ClearPilot ding loaded: {length} frames")
|
||||
except Exception as e:
|
||||
cloudlog.error(f"Failed to load ding sound: {e}")
|
||||
self.ding_sound = None
|
||||
|
||||
def load_sounds(self):
|
||||
self.loaded_sounds: dict[int, np.ndarray] = {}
|
||||
|
||||
@@ -137,7 +158,20 @@ class Soundd:
|
||||
written_frames += frames_to_write
|
||||
self.current_sound_frame += frames_to_write
|
||||
|
||||
return ret * self.current_volume
|
||||
ret = ret * self.current_volume
|
||||
|
||||
# Mix in ClearPilot ding (independent of alerts, always max volume)
|
||||
if self.ding_playing and self.ding_sound is not None:
|
||||
ding_remaining = len(self.ding_sound) - self.ding_frame
|
||||
if ding_remaining > 0:
|
||||
frames_to_write = min(ding_remaining, frames)
|
||||
ret[:frames_to_write] += self.ding_sound[self.ding_frame:self.ding_frame + frames_to_write] * MAX_VOLUME
|
||||
self.ding_frame += frames_to_write
|
||||
else:
|
||||
self.ding_playing = False
|
||||
self.ding_frame = 0
|
||||
|
||||
return ret
|
||||
|
||||
def callback(self, data_out: np.ndarray, frames: int, time, status) -> None:
|
||||
if status:
|
||||
@@ -197,6 +231,14 @@ class Soundd:
|
||||
|
||||
self.get_audible_alert(sm)
|
||||
|
||||
# ClearPilot: check for ding trigger at ~2Hz
|
||||
self.ding_check_counter += 1
|
||||
if self.ding_check_counter % 10 == 0 and self.ding_sound is not None:
|
||||
if self.params_memory.get("ClearpilotPlayDing") == b"1":
|
||||
self.params_memory.put("ClearpilotPlayDing", "0")
|
||||
self.ding_playing = True
|
||||
self.ding_frame = 0
|
||||
|
||||
rk.keep_time()
|
||||
|
||||
assert stream.active
|
||||
|
||||
@@ -565,7 +565,10 @@ void Device::updateWakefulness(const UIState &s) {
|
||||
emit interactiveTimeout();
|
||||
}
|
||||
|
||||
if (s.scene.screen_brightness_onroad != 0) {
|
||||
// CLEARPILOT: ScreenDisplayMode 3 = screen off — override awake state
|
||||
if (paramsMemory.getInt("ScreenDisplayMode") == 3) {
|
||||
setAwake(false);
|
||||
} else if (s.scene.screen_brightness_onroad != 0) {
|
||||
setAwake(s.scene.ignition || interactive_timeout > 0);
|
||||
} else {
|
||||
setAwake(interactive_timeout > 0);
|
||||
|
||||
@@ -9,10 +9,39 @@ echo -n 1 > /data/params/d/SshEnabled
|
||||
sudo systemctl enable ssh 2>/dev/null
|
||||
sudo systemctl start ssh
|
||||
|
||||
# Decrypt and install SSH identity keys for root (git auth)
|
||||
serial=$(sed 's/.*androidboot.serialno=\([^ ]*\).*/\1/' /proc/cmdline)
|
||||
ssh_dir="/root/.ssh"
|
||||
if [[ $serial == 3889765b ]] && [[ ! -f "$ssh_dir/id_ed25519" || ! -f "$ssh_dir/id_ed25519.pub" ]]; then
|
||||
echo "Decrypting SSH identity keys for root (serial=$serial)..."
|
||||
tmpdir=$(mktemp -d)
|
||||
bash /data/openpilot/system/clearpilot/tools/decrypt /data/openpilot/system/clearpilot/dev/id_ed25519.cpt "$tmpdir/id_ed25519"
|
||||
bash /data/openpilot/system/clearpilot/tools/decrypt /data/openpilot/system/clearpilot/dev/id_ed25519.pub.cpt "$tmpdir/id_ed25519.pub"
|
||||
sudo mkdir -p "$ssh_dir"
|
||||
sudo cp "$tmpdir/id_ed25519" "$tmpdir/id_ed25519.pub" "$ssh_dir/"
|
||||
rm -rf "$tmpdir"
|
||||
sudo chmod 700 "$ssh_dir"
|
||||
sudo chmod 600 "$ssh_dir/id_ed25519"
|
||||
sudo chmod 644 "$ssh_dir/id_ed25519.pub"
|
||||
echo "SSH identity keys installed to $ssh_dir"
|
||||
fi
|
||||
|
||||
# Ensure root SSH config has git.hanson.xyz entry
|
||||
if ! grep -q "Host git.hanson.xyz" "$ssh_dir/config" 2>/dev/null; then
|
||||
sudo tee -a "$ssh_dir/config" > /dev/null <<'SSHCFG'
|
||||
|
||||
Host git.hanson.xyz
|
||||
IdentityFile /root/.ssh/id_ed25519
|
||||
StrictHostKeyChecking no
|
||||
SSHCFG
|
||||
sudo chmod 600 "$ssh_dir/config"
|
||||
echo "SSH config updated for git.hanson.xyz"
|
||||
fi
|
||||
|
||||
# Always ensure WiFi radio is on
|
||||
nmcli radio wifi on 2>/dev/null
|
||||
|
||||
# Provision (packages, git pull, build) if no quick_boot flag
|
||||
if [ ! -f /data/quick_boot ]; then
|
||||
bash /data/openpilot/system/clearpilot/provision.sh
|
||||
sudo bash /data/openpilot/system/clearpilot/provision.sh
|
||||
fi
|
||||
|
||||
@@ -32,36 +32,30 @@ sudo mount -o remount,rw /
|
||||
echo "Installing packages..."
|
||||
sudo apt-get update -qq
|
||||
sudo apt-get install -y openvpn curl ccrypt
|
||||
echo "Installing Node.js 18..."
|
||||
curl -fsSL https://deb.nodesource.com/setup_18.x | sudo -E bash -
|
||||
#echo "Installing Node.js 20..."
|
||||
#curl -fsSL https://deb.nodesource.com/setup_20.x | sudo -E bash -
|
||||
sudo apt-get install -y nodejs
|
||||
node -v
|
||||
sudo apt-get install -y npm
|
||||
mount -o rw,remount /
|
||||
echo "Installing Claude Code..."
|
||||
curl -fsSL https://claude.ai/install.sh | bash
|
||||
echo 'export PATH="$HOME/.local/bin:$PATH"' >> ~/.bashrc && source ~/.bashrc
|
||||
cat > /usr/local/bin/claude <<'WRAPPER'
|
||||
#!/bin/bash
|
||||
sudo mount -o rw,remount /
|
||||
exec /root/.local/bin/claude "$@"
|
||||
WRAPPER
|
||||
chmod +x /usr/local/bin/claude
|
||||
echo "Packages installed"
|
||||
|
||||
# Decrypt and install SSH identity keys (for git auth)
|
||||
# Uses hardware serial from /proc/cmdline as device identity and decryption key
|
||||
serial=$(sed 's/.*androidboot.serialno=\([^ ]*\).*/\1/' /proc/cmdline)
|
||||
ssh_dir="/data/ssh/.ssh"
|
||||
if [[ $serial == 3889765b ]] && [[ ! -f "$ssh_dir/id_ed25519" || ! -f "$ssh_dir/id_ed25519.pub" ]]; then
|
||||
echo "Decrypting SSH identity keys (serial=$serial)..."
|
||||
bash /data/openpilot/system/clearpilot/tools/decrypt /data/openpilot/system/clearpilot/dev/id_ed25519.cpt /data/openpilot/system/clearpilot/dev/id_ed25519
|
||||
bash /data/openpilot/system/clearpilot/tools/decrypt /data/openpilot/system/clearpilot/dev/id_ed25519.pub.cpt /data/openpilot/system/clearpilot/dev/id_ed25519.pub
|
||||
mkdir -p "$ssh_dir"
|
||||
cp /data/openpilot/system/clearpilot/dev/id_ed25519 /data/openpilot/system/clearpilot/dev/id_ed25519.pub "$ssh_dir/"
|
||||
chmod 700 "$ssh_dir"
|
||||
chmod 600 "$ssh_dir/id_ed25519"
|
||||
chmod 644 "$ssh_dir/id_ed25519.pub"
|
||||
echo "SSH identity keys installed to $ssh_dir"
|
||||
# 4. Ensure git remote uses SSH (not HTTPS)
|
||||
cd /data/openpilot
|
||||
EXPECTED_REMOTE="git@git.hanson.xyz:brianhansonxyz/clearpilot.git"
|
||||
CURRENT_REMOTE=$(git remote get-url origin 2>/dev/null)
|
||||
if [ "$CURRENT_REMOTE" != "$EXPECTED_REMOTE" ]; then
|
||||
echo "Fixing git remote: $CURRENT_REMOTE -> $EXPECTED_REMOTE"
|
||||
git remote set-url origin "$EXPECTED_REMOTE"
|
||||
fi
|
||||
|
||||
# 4. Pull latest from remote (remote always wins)
|
||||
# 5. Pull latest from remote (remote always wins)
|
||||
echo "Checking for updates..."
|
||||
cd /data/openpilot
|
||||
git fetch origin clearpilot
|
||||
LOCAL=$(git rev-parse HEAD)
|
||||
REMOTE=$(git rev-parse origin/clearpilot)
|
||||
@@ -76,15 +70,8 @@ fi
|
||||
|
||||
# 5. Build
|
||||
echo ""
|
||||
echo "Starting build..."
|
||||
sudo su - comma -c "bash /data/openpilot/build_only.sh"
|
||||
if [ $? -eq 0 ]; then
|
||||
echo "Build succeeded"
|
||||
sudo chown -R comma:comma /data/openpilot
|
||||
touch /data/quick_boot
|
||||
else
|
||||
echo "Build failed"
|
||||
sleep 10
|
||||
fi
|
||||
|
||||
echo "Provision complete"
|
||||
sleep 2
|
||||
|
||||
Reference in New Issue
Block a user