mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-10-26 08:13:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mazda: Add safety, UI toggles, and implement Blended ACC longitudinal control

Browse Source
This commit is contained in:
Test User
2026-06-23 17:18:44 -05:00
parent e9320b2d04
commit e2d44d8315
9 changed files with 168 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params_keys.h
-1
View File
@@ -178,7 +178,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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}},
opendbc_repo/opendbc/car/mazda/carcontroller.py
+40 -3
View File
@@ -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
opendbc_repo/opendbc/car/mazda/carstate.py
+5 -1
View File
@@ -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),
}
opendbc_repo/opendbc/safety/modes/mazda.h
-1
View File
@@ -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;
scripts/launch_onroad_desktop.sh
+68 -1
View File
@@ -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}"
selfdrive/controls/lib/longcontrol.py
+10
View File
@@ -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.
selfdrive/ui/layouts/settings/starpilot/vehicle.py
+32
View File
@@ -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):
selfdrive/ui/lib/starpilot_state.py
+3
View File
@@ -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")
system/version.py
+10 -2
View File
@@ -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__":