mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-02 12:02:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

offset

Browse Source
This commit is contained in:
firestar5683
2026-03-06 12:50:48 -06:00
parent 8eb6d5b447
commit aa36b2133e
4 changed files with 74 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
frogpilot/common/testing_grounds.py
+19 -4
View File
@@ -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,
frogpilot/system/the_pond/assets/components/tools/testing_ground.css
+10
View File
@@ -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);
}
frogpilot/system/the_pond/assets/components/tools/testing_ground.js
+9
View File
@@ -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() {
<div class="testingGroundDetails">
<h3>${getSelectedSlot().name}</h3>
${() => getSelectedSlot().description ? html`<p>${getSelectedSlot().description}</p>` : ""}
${() => isOffsetTestingSlot(getSelectedSlot())
? html`<p class="testingGroundPlotsLink">${Link("/plots", html`Open Plots to watch I term`, null, "testingGroundInlineLink")}</p>`
: ""
}
</div>
` : ""}
selfdrive/car/gm/interface.py
+36 -5
View File
@@ -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: