diff --git a/frogpilot/common/testing_grounds.py b/frogpilot/common/testing_grounds.py index 1c7f4617f..d1cb66fdf 100644 --- a/frogpilot/common/testing_grounds.py +++ b/frogpilot/common/testing_grounds.py @@ -40,10 +40,25 @@ TESTING_GROUNDS_SLOT_DEFINITIONS = ( }, { "id": TESTING_GROUND_2, - "name": "Unused", - "description": "", - "aLabel": "A", - "bLabel": "B", + "name": "Offset Testing", + "description": "All GM non-linear torque cars: sigmoid d offset sweep for both left/right (-0.100 to +0.100). A proper offset should see near-zero I term in Plots.", + "aLabel": "A - Installed tune", + "bLabel": "B - d -0.030", + "cLabel": "C - d -0.040", + "dLabel": "D - d -0.050", + "eLabel": "E - d -0.060", + "fLabel": "F - d -0.070", + "gLabel": "G - d -0.080", + "hLabel": "H - d -0.090", + "iLabel": "I - d -0.100", + "jLabel": "J - d +0.030", + "kLabel": "K - d +0.040", + "lLabel": "L - d +0.050", + "mLabel": "M - d +0.060", + "nLabel": "N - d +0.070", + "oLabel": "O - d +0.080", + "pLabel": "P - d +0.090", + "qLabel": "Q - d +0.100", }, { "id": TESTING_GROUND_3, diff --git a/frogpilot/system/the_pond/assets/components/tools/testing_ground.css b/frogpilot/system/the_pond/assets/components/tools/testing_ground.css index 2a79e583b..50a84878e 100644 --- a/frogpilot/system/the_pond/assets/components/tools/testing_ground.css +++ b/frogpilot/system/the_pond/assets/components/tools/testing_ground.css @@ -76,6 +76,16 @@ margin: 0 0 var(--margin-xs); } +.testingGroundPlotsLink { + margin-top: var(--margin-xs); +} + +.testingGroundInlineLink { + color: var(--main-fg); + font-size: var(--font-size-sm); + text-decoration: underline; +} + .testingGroundMuted { color: var(--text-muted); } diff --git a/frogpilot/system/the_pond/assets/components/tools/testing_ground.js b/frogpilot/system/the_pond/assets/components/tools/testing_ground.js index 37738edc9..541709e67 100644 --- a/frogpilot/system/the_pond/assets/components/tools/testing_ground.js +++ b/frogpilot/system/the_pond/assets/components/tools/testing_ground.js @@ -1,4 +1,5 @@ import { html, reactive } from "https://esm.sh/@arrow-js/core" +import { Link } from "/assets/components/router.js" const state = reactive({ loading: true, @@ -45,6 +46,10 @@ function getActiveSlot() { return slots.find((slot) => slotId(slot) === activeId) || null } +function isOffsetTestingSlot(slot) { + return String(slot?.id || "").trim() === "2" +} + function getVariantLabels(slot) { if (!slot || typeof slot !== "object") { return { A: "A" } @@ -257,6 +262,10 @@ export function TestingGround() {

${getSelectedSlot().name}

${() => getSelectedSlot().description ? html`

${getSelectedSlot().description}

` : ""} + ${() => isOffsetTestingSlot(getSelectedSlot()) + ? html`

${Link("/plots", html`Open Plots to watch I term`, null, "testingGroundInlineLink")}

` + : "" + }
` : ""} diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 0b317a97c..e798e5327 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -10,6 +10,7 @@ from openpilot.selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus, GMFlags, CC_ONLY_CAR, SDGM_CAR, ASCM_INT, CC_REGEN_PADDLE_CAR, set_red_panda_canbus from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LateralAccelFromTorqueCallbackType, get_friction_threshold from openpilot.selfdrive.controls.lib.drive_helpers import get_friction +from openpilot.frogpilot.common.testing_grounds import testing_ground ButtonType = car.CarState.ButtonEvent.Type FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type @@ -70,11 +71,33 @@ NON_LINEAR_TORQUE_PARAMS = { }, } +NON_LINEAR_D_SWEEP_VARIANT_OFFSETS = { + "B": -0.030, + "C": -0.040, + "D": -0.050, + "E": -0.060, + "F": -0.070, + "G": -0.080, + "H": -0.090, + "I": -0.100, + "J": 0.030, + "K": 0.040, + "L": 0.050, + "M": 0.060, + "N": 0.070, + "O": 0.080, + "P": 0.090, + "Q": 0.100, +} + class CarInterface(CarInterfaceBase): def __init__(self, CP, FPCP, CarController, CarState): super().__init__(CP, FPCP, CarController, CarState) self.steer_offset = 0.0 + self._siglin_cache_key = None + self._siglin_cache_torque_values = None + self._siglin_cache_lataccel_values = None @staticmethod def get_pid_accel_limits(CP, current_speed, cruise_speed): @@ -94,6 +117,12 @@ class CarInterface(CarInterfaceBase): return CarInterfaceBase.get_steer_feedforward_default def get_lataccel_torque_siglin(self) -> float: + active_slot, active_variant = testing_ground.selection() + d_offset = NON_LINEAR_D_SWEEP_VARIANT_OFFSETS.get(active_variant, 0.0) if active_slot == testing_ground.id_2 else 0.0 + cache_key = (self.CP.carFingerprint, active_slot, active_variant, d_offset) + + if self._siglin_cache_key == cache_key and self._siglin_cache_torque_values is not None and self._siglin_cache_lataccel_values is not None: + return self._siglin_cache_torque_values, self._siglin_cache_lataccel_values def torque_from_lateral_accel_siglin_func(lateral_acceleration: float) -> float: # The "lat_accel vs torque" relationship is assumed to be the sum of "sigmoid + linear" curves @@ -104,6 +133,7 @@ class CarInterface(CarInterfaceBase): # Left is positive side_key = "left" if lateral_acceleration >= 0 else "right" a, b, c, d = non_linear_torque_params[side_key] + d += d_offset sig_input = a * lateral_acceleration sig = np.sign(sig_input) * (1 / (1 + exp(-fabs(sig_input))) - 0.5) steer_torque = (sig * b) + (lateral_acceleration * c) + d @@ -112,13 +142,15 @@ class CarInterface(CarInterfaceBase): lataccel_values = np.arange(-5.0, 5.0, 0.01) torque_values = [torque_from_lateral_accel_siglin_func(x) for x in lataccel_values] assert min(torque_values) < -1 and max(torque_values) > 1, "The torque values should cover the range [-1, 1]" - return torque_values, lataccel_values + self._siglin_cache_key = cache_key + self._siglin_cache_torque_values = torque_values + self._siglin_cache_lataccel_values = lataccel_values + return self._siglin_cache_torque_values, self._siglin_cache_lataccel_values def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: if self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS: - torque_values, lataccel_values = self.get_lataccel_torque_siglin() - def torque_from_lateral_accel_siglin(lateral_acceleration: float, torque_params: car.CarParams.LateralTorqueTuning): + torque_values, lataccel_values = self.get_lataccel_torque_siglin() return float(np.interp(lateral_acceleration, lataccel_values, torque_values) + self.steer_offset) return torque_from_lateral_accel_siglin else: @@ -128,9 +160,8 @@ class CarInterface(CarInterfaceBase): def lateral_accel_from_torque(self) -> LateralAccelFromTorqueCallbackType: if self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS: - torque_values, lataccel_values = self.get_lataccel_torque_siglin() - def lateral_accel_from_torque_siglin(torque: float, torque_params: car.CarParams.LateralTorqueTuning): + torque_values, lataccel_values = self.get_lataccel_torque_siglin() return np.interp(torque - self.steer_offset, torque_values, lataccel_values) return lateral_accel_from_torque_siglin else: