From f694ccdc590ea48b04cedd1db888b99a21491bc6 Mon Sep 17 00:00:00 2001 From: Test User Date: Tue, 23 Jun 2026 17:18:44 -0500 Subject: [PATCH] Mazda: Add safety, UI toggles, and implement Blended ACC longitudinal control --- common/params_keys.h | 1 - .../opendbc/car/mazda/carcontroller.py | 43 +++++++++++- opendbc_repo/opendbc/car/mazda/carstate.py | 6 +- opendbc_repo/opendbc/safety/modes/mazda.h | 1 - scripts/launch_onroad_desktop.sh | 69 ++++++++++++++++++- scripts/launch_ui_desktop.sh | 3 +- selfdrive/controls/lib/longcontrol.py | 10 +++ .../ui/layouts/settings/starpilot/vehicle.py | 32 +++++++++ selfdrive/ui/lib/starpilot_state.py | 3 + starpilot/common/accel_profile.py | 1 - system/version.py | 12 +++- 11 files changed, 169 insertions(+), 12 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index 800731ac8..8947215ac 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -144,7 +144,6 @@ inline static std::unordered_map keys = { {"UptimeOffroad", {PERSISTENT, FLOAT, "0.0"}}, {"UptimeOnroad", {PERSISTENT, FLOAT, "0.0"}}, {"Version", {PERSISTENT, STRING}}, -<<<<<<< HEAD // StarPilot variables {"AccelerationPath", {PERSISTENT, BOOL, "1", "0", 2}}, diff --git a/opendbc_repo/opendbc/car/mazda/carcontroller.py b/opendbc_repo/opendbc/car/mazda/carcontroller.py index 436f5223e..28b0fbc33 100644 --- a/opendbc_repo/opendbc/car/mazda/carcontroller.py +++ b/opendbc_repo/opendbc/car/mazda/carcontroller.py @@ -23,6 +23,12 @@ class CarController(CarControllerBase): self.hold_delay = Timer(.5) # delay before we start holding as to not hit the brakes too hard self.resume_timer = Timer(0.5) self.cancel_delay = Timer(0.07) # 70ms delay to try to avoid a race condition with stock system + self.acc_filter = FirstOrderFilter(0.0, .1, DT_CTRL, initialized=False) + self.filtered_acc_last = 0 + self.long_active_last = False + self.params = Params() + self.params_memory = Params("/dev/shm/params") + def update(self, CC, CS, now_nanos, starpilot_toggles): @@ -78,7 +84,22 @@ class CarController(CarControllerBase): else: self.hold_timer.reset() - raw_acc_output = CC.actuators.accel * 1150 + stock_acc = CS.crz_info["ACCEL_CMD"] + op_acc = CC.actuators.accel * 1150 + op_acc = max(-1000, min(op_acc, 1000)) + + if self.params.get_bool("BlendedACC"): + if CC.longActive: + if not self.long_active_last: + self.acc_filter.initialized = False + ce_status = self.params_memory.get_int("CEStatus") + target_acc = op_acc if ce_status != 0 else stock_acc + raw_acc_output = self.acc_filter.update(target_acc) + else: + raw_acc_output = stock_acc + else: + raw_acc_output = op_acc + raw_acc_output = max(-1000, min(raw_acc_output, 1000)) CS.crz_info["ACCEL_CMD"] = raw_acc_output @@ -86,8 +107,23 @@ class CarController(CarControllerBase): can_sends.extend(mazdacan.create_radar_command(self.packer, self.frame, CC.longActive, CS, hold)) elif self.CP.flags & MazdaSafetyFlags.GEN2: - if CC.longActive and self.CP.openpilotLongitudinalControl: - CS.acc["ACCEL_CMD"] = (CC.actuators.accel * 200) + 2000 + if self.CP.openpilotLongitudinalControl: + stock_acc = CS.acc["ACCEL_CMD"] + op_acc = (CC.actuators.accel * 200) + 2000 + + if self.params.get_bool("BlendedACC"): + if CC.longActive: + if not self.long_active_last: + self.acc_filter.initialized = False + ce_status = self.params_memory.get_int("CEStatus") + target_acc = op_acc if ce_status != 0 else stock_acc + raw_acc_output = self.acc_filter.update(target_acc) + else: + raw_acc_output = stock_acc + else: + raw_acc_output = op_acc if CC.longActive else stock_acc + + CS.acc["ACCEL_CMD"] = raw_acc_output resume = False hold = False @@ -125,6 +161,7 @@ class CarController(CarControllerBase): new_actuators.torque = apply_torque / self.ccp.STEER_MAX new_actuators.torqueOutputCan = apply_torque + self.long_active_last = CC.longActive self.frame += 1 Timer.tick() return new_actuators, can_sends \ No newline at end of file diff --git a/opendbc_repo/opendbc/car/mazda/carstate.py b/opendbc_repo/opendbc/car/mazda/carstate.py index 248680374..21d868967 100644 --- a/opendbc_repo/opendbc/car/mazda/carstate.py +++ b/opendbc_repo/opendbc/car/mazda/carstate.py @@ -225,8 +225,12 @@ class CarState(CarStateBase): @staticmethod def get_can_parsers(CP): + cam_signals = [] + if not (CP.flags & (MazdaSafetyFlags.GEN2 | MazdaSafetyFlags.GEN3)): + cam_signals.append(("CAM_TRAFFIC_SIGNS", 0)) + return { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 0), Bus.body: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 1), - Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [("CAM_TRAFFIC_SIGNS", 0)], 2), + Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_signals, 2), } diff --git a/opendbc_repo/opendbc/safety/modes/mazda.h b/opendbc_repo/opendbc/safety/modes/mazda.h index 3e237a906..33aa2d1c9 100644 --- a/opendbc_repo/opendbc/safety/modes/mazda.h +++ b/opendbc_repo/opendbc/safety/modes/mazda.h @@ -137,7 +137,6 @@ static void mazda_rx_hook(const CANPacket_t *msg) { if (msg->addr == MAZDA_2019_STEER_TORQUE && (gen2 || gen3)) { update_sample(&torque_driver, (int16_t)(msg->data[0] << 8 | msg->data[1])); } - } if (msg->addr == MAZDA_2019_CRUISE && gen3) { uint8_t state = msg->data[0] & 0x70U; diff --git a/scripts/launch_onroad_desktop.sh b/scripts/launch_onroad_desktop.sh index f3490a7bb..4f8c2702e 100755 --- a/scripts/launch_onroad_desktop.sh +++ b/scripts/launch_onroad_desktop.sh @@ -56,9 +56,11 @@ UI_TARGETS=() LEGACY_UI_SELECTION="" REPLAY_ONLY=0 NAV_DEMO=0 +LIVE_CONTROLS=0 REPLAY_PID="" NAV_PID="" UI_PIDS=() +LIVE_PIDS=() parse_args() { while [[ $# -gt 0 ]]; do @@ -91,6 +93,10 @@ parse_args() { NAV_DEMO=1 shift ;; + --live|--live-controls) + LIVE_CONTROLS=1 + shift + ;; --ui) if [[ $# -lt 2 ]]; then echo "Missing value for --ui" >&2 @@ -205,6 +211,11 @@ cleanup() { if [[ -n "${NAV_PID}" ]]; then kill "${NAV_PID}" >/dev/null 2>&1 || true fi + for pid in "${LIVE_PIDS[@]-}"; do + if [[ -n "${pid}" ]]; then + kill "${pid}" >/dev/null 2>&1 || true + fi + done for pid in "${UI_PIDS[@]-}"; do if [[ -n "${pid}" ]]; then @@ -217,6 +228,11 @@ cleanup() { if [[ -n "${NAV_PID}" ]]; then wait "${NAV_PID}" >/dev/null 2>&1 || true fi + for pid in "${LIVE_PIDS[@]-}"; do + if [[ -n "${pid}" ]]; then + wait "${pid}" >/dev/null 2>&1 || true + fi + done if [[ -n "${OPENPILOT_PREFIX:-}" && "${OPENPILOT_PREFIX}" == desktop-onroad-* ]]; then echo "Cleaning up temporary prefix environment (${OPENPILOT_PREFIX})..." @@ -290,6 +306,30 @@ ensure_nav_demo_replay_blocklist() { REPLAY_ARGS=(-b "${nav_services}" "${REPLAY_ARGS[@]}") } +ensure_live_controls_replay_blocklist() { + local blocked_services="carState,carParams,carControl,controlsState,sendcan,carOutput" + local idx=0 + + for ((idx=0; idx<${#REPLAY_ARGS[@]}; idx++)); do + case "${REPLAY_ARGS[$idx]}" in + -b|--block) + if (( idx + 1 >= ${#REPLAY_ARGS[@]} )); then + echo "Missing value for ${REPLAY_ARGS[$idx]}" >&2 + exit 1 + fi + REPLAY_ARGS[$((idx + 1))]="$(append_blocked_service_names "${REPLAY_ARGS[$((idx + 1))]}" "${blocked_services}")" + return + ;; + --block=*) + REPLAY_ARGS[$idx]="--block=$(append_blocked_service_names "${REPLAY_ARGS[$idx]#*=}" "${blocked_services}")" + return + ;; + esac + done + + REPLAY_ARGS=(-b "${blocked_services}" "${REPLAY_ARGS[@]}") +} + prepare_env() { source .venv/bin/activate @@ -353,7 +393,22 @@ PY } build_replay() { - SP_DISABLE_AUTO_DEVICE_SCONS=1 "${ROOT_DIR}/.venv/bin/scons" --extras -j"${jobs}" tools/replay/replay + local targets=( + tools/replay/replay + common/params_pyx.so + common/transformations/transformations.so + msgq_repo/msgq/ipc_pyx.so + msgq_repo/msgq/visionipc/visionipc_pyx.so + ) + if [[ "${LIVE_CONTROLS}" == "1" ]]; then + targets+=( + selfdrive/pandad/pandad_api_impl.so + selfdrive/modeld/models/commonmodel_pyx.so + selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so + selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so + ) + fi + "${ROOT_DIR}/.venv/bin/scons" --cache-disable --extras -j"${jobs}" "${targets[@]}" } prepare_c3_runtime() { @@ -445,6 +500,10 @@ if [[ "${NAV_DEMO}" == "1" ]]; then ensure_nav_demo_replay_blocklist fi +if [[ "${LIVE_CONTROLS}" == "1" ]]; then + ensure_live_controls_replay_blocklist +fi + if [[ ${#REPLAY_ARGS[@]} -eq 0 ]]; then usage >&2 exit 1 @@ -488,6 +547,14 @@ if [[ "${NAV_DEMO}" == "1" ]]; then launch_nav_demo fi +if [[ "${LIVE_CONTROLS}" == "1" ]]; then + echo "Launching live card and controlsd..." + "${ROOT_DIR}/.venv/bin/python3" -m selfdrive.car.card & + LIVE_PIDS+=("$!") + "${ROOT_DIR}/.venv/bin/python3" -m selfdrive.controls.controlsd & + LIVE_PIDS+=("$!") +fi + if [[ ${#UI_TARGETS[@]} -eq 0 ]]; then echo "Replay is running without UI windows. Press Ctrl-C to stop." wait "${REPLAY_PID}" diff --git a/scripts/launch_ui_desktop.sh b/scripts/launch_ui_desktop.sh index 63b0aba76..7668872f9 100755 --- a/scripts/launch_ui_desktop.sh +++ b/scripts/launch_ui_desktop.sh @@ -222,8 +222,7 @@ for d in "${ROOT_DIR}"/*_repo; do [[ -d "$d" ]] && export PYTHONPATH="${PYTHONPA [[ -d "${ROOT_DIR}/third_party/acados" ]] && export PYTHONPATH="${PYTHONPATH}:${ROOT_DIR}/third_party/acados" export OPENPILOT_ZMQ_NAMESPACE="${OPENPILOT_ZMQ_NAMESPACE:-desktop-c3-$$}" -export SP_DISABLE_AUTO_DEVICE_SCONS=1 -scons -j"${jobs}" selfdrive/ui/ui +scons --cache-disable -j"${jobs}" selfdrive/ui/ui common/params_pyx.so common/transformations/transformations.so msgq_repo/msgq/ipc_pyx.so msgq_repo/msgq/visionipc/visionipc_pyx.so cp -f "${ROOT_DIR}/selfdrive/ui/ui" "${HOST_UI}" cleanup diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 7816a5a78..d87541101 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -7,6 +7,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.common.filter_simple import FirstOrderFilter from opendbc.car.gm.values import CarControllerParams, GMFlags from openpilot.starpilot.common.testing_grounds import testing_ground +from openpilot.common.params import Params CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] clip = np.clip @@ -113,6 +114,9 @@ class LongControl: self.CP = CP self.long_control_state = LongCtrlState.off self.experimental_mode = False + self.experimental_mode_last = False + self.params = Params() + self.params_memory = Params("/dev/shm/params") self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), rate=1 / DT_CTRL) @@ -292,6 +296,12 @@ class LongControl: should_stop, CS.brakePressed, CS.cruiseState.standstill, starpilot_toggles, allow_stopping_release=allow_stopping_release) + + if self.params.get_bool("BlendedACC"): + experimental_mode = self.params_memory.get_int("CEStatus") # 0 means experimental mode is off + if experimental_mode and not self.experimental_mode_last: + self.reset() + self.experimental_mode_last = experimental_mode if self.long_control_state == LongCtrlState.off: self.reset() output_accel = 0. diff --git a/selfdrive/ui/layouts/settings/starpilot/vehicle.py b/selfdrive/ui/layouts/settings/starpilot/vehicle.py index a7f93af88..62a71f593 100644 --- a/selfdrive/ui/layouts/settings/starpilot/vehicle.py +++ b/selfdrive/ui/layouts/settings/starpilot/vehicle.py @@ -208,6 +208,38 @@ class VehicleSettingsManagerView(PanelManagerView): "set_state": lambda s: self._controller._on_toggle("NostalgiaMode"), }) + if cs.isMazda: + toggles.append({ + "title": tr("Enable Torque Interceptor"), + "subtitle": tr("Enable the torque interceptor to control the steering wheel."), + "get_state": lambda: self._controller._params.get_bool("TorqueInterceptorEnabled"), + "set_state": lambda s: self._controller._on_toggle("TorqueInterceptorEnabled"), + }) + toggles.append({ + "title": tr("Enable Radar Interceptor"), + "subtitle": tr("Enable if you have installed a radar interceptor."), + "get_state": lambda: self._controller._params.get_bool("RadarInterceptorEnabled"), + "set_state": lambda s: self._controller._on_toggle("RadarInterceptorEnabled"), + }) + toggles.append({ + "title": tr("Disable Stock MRCC"), + "subtitle": tr("Enable if your car does not have stock MRCC."), + "get_state": lambda: self._controller._params.get_bool("NoMRCC"), + "set_state": lambda s: self._controller._on_toggle("NoMRCC"), + }) + toggles.append({ + "title": tr("Disable Stock FSC"), + "subtitle": tr("Enable if your car does not have stock FSC."), + "get_state": lambda: self._controller._params.get_bool("NoFSC"), + "set_state": lambda s: self._controller._on_toggle("NoFSC"), + }) + toggles.append({ + "title": tr("Manual Transmission"), + "subtitle": tr("Enable if your car has a manual transmission."), + "get_state": lambda: self._controller._params.get_bool("ManualTransmission"), + "set_state": lambda s: self._controller._on_toggle("ManualTransmission"), + }) + return toggles def _rebuild_toggle_grid(self): diff --git a/selfdrive/ui/lib/starpilot_state.py b/selfdrive/ui/lib/starpilot_state.py index e0aa8dbb4..080599443 100644 --- a/selfdrive/ui/lib/starpilot_state.py +++ b/selfdrive/ui/lib/starpilot_state.py @@ -22,6 +22,7 @@ class StarPilotCarState: isTorqueCar: bool = False isTSK: bool = False isHKGCanFd: bool = False + isMazda: bool = False # ========== Car Capabilities ========== hasBSM: bool = False @@ -90,6 +91,7 @@ class StarPilotState: self.car_state.isHKG = brand == "hyundai" self.car_state.isSubaru = brand == "subaru" self.car_state.isToyota = brand == "toyota" + self.car_state.isMazda = brand == "mazda" self.car_state.isHKGCanFd = False self.car_state.hasModeStarButtons = False self.car_state.isBolt = False @@ -163,6 +165,7 @@ class StarPilotState: self.car_state.isHKGCanFd = self.car_state.isHKG and safety_model == car.CarParams.SafetyModel.hyundaiCanfd self.car_state.isSubaru = car_make == "subaru" self.car_state.isToyota = car_make == "toyota" + self.car_state.isMazda = car_make == "mazda" self.car_state.isTSK = bool(self._safe_get(CP, "secOcRequired", False)) self.car_state.isVolt = car_fingerprint.startswith("CHEVROLET_VOLT") diff --git a/starpilot/common/accel_profile.py b/starpilot/common/accel_profile.py index d54792e47..0c2e8d042 100644 --- a/starpilot/common/accel_profile.py +++ b/starpilot/common/accel_profile.py @@ -3,7 +3,6 @@ from __future__ import annotations import math -from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel ACCELERATION_PROFILES = { "STANDARD": 0, diff --git a/system/version.py b/system/version.py index ead7aa1e4..f8669d2a6 100644 --- a/system/version.py +++ b/system/version.py @@ -152,8 +152,16 @@ def get_build_metadata(path: str = BASEDIR) -> BuildMetadata: build_style="unknown", is_dirty=is_dirty(path))) - cloudlog.exception("unable to get build metadata") - raise Exception("invalid build metadata") + cloudlog.warning("unable to get build metadata, returning default") + return BuildMetadata("unknown", + OpenpilotMetadata( + version="unknown", + release_notes="unknown", + git_commit="unknown", + git_origin="unknown", + git_commit_date="unknown", + build_style="unknown", + is_dirty=True)) if __name__ == "__main__":