brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Actions 60 Packages Projects Releases Wiki Activity

port clearpilot gpsd; decouple self-driving from GPS

Browse Source 
Adds the AT-command-based GPS daemon for the Quectel EC25 modem (the
device has no u-blox chip and qcomgpsd's diag interface hangs on this
hardware). Trimmed from broken's version: dropped cloudlog calls and
the IsDaylight / ScreenDisplayMode auto-switching (those belong to the
display-modes feature, port later).

Used solely for system clock initialization, on-screen UI speed, and
per-segment dashcam GPS metadata.

Self-driving must NOT consume gpsLocation — feeding it to locationd's
kalman filter screws up the math. Patch locationd to skip GPS:
- locationd_thread() no longer subscribes to gpsLocation/gpsLocationExternal
- handle_msg's GPS branches commented (dead code without subscription)
- the "save LastGPSPosition once a minute when gpsOK" block commented
  (dead because gpsOK is now permanently false)

Result: liveLocationKalman.gpsOK = false for all self-driving consumers
(controlsd, paramsd, torqued, frogpilot_planner). They already handle
that case. Other liveLocationKalman fields still publish from the
camera-odometry + IMU + calibration kalman state.

system/clearpilot/__init__.py added so system.clearpilot.gpsd is a
valid Python module.
This commit is contained in:
Brian Hanson
2026-05-03 22:29:07 -05:00
parent 54c566c68f
commit 3ebfc29d35
4 changed files with 196 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/locationd.cc
+25 -24
View File
@@ -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++;
}
}
selfdrive/manager/process_config.py
+4
View File
@@ -82,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),