only send adrv messages when not interceptor for now

- Removed redundant safety parameter check for improved code clarity.
- Ensures consistent handling of ADAS_ECU_INTERCEPTOR flag.

.codespellignore

@@ -2,3 +2,4 @@ Wen
 REGIST
 PullRequest
 cancelled
+indeces

.dockerignore

@@ -18,19 +18,6 @@
 
 venv/
 .venv/
-**/.idea
-**/.hypothesis
-**/.mypy_cache
-
-**/.venv
-**/.venv/
-
-**/.ci_cache
-**/*.rlog
-
-**/Dockerfile*
-**/dockerfile*
-**/build_output
 
 notebooks
 phone

Dockerfile.sunnypilot

@@ -1,66 +0,0 @@
-FROM sunnypilot-base
-
-ARG RUNNER_DEBUG=0
-
-ENV PYTHONUNBUFFERED=1
-ENV OPENPILOT_SRC_PATH=/tmp/openpilot
-ENV BUILD_DIR=/data/openpilot
-ENV OUTPUT_DIR=/output
-
-RUN sudo apt update && sudo apt install -y rsync
-
-RUN mkdir -p ${OPENPILOT_SRC_PATH}
-RUN mkdir -p ${BUILD_DIR}
-COPY . ${OPENPILOT_SRC_PATH}
-ENV PYTHONPATH=${BUILD_DIR}
-
-WORKDIR ${OPENPILOT_SRC_PATH}
-RUN ./tools/ubuntu_setup.sh
-
-RUN ./release/release_files.py | sort | uniq | rsync -rRl${RUNNER_DEBUG:+v} --files-from=- . $BUILD_DIR/
-WORKDIR ${BUILD_DIR}
-RUN sed -i '/from .board.jungle import PandaJungle, PandaJungleDFU/s/^/#/' panda/__init__.py
-RUN scons --cache-readonly -j$(nproc) --minimal
-RUN touch ${BUILD_DIR}/prebuilt
-RUN sudo rm -rf ${OUTPUT_DIR}
-RUN mkdir -p ${OUTPUT_DIR}
-
-ENTRYPOINT [\
-  "rsync", \
-  "-am", \
-  "--include=**/panda/board/", \
-  "--include=**/panda/board/obj", \
-  "--include=**/panda/board/obj/panda.bin.signed", \
-  "--include=**/panda/board/obj/panda_h7.bin.signed", \
-  "--include=**/panda/board/obj/bootstub.panda.bin", \
-  "--include=**/panda/board/obj/bootstub.panda_h7.bin", \
-  "--exclude=.sconsign.dblite", \
-  "--exclude=*.a", \
-  "--exclude=*.o", \
-  "--exclude=*.os", \
-  "--exclude=*.pyc", \
-  "--exclude=moc_*", \
-  "--exclude=*.cc", \
-  "--exclude=Jenkinsfile", \
-  "--exclude=supercombo.onnx", \
-  "--exclude=**/panda/board/*", \
-  "--exclude=**/panda/board/obj/**", \
-  "--exclude=**/panda/certs/", \
-  "--exclude=**/panda/crypto/", \
-  "--exclude=**/release/", \
-  "--exclude=**/.github/", \
-  "--exclude=**/selfdrive/ui/replay/", \
-  "--exclude=**/__pycache__/", \
-  "--exclude=**/selfdrive/ui/*.h", \
-  "--exclude=**/selfdrive/ui/**/*.h", \
-  "--exclude=**/selfdrive/ui/qt/offroad/sunnypilot/", \
-  #"--exclude=${SCONS_CACHE_DIR:-}", \
-  "--exclude=**/.git/", \
-  "--exclude=**/SConstruct", \
-  "--exclude=**/SConscript", \
-  "--exclude=**/.venv/", \
-  "--delete-excluded", \
-  "--chown=1000:1000", \
-  "/data/openpilot/", \
-  "/output/" \
-]

common/params_keys.h

@@ -136,7 +136,6 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"CustomAccShortPressIncrement", PERSISTENT | BACKUP},
     {"DeviceBootMode", PERSISTENT | BACKUP},
     {"EnableGithubRunner", PERSISTENT | BACKUP},
-    {"InteractivityTimeout", PERSISTENT | BACKUP},
     {"MaxTimeOffroad", PERSISTENT | BACKUP},
     {"Brightness", PERSISTENT | BACKUP},
     {"ModelRunnerTypeCache", CLEAR_ON_ONROAD_TRANSITION},
@@ -179,8 +178,6 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
 
     // model panel params
     {"LagdToggle", PERSISTENT | BACKUP},
-    {"LagdToggleDesc", PERSISTENT},
-    {"LagdToggledelay", PERSISTENT | BACKUP},
 
     // mapd
     {"MapAdvisorySpeedLimit", CLEAR_ON_ONROAD_TRANSITION},
@@ -201,4 +198,15 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"OsmStateTitle", PERSISTENT},
     {"OsmWayTest", PERSISTENT},
     {"RoadName", CLEAR_ON_ONROAD_TRANSITION},
+
+    // Tuning keys
+    {"EnableHkgTuningAngleSmoothingFactor", PERSISTENT | BACKUP},
+    {"HkgTuningAngleMinTorqueReductionGain", PERSISTENT | BACKUP},
+    {"HkgTuningAngleMaxTorqueReductionGain", PERSISTENT | BACKUP},
+    {"HkgTuningAngleActiveTorqueReductionGain", PERSISTENT | BACKUP},
+    {"HkgTuningOverridingCycles", PERSISTENT | BACKUP},
+    {"HkgAngleLiveTuning", CLEAR_ON_MANAGER_START},
+
+    {"AdasDrvEcuInterceptorEnabled", PERSISTENT | BACKUP}
+    
 };

pyproject.toml

@@ -59,7 +59,7 @@ dependencies = [
   "future-fstrings",
 
   # joystickd
-  "inputs",
+  "pygame",
 
   # these should be removed
   "psutil",
@@ -110,7 +110,6 @@ dev = [
   "opencv-python-headless",
   "parameterized >=0.8, <0.9",
   "pyautogui",
-  "pygame",
   "pyopencl; platform_machine != 'aarch64'", # broken on arm64
   "pytools < 2024.1.11; platform_machine != 'aarch64'", # pyopencl use a broken version
   "pywinctl",

release/ci/docker_build_sp.sh

@@ -1,51 +0,0 @@
-#!/bin/sh
-# run_openpilot_docker.sh
-# POSIX-compliant script to run openpilot in Docker for local testing
-
-# === Configurable Variables ===
-
-# Base image to use (required)
-BASE_IMAGE="${BASE_IMAGE:-commaai/openpilot-base:latest}"
-
-# Working directory inside the container
-WORKDIR="/tmp/openpilot"
-
-# Local project path
-LOCAL_DIR="$PWD"
-
-# Shared memory size (adjust for large builds/tests)
-SHM_SIZE="2G"
-
-# Environment configuration
-CI=1
-PYTHONWARNINGS="error"
-FILEREADER_CACHE=1
-PYTHONPATH="$WORKDIR"
-
-# Optional: GitHub Actions env vars — set them only if needed for local mirroring/debug
-USE_GITHUB_ENV_VARS=false  # set to true to enable GitHub-related mounts/envs
-GITHUB_WORKSPACE="${GITHUB_WORKSPACE:-$HOME/openpilot_ci}"  # fallback path
-
-# === Docker Command ===
-
-docker run --rm \
-  --shm-size "$SHM_SIZE" \
-  -v "$LOCAL_DIR":"$WORKDIR" \
-  -w "$WORKDIR" \
-  -e CI="$CI" \
-  -e PYTHONWARNINGS="$PYTHONWARNINGS" \
-  -e FILEREADER_CACHE="$FILEREADER_CACHE" \
-  -e PYTHONPATH="$PYTHONPATH" \
-  ${USE_GITHUB_ENV_VARS:+\
-  -e NUM_JOBS \
-  -e JOB_ID \
-  -e GITHUB_ACTION \
-  -e GITHUB_REF \
-  -e GITHUB_HEAD_REF \
-  -e GITHUB_SHA \
-  -e GITHUB_REPOSITORY \
-  -e GITHUB_RUN_ID \
-  -v "$GITHUB_WORKSPACE/.ci_cache/scons_cache":/tmp/scons_cache \
-  -v "$GITHUB_WORKSPACE/.ci_cache/comma_download_cache":/tmp/comma_download_cache \
-  -v "$GITHUB_WORKSPACE/.ci_cache/openpilot_cache":/tmp/openpilot_cache } \
-  "$BASE_IMAGE" /bin/bash -c "${1:-/bin/bash}"

release/ci/install_github_runner.sh

@@ -5,7 +5,6 @@ set -e
 DEFAULT_REPO_URL="https://github.com/sunnypilot"
 START_AT_BOOT=false
 RESTORE_MODE=false
-RUNNER_VERSION="2.325.0"
 
 # Parse command line arguments
 while [[ $# -gt 0 ]]; do
@@ -109,9 +108,9 @@ setup_system_configs() {
 install_runner() {
     echo "Downloading and setting up runner..."
     cd "$RUNNER_DIR"
-    curl -o actions-runner-linux-arm64-${RUNNER_VERSION}.tar.gz -L https://github.com/actions/runner/releases/download/v${RUNNER_VERSION}/actions-runner-linux-arm64-${RUNNER_VERSION}.tar.gz
-    sudo -u ${RUNNER_USER} tar -xzf ./actions-runner-linux-arm64-${RUNNER_VERSION}.tar.gz
-    sudo rm ./actions-runner-linux-arm64-${RUNNER_VERSION}.tar.gz
+    curl -o actions-runner-linux-arm64-2.322.0.tar.gz -L https://github.com/actions/runner/releases/download/v2.322.0/actions-runner-linux-arm64-2.322.0.tar.gz
+    sudo -u ${RUNNER_USER} tar -xzf ./actions-runner-linux-arm64-2.322.0.tar.gz
+    sudo rm ./actions-runner-linux-arm64-2.322.0.tar.gz
     sudo chmod +x ./config.sh
 }
 
@@ -150,32 +149,25 @@ EOL
 }
 
 install_service() {
-    local service_name
-    if [ -f "${RUNNER_DIR}/.service" ]; then
-        service_name=$(cat "${RUNNER_DIR}/.service")
-    else
-        service_name="actions.runner.sunnypilot.$(uname -n)"
-    fi
-  
-    create_service_template
     remount_rw
-    local service_path="/etc/systemd/system/${service_name}"
     echo "Installing systemd service..."
-    if [ -f "${service_path}" ]; then
-        echo "Service ${service_path} found in systemd, we will delete it"
-        sudo rm -f "${service_path}"
-    fi
-    
     cd "$RUNNER_DIR"
     sudo ./svc.sh install $RUNNER_USER
 
     if [ "$START_AT_BOOT" = false ]; then
+        local service_name
+        if [ -f "${RUNNER_DIR}/.service" ]; then
+            service_name=$(cat "${RUNNER_DIR}/.service")
+        else
+            service_name="actions.runner.sunnypilot.$(uname -n)"
+        fi
         sudo systemctl disable "${service_name}"
     fi
     remount_ro
 }
 
 check_restore_prerequisites() {
+    local needs_restore=false
     local can_restore=false
     local service_name=""
 
@@ -197,16 +189,25 @@ check_restore_prerequisites() {
         exit 1
     fi
 
+    # Then check if restoration is needed (if either service or user is missing)
+    if ! systemctl list-unit-files "${service_name}" &>/dev/null; then
+        echo "Service ${service_name} not found in systemd"
+        needs_restore=true
+    fi
+
     if ! id "${RUNNER_USER}" &>/dev/null; then
         echo "User ${RUNNER_USER} does not exist"
+        needs_restore=true
     fi
 
     # Only proceed if we can restore AND need to restore
-    if [ "$can_restore" = true ]; then
-        echo "Restoration is possible"
+    if [ "$can_restore" = true ] && [ "$needs_restore" = true ]; then
+        echo "Restoration is needed and possible"
         return 0
     else
-        echo "No restoration possible"
+        if [ "$needs_restore" = false ]; then
+            echo "System is already properly configured (user and service exist)"
+        fi
         exit 0
     fi
 }
@@ -225,6 +226,7 @@ perform_install() {
     setup_system_configs
     install_runner
     set_directory_permissions
+    create_service_template
     configure_runner
     install_service
     echo "Installation completed successfully"

scripts/lint/lint.sh

@@ -13,7 +13,7 @@ cd $ROOT
 
 FAILED=0
 
-IGNORED_FILES="uv\.lock|docs\/CARS.md|LICENSE\.md"
+IGNORED_FILES="uv\.lock|docs\/CARS.md|LICENSE\.md|layouts\/.*\.xml"
 IGNORED_DIRS="^third_party.*|^msgq.*|^msgq_repo.*|^opendbc.*|^opendbc_repo.*|^cereal.*|^panda.*|^rednose.*|^rednose_repo.*|^tinygrad.*|^tinygrad_repo.*|^teleoprtc.*|^teleoprtc_repo.*"
 
 function run() {

selfdrive/controls/controlsd.py

@@ -17,6 +17,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
+from openpilot.selfdrive.controls.lib.latcontrol_angle_torque import LatControlAngleTorque
 from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
@@ -58,7 +59,9 @@ class Controls(ControlsExt):
     self.LoC = LongControl(self.CP)
     self.VM = VehicleModel(self.CP)
     self.LaC: LatControl
-    if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
+    if self.CP.steerControlType == car.CarParams.SteerControlType.angle and self.CP.lateralTuning.which() == 'torque':
+      self.LaC = LatControlAngleTorque(self.CP, self.CP_SP, self.CI)
+    elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
       self.LaC = LatControlAngle(self.CP, self.CP_SP, self.CI)
     elif self.CP.lateralTuning.which() == 'pid':
       self.LaC = LatControlPID(self.CP, self.CP_SP, self.CI)
@@ -93,8 +96,7 @@ class Controls(ControlsExt):
                                            torque_params.frictionCoefficientFiltered)
 
       self.LaC.extension.update_model_v2(self.sm['modelV2'])
-      calculated_lag = self.LaC.extension.lagd_torqued_main(self.CP, self.sm['liveDelay'])
-      self.LaC.extension.update_lateral_lag(calculated_lag)
+      self.LaC.extension.update_lateral_lag(self.sm['liveDelay'].lateralDelay)
 
     long_plan = self.sm['longitudinalPlan']
     model_v2 = self.sm['modelV2']

selfdrive/controls/lib/latcontrol_angle_torque.py

@@ -0,0 +1,13 @@
+from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
+
+
+class LatControlAngleTorque(LatControlTorque, LatControlAngle):
+  def __init__(self, CP, CP_SP, CI):
+    LatControlTorque.__init__(self, CP, CP_SP, CI)
+    LatControlAngle.__init__(self, CP, CP_SP, CI)
+
+  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):
+    torque, _, _ = LatControlTorque.update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited)
+    _, angle, angle_log = LatControlAngle.update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited)
+    return torque, angle, angle_log

selfdrive/controls/lib/longitudinal_planner.py

@@ -94,13 +94,9 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
   def update(self, sm):
     self.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
-    if not self.mlsim:
-      self.mpc.mode = self.mode
     LongitudinalPlannerSP.update(self, sm)
     if dec_mpc_mode := self.get_mpc_mode():
       self.mode = dec_mpc_mode
-      if not self.mlsim:
-        self.mpc.mode = dec_mpc_mode
 
     if len(sm['carControl'].orientationNED) == 3:
       accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
@@ -175,7 +171,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
-    if self.mode == 'acc' or not self.mlsim:
+    if self.mode == 'acc':
       output_a_target = output_a_target_mpc
       self.output_should_stop = output_should_stop_mpc
     else:

selfdrive/controls/radard.py

@@ -189,7 +189,7 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn
 
 def get_custom_yrel(CP: structs.CarParams, CP_SP: structs.CarParamsSP, lead_dict: dict[str, Any],
                     lead_msg: capnp._DynamicStructReader) -> dict[str, Any]:
-  if CP.brand == "hyundai" and (CP_SP.flags & HyundaiFlagsSP.ENHANCED_SCC or
+  if CP.brand == "hyundai" and (CP_SP.flags & (HyundaiFlagsSP.ENHANCED_SCC | HyundaiFlagsSP.ADAS_ECU_INTERCEPTOR) or
                                 CP.flags & (HyundaiFlags.CANFD_CAMERA_SCC | HyundaiFlags.CAMERA_SCC)):
     lead_dict['yRel'] = float(-lead_msg.y[0])
 

selfdrive/locationd/torqued.py

@@ -11,8 +11,6 @@ from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose
 
-from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle
-
 HISTORY = 5  # secs
 POINTS_PER_BUCKET = 1500
 MIN_POINTS_TOTAL = 4000
@@ -51,10 +49,8 @@ class TorqueBuckets(PointBuckets):
         break
 
 
-class TorqueEstimator(ParameterEstimator, LagdToggle):
+class TorqueEstimator(ParameterEstimator):
   def __init__(self, CP, decimated=False, track_all_points=False):
-    super().__init__()
-    self.CP = CP
     self.hist_len = int(HISTORY / DT_MDL)
     self.lag = 0.0
     self.track_all_points = track_all_points  # for offline analysis, without max lateral accel or max steer torque filters
@@ -180,7 +176,7 @@ class TorqueEstimator(ParameterEstimator, LagdToggle):
     elif which == "liveCalibration":
       self.calibrator.feed_live_calib(msg)
     elif which == "liveDelay":
-      self.lag = self.lagd_torqued_main(self.CP, msg)
+      self.lag = msg.lateralDelay
     # calculate lateral accel from past steering torque
     elif which == "livePose":
       if len(self.raw_points['steer_torque']) == self.hist_len:

selfdrive/modeld/modeld.py

@@ -37,8 +37,6 @@ from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
 from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext
 
-from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle
-
 
 PROCESS_NAME = "selfdrive.modeld.modeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -88,7 +86,6 @@ class ModelState:
   prev_desire: np.ndarray  # for tracking the rising edge of the pulse
 
   def __init__(self, context: CLContext):
-    self.LAT_SMOOTH_SECONDS = 0.0
     with open(VISION_METADATA_PATH, 'rb') as f:
       vision_metadata = pickle.load(f)
       self.vision_input_shapes =  vision_metadata['input_shapes']
@@ -254,8 +251,6 @@ def main(demo=False):
     CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
   cloudlog.info("modeld got CarParams: %s", CP.brand)
 
-  modeld_lagd = LagdToggle()
-
   # TODO this needs more thought, use .2s extra for now to estimate other delays
   # TODO Move smooth seconds to action function
   long_delay = CP.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS
@@ -301,7 +296,7 @@ def main(demo=False):
     is_rhd = sm["driverMonitoringState"].isRHD
     frame_id = sm["roadCameraState"].frameId
     v_ego = max(sm["carState"].vEgo, 0.)
-    lat_delay = modeld_lagd.lagd_main(CP, sm, model)
+    lat_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
     lateral_control_params = np.array([v_ego, lat_delay], dtype=np.float32)
     if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)

selfdrive/ui/sunnypilot/SConscript

@@ -46,6 +46,7 @@ lateral_panel_qt_src = [
   "sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.cc",
   "sunnypilot/qt/offroad/settings/lateral/lane_change_settings.cc",
   "sunnypilot/qt/offroad/settings/lateral/mads_settings.cc",
+  "sunnypilot/qt/offroad/settings/lateral/angle_tuning_settings.cc",
   "sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc",
 ]
 

selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.cc

@@ -87,17 +87,6 @@ DevicePanelSP::DevicePanelSP(SettingsWindowSP *parent) : DevicePanel(parent) {
     params.put("DeviceBootMode", QString::number(index).toStdString());
     updateState();
   });
-
-  interactivityTimeout =  new OptionControlSP("InteractivityTimeout", tr("Interactivity Timeout"),
-                                     tr("Apply a custom timeout for settings UI."
-                                        "\nThis is the time after which settings UI closes automatically if user is not interacting with the screen."),
-                                     "", {0, 120}, 10, true, nullptr, false);
-
-  connect(interactivityTimeout, &OptionControlSP::updateLabels, [=]() {
-    updateState();
-  });
-
-  addItem(interactivityTimeout);
   
   // Brightness
   brightness = new Brightness();
@@ -209,11 +198,4 @@ void DevicePanelSP::updateState() {
     currStatus = DeviceSleepModeStatus::OFFROAD;
   }
   toggleDeviceBootMode->setDescription(deviceSleepModeDescription(currStatus));
-
-  QString timeoutValue = QString::fromStdString(params.get("InteractivityTimeout"));
-  if (timeoutValue == "0") {
-    interactivityTimeout->setLabel("DEFAULT");
-  } else {
-    interactivityTimeout->setLabel(timeoutValue + "s");
-  }
 }

selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h

@@ -33,7 +33,6 @@ private:
   MaxTimeOffroad *maxTimeOffroad;
   ButtonParamControlSP *toggleDeviceBootMode;
   Brightness *brightness;
-  OptionControlSP *interactivityTimeout;
 
   const QString alwaysOffroadStyle = R"(
     PushButtonSP {

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/angle_tuning_settings.cc

@@ -0,0 +1,87 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/angle_tuning_settings.h"
+
+#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
+
+AngleTunningSettings::AngleTunningSettings(QWidget *parent) : QWidget(parent) {
+  QVBoxLayout *main_layout = new QVBoxLayout(this);
+  main_layout->setContentsMargins(50, 20, 50, 20);
+  main_layout->setSpacing(20);
+
+  // Back button
+  PanelBackButton *back = new PanelBackButton();
+  connect(back, &QPushButton::clicked, [=]() { emit backPress(); });
+  main_layout->addWidget(back, 0, Qt::AlignLeft);
+
+  auto *list = new ListWidgetSP(this, false);
+
+  main_layout->addWidget(new QWidget());
+
+  enableHkgAngleSmoothingFactor = new ExpandableToggleRow("EnableHkgTuningAngleSmoothingFactor", tr("HKG Angle Smoothing Factor"), tr("Applies EMA (Exponential Moving Average) to the desired angle steering and avoid overcorrections."), "../assets/offroad/icon_blank.png");
+  list->addItem(enableHkgAngleSmoothingFactor);
+
+  auto first_row = new QHBoxLayout();
+  hkgTuningOverridingCycles = new OptionControlSP("HkgTuningOverridingCycles", tr("Override Ramp-Down Cycles"), tr("Number of cycles to ramp down the current amount of torque on the steering wheel.<br/>A smaller value means a faster override by the user (less effort)"), "../assets/offroad/icon_blank.png", {10, 30}, 1);
+  connect(hkgTuningOverridingCycles, &OptionControlSP::updateLabels, hkgTuningOverridingCycles, [=]() {
+    this->updateToggles(offroad);
+  });
+  first_row->addWidget(hkgTuningOverridingCycles);
+
+  hkgAngleMinTorque = new OptionControlSP("HkgTuningAngleMinTorqueReductionGain", tr("Override Steering Effort"), tr("Sets the steering effort percentage used when the driver is overriding lateral control.<br/>Higher values increase resistance and make the wheel feel stiffer."), "../assets/offroad/icon_blank.png", {5, 60}, 1);
+  connect(hkgAngleMinTorque, &OptionControlSP::updateLabels, hkgAngleMinTorque, [=]() {
+    this->updateToggles(offroad);
+  });
+  first_row->addWidget(hkgAngleMinTorque);
+  list->addItem(first_row);
+
+  auto second_row = new QHBoxLayout();
+  hkgAngleActiveTorque = new OptionControlSP("HkgTuningAngleActiveTorqueReductionGain", tr("Min Active Torque"), tr("Torque applied when lateral control is active but the vehicle is not turning.<br/>Used to maintain lane centering on straight paths when no user input is detected."), "../assets/offroad/icon_blank.png", {10, 100}, 1);
+  connect(hkgAngleActiveTorque, &OptionControlSP::updateLabels, hkgAngleActiveTorque, [=]() {
+    this->updateToggles(offroad);
+  });
+  second_row->addWidget(hkgAngleActiveTorque);
+
+  hkgAngleMaxTorque = new OptionControlSP("HkgTuningAngleMaxTorqueReductionGain", tr("Max Torque Allowance"), tr("Sets the maximum torque reduction percentage the controller can apply during normal lateral control.<br/>"), "../assets/offroad/icon_blank.png", {10, 100}, 1);
+  connect(hkgAngleMaxTorque, &OptionControlSP::updateLabels, hkgAngleMaxTorque, [=]() {
+    this->updateToggles(offroad);
+  });
+  second_row->addWidget(hkgAngleMaxTorque);
+  list->addItem(second_row);
+  
+  QObject::connect(uiState(), &UIState::offroadTransition, this, &AngleTunningSettings::updateToggles);
+
+  main_layout->addWidget(new ScrollViewSP(list, this));
+
+  auto *warning = new QLabel(tr("Reboot required for settings to apply; Tap on each setting to see more details."));
+  warning->setStyleSheet("font-size: 30px; font-weight: 500; font-family: 'Noto Color Emoji'; color: orange;");
+  main_layout->addWidget(warning, 0, Qt::AlignCenter);
+}
+
+void AngleTunningSettings::showEvent(QShowEvent *event) {
+  updateToggles(offroad);
+}
+
+void AngleTunningSettings::updateToggles(bool _offroad) {
+  auto HkgAngleSmoothingFactorValue = params.getBool("EnableHkgTuningAngleSmoothingFactor");
+  enableHkgAngleSmoothingFactor->toggleFlipped(HkgAngleSmoothingFactorValue);
+
+  auto HkgAngleMinTorqueValue = QString::fromStdString(params.get("HkgTuningAngleMinTorqueReductionGain")).toInt();
+  hkgAngleMinTorque->setLabel(QString::number(HkgAngleMinTorqueValue)+"%");
+
+  auto HkgAngleActiveTorqueValue = QString::fromStdString(params.get("HkgTuningAngleActiveTorqueReductionGain")).toInt();
+  hkgAngleActiveTorque->setLabel(QString::number(HkgAngleActiveTorqueValue)+"%");
+
+  auto HkgAngleMaxTorqueValue = QString::fromStdString(params.get("HkgTuningAngleMaxTorqueReductionGain")).toInt();
+  hkgAngleMaxTorque->setLabel(QString::number(HkgAngleMaxTorqueValue)+"%");
+
+  auto HkgTuningOverridingCyclesValue = QString::fromStdString(params.get("HkgTuningOverridingCycles")).toInt();
+  hkgTuningOverridingCycles->setLabel(QString::number(HkgTuningOverridingCyclesValue));
+
+  offroad = _offroad;
+}

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/angle_tuning_settings.h

@@ -0,0 +1,40 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#pragma once
+
+#include "selfdrive/ui/qt/util.h"
+#include "selfdrive/ui/sunnypilot/ui.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/expandable_row.h"
+
+class AngleTunningSettings : public QWidget {
+  Q_OBJECT
+
+public:
+  explicit AngleTunningSettings(QWidget *parent = nullptr);
+
+  void showEvent(QShowEvent *event) override;
+
+signals:
+  void backPress();
+
+public slots:
+  void updateToggles(bool _offroad);
+
+private:
+  Params params;
+  bool offroad;
+
+  ExpandableToggleRow* enableHkgAngleSmoothingFactor;
+  OptionControlSP* hkgAngleMinTorque;
+  OptionControlSP* hkgAngleActiveTorque;
+  OptionControlSP* hkgAngleMaxTorque;
+  OptionControlSP* hkgTuningOverridingCycles;
+  ParamControlSP* hkgAngleLiveTuning;
+};

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc

@@ -89,6 +89,36 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
     nnlcToggle->updateToggle();
   });
 
+#pragma region hkg angle tuning
+  list->addItem(vertical_space());
+  list->addItem(horizontal_line());
+  list->addItem(vertical_space());
+
+  // HKG Angle Tuning
+  // angleTuningToggle = new ParamControl(
+  //   "AngleTuning",
+  //   tr("Modular Assistive Driving System (MADS)"),
+  //   tr("Enable the beloved MADS feature. Disable toggle to revert back to stock sunnypilot engagement/disengagement."),
+  //   "");
+  // angleTuningToggle->setConfirmation(true, false);
+  // list->addItem(angleTuningToggle);
+
+  angleTuningSettingsButton = new PushButtonSP(tr("Customize ANGLE Tuning"));
+  angleTuningSettingsButton->setObjectName("angle_btn");
+  connect(angleTuningSettingsButton, &QPushButton::clicked, [=]() {
+    sunnypilotScroller->setLastScrollPosition();
+    main_layout->setCurrentWidget(angleTuningWidget);
+  });
+  // QObject::connect(angleTuningToggle, &ToggleControl::toggleFlipped, angleTuningSettingsButton, &PushButtonSP::setEnabled);
+
+  angleTuningWidget = new AngleTunningSettings(this);
+  connect(angleTuningWidget, &AngleTunningSettings::backPress, [=]() {
+    sunnypilotScroller->restoreScrollPosition();
+    main_layout->setCurrentWidget(sunnypilotScreen);
+  });
+  list->addItem(angleTuningSettingsButton);
+#pragma endregion
+
   toggleOffroadOnly = {
     madsToggle, nnlcToggle,
   };
@@ -99,6 +129,7 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
 
   main_layout->addWidget(sunnypilotScreen);
   main_layout->addWidget(madsWidget);
+  main_layout->addWidget(angleTuningWidget);
   main_layout->addWidget(laneChangeWidget);
 
   setStyleSheet(R"(
@@ -149,6 +180,7 @@ void LateralPanel::updateToggles(bool _offroad) {
   }
 
   madsSettingsButton->setEnabled(madsToggle->isToggled());
+  // angleTuningSettingsButton->setEnabled(angleTuningToggle->isToggled());
 
   blinkerPauseLateralSettings->refresh();
 

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h

@@ -13,6 +13,7 @@
 #include "selfdrive/ui/sunnypilot/ui.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/angle_tuning_settings.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.h"
 #include "selfdrive/ui/qt/util.h"
@@ -39,8 +40,11 @@ private:
   bool offroad;
 
   ParamControl *madsToggle;
+  // ParamControl *angleTuningToggle;
   PushButtonSP *madsSettingsButton;
+  PushButtonSP *angleTuningSettingsButton;
   MadsSettings *madsWidget = nullptr;
+  AngleTunningSettings *angleTuningWidget = nullptr;
   PushButtonSP *laneChangeSettingsButton;
   LaneChangeSettings *laneChangeWidget = nullptr;
   NeuralNetworkLateralControl *nnlcToggle = nullptr;

selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc

@@ -90,25 +90,11 @@ ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) {
   list->addItem(horizontal_line());
 
   // LiveDelay toggle
-  lagd_toggle_control = new ParamControlSP("LagdToggle", tr("Live Learning Steer Delay"), "", "../assets/offroad/icon_shell.png");
-  lagd_toggle_control->showDescription();
-  list->addItem(lagd_toggle_control);
-
-  // Software delay control
-  delay_control = new OptionControlSP("LagdToggledelay", tr("Adjust Software Delay"),
-                                     tr("Adjust the software delay when Live Learning Steer Delay is toggled off."
-                                        "\nThe default software delay value is 0.2"),
-                                     "", {10, 30}, 1, false, nullptr, true);
-
-  connect(delay_control, &OptionControlSP::updateLabels, [=]() {
-    float value = QString::fromStdString(params.get("LagdToggledelay")).toFloat();
-    delay_control->setLabel(QString::number(value, 'f', 2) + "s");
-  });
-  connect(lagd_toggle_control, &ParamControlSP::toggleFlipped, [=](bool state) {
-    delay_control->setVisible(!state);
-  });
-  delay_control->showDescription();
-  list->addItem(delay_control);
+  list->addItem(new ParamControlSP("LagdToggle",
+                                   tr("Live Learning Steer Delay"),
+                                   tr("Enable this for the car to learn and adapt its steering response time. "
+                                      "Disable to use a fixed steering response time. Keeping this on provides the stock openpilot experience."),
+                                   "../assets/offroad/icon_shell.png"));
 }
 
 QProgressBar* ModelsPanel::createProgressBar(QWidget *parent) {
@@ -304,24 +290,6 @@ void ModelsPanel::updateLabels() {
   handleBundleDownloadProgress();
   currentModelLblBtn->setEnabled(!is_onroad && !isDownloading());
   currentModelLblBtn->setValue(GetActiveModelInternalName());
-
-  // Update lagdToggle description with current value
-  QString desc = tr("Enable this for the car to learn and adapt its steering response time. "
-                   "Disable to use a fixed steering response time. Keeping this on provides the stock openpilot experience. "
-                   "The Current value is updated automatically when the vehicle is Onroad.");
-  QString current = QString::fromStdString(params.get("LagdToggleDesc", false));
-  if (!current.isEmpty()) {
-    desc += "<br><br><b><span style=\"color:#e0e0e0\">" + tr("Current:") + "</span></b> <span style=\"color:#e0e0e0\">" + current + "</span>";
-  }
-  lagd_toggle_control->setDescription(desc);
-  lagd_toggle_control->showDescription();
-
-  delay_control->setVisible(!params.getBool("LagdToggle"));
-  if (delay_control->isVisible()) {
-    float value = QString::fromStdString(params.get("LagdToggledelay")).toFloat();
-    delay_control->setLabel(QString::number(value, 'f', 2) + "s");
-    delay_control->showDescription();
-  }
 }
 
 /**

selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h

@@ -64,8 +64,6 @@ private:
   bool is_onroad = false;
 
   ButtonControlSP *currentModelLblBtn;
-  ParamControlSP *lagd_toggle_control;
-  OptionControlSP *delay_control;
   QProgressBar *supercomboProgressBar;
   QFrame *supercomboFrame;
   QProgressBar *navigationProgressBar;

selfdrive/ui/sunnypilot/qt/widgets/controls.h

@@ -480,16 +480,6 @@ private:
     return result.toInt();
   }
 
-  int getParamValueScaled() {
-    const auto param_value = QString::fromStdString(params.get(key));
-    return static_cast<int>(param_value.toFloat() * 100);
-  }
-
-  void setParamValueScaled(const int new_value) {
-    const float scaled_value = new_value / 100.0f;
-    params.put(key, QString::number(scaled_value, 'f', 2).toStdString());
-  }
-
   // Although the method is not static, and thus has access to the value property, I prefer to be explicit about the value.
   void setParamValue(const int new_value) {
     const auto value_str = valueMap != nullptr ? valueMap->value(QString::number(new_value)) : QString::number(new_value);
@@ -498,8 +488,7 @@ private:
 
 public:
   OptionControlSP(const QString &param, const QString &title, const QString &desc, const QString &icon,
-                  const MinMaxValue &range, const int per_value_change = 1, const bool inline_layout = false,
-                  const QMap<QString, QString> *valMap = nullptr, bool scale_float = false) : AbstractControlSP_SELECTOR(title, desc, icon, nullptr), _title(title), valueMap(valMap), is_inline_layout(inline_layout), use_float_scaling(scale_float) {
+                  const MinMaxValue &range, const int per_value_change = 1, const bool inline_layout = false, const QMap<QString, QString> *valMap = nullptr) : AbstractControlSP_SELECTOR(title, desc, icon, nullptr), _title(title), valueMap(valMap), is_inline_layout(inline_layout) {
     const QString style = R"(
       QPushButton {
         border-radius: 20px;
@@ -539,7 +528,7 @@ public:
     const std::vector<QString> button_texts{"－", "＋"};
 
     key = param.toStdString();
-    value = use_float_scaling ? getParamValueScaled() : getParamValue();
+    value = getParamValue();
 
     button_group = new QButtonGroup(this);
     button_group->setExclusive(true);
@@ -557,15 +546,10 @@ public:
 
       QObject::connect(button, &QPushButton::clicked, [=]() {
         int change_value = (i == 0) ? -per_value_change : per_value_change;
-        value = use_float_scaling ? getParamValueScaled() : getParamValue();
+        value = getParamValue(); // in case it changed externally, we need to get the latest value.
         value += change_value;
         value = std::clamp(value, range.min_value, range.max_value);
-
-        if (use_float_scaling) {
-          setParamValueScaled(value);
-        } else {
-          setParamValue(value);
-        }
+        setParamValue(value);
 
         button_group->button(0)->setEnabled(!(value <= range.min_value));
         button_group->button(1)->setEnabled(!(value >= range.max_value));
@@ -658,7 +642,6 @@ private:
   const QString label_disabled_style = "font-size: 50px; font-weight: 450; color: #5C5C5C;";
 
   bool button_enabled = true;
-  bool use_float_scaling = false;
 };
 
 class PushButtonSP : public QPushButton {

selfdrive/ui/ui.cc

@@ -164,9 +164,8 @@ void Device::setAwake(bool on) {
 }
 
 void Device::resetInteractiveTimeout(int timeout) {
-  int customTimeout = QString::fromStdString(Params().get("InteractivityTimeout")).toInt();
   if (timeout == -1) {
-    timeout = customTimeout == 0 ? (ignition_on ? 10 : 30) : customTimeout;
+    timeout = (ignition_on ? 10 : 30);
   }
   interactive_timeout = timeout * UI_FREQ;
 }

sunnypilot/livedelay/lagd_toggle.py

@@ -1,51 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-from openpilot.common.params import Params
-from openpilot.common.swaglog import cloudlog
-
-
-class LagdToggle:
-  def __init__(self):
-    self.params = Params()
-    self.lag = 0.0
-    self._last_desc = None
-
-  @property
-  def software_delay(self):
-    return float(self.params.get("LagdToggledelay", encoding='utf8'))
-
-  def _maybe_update_desc(self, desc):
-    if desc != self._last_desc:
-      self.params.put_nonblocking("LagdToggleDesc", desc)
-      self._last_desc = desc
-
-  def lagd_main(self, CP, sm, model):
-    if self.params.get_bool("LagdToggle"):
-      lateral_delay = sm["liveDelay"].lateralDelay
-      lat_smooth = model.LAT_SMOOTH_SECONDS
-      result = lateral_delay + lat_smooth
-      desc = f"live steer delay learner ({lateral_delay:.3f}s) + model smoothing filter ({lat_smooth:.3f}s) = total delay ({result:.3f}s)"
-      self._maybe_update_desc(desc)
-      return result
-
-    steer_actuator_delay = CP.steerActuatorDelay
-    lat_smooth = model.LAT_SMOOTH_SECONDS
-    delay = self.software_delay
-    result = (steer_actuator_delay + delay) + lat_smooth
-    desc = (f"Vehicle steering delay ({steer_actuator_delay:.3f}s) + software delay ({delay:.3f}s) + " +
-            f"model smoothing filter ({lat_smooth:.3f}s) = total delay ({result:.3f}s)")
-    self._maybe_update_desc(desc)
-    return result
-
-  def lagd_torqued_main(self, CP, msg):
-    if self.params.get_bool("LagdToggle"):
-      self.lag = msg.lateralDelay
-      cloudlog.debug(f"TORQUED USING LIVE DELAY: {self.lag:.3f}")
-    else:
-      self.lag = CP.steerActuatorDelay + self.software_delay
-      cloudlog.debug(f"TORQUED USING STEER ACTUATOR: {self.lag:.3f}")
-    return self.lag

sunnypilot/modeld/modeld.py

@@ -25,7 +25,7 @@ from openpilot.sunnypilot.modeld.parse_model_outputs import Parser
 from openpilot.sunnypilot.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
 from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan
 from openpilot.sunnypilot.models.helpers import get_active_bundle, get_model_path, load_metadata, prepare_inputs, load_meta_constants
-from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle
+from openpilot.sunnypilot.models.modeld_lagd import ModeldLagd
 from openpilot.sunnypilot.modeld.models.commonmodel_pyx import ModelFrame, CLContext
 
 
@@ -202,7 +202,7 @@ def main(demo=False):
 
   cloudlog.info("modeld got CarParams: %s", CP.brand)
 
-  modeld_lagd = LagdToggle()
+  modeld_lagd = ModeldLagd()
 
   # Enable lagd support for sunnypilot modeld
   long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS

sunnypilot/modeld_v2/modeld.py

@@ -23,7 +23,7 @@ from openpilot.sunnypilot.modeld_v2.models.commonmodel_pyx import DrivingModelFr
 from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants
 
 from openpilot.sunnypilot.models.helpers import get_active_bundle
-from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle
+from openpilot.sunnypilot.models.modeld_lagd import ModeldLagd
 from openpilot.sunnypilot.models.runners.helpers import get_model_runner
 
 PROCESS_NAME = "selfdrive.modeld.modeld"
@@ -239,7 +239,7 @@ def main(demo=False):
   cloudlog.info("modeld got CarParams: %s", CP.brand)
 
 
-  modeld_lagd = LagdToggle()
+  modeld_lagd = ModeldLagd()
 
   # TODO Move smooth seconds to action function
   long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS

sunnypilot/models/modeld_lagd.py

@@ -0,0 +1,26 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+
+
+class ModeldLagd:
+  def __init__(self):
+    self.params = Params()
+
+  def lagd_main(self, CP, sm, model):
+    if self.params.get_bool("LagdToggle"):
+      lateral_delay = sm["liveDelay"].lateralDelay
+      lat_smooth = model.LAT_SMOOTH_SECONDS
+      result = lateral_delay + lat_smooth
+      cloudlog.debug(f"LAGD USING LIVE DELAY: {lateral_delay:.3f} + {lat_smooth:.3f} = {result:.3f}")
+      return result
+    steer_actuator_delay = CP.steerActuatorDelay
+    lat_smooth = model.LAT_SMOOTH_SECONDS
+    result = (steer_actuator_delay + 0.2) + lat_smooth
+    cloudlog.debug(f"LAGD USING STEER ACTUATOR: {steer_actuator_delay:.3f} + 0.2 + {lat_smooth:.3f} = {result:.3f}")
+    return result

sunnypilot/selfdrive/controls/lib/dec/constants.py

@@ -1,17 +1,25 @@
 class WMACConstants:
-  # Lead detection parameters
-  LEAD_WINDOW_SIZE = 6  # Stable detection window
-  LEAD_PROB = 0.45  # Balanced threshold for lead detection
+  LEAD_WINDOW_SIZE = 5
+  LEAD_PROB = 0.5
 
-  # Slow down detection parameters
-  SLOW_DOWN_WINDOW_SIZE = 5  # Responsive but stable
-  SLOW_DOWN_PROB = 0.3  # Balanced threshold for slow down scenarios
+  SLOW_DOWN_WINDOW_SIZE = 5
+  SLOW_DOWN_PROB = 0.6
 
-  # Optimized slow down distance curve - smooth and progressive
   SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
-  SLOW_DOWN_DIST = [32., 46., 64., 86., 108., 130., 145., 165.]
+  SLOW_DOWN_DIST = [25., 38., 55., 75., 95., 115., 130., 150.]
 
-  # Slowness detection parameters
-  SLOWNESS_WINDOW_SIZE = 10  # Stable slowness detection
-  SLOWNESS_PROB = 0.55  # Clear threshold for slowness
-  SLOWNESS_CRUISE_OFFSET = 1.025  # Conservative cruise speed offset
+  SLOWNESS_WINDOW_SIZE = 12
+  SLOWNESS_PROB = 0.5
+  SLOWNESS_CRUISE_OFFSET = 1.05
+
+  DANGEROUS_TTC_WINDOW_SIZE = 3
+  DANGEROUS_TTC = 2.3
+
+  MPC_FCW_WINDOW_SIZE = 10
+  MPC_FCW_PROB = 0.5
+
+
+class SNG_State:
+  off = 0
+  stopped = 1
+  going = 2

sunnypilot/selfdrive/controls/lib/dec/dec.py

@@ -1,133 +1,88 @@
-"""
-Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
+# The MIT License
+#
+# Copyright (c) 2019-, Rick Lan, dragonpilot community, and a number of other of contributors.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+#
+# Version = 2025-1-18
 
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-# Version = 2025-6-30
+import numpy as np
 
 from cereal import messaging
 from opendbc.car import structs
 from numpy import interp
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
-from typing import Literal
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants, SNG_State
 
 # d-e2e, from modeldata.h
 TRAJECTORY_SIZE = 33
-SET_MODE_TIMEOUT = 15
 
-# Define the valid mode types
-ModeType = Literal['acc', 'blended']
+HIGHWAY_CRUISE_KPH = 70
+
+STOP_AND_GO_FRAME = 60
+
+SET_MODE_TIMEOUT = 10
+
+V_ACC_MIN = 9.72
 
 
-class SmoothKalmanFilter:
-  """Enhanced Kalman filter with smoothing for stable decision making."""
+class GenericMovingAverageCalculator:
+  def __init__(self, window_size):
+    self.window_size = window_size
+    self.data = []
+    self.total = 0
 
-  def __init__(self, initial_value=0, measurement_noise=0.1, process_noise=0.01,
-               alpha=1.0, smoothing_factor=0.85):
-    self.x = initial_value
-    self.P = 1.0
-    self.R = measurement_noise
-    self.Q = process_noise
-    self.alpha = alpha
-    self.smoothing_factor = smoothing_factor
-    self.initialized = False
-    self.history = []
-    self.max_history = 10
-    self.confidence = 0.0
+  def add_data(self, value: float) -> None:
+    if len(self.data) == self.window_size:
+      self.total -= self.data.pop(0)
+    self.data.append(value)
+    self.total += value
 
-  def add_data(self, measurement):
-    if len(self.history) >= self.max_history:
-      self.history.pop(0)
-    self.history.append(measurement)
+  def get_moving_average(self) -> float | None:
+    return None if len(self.data) == 0 else self.total / len(self.data)
 
-    if not self.initialized:
-      self.x = measurement
-      self.initialized = True
-      self.confidence = 0.1
-      return
-
-    self.P = self.alpha * self.P + self.Q
-
-    K = self.P / (self.P + self.R)
-    effective_K = K * (1.0 - self.smoothing_factor) + self.smoothing_factor * 0.1
-
-    innovation = measurement - self.x
-    self.x = self.x + effective_K * innovation
-    self.P = (1 - effective_K) * self.P
-
-    if abs(innovation) < 0.1:
-      self.confidence = min(1.0, self.confidence + 0.05)
-    else:
-      self.confidence = max(0.1, self.confidence - 0.02)
-
-  def get_value(self):
-    return self.x if self.initialized else None
-
-  def get_confidence(self):
-    return self.confidence
-
-  def reset_data(self):
-    self.initialized = False
-    self.history = []
-    self.confidence = 0.0
+  def reset_data(self) -> None:
+    self.data = []
+    self.total = 0
 
 
-class ModeTransitionManager:
-  """Manages smooth transitions between driving modes with hysteresis."""
+class WeightedMovingAverageCalculator:
+  def __init__(self, window_size):
+    self.window_size = window_size
+    self.data = []
+    self.weights = np.linspace(1, 3, window_size)  # Linear weights, adjust as needed
 
-  def __init__(self):
-    self.current_mode: ModeType = 'acc'
-    self.mode_confidence = {'acc': 1.0, 'blended': 0.0}
-    self.transition_timeout = 0
-    self.min_mode_duration = 10
-    self.mode_duration = 0
-    self.emergency_override = False
+  def add_data(self, value: float) -> None:
+    if len(self.data) == self.window_size:
+      self.data.pop(0)
+    self.data.append(value)
 
-  def request_mode(self, mode: ModeType, confidence: float = 1.0, emergency: bool = False):
-    # Emergency override for critical situations (stops, collisions)
-    if emergency:
-      self.emergency_override = True
-      self.current_mode = mode
-      self.transition_timeout = SET_MODE_TIMEOUT
-      self.mode_duration = 0
-      return
+  def get_weighted_average(self) -> float | None:
+    if len(self.data) == 0:
+      return None
+    weighted_sum: float = float(np.dot(self.data, self.weights[-len(self.data):]))
+    weight_total: float = float(np.sum(self.weights[-len(self.data):]))
+    return weighted_sum / weight_total
 
-    self.mode_confidence[mode] = min(1.0, self.mode_confidence[mode] + 0.1 * confidence)
-    for m in self.mode_confidence:
-      if m != mode:
-        self.mode_confidence[m] = max(0.0, self.mode_confidence[m] - 0.05)
-
-    # Require minimum duration in current mode (unless emergency)
-    if self.mode_duration < self.min_mode_duration and not self.emergency_override:
-      return
-
-    # Hysteresis: higher threshold for mode changes
-    confidence_threshold = 0.6 if mode != self.current_mode else 0.3  # Lower threshold for faster response
-
-    if self.mode_confidence[mode] > confidence_threshold:
-      if mode != self.current_mode and self.transition_timeout == 0:
-        self.transition_timeout = SET_MODE_TIMEOUT
-        self.current_mode = mode
-        self.mode_duration = 0
-
-  def update(self):
-    if self.transition_timeout > 0:
-      self.transition_timeout -= 1
-    self.mode_duration += 1
-
-    # Reset emergency override after some time
-    if self.emergency_override and self.mode_duration > 20:
-      self.emergency_override = False
-
-    # Gradual confidence decay
-    for mode in self.mode_confidence:
-      self.mode_confidence[mode] *= 0.98
-
-  def get_mode(self) -> ModeType:
-    return self.current_mode
+  def reset_data(self) -> None:
+    self.data = []
 
 
 class DynamicExperimentalController:
@@ -137,59 +92,51 @@ class DynamicExperimentalController:
     self._params = params or Params()
     self._enabled: bool = self._params.get_bool("DynamicExperimentalControl")
     self._active: bool = False
+    self._mode: str = 'acc'
     self._frame: int = 0
-    self._urgency = 0.0
 
-    self._mode_manager = ModeTransitionManager()
-
-    # Smooth filters for stable decision making with faster response for critical scenarios
-    self._lead_filter = SmoothKalmanFilter(
-      measurement_noise=0.15,
-      process_noise=0.05,
-      alpha=1.02,
-      smoothing_factor=0.8
-    )
-
-    self._slow_down_filter = SmoothKalmanFilter(
-      measurement_noise=0.1,
-      process_noise=0.1,
-      alpha=1.05,
-      smoothing_factor=0.7
-    )
-
-    self._slowness_filter = SmoothKalmanFilter(
-      measurement_noise=0.1,
-      process_noise=0.06,
-      alpha=1.015,
-      smoothing_factor=0.92
-    )
-
-    self._mpc_fcw_filter = SmoothKalmanFilter(
-      measurement_noise=0.2,
-      process_noise=0.1,
-      alpha=1.1,
-      smoothing_factor=0.5
-    )
+    # Use weighted moving average for filtering leads
+    self._lead_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.LEAD_WINDOW_SIZE)
     self._has_lead_filtered = False
-    self._has_slow_down = False
-    self._has_slowness = False
-    self._has_mpc_fcw = False
-    self._v_ego_kph = 0.0
-    self._v_cruise_kph = 0.0
+    self._has_lead_filtered_prev = False
+
+    self._slow_down_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.SLOW_DOWN_WINDOW_SIZE)
+    self._has_slow_down: bool = False
+    self._slow_down_confidence: float = 0.0
+
+    self._has_blinkers = False
+
+    self._slowness_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.SLOWNESS_WINDOW_SIZE)
+    self._has_slowness: bool = False
+
+    self._has_nav_instruction = False
+
+    self._dangerous_ttc_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.DANGEROUS_TTC_WINDOW_SIZE)
+    self._has_dangerous_ttc: bool = False
+
+    self._v_ego_kph = 0.
+    self._v_cruise_kph = 0.
+
+    self._has_lead = False
+
     self._has_standstill = False
+    self._has_standstill_prev = False
+
+    self._sng_transit_frame = 0
+    self._sng_state = SNG_State.off
+
+    self._mpc_fcw_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.MPC_FCW_WINDOW_SIZE)
+    self._has_mpc_fcw: bool = False
     self._mpc_fcw_crash_cnt = 0
-    self._standstill_count = 0
-    # debug
-    self._endpoint_x = float('inf')
-    self._expected_distance = 0.0
-    self._trajectory_valid = False
+
+    self._set_mode_timeout = 0
 
   def _read_params(self) -> None:
     if self._frame % int(1. / DT_MDL) == 0:
       self._enabled = self._params.get_bool("DynamicExperimentalControl")
 
   def mode(self) -> str:
-    return self._mode_manager.get_mode()
+    return str(self._mode)
 
   def enabled(self) -> bool:
     return self._enabled
@@ -197,9 +144,46 @@ class DynamicExperimentalController:
   def active(self) -> bool:
     return self._active
 
-  def set_mpc_fcw_crash_cnt(self) -> None:
-    """Set MPC FCW crash count"""
-    self._mpc_fcw_crash_cnt = self._mpc.crash_cnt
+  @staticmethod
+  def _anomaly_detection(recent_data: list[float], threshold: float = 2.0, context_check: bool = True) -> bool:
+    """
+    Basic anomaly detection using standard deviation.
+    """
+    if len(recent_data) < 5:
+      return False
+    mean: float = float(np.mean(recent_data))
+    std_dev: float = float(np.std(recent_data))
+    anomaly: bool = bool(recent_data[-1] > mean + threshold * std_dev)
+
+    # Context check to ensure repeated anomaly
+    if context_check:
+      return np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1
+    return anomaly
+
+  def _adaptive_slowdown_threshold(self) -> float:
+    """
+    Adapts the slow-down threshold based on vehicle speed and recent behavior.
+    """
+    slowdown_scaling_factor: float = (1.0 + 0.03 * np.log(1 + len(self._slow_down_gmac.data)))
+    adaptive_threshold: float = float(
+      interp(self._v_ego_kph, WMACConstants.SLOW_DOWN_BP, WMACConstants.SLOW_DOWN_DIST) * slowdown_scaling_factor
+    )
+    return adaptive_threshold
+
+  def _smoothed_lead_detection(self, lead_prob: float, smoothing_factor: float = 0.2):
+    """
+    Smoothing the lead detection to avoid erratic behavior.
+    """
+    lead_filtering: float = (1 - smoothing_factor) * self._has_lead_filtered + smoothing_factor * lead_prob
+    return lead_filtering > WMACConstants.LEAD_PROB
+
+  def _adaptive_lead_prob_threshold(self) -> float:
+    """
+    Adapts lead probability threshold based on driving conditions.
+    """
+    if self._v_ego_kph > HIGHWAY_CRUISE_KPH:
+      return float(WMACConstants.LEAD_PROB + 0.1)  # Increase the threshold on highways
+    return float(WMACConstants.LEAD_PROB)
 
   def _update_calculations(self, sm: messaging.SubMaster) -> None:
     car_state = sm['carState']
@@ -208,168 +192,186 @@ class DynamicExperimentalController:
 
     self._v_ego_kph = car_state.vEgo * 3.6
     self._v_cruise_kph = car_state.vCruise
+    self._has_lead = lead_one.status
     self._has_standstill = car_state.standstill
 
-    # standstill detection
-    if self._has_standstill:
-      self._standstill_count = min(20, self._standstill_count + 1)
+    # fcw detection
+    self._mpc_fcw_gmac.add_data(self._mpc_fcw_crash_cnt > 0)
+    if _mpc_fcw_weighted_average := self._mpc_fcw_gmac.get_weighted_average():
+      self._has_mpc_fcw = _mpc_fcw_weighted_average > WMACConstants.MPC_FCW_PROB
     else:
-      self._standstill_count = max(0, self._standstill_count - 1)
+      self._has_mpc_fcw = False
 
-    # Lead detection
-    self._lead_filter.add_data(float(lead_one.status))
-    lead_value = self._lead_filter.get_value() or 0.0
-    self._has_lead_filtered = lead_value > WMACConstants.LEAD_PROB
+    # nav enable detection
+    # self._has_nav_instruction = md.navEnabledDEPRECATED and maneuver_distance / max(car_state.vEgo, 1) < 13
 
-    # MPC FCW detection
-    fcw_filtered_value = self._mpc_fcw_filter.get_value() or 0.0
-    self._mpc_fcw_filter.add_data(float(self._mpc_fcw_crash_cnt > 0))
-    self._has_mpc_fcw = fcw_filtered_value > 0.5
+    # lead detection with smoothing
+    self._lead_gmac.add_data(lead_one.status)
+    self._has_lead_filtered = (self._lead_gmac.get_weighted_average() or -1.) > WMACConstants.LEAD_PROB
+    #lead_prob = self._lead_gmac.get_weighted_average() or 0
+    #self._has_lead_filtered = self._smoothed_lead_detection(lead_prob)
 
-    # Slow down detection
-    self._calculate_slow_down(md)
+    # adaptive slow down detection
+    adaptive_threshold = self._adaptive_slowdown_threshold()
+    slow_down_trigger = len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE and md.position.x[TRAJECTORY_SIZE - 1] < adaptive_threshold
+    self._slow_down_gmac.add_data(slow_down_trigger)
+    if _has_slow_down_weighted_average := self._slow_down_gmac.get_weighted_average():
+      self._has_slow_down = _has_slow_down_weighted_average > WMACConstants.SLOW_DOWN_PROB
+      self._slow_down_confidence = _has_slow_down_weighted_average  # Store confidence level
+    else:
+      self._has_slow_down = False
+      self._slow_down_confidence = 0.0  # No confidence if no slowdown
 
-    # Slowness detection
-    if not (self._standstill_count > 5) and not self._has_slow_down:
-      current_slowness = float(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET))
-      self._slowness_filter.add_data(current_slowness)
-      slowness_value = self._slowness_filter.get_value() or 0.0
+    # anomaly detection for slow down events
+    if self._anomaly_detection(self._slow_down_gmac.data):
+      self._slow_down_confidence *= 0.85  # Reduce confidence
+      self._has_slow_down = self._slow_down_confidence > WMACConstants.SLOW_DOWN_PROB
 
-      # Hysteresis for slowness
-      threshold = WMACConstants.SLOWNESS_PROB * (0.8 if self._has_slowness else 1.1)
-      self._has_slowness = slowness_value > threshold
+    # blinker detection
+    self._has_blinkers = car_state.leftBlinker or car_state.rightBlinker
 
-  def _calculate_slow_down(self, md):
-    """Calculate urgency based on trajectory endpoint vs expected distance."""
+    # sng detection
+    if self._has_standstill:
+      self._sng_state = SNG_State.stopped
+      self._sng_transit_frame = 0
+    else:
+      if self._sng_transit_frame == 0:
+        if self._sng_state == SNG_State.stopped:
+          self._sng_state = SNG_State.going
+          self._sng_transit_frame = STOP_AND_GO_FRAME
+        elif self._sng_state == SNG_State.going:
+          self._sng_state = SNG_State.off
+      elif self._sng_transit_frame > 0:
+        self._sng_transit_frame -= 1
 
-    # Reset to safe defaults
-    urgency = 0.0
-    self._endpoint_x = float('inf')
-    self._trajectory_valid = False
+    # slowness detection
+    if not self._has_standstill:
+      self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET))
+      if _slowness_weighted_average := self._slowness_gmac.get_weighted_average():
+        self._has_slowness = _slowness_weighted_average > WMACConstants.SLOWNESS_PROB
+      else:
+        self._has_slowness = False
 
-    #Require exact trajectory size
-    position_valid = len(md.position.x) == TRAJECTORY_SIZE
-    orientation_valid = len(md.orientation.x) == TRAJECTORY_SIZE
+    # dangerous TTC detection
+    if not self._has_lead_filtered and self._has_lead_filtered_prev:
+      self._dangerous_ttc_gmac.reset_data()
+      self._has_dangerous_ttc = False
 
-    if not (position_valid and orientation_valid):
-      # Invalid trajectory - this itself might indicate a stop scenario
-      # Apply moderate urgency for incomplete trajectories at speed
-      if self._v_ego_kph > 20.0:
-        urgency = 0.3
+    if self._has_lead and car_state.vEgo >= 0.01:
+      self._dangerous_ttc_gmac.add_data(lead_one.dRel / car_state.vEgo)
 
-      self._slow_down_filter.add_data(urgency)
-      urgency_filtered = self._slow_down_filter.get_value() or 0.0
-      self._has_slow_down = urgency_filtered > WMACConstants.SLOW_DOWN_PROB
-      self._urgency = urgency_filtered
-      return
+    if _dangerous_ttc_weighted_average := self._dangerous_ttc_gmac.get_weighted_average():
+      self._has_dangerous_ttc = _dangerous_ttc_weighted_average <= WMACConstants.DANGEROUS_TTC
+    else:
+      self._has_dangerous_ttc = False
 
-    # We have a valid full trajectory
-    self._trajectory_valid = True
-
-    # Use the exact endpoint (33rd point, index 32)
-    endpoint_x = md.position.x[TRAJECTORY_SIZE - 1]
-    self._endpoint_x = endpoint_x
-
-    # Get expected distance based on current speed using tuned constants
-    expected_distance = interp(self._v_ego_kph,
-                               WMACConstants.SLOW_DOWN_BP,
-                               WMACConstants.SLOW_DOWN_DIST)
-    self._expected_distance = expected_distance
-
-    # Calculate urgency based on trajectory shortage
-    if endpoint_x < expected_distance:
-      shortage = expected_distance - endpoint_x
-      shortage_ratio = shortage / expected_distance
-
-      # Base urgency on shortage ratio
-      urgency = min(1.0, shortage_ratio * 2.0)
-
-      # Increase urgency for very short trajectories (imminent stops)
-      critical_distance = expected_distance * 0.3
-      if endpoint_x < critical_distance:
-        urgency = min(1.0, urgency * 2.0)
-
-      # Speed-based urgency adjustment
-      if self._v_ego_kph > 25.0:
-        speed_factor = 1.0 + (self._v_ego_kph - 25.0) / 80.0
-        urgency = min(1.0, urgency * speed_factor)
-
-    # Apply filtering but with less smoothing for stops
-    self._slow_down_filter.add_data(urgency)
-    urgency_filtered = self._slow_down_filter.get_value() or 0.0
-
-    # Update state with lower threshold for better stop detection
-    self._has_slow_down = urgency_filtered > (WMACConstants.SLOW_DOWN_PROB * 0.8)
-    self._urgency = urgency_filtered
+    # keep prev values
+    self._has_standstill_prev = self._has_standstill
+    self._has_lead_filtered_prev = self._has_lead_filtered
 
   def _radarless_mode(self) -> None:
-    """Radarless mode decision logic with emergency handling."""
-
-    # EMERGENCY: MPC FCW - immediate blended mode
+    # when mpc fcw crash prob is high
+    # use blended to slow down quickly
     if self._has_mpc_fcw:
-      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      self._set_mode('blended')
       return
 
-    # Standstill: use blended
-    if self._standstill_count > 3:
-      self._mode_manager.request_mode('blended', confidence=0.9)
+    # Nav enabled and distance to upcoming turning is 300 or below
+    # if self._has_nav_instruction:
+    #  self._set_mode('blended')
+    #  return
+
+    # when blinker is on and speed is driving below V_ACC_MIN: blended
+    # we don't want it to switch mode at higher speed, blended may trigger hard brake
+    # if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
+    #  self._set_mode('blended')
+    #  return
+
+    # when at highway cruise and SNG: blended
+    # ensuring blended mode is used because acc is bad at catching SNG lead car
+    # especially those who accel very fast and then brake very hard.
+    # if self._sng_state == SNG_State.going and self._v_cruise_kph >= V_ACC_MIN:
+    #  self._set_mode('blended')
+    #  return
+
+    # when standstill: blended
+    # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
+    if self._has_standstill:
+      self._set_mode('blended')
       return
 
-    # Slow down scenarios: emergency for high urgency, normal for lower urgency
+    # when detecting slow down scenario: blended
+    # e.g. traffic light, curve, stop sign etc.
     if self._has_slow_down:
-      if self._urgency > 0.7:
-        # Emergency: immediate blended mode for high urgency stops
-        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      else:
-        # Normal: blended with urgency-based confidence
-        confidence = min(1.0, self._urgency * 1.5)
-        self._mode_manager.request_mode('blended', confidence=confidence)
+      self._set_mode('blended')
       return
 
-    # Driving slow: use ACC (but not if actively slowing down)
-    if self._has_slowness and not self._has_slow_down:
-      self._mode_manager.request_mode('acc', confidence=0.8)
+    # when detecting lead slow down: blended
+    # use blended for higher braking capability
+    if self._has_dangerous_ttc:
+      self._set_mode('blended')
       return
 
-    # Default: ACC
-    self._mode_manager.request_mode('acc', confidence=0.7)
+    # car driving at speed lower than set speed: acc
+    if self._has_slowness:
+      self._set_mode('acc')
+      return
+
+    self._set_mode('acc')
 
   def _radar_mode(self) -> None:
-    """Radar mode with emergency handling."""
-
-    # EMERGENCY: MPC FCW - immediate blended mode
+    # when mpc fcw crash prob is high
+    # use blended to slow down quickly
     if self._has_mpc_fcw:
-      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      self._set_mode('blended')
       return
 
-    # If lead detected and not in standstill: always use ACC
-    if self._has_lead_filtered and not (self._standstill_count > 3):
-      self._mode_manager.request_mode('acc', confidence=1.0)
+    # If there is a filtered lead, the vehicle is not in standstill, and the lead vehicle's yRel meets the condition,
+    if self._has_lead_filtered and not self._has_standstill:
+      self._set_mode('acc')
       return
 
-    # Slow down scenarios: emergency for high urgency, normal for lower urgency
+    # when blinker is on and speed is driving below V_ACC_MIN: blended
+    # we don't want it to switch mode at higher speed, blended may trigger hard brake
+    # if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
+    #  self._set_mode('blended')
+    #  return
+
+    # when standstill: blended
+    # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
+    if self._has_standstill:
+      self._set_mode('blended')
+      return
+
+    # when detecting slow down scenario: blended
+    # e.g. traffic light, curve, stop sign etc.
     if self._has_slow_down:
-      if self._urgency > 0.7:
-        # Emergency: immediate blended mode for high urgency stops
-        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      else:
-        # Normal: blended with urgency-based confidence
-        confidence = min(1.0, self._urgency * 1.3)
-        self._mode_manager.request_mode('blended', confidence=confidence)
+      self._set_mode('blended')
       return
 
-    # Standstill: use blended
-    if self._standstill_count > 3:
-      self._mode_manager.request_mode('blended', confidence=0.9)
+    # car driving at speed lower than set speed: acc
+    if self._has_slowness:
+      self._set_mode('acc')
       return
 
-    # Driving slow: use ACC (but not if actively slowing down)
-    if self._has_slowness and not self._has_slow_down:
-      self._mode_manager.request_mode('acc', confidence=0.8)
-      return
+    # Nav enabled and distance to upcoming turning is 300 or below
+    # if self._has_nav_instruction:
+    #  self._set_mode('blended')
+    #  return
 
-    # Default: ACC
-    self._mode_manager.request_mode('acc', confidence=0.7)
+    self._set_mode('acc')
+
+  def set_mpc_fcw_crash_cnt(self) -> None:
+    self._mpc_fcw_crash_cnt = self._mpc.crash_cnt
+
+  def _set_mode(self, mode: str) -> None:
+    if self._set_mode_timeout == 0:
+      self._mode = mode
+      if mode == 'blended':
+        self._set_mode_timeout = SET_MODE_TIMEOUT
+
+    if self._set_mode_timeout > 0:
+      self._set_mode_timeout -= 1
 
   def update(self, sm: messaging.SubMaster) -> None:
     self._read_params()
@@ -383,6 +385,6 @@ class DynamicExperimentalController:
     else:
       self._radar_mode()
 
-    self._mode_manager.update()
     self._active = sm['selfdriveState'].experimentalMode and self._enabled
+
     self._frame += 1

sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py

@@ -1,94 +1,248 @@
 import pytest
+import numpy as np
+from openpilot.common.params import Params
 
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants, SNG_State
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, TRAJECTORY_SIZE, STOP_AND_GO_FRAME
 
-class MockLeadOne:
-  def __init__(self, status=0.0):
-    self.status = status
-
-class MockRadarState:
-  def __init__(self, status=0.0):
-    self.leadOne = MockLeadOne(status=status)
+class MockInterp:
+  def __call__(self, x, xp, fp):
+    return np.interp(x, xp, fp)
 
 class MockCarState:
-  def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False):
-    self.vEgo = vEgo
-    self.vCruise = vCruise
+  def __init__(self, v_ego=0., standstill=False, left_blinker=False, right_blinker=False):
+    self.vEgo = v_ego
     self.standstill = standstill
+    self.leftBlinker = left_blinker
+    self.rightBlinker = right_blinker
+
+class MockLeadOne:
+  def __init__(self, status=False, d_rel=0):
+    self.status = status
+    self.dRel = d_rel
 
 class MockModelData:
-  def __init__(self, valid=True):
-    size = 33 if valid else 10  # incomplete if invalid
-    self.position = type("Pos", (), {"x": [0.0] * size})()
-    self.orientation = type("Ori", (), {"x": [0.0] * size})()
+  def __init__(self, x_vals=None, positions=None):
+    self.orientation = type('Orientation', (), {'x': x_vals})()
+    self.position = type('Position', (), {'x': positions})()
 
-class MockSelfDriveState:
-  def __init__(self, experimentalMode=False):
-    self.experimentalMode = experimentalMode
-
-class MockParams:
-  def get_bool(self, name):
-    return True
+class MockControlState:
+  def __init__(self, v_cruise=0):
+    self.vCruise = v_cruise
 
 @pytest.fixture
-def default_sm():
-  sm = {
-    'carState': MockCarState(vEgo=10.0, vCruise=20.0),
-    'radarState': MockRadarState(status=1.0),
-    'modelV2': MockModelData(valid=True),
-    'selfdriveState': MockSelfDriveState(experimentalMode=True),
-  }
-  return sm
+def interp(monkeypatch):
+  mock_interp = MockInterp()
+  monkeypatch.setattr('numpy.interp', mock_interp)
+  return mock_interp
 
 @pytest.fixture
-def mock_cp():
-  class CP:
-    radarUnavailable = False
-  return CP()
+def controller(interp):
+  params = Params()
+  params.put_bool("DynamicExperimentalControl", True)
+  return DynamicExperimentalController()
 
-@pytest.fixture
-def mock_mpc():
-  class MPC:
-    crash_cnt = 0
-  return MPC()
+def test_initial_state(controller):
+  """Test initial state of the controller"""
+  assert controller._mode == 'acc'
+  assert not controller._has_lead
+  assert not controller._has_standstill
+  assert controller._sng_state == SNG_State.off
+  assert not controller._has_lead_filtered
+  assert not controller._has_slow_down
+  assert not controller._has_dangerous_ttc
+  assert not controller._has_mpc_fcw
 
-# Fake Kalman Filter that always returns a given value
-class FakeKalman:
-  def __init__(self, value=1.0):
-    self.value = value
-  def add_data(self, v): pass
-  def get_value(self): return self.value
-  def get_confidence(self): return 1.0
-  def reset_data(self): pass
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_standstill_detection(controller, has_radar):
+  """Test standstill detection and state transitions"""
+  car_state = MockCarState(standstill=True)
+  lead_one = MockLeadOne()
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
+  controls_state = MockControlState()
 
-def test_initial_mode_is_acc(mock_cp, mock_mpc):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  assert controller.mode() == "acc"
+  # Test transition to standstill
+  controller.update(not has_radar, car_state, lead_one, md, controls_state)
+  assert controller._sng_state == SNG_State.stopped
+  assert controller.get_mpc_mode() == 'blended'
 
-def test_standstill_triggers_blended(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['carState'].standstill = True
-  for _ in range(10):
-    controller.update(default_sm)
-  assert controller.mode() == "blended"
+  # Test transition from standstill to moving
+  car_state.standstill = False
+  controller.update(not has_radar, car_state, lead_one, md, controls_state)
+  assert controller._sng_state == SNG_State.going
 
-def test_emergency_blended_on_fcw(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  mock_mpc.crash_cnt = 1  # simulate FCW
-  for _ in range(2):
-    controller.update(default_sm)
-  assert controller.mode() == "blended"
+  # Test complete transition to normal driving
+  for _ in range(STOP_AND_GO_FRAME + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+  assert controller._sng_state == SNG_State.off
 
-def test_radarless_slowdown_triggers_blended(mock_cp, mock_mpc, default_sm):
-  mock_cp.radarUnavailable = True
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_lead_detection(controller, has_radar):
+  """Test lead vehicle detection and filtering"""
+  car_state = MockCarState(v_ego=20)  # 72 kph
+  lead_one = MockLeadOne(status=True, d_rel=50)  # Safe distance
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
+  controls_state = MockControlState(v_cruise=72)
 
-  # Force conditions to simulate slowdown
-  controller._slow_down_filter = FakeKalman(value=1.0)  # Ensure urgency triggers slowdown
-  controller._v_ego_kph = 35.0
-  default_sm['modelV2'] = MockModelData(valid=False)  # Incomplete trajectory
+  # Let moving average stabilize
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
 
-  for _ in range(3):
-    controller.update(default_sm)
+  assert controller._has_lead_filtered
+  expected_mode = 'acc' if has_radar else 'blended'
+  assert controller.get_mpc_mode() == expected_mode
 
-  assert controller.mode() == "blended"
+  # Test lead loss detection
+  lead_one.status = False
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert not controller._has_lead_filtered
+
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_slow_down_detection(controller, has_radar):
+  """Test slow down detection based on trajectory"""
+  car_state = MockCarState(v_ego=10/3.6)  # 10 kph
+  lead_one = MockLeadOne()
+  x_vals = [0] * TRAJECTORY_SIZE
+  positions = [20] * TRAJECTORY_SIZE  # Position within slow down threshold
+  md = MockModelData(x_vals=x_vals, positions=positions)
+  controls_state = MockControlState(v_cruise=30)
+
+  # Test slow down detection
+  for _ in range(WMACConstants.SLOW_DOWN_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_slow_down
+  assert controller.get_mpc_mode() == 'blended'
+
+  # Test slow down recovery
+  positions = [200] * TRAJECTORY_SIZE  # Position outside slow down threshold
+  md = MockModelData(x_vals=x_vals, positions=positions)
+  for _ in range(WMACConstants.SLOW_DOWN_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert not controller._has_slow_down
+
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_dangerous_ttc_detection(controller, has_radar):
+  """Test Time-To-Collision detection and handling"""
+  car_state = MockCarState(v_ego=10)  # 36 kph
+  lead_one = MockLeadOne(status=True)
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
+  controls_state = MockControlState(v_cruise=36)
+
+  # First establish normal conditions with lead
+  lead_one.dRel = 100  # Safe distance
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):  # First establish lead detection
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_lead_filtered  # Verify lead is detected
+
+  # Now test dangerous TTC detection
+  lead_one.dRel = 10  # 10m distance - should trigger dangerous TTC
+  # TTC = dRel/vEgo = 10/10 = 1s (which is less than DANGEROUS_TTC = 2.3s)
+
+  # Need to update multiple times to allow the weighted average to stabilize
+  for _ in range(WMACConstants.DANGEROUS_TTC_WINDOW_SIZE * 2):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_dangerous_ttc, "TTC of 1s should be considered dangerous"
+  expected_mode = 'acc' if has_radar else 'blended'
+  assert controller.get_mpc_mode() == expected_mode, f"Should be in [{expected_mode}] mode with dangerous TTC"
+
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_mode_transitions(controller, has_radar):
+  """Test comprehensive mode transitions under different conditions"""
+  # Initialize with normal driving conditions
+  car_state = MockCarState(v_ego=25)  # 90 kph
+  lead_one = MockLeadOne(status=False)
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[200] * TRAJECTORY_SIZE)
+  controls_state = MockControlState(v_cruise=100)
+
+  def stabilize_filters():
+    """Helper to let all moving averages stabilize"""
+    for _ in range(max(WMACConstants.LEAD_WINDOW_SIZE, WMACConstants.SLOW_DOWN_WINDOW_SIZE,
+                       WMACConstants.DANGEROUS_TTC_WINDOW_SIZE, WMACConstants.MPC_FCW_WINDOW_SIZE) + 1):
+      controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  # Test 1: Normal driving -> ACC mode
+  stabilize_filters()
+  assert controller.get_mpc_mode() == 'acc', "Should be in ACC mode under normal driving conditions"
+
+  # Test 2: Standstill -> Blended mode
+  car_state.standstill = True
+  controller.update(not has_radar, car_state, lead_one, md, controls_state)
+  assert controller.get_mpc_mode() == 'blended', "Should be in blended mode during standstill"
+
+  # Test 3: Lead car appears -> ACC mode
+  car_state = MockCarState(v_ego=20)  # Reset car state
+  lead_one.status = True
+  lead_one.dRel = 50  # Safe distance
+  stabilize_filters()
+  assert not controller._has_dangerous_ttc, "Should not have dangerous TTC"
+  assert controller.get_mpc_mode() == 'acc', "Should be in ACC mode with safe lead distance"
+
+  # Test 4: Dangerous TTC -> Blended mode
+  car_state = MockCarState(v_ego=20)  # 72 kph
+  lead_one.status = True
+  lead_one.dRel = 50  # First establish normal lead detection
+
+  # First establish lead detection
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_lead_filtered  # Verify lead is detected
+
+  # Now create dangerous TTC condition
+  lead_one.dRel = 20  # This creates a TTC of 1s, well below DANGEROUS_TTC
+
+  for _ in range(WMACConstants.DANGEROUS_TTC_WINDOW_SIZE * 2):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_dangerous_ttc, "Should detect dangerous TTC condition"
+  expected_mode = 'acc' if has_radar else 'blended'
+  assert controller.get_mpc_mode() == expected_mode, f"Should be in [{expected_mode}] mode with dangerous TTC"
+
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_mpc_fcw_handling(controller, has_radar):
+  """Test MPC FCW crash count handling and mode transitions"""
+  car_state = MockCarState(v_ego=20)
+  lead_one = MockLeadOne()
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
+  controls_state = MockControlState(v_cruise=72)
+
+  # Test FCW activation
+  controller.set_mpc_fcw_crash_cnt(5)
+  for _ in range(WMACConstants.MPC_FCW_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_mpc_fcw
+  assert controller.get_mpc_mode() == 'blended'
+
+  # Test FCW recovery
+  controller.set_mpc_fcw_crash_cnt(0)
+  for _ in range(WMACConstants.MPC_FCW_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert not controller._has_mpc_fcw
+
+def test_radar_unavailable_handling(controller):
+  """Test behavior transitions between radar available and unavailable states"""
+  car_state = MockCarState(v_ego=27.78)  # 100 kph
+  lead_one = MockLeadOne(status=True, d_rel=50)
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
+  controls_state = MockControlState(v_cruise=100)
+
+  # Test with radar available
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):
+    controller.update(False, car_state, lead_one, md, controls_state)
+  radar_mode = controller.get_mpc_mode()
+
+  # Test with radar unavailable
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):
+    controller.update(True, car_state, lead_one, md, controls_state)
+  radarless_mode = controller.get_mpc_mode()
+
+  assert radar_mode is not None
+  assert radarless_mode is not None

sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py

@@ -10,8 +10,6 @@ import numpy as np
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.selfdrive.modeld.constants import ModelConstants
 
-from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle
-
 LAT_PLAN_MIN_IDX = 5
 LATERAL_LAG_MOD = 0.0 # seconds, modifies how far in the future we look ahead for the lateral plan
 
@@ -43,9 +41,8 @@ def get_lookahead_value(future_vals, current_val):
   return min_val
 
 
-class LatControlTorqueExtBase(LagdToggle):
+class LatControlTorqueExtBase:
   def __init__(self, lac_torque, CP, CP_SP):
-    LagdToggle.__init__(self)
     self.model_v2 = None
     self.model_valid = False
     self.use_steering_angle = lac_torque.use_steering_angle

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -8,7 +8,6 @@ See the LICENSE.md file in the root directory for more details.
 from cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
-from openpilot.sunnypilot.models.helpers import get_active_bundle
 
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
 
@@ -16,12 +15,6 @@ DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimen
 class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, mpc):
     self.dec = DynamicExperimentalController(CP, mpc)
-    model_bundle = get_active_bundle()
-    self.generation = model_bundle.generation if model_bundle is not None else None
-
-  @property
-  def mlsim(self) -> bool:
-    return self.generation == 11
 
   def get_mpc_mode(self) -> str | None:
     if not self.dec.active():

sunnypilot/selfdrive/controls/lib/param_store.py

@@ -22,16 +22,14 @@ class ParamStore:
 
     self.keys = universal_params + brand_params
     self.values = {}
-    self.cached_params_list: list[capnp.lib.capnp._DynamicStructBuilder] | None = None
 
   def update(self, params: Params) -> None:
-    old_values = dict(self.values)
     self.values = {k: params.get(k, encoding='utf8') or "0" for k in self.keys}
-    if old_values != self.values:
-      self.cached_params_list = None
 
   def publish(self) -> list[capnp.lib.capnp._DynamicStructBuilder]:
-    if self.cached_params_list is None:
-      # TODO-SP: Why are we doing a list instead of a dictionary here?
-      self.cached_params_list = [custom.CarControlSP.Param(key=k, value=self.values[k]) for k in self.keys]
-    return self.cached_params_list
+    params_list: list[capnp.lib.capnp._DynamicStructBuilder] = []
+
+    for k in self.keys:
+      params_list.append(custom.CarControlSP.Param(key=k, value=self.values[k]))
+
+    return params_list

system/manager/manager.py

@@ -56,9 +56,7 @@ def manager_init() -> None:
     ("DeviceBootMode", "0"),
     ("DynamicExperimentalControl", "0"),
     ("HyundaiLongitudinalTuning", "0"),
-    ("InteractivityTimeout", "0"),
     ("LagdToggle", "1"),
-    ("LagdToggledelay", "0.2"),
     ("Mads", "1"),
     ("MadsMainCruiseAllowed", "1"),
     ("MadsSteeringMode", "0"),
@@ -70,6 +68,13 @@ def manager_init() -> None:
     ("ModelManager_ModelsCache", ""),
     ("NeuralNetworkLateralControl", "0"),
     ("QuietMode", "0"),
+    ("EnableHkgTuningAngleSmoothingFactor", "1"),
+    ("HkgTuningAngleMinTorqueReductionGain", "10"),
+    ("HkgTuningAngleActiveTorqueReductionGain", "60"),
+    ("HkgTuningAngleMaxTorqueReductionGain", "100"),
+    ("HkgTuningOverridingCycles", "17"),
+    ("HkgAngleLiveTuning", "0"),
+    ("AdasDrvEcuInterceptorEnabled", "0"),
   ]
 
   # device boot mode

system/manager/process_config.py

@@ -114,7 +114,7 @@ procs = [
 
   PythonProcess("sensord", "system.sensord.sensord", only_onroad, enabled=not PC),
   NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)),
-  PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad),
+  PythonProcess("soundd", "selfdrive.ui.soundd", and_(only_onroad, not_joystick)),
   PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad),
   NativeProcess("_pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False),
   PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad),

tools/joystick/README.md

@@ -5,6 +5,10 @@
 With joystick_control, you can connect your laptop to your comma device over the network and debug controls using a joystick or keyboard.
 joystick_control uses [inputs](https://pypi.org/project/inputs) which supports many common gamepads and joysticks.
 
+## Note
+You might need to install [xow](https://github.com/medusalix/xow) to use the Xbox One controller on comma device. 
+Even though **xone** is recommended on the xow page, **xow** is the one that works with the comma device.
+
 ## Usage
 
 The car must be off, and openpilot must be offroad before starting `joystick_control`.

tools/joystick/joystick_control.py

@@ -3,7 +3,10 @@ import os
 import argparse
 import threading
 import numpy as np
-from inputs import UnpluggedError, get_gamepad
+try:
+  import pygame as pg
+except ImportError:
+  print("pygame is not installed. Please install it with 'uv pip install pygame'")
 
 from cereal import messaging
 from openpilot.common.params import Params
@@ -11,7 +14,10 @@ from openpilot.common.realtime import Ratekeeper
 from openpilot.system.hardware import HARDWARE
 from openpilot.tools.lib.kbhit import KBHit
 
-EXPO = 0.4
+# Set SDL environment variable to avoid using a video driver for headless operation
+os.environ["SDL_VIDEODRIVER"] = "dummy"
+
+EXPO = 0.4  # Exponential factor for joystick response curve
 
 
 class Keyboard:
@@ -40,60 +46,122 @@ class Keyboard:
     return True
 
 
-class Joystick:
+class PyGameJoystick:
   def __init__(self):
-    # This class supports a PlayStation 5 DualSense controller on the comma 3X
-    # TODO: find a way to get this from API or detect gamepad/PC, perhaps "inputs" doesn't support it
-    self.cancel_button = 'BTN_NORTH'  # BTN_NORTH=X/triangle
-    if HARDWARE.get_device_type() == 'pc':
-      accel_axis = 'ABS_Z'
-      steer_axis = 'ABS_RX'
-      # TODO: once the longcontrol API is finalized, we can replace this with outputting gas/brake and steering
-      self.flip_map = {'ABS_RZ': accel_axis}
-    else:
-      accel_axis = 'ABS_RX'
-      steer_axis = 'ABS_Z'
-      self.flip_map = {'ABS_RY': accel_axis}
+    # Initialize pygame and joystick subsystem
+    pg.init()
+    if not pg.joystick.get_init():
+      pg.joystick.init()
 
-    self.min_axis_value = {accel_axis: 0., steer_axis: 0.}
-    self.max_axis_value = {accel_axis: 255., steer_axis: 255.}
-    self.axes_values = {accel_axis: 0., steer_axis: 0.}
-    self.axes_order = [accel_axis, steer_axis]
+    # Find connected joysticks
+    joystick_count = pg.joystick.get_count()
+    if joystick_count == 0:
+      print("No joysticks found. Please connect a controller.")
+      exit(1)
+
+    # Initialize the first joystick
+    self.joystick = pg.joystick.Joystick(0)
+    self.joystick.init()
+
+    print(f"Using joystick: {self.joystick.get_name()}")
+    print(f"Number of axes: {self.joystick.get_numaxes()}")
+    print(f"Number of buttons: {self.joystick.get_numbuttons()}")
+    print(f"Number of hats: {self.joystick.get_numhats()}")
+
+    # This supports PlayStation and Xbox controllers on different platforms
+    if HARDWARE.get_device_type() == 'pc':
+      # Xbox mapping on PC
+      self.accel_axis = 5  # Right trigger
+      self.brake_axis = 4  # Left trigger
+      self.steer_axis = 0  # Left stick horizontal
+      self.cancel_button = 3  # Y/Triangle button
+    else:
+      # PlayStation mapping on comma device
+      self.accel_axis = 5  # R2
+      self.brake_axis = 4  # L2
+      self.steer_axis = 0  # Left stick horizontal
+      self.cancel_button = 3  # Triangle button
+
+    # Configure for adaptive mappings based on detected controller
+    controller_name = self.joystick.get_name().lower()
+    if "xbox" in controller_name:
+      print("Xbox controller detected, using Xbox mappings")
+      self.accel_axis = 5  # Right trigger (RT)
+      self.brake_axis = 4  # Left trigger (LT)
+      self.cancel_button = 3  # Y button
+    elif "playstation" in controller_name or "dual" in controller_name:
+      print("PlayStation controller detected, using PlayStation mappings")
+      self.accel_axis = 5  # R2
+      self.brake_axis = 4  # L2
+      self.cancel_button = 3  # Triangle
+
+    # Initialize values
+    self.axes_values = {'gb': 0., 'steer': 0.}  # Maintain same keys as Keyboard class
+    self.axes_order = ['gb', 'steer']  # Match expected format
     self.cancel = False
+    self.deadzone = 0.03  # 3% deadzone for noisy joysticks
+
+    # Process events once to clear the event queue
+    pg.event.pump()
 
   def update(self):
+    # Process pygame events
     try:
-      joystick_event = get_gamepad()[0]
-    except (OSError, UnpluggedError):
-      self.axes_values = dict.fromkeys(self.axes_values, 0.)
+      for event in pg.event.get():
+        if event.type == pg.JOYDEVICEREMOVED:
+          if event.instance_id == self.joystick.get_instance_id():
+            print("Joystick disconnected!")
+            self.axes_values = {'gb': 0., 'steer': 0.}
+            return False
+
+        elif event.type == pg.JOYBUTTONDOWN:
+          if event.button == self.cancel_button:
+            self.cancel = True
+
+        elif event.type == pg.JOYBUTTONUP:
+          if event.button == self.cancel_button:
+            self.cancel = False
+    except Exception as e:
+      print(f"Error processing events: {e}")
       return False
 
-    event = (joystick_event.code, joystick_event.state)
+    # Read current joystick state directly
+    try:
+      if not self.joystick.get_init():
+        print("Joystick not initialized")
+        return False
 
-    # flip left trigger to negative accel
-    if event[0] in self.flip_map:
-      event = (self.flip_map[event[0]], -event[1])
+      # Read steering (left stick horizontal)
+      steer_raw = self.joystick.get_axis(self.steer_axis) * -1
+      steer = steer_raw if abs(steer_raw) > self.deadzone else 0.0
 
-    if event[0] == self.cancel_button:
-      if event[1] == 1:
-        self.cancel = True
-      elif event[1] == 0:   # state 0 is falling edge
-        self.cancel = False
-    elif event[0] in self.axes_values:
-      self.max_axis_value[event[0]] = max(event[1], self.max_axis_value[event[0]])
-      self.min_axis_value[event[0]] = min(event[1], self.min_axis_value[event[0]])
+      # Read gas (right trigger)
+      accel_raw = self.joystick.get_axis(self.accel_axis)
+      # Convert from [-1, 1] to [0, 1] range (triggers often start at -1 when not pressed)
+      accel = (accel_raw + 1) / 2 if self.accel_axis in [4, 5] else accel_raw
+      accel = accel if accel > self.deadzone else 0.0
 
-      norm = -float(np.interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.]))
-      norm = norm if abs(norm) > 0.03 else 0.  # center can be noisy, deadzone of 3%
-      self.axes_values[event[0]] = EXPO * norm ** 3 + (1 - EXPO) * norm  # less action near center for fine control
-    else:
+      # Read brake (left trigger)
+      brake_raw = self.joystick.get_axis(self.brake_axis)
+      # Convert from [-1, 1] to [0, 1] range (triggers often start at -1 when not pressed)
+      brake = (brake_raw + 1) / 2 if self.brake_axis in [4, 5] else brake_raw
+      brake = brake if brake > self.deadzone else 0.0
+
+      # Apply expo for steering
+      self.axes_values['steer'] = EXPO * steer**3 + (1 - EXPO) * steer  # Apply expo for fine control
+
+      # Calculate combined gas/brake value for output [-1, 1] where negative is brake
+      self.axes_values['gb'] = accel - brake
+
+    except Exception as e:
+      print(f"Error reading joystick: {e}")
+      self.axes_values = {'gb': 0., 'steer': 0.}
       return False
     return True
 
 
 def send_thread(joystick):
   pm = messaging.PubMaster(['testJoystick'])
-
   rk = Ratekeeper(100, print_delay_threshold=None)
 
   while True:
@@ -105,7 +173,6 @@ def send_thread(joystick):
     joystick_msg.testJoystick.axes = [joystick.axes_values[ax] for ax in joystick.axes_order]
 
     pm.send('testJoystick', joystick_msg)
-
     rk.keep_time()
 
 
@@ -117,13 +184,12 @@ def joystick_control_thread(joystick):
 
 
 def main():
-  joystick_control_thread(Joystick())
-
+  joystick_control_thread(PyGameJoystick())
 
 if __name__ == '__main__':
   parser = argparse.ArgumentParser(description='Publishes events from your joystick to control your car.\n' +
                                                'openpilot must be offroad before starting joystick_control. This tool supports ' +
-                                               'a PlayStation 5 DualSense controller on the comma 3X.',
+                                               'PlayStation and Xbox controllers on various platforms.',
                                    formatter_class=argparse.ArgumentDefaultsHelpFormatter)
   parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a joystick')
   args = parser.parse_args()
@@ -139,9 +205,12 @@ if __name__ == '__main__':
     print('Buttons')
     print('- `R`: Resets axes')
     print('- `C`: Cancel cruise control')
+    joystick_control_thread(Keyboard())
   else:
-    print('Using joystick, make sure to run cereal/messaging/bridge on your device if running over the network!')
-    print('If not running on a comma device, the mapping may need to be adjusted.')
-
-  joystick = Keyboard() if args.keyboard else Joystick()
-  joystick_control_thread(joystick)
+    print('Using pygame joystick')
+    print('Standard controller mapping:')
+    print('- Left stick: Steering')
+    print('- Right trigger (R2): Gas')
+    print('- Left trigger (L2): Brake')
+    print('- Triangle/Y button: Cancel')
+    joystick_control_thread(PyGameJoystick())

tools/joystick/joystickd.py

@@ -19,18 +19,24 @@ def joystickd_thread():
   CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
   VM = VehicleModel(CP)
 
-  sm = messaging.SubMaster(['carState', 'onroadEvents', 'liveParameters', 'selfdriveState', 'testJoystick'], frequency=1. / DT_CTRL)
+  sm = messaging.SubMaster(['carState', 'onroadEvents', 'liveParameters', 'selfdriveState', 'testJoystick', 'selfdriveStateSP'], frequency=1. / DT_CTRL)
   pm = messaging.PubMaster(['carControl', 'controlsState'])
 
   rk = Ratekeeper(100, print_delay_threshold=None)
   while 1:
     sm.update(0)
+    ss_sp = sm['selfdriveStateSP']
+    _lat_active = False
+    if ss_sp.mads.available:
+      _lat_active = ss_sp.mads.active
+    else:
+      _lat_active = sm['selfdriveState'].active
 
     cc_msg = messaging.new_message('carControl')
     cc_msg.valid = True
     CC = cc_msg.carControl
     CC.enabled = sm['selfdriveState'].enabled
-    CC.latActive = sm['selfdriveState'].active and not sm['carState'].steerFaultTemporary and not sm['carState'].steerFaultPermanent
+    CC.latActive = _lat_active and not sm['carState'].steerFaultTemporary and not sm['carState'].steerFaultPermanent
     CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in sm['onroadEvents']) and CP.openpilotLongitudinalControl
     CC.cruiseControl.cancel = sm['carState'].cruiseState.enabled and (not CC.enabled or not CP.pcmCruise)
     CC.hudControl.leadDistanceBars = 2

tools/plotjuggler/layouts/analyzing-panda-block-angle-hkg.xml

@@ -0,0 +1,131 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget parent="main_window" name="Main Window">
+  <Tab tab_name="actuator data" containers="1">
+   <Container>
+    <DockSplitter sizes="0.25;0.25;0.25;0.25" count="4" orientation="-">
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-1.050000" top="1.050000" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#17becf" name="/carControl/actuators/torque"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-385.132391" top="536.899115" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#1f77b4" name="/carControl/actuators/steeringAngleDeg"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-0.364800" top="14.956815" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#ff7f0e" name="/carState/vEgoRaw"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-0.208517" top="0.149191" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#f14cc1" name="/carControl/actuators/curvature"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <Tab tab_name="steering messages (CAN)" containers="1">
+   <Container>
+    <DockSplitter sizes="0.200218;0.200218;0.199129;0.200218;0.200218" count="5" orientation="-">
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-189.000000" top="189.000000" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#1ac938" name="/can/2/LKAS_ALT/LKAS_ANGLE_CMD"/>
+       <curve color="#9467bd" name="/can/128/LKAS_ALT/LKAS_ANGLE_CMD"/>
+       <curve color="#ff0e1c" name="/can/192/LKAS_ALT/LKAS_ANGLE_CMD"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-6.200000" top="254.200000" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#f14cc1" name="/can/128/LKAS_ALT/LKAS_ANGLE_MAX_TORQUE"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-0.025000" top="1.025000" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#d62728" name="/carState/steeringPressed"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="460.325000" top="1874.675000" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#1f77b4" name="/pandaStates/0/safetyTxBlocked"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-0.025000" top="1.025000" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#f14cc1" name="/can/1/CCNC_0x161/DAW_ICON"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <Tab tab_name="Understanding Torque" containers="1">
+   <Container>
+    <DockSplitter sizes="0.5;0.5" count="2" orientation="-">
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-6.175000" top="253.175000" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#1f77b4" name="/can/1/LFA_ALT/LKAS_ANGLE_MAX_TORQUE"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
+       <range bottom="-41.182501" top="42.082498" right="590.696728" left="429.901019"/>
+       <limitY/>
+       <curve color="#1ac938" name="/carState/steeringTorqueEps"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <currentTabIndex index="1"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <Plugins>
+  <plugin ID="DataLoad CSV">
+   <default time_axis="" delimiter="0"/>
+  </plugin>
+  <plugin ID="DataLoad MCAP"/>
+  <plugin ID="DataLoad Rlog"/>
+  <plugin ID="DataLoad ULog"/>
+  <plugin ID="Cereal Subscriber"/>
+  <plugin ID="UDP Server"/>
+  <plugin ID="WebSocket Server"/>
+  <plugin ID="ZMQ Subscriber"/>
+  <plugin ID="Fast Fourier Transform"/>
+  <plugin ID="Quaternion to RPY"/>
+  <plugin ID="Reactive Script Editor">
+   <library code="--[[ Helper function to create a series from arrays&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix:     prefix of the timeseries, before the index of the array&#xa; suffix_X:   suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y:   suffix to complete the name of the series containing the Y value&#xa; timestamp:   usually the tracker_time variable&#xa;              &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa;   /trajectory/node.{X}/position/x&#xa;   /trajectory/node.{X}/position/y&#xa;   &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa;   new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa;   CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa;  &#xa;  --- clear previous values&#xa;  new_series:clear()&#xa;  &#xa;  --- Append points to new_series&#xa;  index = 0&#xa;  while(true) do&#xa;&#xa;    x = index;&#xa;    -- if not nil, get the X coordinate from a series&#xa;    if suffix_X ~= nil then &#xa;      series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa;      if series_x == nil then break end&#xa;      x = series_x:atTime(timestamp)&#x9; &#xa;    end&#xa;    &#xa;    series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa;    if series_y == nil then break end &#xa;    y = series_y:atTime(timestamp)&#xa;    &#xa;    new_series:push_back(x,y)&#xa;    index = index+1&#xa;  end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa;  -- GetSeriesNames(9 is a built-in function&#xa;  all_names = GetSeriesNames()&#xa;  filtered_names = {}&#xa;  for i, name in ipairs(all_names)  do&#xa;    -- check the prefix&#xa;    if name:find(prefix, 1, #prefix) then&#xa;      table.insert(filtered_names, name);&#xa;    end&#xa;  end&#xa;  return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa;  &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa;  -- use C++ indeces, not Lua indeces&#xa;  for index=0, series:size()-1 do&#xa;    x,y = series:at(index)&#xa;    series:set(index, x + delta_x, y + delta_y)&#xa;  end&#xa;end&#xa;"/>
+   <scripts/>
+  </plugin>
+  <plugin ID="CSV Exporter"/>
+ </Plugins>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <customMathEquations/>
+ <snippets/>
+ <!-- - - - - - - - - - - - - - - -->
+</root>
+

tools/plotjuggler/layouts/analyzing-torque-angle-hkg.xml

@@ -0,0 +1,209 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget parent="main_window" name="Main Window">
+  <Tab tab_name="actuator data" containers="1">
+   <Container>
+    <DockSplitter orientation="-" sizes="0.25;0.25;0.25;0.25" count="4">
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-1.050000" top="1.050000" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#17becf" name="/carControl/actuators/torque"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-453.043646" top="538.555487" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#1f77b4" name="/carControl/actuators/steeringAngleDeg"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-0.647461" top="26.545898" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#ff7f0e" name="/carState/vEgoRaw"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-0.209157" top="0.175448" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#f14cc1" name="/carControl/actuators/curvature"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <Tab tab_name="steering messages (CAN)" containers="1">
+   <Container>
+    <DockSplitter orientation="-" sizes="0.200218;0.200218;0.199129;0.200218;0.200218" count="5">
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-189.000000" top="189.000000" right="723.115449" left="6.371879"/>
+       <limitY/>
+       <curve color="#b6ff00" name="/can/128/LKAS_ALT/LKAS_ANGLE_CMD"/>
+       <curve color="#00fab2" name="/can/0/LKAS_ALT/LKAS_ANGLE_CMD"/>
+       <curve color="#d62728" name="/can/192/LKAS_ALT/LKAS_ANGLE_CMD"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-6.250000" top="256.250000" right="722.420439" left="6.497277"/>
+       <limitY/>
+       <curve color="#00ffe8" name="/can/128/LKAS_ALT/LKAS_ANGLE_MAX_TORQUE"/>
+       <curve color="#e5fd00" name="/can/0/LKAS_ALT/LKAS_ANGLE_MAX_TORQUE"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-4.399729" top="4.056632" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#ff7f0e" name="IMU_LatAccelVal_ms^2_roll_compensated"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-0.025000" top="1.025000" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#d62728" name="/carState/steeringPressed"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-0.025000" top="1.025000" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#f14cc1" name="/can/1/CCNC_0x161/DAW_ICON"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <Tab tab_name="Understanding Torque" containers="1">
+   <Container>
+    <DockSplitter orientation="-" sizes="0.5;0.5" count="2">
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-6.250000" top="256.250000" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#1f77b4" name="/can/1/LFA_ALT/LKAS_ANGLE_MAX_TORQUE"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
+       <range bottom="-42.062501" top="78.162503" right="722.420439" left="0.000000"/>
+       <limitY/>
+       <curve color="#1ac938" name="/carState/steeringTorqueEps"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <currentTabIndex index="1"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <Plugins>
+  <plugin ID="DataLoad CSV">
+   <default delimiter="0" time_axis=""/>
+  </plugin>
+  <plugin ID="DataLoad MCAP"/>
+  <plugin ID="DataLoad Rlog"/>
+  <plugin ID="DataLoad ULog"/>
+  <plugin ID="Cereal Subscriber"/>
+  <plugin ID="UDP Server"/>
+  <plugin ID="WebSocket Server"/>
+  <plugin ID="ZMQ Subscriber"/>
+  <plugin ID="Fast Fourier Transform"/>
+  <plugin ID="Quaternion to RPY"/>
+  <plugin ID="Reactive Script Editor">
+   <library code="--[[ Helper function to create a series from arrays&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix:     prefix of the timeseries, before the index of the array&#xa; suffix_X:   suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y:   suffix to complete the name of the series containing the Y value&#xa; timestamp:   usually the tracker_time variable&#xa;              &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa;   /trajectory/node.{X}/position/x&#xa;   /trajectory/node.{X}/position/y&#xa;   &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa;   new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa;   CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa;  &#xa;  --- clear previous values&#xa;  new_series:clear()&#xa;  &#xa;  --- Append points to new_series&#xa;  index = 0&#xa;  while(true) do&#xa;&#xa;    x = index;&#xa;    -- if not nil, get the X coordinate from a series&#xa;    if suffix_X ~= nil then &#xa;      series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa;      if series_x == nil then break end&#xa;      x = series_x:atTime(timestamp)&#x9; &#xa;    end&#xa;    &#xa;    series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa;    if series_y == nil then break end &#xa;    y = series_y:atTime(timestamp)&#xa;    &#xa;    new_series:push_back(x,y)&#xa;    index = index+1&#xa;  end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa;  -- GetSeriesNames(9 is a built-in function&#xa;  all_names = GetSeriesNames()&#xa;  filtered_names = {}&#xa;  for i, name in ipairs(all_names)  do&#xa;    -- check the prefix&#xa;    if name:find(prefix, 1, #prefix) then&#xa;      table.insert(filtered_names, name);&#xa;    end&#xa;  end&#xa;  return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa;  &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa;  -- use C++ indeces, not Lua indeces&#xa;  for index=0, series:size()-1 do&#xa;    x,y = series:at(index)&#xa;    series:set(index, x + delta_x, y + delta_y)&#xa;  end&#xa;end&#xa;"/>
+   <scripts/>
+  </plugin>
+  <plugin ID="CSV Exporter"/>
+ </Plugins>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <customMathEquations>
+  <snippet name="Desired lateral accel (roll compensated)">
+   <global></global>
+   <function>return (value * v1 ^ 2) - (v2 * 9.81)</function>
+   <linked_source>/controlsState/desiredCurvature</linked_source>
+   <additional_sources>
+    <v1>/carState/vEgo</v1>
+    <v2>/liveParameters/roll</v2>
+   </additional_sources>
+  </snippet>
+  <snippet name="Actual lateral accel (roll compensated)">
+   <global></global>
+   <function>return (value * v1 ^ 2) - (v2 * 9.81)</function>
+   <linked_source>/controlsState/curvature</linked_source>
+   <additional_sources>
+    <v1>/carState/vEgo</v1>
+    <v2>/liveParameters/roll</v2>
+   </additional_sources>
+  </snippet>
+  <snippet name="IMU_LatAccelVal_ms^2_roll_compensated">
+   <global></global>
+   <function>if (v1 == 0 and v2 == 1) then
+  return  (value * -9.8) - (v3 * 9.81)
+end
+--return 0
+  return  (value * -9.8) - (v3 * 9.81)</function>
+   <linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
+   <additional_sources>
+    <v1>/carState/steeringPressed</v1>
+    <v2>/carControl/latActive</v2>
+    <v3>/liveParameters/roll</v3>
+   </additional_sources>
+  </snippet>
+  <snippet name="IMU_LatAccelVal_Ms^3">
+   <global></global>
+   <function>return  value * -9.8</function>
+   <linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
+   <additional_sources>
+    <v1>/carState/steeringPressed</v1>
+    <v2>/carControl/latActive</v2>
+   </additional_sources>
+  </snippet>
+  <snippet name="IMU_LatAccelVal_Ms^2">
+   <global></global>
+   <function>if (v1 == 0 and v2 == 1) then
+  return  value * -9.8
+end
+return 0</function>
+   <linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
+   <additional_sources>
+    <v1>/carState/steeringPressed</v1>
+    <v2>/carControl/latActive</v2>
+   </additional_sources>
+  </snippet>
+  <snippet name="roll compensated lateral acceleration">
+   <global></global>
+   <function>if (v3 == 0 and v4 == 1) then
+  return  (value * v1 ^ 2) - (v2 * 9.81)
+end
+return 0</function>
+   <linked_source>/controlsState/curvature</linked_source>
+   <additional_sources>
+    <v1>/carState/vEgo</v1>
+    <v2>/liveParameters/roll</v2>
+    <v3>/carState/steeringPressed</v3>
+    <v4>/carControl/latActive</v4>
+   </additional_sources>
+  </snippet>
+  <snippet name="IMU_LatAccelVal_ms^2">
+   <global></global>
+   <function>return  value * -9.8</function>
+   <linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
+   <additional_sources>
+    <v1>/carState/steeringPressed</v1>
+    <v2>/carControl/latActive</v2>
+   </additional_sources>
+  </snippet>
+ </customMathEquations>
+ <snippets/>
+ <!-- - - - - - - - - - - - - - - -->
+</root>
+

tools/plotjuggler/layouts/hkg_angle_control.xml

@@ -0,0 +1,281 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget name="Main Window" parent="main_window">
+  <Tab containers="1" tab_name="tab1">
+   <Container>
+    <DockSplitter sizes="0.500397;0.499603" count="2" orientation="-">
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" mode="TimeSeries" style="Lines">
+       <range top="256.250000" left="0.000000" right="421.102293" bottom="-6.250000"/>
+       <limitY/>
+       <curve color="#1ac938" name="/can/1/LFA_ALT/LKAS_ANGLE_MAX_TORQUE"/>
+       <curve color="#17becf" name="max torque(calc)">
+        <transform name="Moving Average" alias="max torque(calc)[Moving Average]">
+         <options compensate_offset="true" value="10"/>
+        </transform>
+       </curve>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" flip_y="false" mode="TimeSeries" style="Lines">
+       <range top="2.776497" left="0.000000" right="421.102293" bottom="-2.918548"/>
+       <limitY/>
+       <curve color="#f14cc1" name="desired lat accel"/>
+       <curve color="#888888" name="zero"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <currentTabIndex index="0"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <Plugins>
+  <plugin ID="DataLoad CSV">
+   <default time_axis="" delimiter="0"/>
+  </plugin>
+  <plugin ID="DataLoad MCAP"/>
+  <plugin ID="DataLoad Rlog"/>
+  <plugin ID="DataLoad ULog"/>
+  <plugin ID="Cereal Subscriber"/>
+  <plugin ID="UDP Server"/>
+  <plugin ID="WebSocket Server"/>
+  <plugin ID="ZMQ Subscriber"/>
+  <plugin ID="Fast Fourier Transform"/>
+  <plugin ID="Quaternion to RPY"/>
+  <plugin ID="Reactive Script Editor">
+   <library code="--[[ Helper function to create a series from arrays&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix:     prefix of the timeseries, before the index of the array&#xa; suffix_X:   suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y:   suffix to complete the name of the series containing the Y value&#xa; timestamp:   usually the tracker_time variable&#xa;              &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa;   /trajectory/node.{X}/position/x&#xa;   /trajectory/node.{X}/position/y&#xa;   &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa;   new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa;   CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa;  &#xa;  --- clear previous values&#xa;  new_series:clear()&#xa;  &#xa;  --- Append points to new_series&#xa;  index = 0&#xa;  while(true) do&#xa;&#xa;    x = index;&#xa;    -- if not nil, get the X coordinate from a series&#xa;    if suffix_X ~= nil then &#xa;      series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa;      if series_x == nil then break end&#xa;      x = series_x:atTime(timestamp)&#x9; &#xa;    end&#xa;    &#xa;    series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa;    if series_y == nil then break end &#xa;    y = series_y:atTime(timestamp)&#xa;    &#xa;    new_series:push_back(x,y)&#xa;    index = index+1&#xa;  end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa;  -- GetSeriesNames(9 is a built-in function&#xa;  all_names = GetSeriesNames()&#xa;  filtered_names = {}&#xa;  for i, name in ipairs(all_names)  do&#xa;    -- check the prefix&#xa;    if name:find(prefix, 1, #prefix) then&#xa;      table.insert(filtered_names, name);&#xa;    end&#xa;  end&#xa;  return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa;  &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa;  -- use C++ indeces, not Lua indeces&#xa;  for index=0, series:size()-1 do&#xa;    x,y = series:at(index)&#xa;    series:set(index, x + delta_x, y + delta_y)&#xa;  end&#xa;end&#xa;"/>
+   <scripts/>
+  </plugin>
+  <plugin ID="CSV Exporter"/>
+ </Plugins>
+ <!-- - - - - - - - - - - - - - - -->
+ <previouslyLoaded_Datafiles>
+  <fileInfo prefix="" filename="../tmpa_e8kwpj.rlog">
+   <selected_datasources value=""/>
+  </fileInfo>
+ </previouslyLoaded_Datafiles>
+ <!-- - - - - - - - - - - - - - - -->
+ <customMathEquations>
+  <snippet name="zero">
+   <global>min=0
+max=250
+max_from_speed=96
+
+rate_lim = 500
+
+la_deadzone = 0.38
+
+k1=200
+k2=20
+k3=1.0
+k4=1
+k5=10
+
+old = 0
+
+function sign(number)
+    return number > 0 and 1 or (number == 0 and 0 or -1)
+end
+
+function apply_rate_limit(old, new, limit)
+    return math.min(math.max(new, old - limit), old + limit)
+end
+
+function apply_deadzone(val, deadzone)
+    if math.abs(val) &lt;= deadzone then
+        return 0.0
+    elseif val &lt; 0.0 then
+        return val + deadzone
+    else
+        return val - deadzone
+    end
+end</global>
+   <function>return 0</function>
+   <linked_source>/carState/aEgo</linked_source>
+  </snippet>
+  <snippet name="max torque lj adj">
+   <global>min=0
+max=250
+max_from_speed=96
+
+k1=200
+k2=30
+k3=1    
+k4=1
+k5=10
+
+function sign(number)
+    return number > 0 and 1 or (number == 0 and 0 or -1)
+end</global>
+   <function>return 250 - value * 20</function>
+   <linked_source>desired lateral jark</linked_source>
+  </snippet>
+  <snippet name="ang_cmd rate">
+   <global>firstX = 0
+firstY = 0
+is_first = true
+secondX = 0
+secondY = 0
+is_second = false</global>
+   <function>-- Wait for initial values
+if (is_first) then
+  is_first = false
+  is_second = true
+  firstX = time
+  firstY = value
+end
+
+if (is_second) then
+  is_second = false
+  secondX = time
+  secondY = value
+end
+
+-- Central derivative: dy/dx ~= f(x+delta_x)-f(x-delta_x)/(2*delta_x)
+dx = time - firstX
+dy = value - firstY
+-- Increment
+firstX = secondX
+firstY = secondY
+secondX = time
+secondY = value
+
+return dy/dx</function>
+   <linked_source>/can/1/LFA_ALT/LKAS_ANGLE_CMD</linked_source>
+  </snippet>
+  <snippet name="max torque(calc)">
+   <global>min=0
+max=250
+max_from_speed=96
+
+rate_lim = 500
+
+la_deadzone = 0.38
+
+k1=200
+k2=20
+k3=1.0
+k4=1
+k5=10
+
+old = 0
+
+function sign(number)
+    return number > 0 and 1 or (number == 0 and 0 or -1)
+end
+
+function apply_rate_limit(old, new, limit)
+    return math.min(math.max(new, old - limit), old + limit)
+end
+
+function apply_deadzone(val, deadzone)
+    if math.abs(val) &lt;= deadzone then
+        return 0.0
+    elseif val &lt; 0.0 then
+        return val + deadzone
+    else
+        return val - deadzone
+    end
+end</global>
+   <function>la = apply_deadzone(v2, la_deadzone)
+lj = v3
+
+if la == 0.0 then
+    lj = 0.0
+end
+
+fla = math.min(math.abs(k1 * la)^k3, max)
+flj = math.min(math.abs(k2 * lj)^k4, max)
+
+out = fla
+
+flv = math.min(max_from_speed, k5 * v4)
+
+out = out + flv
+
+out = math.max(math.min(out, max), min)
+
+if sign(la) == sign(lj) then
+    out = out - flj
+else
+    out = out + flj
+end
+
+
+if v5 == 1.0 then
+    out = 0.0
+end
+
+out = math.max(math.min(out, max), min)
+out = apply_rate_limit(old, out, rate_lim)
+old = out
+
+return out</function>
+   <linked_source>/can/1/LFA_ALT/LKAS_ANGLE_CMD</linked_source>
+   <additional_sources>
+    <v1>ang_cmd rate</v1>
+    <v2>desired lat accel</v2>
+    <v3>desired lateral jark</v3>
+    <v4>/carState/vEgo</v4>
+    <v5>/can/1/LFA_ALT/LKAS_ANGLE_ACTIVE</v5>
+   </additional_sources>
+  </snippet>
+  <snippet name="desired lateral jark">
+   <global>firstX = 0
+firstY = 0
+is_first = true
+secondX = 0
+secondY = 0
+is_second = false</global>
+   <function>-- Wait for initial values
+if (is_first) then
+  is_first = false
+  is_second = true
+  firstX = time
+  firstY = value
+end
+
+if (is_second) then
+  is_second = false
+  secondX = time
+  secondY = value
+end
+
+-- Central derivative: dy/dx ~= f(x+delta_x)-f(x-delta_x)/(2*delta_x)
+dx = time - firstX
+dy = value - firstY
+-- Increment
+firstX = secondX
+firstY = secondY
+secondX = time
+secondY = value
+
+return dy/dx</function>
+   <linked_source>desired lat accel</linked_source>
+  </snippet>
+  <snippet name="abs(des la accel)">
+   <global></global>
+   <function>return math.abs(value)</function>
+   <linked_source>desired lat accel</linked_source>
+  </snippet>
+  <snippet name="desired lat accel">
+   <global></global>
+   <function>return value * v1^2</function>
+   <linked_source>/controlsState/desiredCurvature</linked_source>
+   <additional_sources>
+    <v1>/carState/vEgo</v1>
+   </additional_sources>
+  </snippet>
+  <snippet name="abs(ang_cmd)">
+   <global></global>
+   <function>return math.abs(value)</function>
+   <linked_source>/can/1/LFA_ALT/LKAS_ANGLE_CMD</linked_source>
+  </snippet>
+ </customMathEquations>
+ <snippets/>
+ <!-- - - - - - - - - - - - - - - -->
+</root>
+

tools/plotjuggler/layouts/safety-limits-angle-kkg.xml

@@ -0,0 +1,175 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget parent="main_window" name="Main Window">
+  <Tab tab_name="tab1" containers="1">
+   <Container>
+    <DockSplitter sizes="0.25;0.25;0.25;0.25" count="4" orientation="-">
+     <DockArea name="...">
+      <plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
+       <range bottom="-3.582051" right="1431.113121" top="5.314632" left="5.322399"/>
+       <limitY/>
+       <curve name="Actual lateral accel (roll compensated)" color="#1ac938"/>
+       <curve name="Desired lateral accel (roll compensated)" color="#ff7f0e"/>
+       <curve name="IMU_LatAccelVal_ms^2" color="#f14cc1"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
+       <range bottom="-3.741271" right="1431.113121" top="3.756006" left="5.322399"/>
+       <limitY/>
+       <curve name="roll compensated lateral acceleration" color="#ff7f0e"/>
+       <curve name="IMU_LatAccelVal_Ms^2" color="#1ac938"/>
+       <curve name="IMU_LatAccelVal_ms^2_roll_compensated" color="#9467bd"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
+       <range bottom="-0.025000" right="1431.113121" top="1.025000" left="5.322399"/>
+       <limitY/>
+       <curve name="/carState/steeringPressed" color="#0097ff"/>
+       <curve name="/carOutput/actuatorsOutput/torque" color="#d62728"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
+       <range bottom="-1.660728" right="1431.113121" top="67.942958" left="5.322399"/>
+       <limitY/>
+       <curve name="/carState/vEgo" color="#f14cc1">
+        <transform name="Scale/Offset" alias="/carState/vEgo[Scale/Offset]">
+         <options value_scale="2.23694" time_offset="0" value_offset="0"/>
+        </transform>
+       </curve>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <Tab tab_name="tab2" containers="1">
+   <Container>
+    <DockSplitter sizes="0.5;0.5" count="2" orientation="-">
+     <DockArea name="...">
+      <plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
+       <range bottom="30595.900000" right="1431.113121" top="34884.100000" left="5.322399"/>
+       <limitY/>
+       <curve name="/can/1/IMU_01_10ms/IMU_RollRtVal" color="#17becf"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
+       <range bottom="-0.058795" right="1431.113121" top="0.072448" left="5.322399"/>
+       <limitY/>
+       <curve name="/liveParameters/roll" color="#bcbd22"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <currentTabIndex index="1"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <Plugins>
+  <plugin ID="DataLoad CSV">
+   <default time_axis="" delimiter="0"/>
+  </plugin>
+  <plugin ID="DataLoad MCAP"/>
+  <plugin ID="DataLoad Rlog"/>
+  <plugin ID="DataLoad ULog"/>
+  <plugin ID="Cereal Subscriber"/>
+  <plugin ID="UDP Server"/>
+  <plugin ID="WebSocket Server"/>
+  <plugin ID="ZMQ Subscriber"/>
+  <plugin ID="Fast Fourier Transform"/>
+  <plugin ID="Quaternion to RPY"/>
+  <plugin ID="Reactive Script Editor">
+   <library code="--[[ Helper function to create a series from arrays&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix:     prefix of the timeseries, before the index of the array&#xa; suffix_X:   suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y:   suffix to complete the name of the series containing the Y value&#xa; timestamp:   usually the tracker_time variable&#xa;              &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa;   /trajectory/node.{X}/position/x&#xa;   /trajectory/node.{X}/position/y&#xa;   &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa;   new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa;   CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa;  &#xa;  --- clear previous values&#xa;  new_series:clear()&#xa;  &#xa;  --- Append points to new_series&#xa;  index = 0&#xa;  while(true) do&#xa;&#xa;    x = index;&#xa;    -- if not nil, get the X coordinate from a series&#xa;    if suffix_X ~= nil then &#xa;      series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa;      if series_x == nil then break end&#xa;      x = series_x:atTime(timestamp)&#x9; &#xa;    end&#xa;    &#xa;    series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa;    if series_y == nil then break end &#xa;    y = series_y:atTime(timestamp)&#xa;    &#xa;    new_series:push_back(x,y)&#xa;    index = index+1&#xa;  end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa;  -- GetSeriesNames(9 is a built-in function&#xa;  all_names = GetSeriesNames()&#xa;  filtered_names = {}&#xa;  for i, name in ipairs(all_names)  do&#xa;    -- check the prefix&#xa;    if name:find(prefix, 1, #prefix) then&#xa;      table.insert(filtered_names, name);&#xa;    end&#xa;  end&#xa;  return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa;  &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa;  -- use C++ indeces, not Lua indeces&#xa;  for index=0, series:size()-1 do&#xa;    x,y = series:at(index)&#xa;    series:set(index, x + delta_x, y + delta_y)&#xa;  end&#xa;end&#xa;"/>
+   <scripts/>
+  </plugin>
+  <plugin ID="CSV Exporter"/>
+ </Plugins>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <customMathEquations>
+  <snippet name="IMU_LatAccelVal_ms^2">
+   <global></global>
+   <function>return  value * -9.8</function>
+   <linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
+   <additional_sources>
+    <v1>/carState/steeringPressed</v1>
+    <v2>/carControl/latActive</v2>
+   </additional_sources>
+  </snippet>
+  <snippet name="roll compensated lateral acceleration">
+   <global></global>
+   <function>if (v3 == 0 and v4 == 1) then
+  return  (value * v1 ^ 2) - (v2 * 9.81)
+end
+return 0</function>
+   <linked_source>/controlsState/curvature</linked_source>
+   <additional_sources>
+    <v1>/carState/vEgo</v1>
+    <v2>/liveParameters/roll</v2>
+    <v3>/carState/steeringPressed</v3>
+    <v4>/carControl/latActive</v4>
+   </additional_sources>
+  </snippet>
+  <snippet name="IMU_LatAccelVal_Ms^2">
+   <global></global>
+   <function>if (v1 == 0 and v2 == 1) then
+  return  value * -9.8
+end
+return 0</function>
+   <linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
+   <additional_sources>
+    <v1>/carState/steeringPressed</v1>
+    <v2>/carControl/latActive</v2>
+   </additional_sources>
+  </snippet>
+  <snippet name="IMU_LatAccelVal_Ms^3">
+   <global></global>
+   <function>return  value * -9.8</function>
+   <linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
+   <additional_sources>
+    <v1>/carState/steeringPressed</v1>
+    <v2>/carControl/latActive</v2>
+   </additional_sources>
+  </snippet>
+  <snippet name="IMU_LatAccelVal_ms^2_roll_compensated">
+   <global></global>
+   <function>
+
+if (v1 == 0 and v2 == 1) then
+  return  (value * -9.8) - (v3 * 9.81)
+end
+return 0</function>
+   <linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
+   <additional_sources>
+    <v1>/carState/steeringPressed</v1>
+    <v2>/carControl/latActive</v2>
+    <v3>/liveParameters/roll</v3>
+   </additional_sources>
+  </snippet>
+  <snippet name="Actual lateral accel (roll compensated)">
+   <global></global>
+   <function>return (value * v1 ^ 2) - (v2 * 9.81)</function>
+   <linked_source>/controlsState/curvature</linked_source>
+   <additional_sources>
+    <v1>/carState/vEgo</v1>
+    <v2>/liveParameters/roll</v2>
+   </additional_sources>
+  </snippet>
+  <snippet name="Desired lateral accel (roll compensated)">
+   <global></global>
+   <function>return (value * v1 ^ 2) - (v2 * 9.81)</function>
+   <linked_source>/controlsState/desiredCurvature</linked_source>
+   <additional_sources>
+    <v1>/carState/vEgo</v1>
+    <v2>/liveParameters/roll</v2>
+   </additional_sources>
+  </snippet>
+ </customMathEquations>
+ <snippets/>
+ <!-- - - - - - - - - - - - - - - -->
+</root>
+