mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-08-01 22:19:49 +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 5910401d7b
commit f694ccdc59
11 changed files with 169 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params_keys.h
-1
View File
@@ -144,7 +144,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"UptimeOffroad", {PERSISTENT, FLOAT, "0.0"}},
{"UptimeOnroad", {PERSISTENT, FLOAT, "0.0"}},
{"Version", {PERSISTENT, STRING}},
<<<<<<< HEAD
// StarPilot variables
{"AccelerationPath", {PERSISTENT, BOOL, "1", "0", 2}},
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
@@ -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}"
scripts/launch_ui_desktop.sh
+1 -2
View File
@@ -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
selfdrive/controls/lib/longcontrol.py
+10
View File
@@ -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.
selfdrive/ui/layouts/settings/starpilot/vehicle.py
+32
View File
@@ -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):
selfdrive/ui/lib/starpilot_state.py
+3
View File
@@ -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")
starpilot/common/accel_profile.py
-1
View File
@@ -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,
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__":