diff --git a/common/params_keys.h b/common/params_keys.h index 12c0fca995..5c3582be9a 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -178,7 +178,6 @@ inline static std::unordered_map keys = { {"UsbGpuLoading", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, {"UsbGpuPresent", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, {"Version", {PERSISTENT, STRING}}, -<<<<<<< HEAD // StarPilot variables {"AccelerationPath", {PERSISTENT, BOOL, "1", "0", 2, SETTINGS_SIMPLE}}, diff --git a/opendbc_repo/opendbc/car/mazda/carcontroller.py b/opendbc_repo/opendbc/car/mazda/carcontroller.py index 436f5223e9..28b0fbc334 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 248680374b..21d8689671 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 3e237a906c..33aa2d1c92 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 6cca1b28be..7218d98989 100755 --- a/scripts/launch_onroad_desktop.sh +++ b/scripts/launch_onroad_desktop.sh @@ -73,6 +73,7 @@ CEM_DEMO=0 ALERT_DEMO=0 CSC_DEMO=0 GALAXY=0 +LIVE_CONTROLS=0 REPLAY_PID="" NAV_PID="" CEM_PID="" @@ -84,6 +85,7 @@ GALAXY_PORT="" GALAXY_URL="" ONROAD_TEMP_PREFIX="" UI_PIDS=() +LIVE_PIDS=() parse_args() { while [[ $# -gt 0 ]]; do @@ -135,6 +137,10 @@ parse_args() { ALERT_DEMO=1 shift ;; + --live|--live-controls) + LIVE_CONTROLS=1 + shift + ;; --ui) if [[ $# -lt 2 ]]; then echo "Missing value for --ui" >&2 @@ -266,6 +272,11 @@ cleanup() { if [[ -n "${GPU_SYNC_PID}" ]]; then kill "${GPU_SYNC_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 @@ -293,6 +304,11 @@ cleanup() { if [[ -n "${GPU_SYNC_PID}" ]]; then wait "${GPU_SYNC_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 "${ONROAD_TEMP_PREFIX:-}" && "${ONROAD_TEMP_PREFIX}" == desktop-onroad-* ]]; then echo "Cleaning up temporary prefix environment (${ONROAD_TEMP_PREFIX})..." @@ -382,6 +398,30 @@ ensure_galaxy_replay_blocklist() { ensure_replay_blocklist "customReserved9" } +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 @@ -445,7 +485,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() { @@ -658,6 +713,10 @@ if [[ "${OFFROAD_DEMO}" == "1" && "${NAV_DEMO}" != "1" ]]; then exit 1 fi +if [[ "${LIVE_CONTROLS}" == "1" ]]; then + ensure_live_controls_replay_blocklist +fi + if [[ "${ALERT_DEMO}" == "1" ]]; then ensure_alert_demo_replay_blocklist fi @@ -740,6 +799,14 @@ if [[ "${CSC_DEMO}" == "1" ]]; then launch_csc_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/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index dbea793485..cec2f723e6 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -6,6 +6,7 @@ from openpilot.common.pid import PIDController from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.common.filter_simple import FirstOrderFilter from openpilot.selfdrive.controls.lib.longcontrol_vehicle_tunes import LongControlVehicleTuning +from openpilot.common.params import Params CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] clip = np.clip @@ -117,6 +118,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) @@ -252,6 +256,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 fc1268a99f..95fc1c29c1 100644 --- a/selfdrive/ui/layouts/settings/starpilot/vehicle.py +++ b/selfdrive/ui/layouts/settings/starpilot/vehicle.py @@ -421,6 +421,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 896da931ed..6aaeecc8dd 100644 --- a/selfdrive/ui/lib/starpilot_state.py +++ b/selfdrive/ui/lib/starpilot_state.py @@ -26,6 +26,7 @@ class StarPilotCarState: isTorqueCar: bool = False isTSK: bool = False isHKGCanFd: bool = False + isMazda: bool = False # ========== Car Capabilities ========== hasBSM: bool = False @@ -96,6 +97,7 @@ class StarPilotState: self.car_state.isJeep = brand == "chrysler" and fallback_model_str.startswith("JEEP_") 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 @@ -171,6 +173,7 @@ class StarPilotState: self.car_state.isJeep = car_make == "chrysler" and car_fingerprint.startswith("JEEP_") 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/system/version.py b/system/version.py index ead7aa1e40..f8669d2a62 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__":