Compare commits
5 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 brianhansonxyz
|
3ebfc29d35 | ||
|
brianhansonxyz
|
54c566c68f | ||
|
brianhansonxyz
|
f28ba340f2 | ||
|
brianhansonxyz
|
6ec4c7bdac | ||
|
brianhansonxyz
|
b57f2d8d70 |
@@ -0,0 +1,241 @@
|
||||
# ClearPilot — CLAUDE.md
|
||||
|
||||
## Project Overview
|
||||
|
||||
ClearPilot is a custom fork of **FrogPilot** (itself a fork of comma.ai's openpilot), purpose-built for Brian Hanson's **Hyundai Tucson** (HDA2 equipped). The vehicle's HDA2 system has specific quirks around how it synchronizes driving state with openpilot that require careful handling.
|
||||
|
||||
The fork was previously in a state where many features were layered on top but the driving model behavior had regressed in ways that couldn't be traced. On **2026-05-03** the working tree was reset back to a known-clean baseline so features can be re-introduced one at a time with proper testing.
|
||||
|
||||
## Current State (post-reset)
|
||||
|
||||
This tree currently contains:
|
||||
|
||||
- **The pristine pre-modification baseline** (commit `c2ab0fa`, "reset to pre-modification baseline").
|
||||
- **Startup-chain customizations** (commit `5624898`): launch scripts, on_start/provision flow, OpenVPN auto-connect, nice-monitor, build helpers, custom logo + spinner, encrypted dev SSH keys.
|
||||
- **Build/launch fixes** (commit `b287fd0`, tagged `working-baseline-2`): make baseline compile cleanly (QtWebEngine removed; `screenDisplayMode` reference fixed), and make `build_only.sh` exit on failure with a detached error window.
|
||||
|
||||
`build_only.sh` succeeds and `launch_openpilot.sh` boots the full manager process set.
|
||||
|
||||
## Where the Old Code Lives
|
||||
|
||||
| Location | What it is |
|
||||
|---|---|
|
||||
| `/data/openpilot/` | This repo. Working baseline + startup port. **Active.** |
|
||||
| `/data/openpilot-broken-2026-05-03/` | Full snapshot (with `.git`) of the prior modified-but-broken tree. Reference for porting features. |
|
||||
| `/data/clearpilot-baseline/` | The original baseline source we copied in. Kept for safety; do not modify. |
|
||||
| `/data/openpilot-features-broken/` | Pre-existing snapshot from an earlier reset attempt — **unverified**, leave alone. |
|
||||
|
||||
| Tag | Commit | What |
|
||||
|---|---|---|
|
||||
| `pre-reset-2026-05-03` | `f7e602c` | Last commit of the broken-but-feature-complete tree. |
|
||||
| `working-baseline-2` | `b287fd0` | Current head after the reset + startup port + compile fixes. |
|
||||
|
||||
Both tags are pushed to `origin/clearpilot`.
|
||||
|
||||
## Pending Feature Port Roadmap
|
||||
|
||||
Everything below is present in `/data/openpilot-broken-2026-05-03/` and **not** in this tree. Port them in small, testable batches — one feature area per commit, build + launch test in between.
|
||||
|
||||
**Driving behavior (HDA2 specifics):**
|
||||
- Lateral disabled (car's radar cruise handles steering; openpilot longitudinal only)
|
||||
- Brief disengage when turn signal is active during lane changes
|
||||
- Driver-monitoring timeout adjustments
|
||||
- Custom driving models in `selfdrive/clearpilot/models/`: `duck-amigo.thneed`, `farmville.onnx`, `wd-40.thneed`
|
||||
|
||||
**Onroad UI:**
|
||||
- Onroad layout changes (number positions, sidebar hidden during drive)
|
||||
- New ready/splash screen and home/offroad menu (the "ClearPilot menu" — sidebar settings panel replacing stock home; General / Network / Dashcam / Debug panels)
|
||||
- Status window with live system stats (temp, fan, storage, RAM, WiFi, VPN, GPS, telemetry, dashcam)
|
||||
- Crash handler in UI with stack-trace dump for SIGSEGV/SIGABRT
|
||||
- Display modes via the LFA steering-wheel button (`ScreenDisplayMode`: auto-normal, nightrider, manual normal, screen off, manual nightrider) — including auto day/night switching driven by GPS sunrise/sunset
|
||||
|
||||
**Speed / cruise logic:**
|
||||
- `selfdrive/clearpilot/speed_logic.py` — speed-limit display, cruise-vs-limit warning signs (different thresholds above/below 50 mph), ding sound on warning transitions
|
||||
- New params: `ClearpilotSpeedDisplay`, `ClearpilotSpeedLimitDisplay`, `ClearpilotCruiseWarning`, `ClearpilotPlayDing`, etc.
|
||||
|
||||
**Dashcam:**
|
||||
- `selfdrive/clearpilot/dashcamd` (native C++) — VisionIPC frames → OMX H.264 hardware encoder, 3-min MP4 segments + SRT GPS subtitles in `/data/media/0/videos/`
|
||||
- Trip lifecycle (waits for time + GPS + drive gear; closes on park/ignition off)
|
||||
- `system/loggerd/deleter.py` trip-aware storage rotation
|
||||
- Disables `encoderd` / `stream_encoderd`; reuses upstream `omx_encoder.cc`
|
||||
|
||||
**GPS:**
|
||||
- `system/clearpilot/gpsd.py` — replacement for broken `qcomgpsd` diag interface; polls Quectel modem via `mmcli` AT commands at 1Hz, publishes `gpsLocation`
|
||||
- NOAA solar-position calc for sunrise/sunset (drives display auto day/night)
|
||||
|
||||
**Telemetry:**
|
||||
- `selfdrive/clearpilot/telemetry.py` (client) + `telemetryd.py` (collector) — diff-based CSV logger over ZMQ
|
||||
- Toggleable via `TelemetryEnabled` memory param from Debug panel
|
||||
- Auto-disabled if `/data` free < 5 GB; auto-disabled on every manager start
|
||||
- Hyundai CAN-FD data logged from `selfdrive/car/hyundai/carstate.py update_canfd()`
|
||||
|
||||
**Bench mode (UI testing without a car):**
|
||||
- `--bench` flag → `BENCH_MODE=1` → enables `bench_onroad.py`, blocks real car processes
|
||||
- `bench_cmd.py` for setting fake vehicle state via params
|
||||
- UI introspection RPC at `ipc:///tmp/clearpilot_ui_rpc` (widget-tree dump)
|
||||
|
||||
**Power/thermal:**
|
||||
- Standstill power saving: `modeld` and `dmonitoringmodeld` throttled to 1fps when stopped
|
||||
- Fan controller uses offroad clamps at standstill
|
||||
- Park CPU savings + virtual battery shutdown fix
|
||||
|
||||
**Memory params (`/dev/shm/params`):**
|
||||
Lots of new keys for runtime UI state — `TelemetryEnabled`, `VpnEnabled`, `ModelStandby`, `ScreenDisplayMode`, `DashcamState`, `DashcamFrames`, `DashcamShutdown`, `LogDirInitialized`, plus the speed/cruise display set.
|
||||
|
||||
**Session logging:**
|
||||
- `/data/log2/current/` per-process stderr capture; aggregate `session.log` of major events
|
||||
- Time-resolved log dir rename via GPS/NTP; 30-day rotation
|
||||
- See `selfdrive/manager/process.py` and `manager.py` changes
|
||||
|
||||
**Already in this tree (just listing for reference, do NOT re-port):**
|
||||
- `system/clearpilot/vpn-monitor.sh` + `vpn.ovpn` — OpenVPN auto-connect to `vpn.hanson.xyz`
|
||||
- `system/clearpilot/nice-monitor.sh`
|
||||
- `system/clearpilot/provision.sh` (apt installs, Claude Code installer, git remote fix, fast-forward)
|
||||
- `system/clearpilot/on_start.sh` (SSH keys, ssh.service, git.hanson.xyz Host config, WiFi radio on)
|
||||
- `system/clearpilot/dev/id_ed25519.{cpt,pub.cpt}` (DongleId-keyed)
|
||||
- `system/clearpilot/startup_logo/bg.jpg` + scripts
|
||||
- `selfdrive/ui/qt/spinner` + `spinner.cc/.h` (custom logo)
|
||||
- `build_only.sh`, `build_preflight.sh`
|
||||
|
||||
## Working Rules
|
||||
|
||||
### CRITICAL: Change Control
|
||||
|
||||
This is self-driving software. All changes must be deliberate and well-understood.
|
||||
|
||||
- **NEVER make changes outside of what is explicitly requested**
|
||||
- **Always explain proposed changes first** — describe the change, the logic, and the architecture; let Brian review and approve before writing any code
|
||||
- **Brian is an expert on this software** — do not override his judgment or assume responsibility for changes he doesn't understand
|
||||
- **Every line must be understood** — work slowly and deliberately
|
||||
- **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` (once that feature is re-ported), 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:
|
||||
|
||||
```bash
|
||||
chown -R comma:comma /data/openpilot
|
||||
```
|
||||
|
||||
### Git
|
||||
|
||||
- Remote: `git@git.hanson.xyz:brianhansonxyz/clearpilot.git`
|
||||
- Branch: `clearpilot`
|
||||
- Large model files are tracked in git (intentional — this is a backup)
|
||||
- The `clearpilot` branch was force-pushed on 2026-05-03 as part of the reset; the prior history is reachable via the `pre-reset-2026-05-03` tag.
|
||||
|
||||
### Samba Share
|
||||
|
||||
- Share name: `openpilot` (e.g. `\\comma-3889765b\openpilot`)
|
||||
- Path: `/data/openpilot`
|
||||
- Username: `comma`
|
||||
- Password: `i-like-to-drive-cars`
|
||||
- Runs as `comma:comma` via force user/group — files created over SMB are owned correctly
|
||||
- Enabled at boot (`smbd` + `nmbd`)
|
||||
|
||||
### Testing Changes
|
||||
|
||||
Use `build_only.sh` to compile, then start the manager separately. Never compile individual targets with scons directly — always use the full build script. Always start the manager after a successful build — don't wait for the user to ask.
|
||||
|
||||
```bash
|
||||
# 1. Fix ownership
|
||||
chown -R comma:comma /data/openpilot
|
||||
|
||||
# 2. Build (kills running manager, removes prebuilt, compiles, exits)
|
||||
# build_only.sh tees output to /tmp/build.log and propagates the build's
|
||||
# exit code via PIPESTATUS. On failure: error text window stays on screen
|
||||
# fully detached; the script exits non-zero and stderr has the compile error.
|
||||
su - comma -c "bash /data/openpilot/build_only.sh"
|
||||
|
||||
# 3. If build succeeded ($? == 0), start openpilot
|
||||
su - comma -c "bash /data/openpilot/launch_openpilot.sh"
|
||||
|
||||
# 4. Inspect logs
|
||||
ls /data/log2/current/
|
||||
cat /data/log2/current/session.log
|
||||
```
|
||||
|
||||
### Adding New Params
|
||||
|
||||
The params system uses a C++ whitelist. Adding a new param name without registering it will crash with `UnknownKeyName`. To add one:
|
||||
|
||||
1. Register the key in `common/params.cc` (alphabetically, with `PERSISTENT` or `CLEAR_ON_*` flag)
|
||||
2. Set the default in `selfdrive/manager/manager.py` `manager_init()`
|
||||
3. Remove `prebuilt`, `common/params.o`, and `common/libcommon.a` to force rebuild
|
||||
|
||||
### Memory Params (paramsMemory)
|
||||
|
||||
Once re-ported, ClearPilot will use memory params (`/dev/shm/params/d/`) for UI toggles that should reset on boot. Conventions:
|
||||
|
||||
- **Registration**: register in `common/params.cc` as `PERSISTENT` (the registration flag does NOT control which path the param lives at)
|
||||
- **C++ access**: `Params{"/dev/shm/params"}` — the Params class appends `/d/` internally, so `Params("/dev/shm/params/d")` would resolve to `/dev/shm/params/d/d/`
|
||||
- **Python access**: `Params("/dev/shm/params")`
|
||||
- **UI toggles**: use `ToggleControl` with manual `toggleFlipped` lambda, not `ParamControl` (which only handles persistent params)
|
||||
- **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.
|
||||
|
||||
### 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 trips `commIssue` just as surely as too slow. If you change how often a process publishes, update the rate in `cereal/services.py` to match.
|
||||
|
||||
## Device: comma 3x
|
||||
|
||||
- Qualcomm Snapdragon SoC (aarch64), serial `comma-3889765b`
|
||||
- Storage: WDC SDINDDH4-128G, 128 GB UFS 2.1
|
||||
- Ubuntu 20.04.6 LTS on AGNOS 9.7
|
||||
- Kernel 4.9.103+ (custom comma.ai PREEMPT build, vendor-patched Qualcomm)
|
||||
- Python 3.11.4 via pyenv at `/usr/local/pyenv/versions/3.11.4/` (system python 3.8 — do not use)
|
||||
- Display: Weston (Wayland) on tty1
|
||||
- Hardware encoding: OMX (`OMX.qcom.video.encoder.avc` / `.hevc`); V4L2 VIDC exists but is not usable from ffmpeg subprocess
|
||||
|
||||
### Filesystem mount quirks
|
||||
|
||||
| Mount | Device | Type | Notes |
|
||||
|---|---|---|---|
|
||||
| `/` | /dev/sda7 | ext4 | rw |
|
||||
| `/data` | /dev/sda12 | ext4 | **persistent** — openpilot lives here |
|
||||
| `/home` | overlay | overlayfs | **volatile** (upper on tmpfs) — changes lost on reboot |
|
||||
| `/tmp` | tmpfs | tmpfs | volatile |
|
||||
| `/persist` | /dev/sda2 | ext4 | persistent config/certs, noexec |
|
||||
| `/dsp` | /dev/sde26 | ext4 | **read-only** Qualcomm DSP firmware |
|
||||
| `/firmware` | /dev/sde4 | vfat | **read-only** firmware blobs |
|
||||
|
||||
### GPS
|
||||
|
||||
The device has **no u-blox chip** (`/dev/ttyHS0` does not exist). GPS is the **Quectel EC25 LTE modem**'s built-in GPS, accessed via AT commands through `mmcli`. The original `qcomgpsd` is broken on this device because the diag interface hangs after setup. Once re-ported, `system/clearpilot/gpsd.py` replaces it.
|
||||
|
||||
## Boot Sequence
|
||||
|
||||
```
|
||||
Power On
|
||||
→ systemd: comma.service (runs as comma user)
|
||||
→ /usr/comma/comma.sh (waits for Weston, handles factory reset)
|
||||
→ /data/continue.sh
|
||||
→ /data/openpilot/launch_openpilot.sh
|
||||
→ kill stale instances (launch_openpilot, launch_chffrplus, manager.py, ./ui, selfdrive/ui/text)
|
||||
→ bash system/clearpilot/on_start.sh (SSH, WiFi, run provision.sh)
|
||||
→ background system/clearpilot/vpn-monitor.sh
|
||||
→ background system/clearpilot/nice-monitor.sh
|
||||
→ exec ./launch_chffrplus.sh
|
||||
→ source launch_env.sh
|
||||
→ run agnos_init
|
||||
→ set PYTHONPATH
|
||||
→ if no `prebuilt`: run build.py (spinner + scons)
|
||||
→ exec selfdrive/manager/manager.py
|
||||
→ manager_init() sets default params
|
||||
→ ensure_running() loop starts managed processes
|
||||
```
|
||||
+4
-1
@@ -10,8 +10,11 @@ BASEDIR="/data/openpilot"
|
||||
# Fix ownership — we edit as root, openpilot builds/runs as comma
|
||||
sudo chown -R comma:comma "$BASEDIR"
|
||||
|
||||
# Kill stale error displays and any running manager/launch/managed processes
|
||||
# Kill stale error displays and any running manager/launch/managed processes.
|
||||
# `text` is a shell wrapper that execs `./_text` — after exec the process is named
|
||||
# `_text` (no path), so we kill by exact comm in addition to the path pattern.
|
||||
pkill -9 -f "selfdrive/ui/text" 2>/dev/null
|
||||
pkill -9 -x _text 2>/dev/null
|
||||
pkill -9 -f 'launch_openpilot.sh' 2>/dev/null
|
||||
pkill -9 -f 'launch_chffrplus.sh' 2>/dev/null
|
||||
pkill -9 -f 'python.*manager.py' 2>/dev/null
|
||||
|
||||
+4
-1
@@ -79,8 +79,11 @@ function launch {
|
||||
agnos_init
|
||||
fi
|
||||
|
||||
# CLEARPILOT: kill stale error display from previous build/run
|
||||
# CLEARPILOT: kill stale error display from previous build/run.
|
||||
# `text` is a wrapper that execs ./_text — running process is named _text
|
||||
# with no path, so kill by exact comm too.
|
||||
pkill -f "selfdrive/ui/text" 2>/dev/null
|
||||
pkill -x _text 2>/dev/null
|
||||
|
||||
# write tmux scrollback to a file
|
||||
tmux capture-pane -pq -S-1000 > /tmp/launch_log
|
||||
|
||||
@@ -13,6 +13,9 @@ pkill -9 -f 'selfdrive\.' 2>/dev/null
|
||||
pkill -9 -f 'system\.' 2>/dev/null
|
||||
pkill -9 -f './ui' 2>/dev/null
|
||||
pkill -9 -f 'selfdrive/ui/text' 2>/dev/null
|
||||
# `text` is a shell wrapper that execs `./_text` — after exec the running
|
||||
# process's argv has no path, so kill by exact comm too.
|
||||
pkill -9 -x _text 2>/dev/null
|
||||
sleep 1
|
||||
|
||||
# CLEARPILOT: ensure params persistence dir is owned by comma:comma. Editing
|
||||
|
||||
@@ -77,7 +77,7 @@ class Controls:
|
||||
self.params_storage = Params("/persist/params")
|
||||
|
||||
self.params_memory.put_bool("CPTLkasButtonAction", False)
|
||||
self.params_memory.put_bool("ScreenDisplayMode", 0)
|
||||
self.params_memory.put_int("ScreenDisplayMode", 0)
|
||||
|
||||
self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS
|
||||
|
||||
|
||||
@@ -594,10 +594,15 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
|
||||
this->handle_sensor(t, log.getAccelerometer());
|
||||
} else if (log.isGyroscope()) {
|
||||
this->handle_sensor(t, log.getGyroscope());
|
||||
} else if (log.isGpsLocation()) {
|
||||
this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
|
||||
} else if (log.isGpsLocationExternal()) {
|
||||
this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
|
||||
// CLEARPILOT: GPS branches removed — locationd no longer subscribes to
|
||||
// gpsLocation/gpsLocationExternal, so these would be dead code regardless.
|
||||
// Self-driving treats GPS as not present: gpsOK stays false (last_gps_msg
|
||||
// never updates), and other liveLocationKalman fields stay at the kalman
|
||||
// filter's IMU+camera-odometry-only state.
|
||||
// } else if (log.isGpsLocation()) {
|
||||
// this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
|
||||
// } else if (log.isGpsLocationExternal()) {
|
||||
// this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
|
||||
//} else if (log.isGnssMeasurements()) {
|
||||
// this->handle_gnss(t, log.getGnssMeasurements());
|
||||
} else if (log.isCarState()) {
|
||||
@@ -676,22 +681,16 @@ void Localizer::configure_gnss_source(const LocalizerGnssSource &source) {
|
||||
}
|
||||
|
||||
int Localizer::locationd_thread() {
|
||||
Params params;
|
||||
LocalizerGnssSource source;
|
||||
const char* gps_location_socket;
|
||||
if (params.getBool("UbloxAvailable")) {
|
||||
source = LocalizerGnssSource::UBLOX;
|
||||
gps_location_socket = "gpsLocationExternal";
|
||||
} else {
|
||||
source = LocalizerGnssSource::QCOM;
|
||||
gps_location_socket = "gpsLocation";
|
||||
}
|
||||
|
||||
this->configure_gnss_source(source);
|
||||
const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration",
|
||||
// CLEARPILOT: do not subscribe to GPS. Our gpsd publishes gpsLocation for
|
||||
// UI/clock/dashcam, but feeding it to the kalman filter screws up the
|
||||
// self-driving math. liveLocationKalman.gpsOK stays false; downstream
|
||||
// self-driving consumers (controlsd, paramsd, torqued, frogpilot_planner)
|
||||
// already handle that case.
|
||||
this->configure_gnss_source(LocalizerGnssSource::QCOM);
|
||||
const std::initializer_list<const char *> service_list = {"cameraOdometry", "liveCalibration",
|
||||
"carState", "accelerometer", "gyroscope"};
|
||||
|
||||
SubMaster sm(service_list, {}, nullptr, {gps_location_socket});
|
||||
SubMaster sm(service_list, {}, nullptr, {});
|
||||
PubMaster pm({"liveLocationKalman"});
|
||||
|
||||
uint64_t cnt = 0;
|
||||
@@ -730,12 +729,14 @@ int Localizer::locationd_thread() {
|
||||
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
|
||||
pm.send("liveLocationKalman", bytes.begin(), bytes.size());
|
||||
|
||||
if (cnt % 1200 == 0 && gpsOK) { // once a minute
|
||||
VectorXd posGeo = this->get_position_geodetic();
|
||||
std::string lastGPSPosJSON = util::string_format(
|
||||
"{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
|
||||
params.putNonBlocking("LastGPSPosition", lastGPSPosJSON);
|
||||
}
|
||||
// CLEARPILOT: dead code now that gpsOK is permanently false (we don't
|
||||
// subscribe to gpsLocation). Was: write LastGPSPosition once a minute.
|
||||
// if (cnt % 1200 == 0 && gpsOK) {
|
||||
// VectorXd posGeo = this->get_position_geodetic();
|
||||
// std::string lastGPSPosJSON = util::string_format(
|
||||
// "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
|
||||
// params.putNonBlocking("LastGPSPosition", lastGPSPosJSON);
|
||||
// }
|
||||
cnt++;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -55,7 +55,8 @@ def manager_init(frogpilot_functions) -> None:
|
||||
frogpilot_boot = threading.Thread(target=frogpilot_boot_functions, args=(frogpilot_functions,))
|
||||
frogpilot_boot.start()
|
||||
|
||||
save_bootlog()
|
||||
# CLEARPILOT: skip writing boot logs to /data/media/0/realdata/boot/
|
||||
# save_bootlog()
|
||||
|
||||
params = Params()
|
||||
params_storage = Params("/persist/params")
|
||||
|
||||
@@ -62,9 +62,12 @@ procs = [
|
||||
PythonProcess("timed", "system.timed", always_run, enabled=not PC),
|
||||
|
||||
PythonProcess("dmonitoringmodeld", "selfdrive.modeld.dmonitoringmodeld", driverview, enabled=(not PC or WEBCAM)),
|
||||
NativeProcess("encoderd", "system/loggerd", ["./encoderd"], allow_logging),
|
||||
NativeProcess("stream_encoderd", "system/loggerd", ["./encoderd", "--stream"], notcar),
|
||||
NativeProcess("loggerd", "system/loggerd", ["./loggerd"], allow_logging),
|
||||
# CLEARPILOT: disabled segment + camera logging — no rlog/qlog or .hevc
|
||||
# files written to /data/media/0/realdata. We don't use comma's upload/
|
||||
# replay pipeline. Keep deleter running for any leftover cleanup.
|
||||
# NativeProcess("encoderd", "system/loggerd", ["./encoderd"], allow_logging),
|
||||
# NativeProcess("stream_encoderd", "system/loggerd", ["./encoderd", "--stream"], notcar),
|
||||
# NativeProcess("loggerd", "system/loggerd", ["./loggerd"], allow_logging),
|
||||
NativeProcess("modeld", "selfdrive/modeld", ["./modeld"], only_onroad),
|
||||
#NativeProcess("mapsd", "selfdrive/navd", ["./mapsd"], only_onroad),
|
||||
#PythonProcess("navmodeld", "selfdrive.modeld.navmodeld", only_onroad),
|
||||
@@ -79,6 +82,10 @@ procs = [
|
||||
PythonProcess("deleter", "system.loggerd.deleter", always_run),
|
||||
PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(not PC or WEBCAM)),
|
||||
# PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", qcomgps, enabled=TICI), # Fixme
|
||||
# CLEARPILOT: replacement for qcomgpsd (whose diag interface is broken on this device).
|
||||
# Uses Quectel modem AT commands via mmcli. Self-driving does NOT consume this; locationd
|
||||
# is patched to skip gpsLocation. Used only for system clock + UI speed + dashcam metadata.
|
||||
PythonProcess("gpsd", "system.clearpilot.gpsd", qcomgps, enabled=TICI),
|
||||
# PythonProcess("ugpsd", "system.ugpsd", only_onroad, enabled=TICI),
|
||||
#PythonProcess("navd", "selfdrive.navd.navd", only_onroad),
|
||||
PythonProcess("pandad", "selfdrive.boardd.pandad", always_run),
|
||||
|
||||
@@ -0,0 +1,167 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
ClearPilot GPS daemon — reads GPS from Quectel EC25 modem via AT commands
|
||||
and publishes gpsLocation messages.
|
||||
|
||||
Replaces qcomgpsd (which uses the diag interface — broken on this device).
|
||||
|
||||
Used solely for: setting system clock on first fix, an on-screen UI
|
||||
speed indicator, and per-segment GPS metadata for the dashcam. Self-
|
||||
driving code does NOT consume this output — locationd is patched to not
|
||||
subscribe to gpsLocation, so liveLocationKalman.gpsOK stays false.
|
||||
"""
|
||||
import os
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
import datetime
|
||||
|
||||
from cereal import log
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.gpio import gpio_init, gpio_set
|
||||
from openpilot.common.time import system_time_valid
|
||||
from openpilot.system.hardware.tici.pins import GPIO
|
||||
|
||||
|
||||
def at_cmd(cmd: str) -> str:
|
||||
try:
|
||||
result = subprocess.check_output(
|
||||
f"mmcli -m any --timeout 10 --command='{cmd}'",
|
||||
shell=True, encoding='utf8', stderr=subprocess.DEVNULL
|
||||
).strip()
|
||||
# mmcli wraps AT responses: response: '+QGPSLOC: ...' — strip the wrapper
|
||||
if result.startswith("response: '") and result.endswith("'"):
|
||||
result = result[len("response: '"):-1]
|
||||
return result
|
||||
except subprocess.CalledProcessError:
|
||||
return ""
|
||||
|
||||
|
||||
def wait_for_modem():
|
||||
print("CLP gpsd: waiting for modem", file=sys.stderr, flush=True)
|
||||
while True:
|
||||
ret = subprocess.call(
|
||||
"mmcli -m any --timeout 10 --command='AT+QGPS?'",
|
||||
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True
|
||||
)
|
||||
if ret == 0:
|
||||
return
|
||||
time.sleep(0.5)
|
||||
|
||||
|
||||
def parse_qgpsloc(response: str):
|
||||
"""Parse AT+QGPSLOC=2 response into a dict.
|
||||
Format: +QGPSLOC: UTC,lat,lon,hdop,alt,fix,cog,spkm,spkn,date,nsat
|
||||
"""
|
||||
if "+QGPSLOC:" not in response:
|
||||
return None
|
||||
try:
|
||||
data = response.split("+QGPSLOC:")[1].strip()
|
||||
fields = data.split(",")
|
||||
if len(fields) < 11:
|
||||
return None
|
||||
|
||||
utc = fields[0] # HHMMSS.S
|
||||
lat = float(fields[1])
|
||||
lon = float(fields[2])
|
||||
hdop = float(fields[3])
|
||||
alt = float(fields[4])
|
||||
fix = int(fields[5]) # 2=2D, 3=3D
|
||||
cog = float(fields[6]) # course over ground
|
||||
spkm = float(fields[7]) # speed km/h
|
||||
date = fields[9] # DDMMYY
|
||||
nsat = int(fields[10])
|
||||
|
||||
# Build unix timestamp from UTC + date
|
||||
hh, mm = int(utc[0:2]), int(utc[2:4])
|
||||
ss = float(utc[4:])
|
||||
dd, mo, yy = int(date[0:2]), int(date[2:4]), 2000 + int(date[4:6])
|
||||
dt = datetime.datetime(yy, mo, dd, hh, mm, int(ss),
|
||||
int((ss % 1) * 1e6), datetime.timezone.utc)
|
||||
|
||||
return {
|
||||
"latitude": lat,
|
||||
"longitude": lon,
|
||||
"altitude": alt,
|
||||
"speed": spkm / 3.6, # km/h -> m/s
|
||||
"bearing": cog,
|
||||
"accuracy": hdop * 5, # rough HDOP -> meters
|
||||
"timestamp_ms": dt.timestamp() * 1e3,
|
||||
"satellites": nsat,
|
||||
"fix": fix,
|
||||
}
|
||||
except (ValueError, IndexError) as e:
|
||||
print(f"CLP gpsd: parse error: {e}", file=sys.stderr, flush=True)
|
||||
return None
|
||||
|
||||
|
||||
def main():
|
||||
print("CLP gpsd: starting", file=sys.stderr, flush=True)
|
||||
|
||||
# Kill system gpsd which may respawn and interfere with modem access
|
||||
subprocess.run("pkill -f /usr/sbin/gpsd", shell=True,
|
||||
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
|
||||
|
||||
wait_for_modem()
|
||||
print("CLP gpsd: modem ready", file=sys.stderr, flush=True)
|
||||
|
||||
# Enable GPS antenna power
|
||||
gpio_init(GPIO.GNSS_PWR_EN, True)
|
||||
gpio_set(GPIO.GNSS_PWR_EN, True)
|
||||
print("CLP gpsd: GPIO power enabled", file=sys.stderr, flush=True)
|
||||
|
||||
# Don't restart GPS if already running (preserve existing fix)
|
||||
resp = at_cmd("AT+QGPS?")
|
||||
print(f"CLP gpsd: QGPS status: {resp}", file=sys.stderr, flush=True)
|
||||
if "QGPS: 1" not in resp:
|
||||
at_cmd('AT+QGPSCFG="dpoenable",0')
|
||||
at_cmd('AT+QGPSCFG="outport","none"')
|
||||
at_cmd("AT+QGPS=1")
|
||||
print("CLP gpsd: GPS started fresh", file=sys.stderr, flush=True)
|
||||
else:
|
||||
print("CLP gpsd: GPS already running, keeping fix", file=sys.stderr, flush=True)
|
||||
|
||||
pm = messaging.PubMaster(["gpsLocation"])
|
||||
clock_set = system_time_valid()
|
||||
print("CLP gpsd: entering main loop", file=sys.stderr, flush=True)
|
||||
|
||||
while True:
|
||||
resp = at_cmd("AT+QGPSLOC=2")
|
||||
fix = parse_qgpsloc(resp)
|
||||
|
||||
if fix:
|
||||
# Set system clock from GPS on first valid fix if clock is invalid
|
||||
if not clock_set:
|
||||
gps_dt = datetime.datetime.utcfromtimestamp(fix["timestamp_ms"] / 1000)
|
||||
ret = subprocess.run(["date", "-s", gps_dt.strftime("%Y-%m-%d %H:%M:%S")],
|
||||
env={**os.environ, "TZ": "UTC"},
|
||||
capture_output=True)
|
||||
if ret.returncode == 0:
|
||||
clock_set = True
|
||||
print(f"CLP gpsd: system clock set from GPS: {gps_dt}", file=sys.stderr, flush=True)
|
||||
else:
|
||||
print(f"CLP gpsd: failed to set clock: {ret.stderr.decode().strip()}", file=sys.stderr, flush=True)
|
||||
|
||||
msg = messaging.new_message("gpsLocation", valid=True)
|
||||
gps = msg.gpsLocation
|
||||
gps.latitude = fix["latitude"]
|
||||
gps.longitude = fix["longitude"]
|
||||
gps.altitude = fix["altitude"]
|
||||
gps.speed = fix["speed"]
|
||||
gps.bearingDeg = fix["bearing"]
|
||||
gps.horizontalAccuracy = fix["accuracy"]
|
||||
gps.unixTimestampMillis = int(fix["timestamp_ms"])
|
||||
gps.source = log.GpsLocationData.SensorSource.qcomdiag
|
||||
gps.hasFix = fix["fix"] >= 2
|
||||
gps.flags = 1
|
||||
gps.vNED = [0.0, 0.0, 0.0]
|
||||
gps.verticalAccuracy = fix["accuracy"]
|
||||
gps.bearingAccuracyDeg = 10.0
|
||||
gps.speedAccuracy = 1.0
|
||||
pm.send("gpsLocation", msg)
|
||||
|
||||
time.sleep(0.5) # 2 Hz polling
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user