mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-12 21:02:13 +08:00
Updates
This commit is contained in:
@@ -11,6 +11,10 @@ let initialized = false
|
||||
let pollHandle = null
|
||||
const POLL_INTERVAL_MS = 1000
|
||||
|
||||
function isLongitudinalManeuversRouteActive() {
|
||||
return window.location.pathname === "/longitudinal_maneuvers"
|
||||
}
|
||||
|
||||
function safeNumber(value, fallback = 0) {
|
||||
const n = Number(value)
|
||||
return Number.isFinite(n) ? n : fallback
|
||||
@@ -54,6 +58,16 @@ function ensurePolling() {
|
||||
if (pollHandle) return
|
||||
|
||||
const poll = async () => {
|
||||
if (!isLongitudinalManeuversRouteActive()) {
|
||||
pollHandle = null
|
||||
return
|
||||
}
|
||||
|
||||
if (document.visibilityState !== "visible") {
|
||||
pollHandle = setTimeout(poll, POLL_INTERVAL_MS)
|
||||
return
|
||||
}
|
||||
|
||||
await fetchStatus()
|
||||
pollHandle = setTimeout(poll, POLL_INTERVAL_MS)
|
||||
}
|
||||
@@ -167,4 +181,3 @@ export function LongitudinalManeuvers() {
|
||||
</div>
|
||||
`
|
||||
}
|
||||
|
||||
|
||||
@@ -187,6 +187,74 @@
|
||||
margin-top: var(--margin-sm);
|
||||
}
|
||||
|
||||
.qualityDetail {
|
||||
color: var(--text-muted);
|
||||
font-size: calc(var(--font-size-sm) * 0.95);
|
||||
}
|
||||
|
||||
.qualitySummaryGrid {
|
||||
display: grid;
|
||||
gap: var(--gap-sm);
|
||||
margin-top: var(--margin-base);
|
||||
}
|
||||
|
||||
.qualitySummaryRow {
|
||||
background: rgba(255, 255, 255, 0.03);
|
||||
border: 1px solid var(--track-color);
|
||||
border-radius: var(--border-radius-sm);
|
||||
padding: var(--padding-sm) var(--padding-base);
|
||||
}
|
||||
|
||||
.qualitySentence {
|
||||
align-items: baseline;
|
||||
display: flex;
|
||||
flex-wrap: wrap;
|
||||
font-size: calc(var(--font-size-lg) * 1.05);
|
||||
font-weight: var(--font-weight-demi-bold);
|
||||
gap: var(--gap-xs);
|
||||
line-height: 1.3;
|
||||
margin: 0;
|
||||
}
|
||||
|
||||
.qualityTone {
|
||||
font-size: calc(var(--font-size-xl) * 1.05);
|
||||
font-weight: var(--font-weight-extra-bold);
|
||||
letter-spacing: 0.01em;
|
||||
text-transform: uppercase;
|
||||
}
|
||||
|
||||
.qualityTone-great {
|
||||
color: #66e3ad;
|
||||
}
|
||||
|
||||
.qualityTone-good {
|
||||
color: #7fc2ff;
|
||||
}
|
||||
|
||||
.qualityTone-fair {
|
||||
color: #f8c36d;
|
||||
}
|
||||
|
||||
.qualityTone-poor {
|
||||
color: #ff8ca5;
|
||||
}
|
||||
|
||||
.qualityTone-na {
|
||||
color: #d7dcec;
|
||||
}
|
||||
|
||||
.qualityMethodNote {
|
||||
color: var(--text-muted);
|
||||
font-size: calc(var(--font-size-sm) * 0.92);
|
||||
margin-top: var(--margin-sm);
|
||||
}
|
||||
|
||||
.qualityUserGuidance {
|
||||
color: var(--text-muted);
|
||||
font-size: calc(var(--font-size-sm) * 0.95);
|
||||
margin: var(--margin-sm) 0 0 0;
|
||||
}
|
||||
|
||||
@media only screen and (max-width: 768px) and (orientation: portrait) {
|
||||
.plotCard,
|
||||
.plotCharts {
|
||||
|
||||
@@ -12,12 +12,45 @@ const state = reactive({
|
||||
let initialized = false
|
||||
let pollHandle = null
|
||||
let lastTimestamp = 0
|
||||
let visibilityListenerAttached = false
|
||||
|
||||
const MAX_POINTS = 240
|
||||
const POLL_INTERVAL_MS = 500
|
||||
const POLL_INTERVAL_MS = 750
|
||||
const SVG_WIDTH = 1000
|
||||
const SVG_HEIGHT = 260
|
||||
const ADVANCED_TERMS_KEY = "plotsShowAdvancedTerms"
|
||||
const QUALITY_WINDOW_SECONDS = 30
|
||||
const QUALITY_MIN_SAMPLES = 12
|
||||
|
||||
const LATERAL_QUALITY_CONFIG = {
|
||||
desiredKey: "desiredLateralAccel",
|
||||
actualKey: "actualLateralAccel",
|
||||
minSpeedMps: 2.0,
|
||||
minDemand: 0.08,
|
||||
requireControlsActive: true,
|
||||
allowLowDemandFallback: true,
|
||||
fallbackMinSpeedMps: 5.0,
|
||||
fallbackMinPeakDemand: 0.12,
|
||||
great: 0.15,
|
||||
good: 0.30,
|
||||
fair: 0.50,
|
||||
}
|
||||
|
||||
const LONGITUDINAL_QUALITY_CONFIG = {
|
||||
desiredKey: "desiredLongitudinalAccel",
|
||||
actualKey: "actualLongitudinalAccel",
|
||||
minSpeedMps: 0.0,
|
||||
minDemand: 0.08,
|
||||
requireControlsActive: true,
|
||||
requireLongitudinalControlActive: true,
|
||||
allowLowDemandFallback: false,
|
||||
applyPersistenceRules: true,
|
||||
warnError: 0.35,
|
||||
severeError: 0.70,
|
||||
great: 0.20,
|
||||
good: 0.35,
|
||||
fair: 0.55,
|
||||
}
|
||||
|
||||
function isPlotsRouteActive() {
|
||||
return window.location.pathname === "/plots"
|
||||
@@ -48,6 +81,9 @@ function formatSourceLabel(kind, source) {
|
||||
}
|
||||
|
||||
if (normalizedKind === "longitudinal") {
|
||||
if (normalizedSource.includes("atarget")) return "Planner target acceleration + measured acceleration"
|
||||
if (normalizedSource.includes("pid sum")) return "PID term sum + measured acceleration"
|
||||
if (normalizedSource.includes("caroutput")) return "Final accel command + measured acceleration"
|
||||
if (normalizedSource.includes("livelocationkalman")) return "Control output + calibrated acceleration"
|
||||
if (normalizedSource === "controlsstate") return "Planner target output"
|
||||
}
|
||||
@@ -64,6 +100,134 @@ function formatSourceLabel(kind, source) {
|
||||
return "Live control signal"
|
||||
}
|
||||
|
||||
function percentile(sortedValues, percentileValue) {
|
||||
const values = Array.isArray(sortedValues) ? sortedValues : []
|
||||
if (!values.length) return 0
|
||||
const p = Math.max(0, Math.min(1, Number(percentileValue)))
|
||||
const index = (values.length - 1) * p
|
||||
const lower = Math.floor(index)
|
||||
const upper = Math.ceil(index)
|
||||
if (lower === upper) return values[lower]
|
||||
const weight = index - lower
|
||||
return values[lower] * (1 - weight) + values[upper] * weight
|
||||
}
|
||||
|
||||
function formatQualityLabel(label) {
|
||||
const text = String(label || "").trim()
|
||||
return text || "N/A"
|
||||
}
|
||||
|
||||
function computeMatchQuality(samples, config) {
|
||||
const safeSamples = Array.isArray(samples) ? samples : []
|
||||
if (safeSamples.length < 2) {
|
||||
return { label: "N/A", value: null, detail: "Waiting for data" }
|
||||
}
|
||||
|
||||
const latestTs = toNumber(safeSamples[safeSamples.length - 1]?.timestamp, 0)
|
||||
const cutoffTs = latestTs > 0 ? latestTs - QUALITY_WINDOW_SECONDS : 0
|
||||
const recentSamples = safeSamples.filter((sample) => toNumber(sample?.timestamp, 0) >= cutoffTs)
|
||||
|
||||
const demandEligibleSamples = recentSamples.filter((sample) => {
|
||||
if (config.requireControlsActive && !sample?.controlsActive) return false
|
||||
if (config.requireLongitudinalControlActive && !sample?.longitudinalControlActive) return false
|
||||
const speed = Math.abs(toNumber(sample?.speed, 0))
|
||||
const desired = Math.abs(toNumber(sample?.[config.desiredKey], 0))
|
||||
|
||||
const speedOk = config.minSpeedMps <= 0 ? true : speed >= config.minSpeedMps
|
||||
const demandOk = config.minDemand <= 0 ? true : desired >= config.minDemand
|
||||
return speedOk && demandOk
|
||||
})
|
||||
|
||||
let eligibleSamples = demandEligibleSamples
|
||||
let usedLowDemandFallback = false
|
||||
const allowLowDemandFallback = config.allowLowDemandFallback !== false
|
||||
if (allowLowDemandFallback && eligibleSamples.length < QUALITY_MIN_SAMPLES && recentSamples.length >= QUALITY_MIN_SAMPLES) {
|
||||
const totalSpeed = recentSamples.reduce((sum, sample) => sum + Math.abs(toNumber(sample?.speed, 0)), 0)
|
||||
const avgSpeed = recentSamples.length > 0 ? (totalSpeed / recentSamples.length) : 0
|
||||
const peakDemand = recentSamples.reduce((peak, sample) => {
|
||||
const desired = Math.abs(toNumber(sample?.[config.desiredKey], 0))
|
||||
return Math.max(peak, desired)
|
||||
}, 0)
|
||||
|
||||
const fallbackMinSpeed = Math.max(0, toNumber(config.fallbackMinSpeedMps, 0))
|
||||
const fallbackMinPeakDemand = Math.max(0, toNumber(config.fallbackMinPeakDemand, 0))
|
||||
if (avgSpeed >= fallbackMinSpeed && peakDemand >= fallbackMinPeakDemand) {
|
||||
eligibleSamples = recentSamples
|
||||
usedLowDemandFallback = true
|
||||
}
|
||||
}
|
||||
|
||||
if (eligibleSamples.length < QUALITY_MIN_SAMPLES) {
|
||||
if (allowLowDemandFallback) {
|
||||
return {
|
||||
label: "N/A",
|
||||
value: null,
|
||||
detail: `Need ${QUALITY_MIN_SAMPLES} engaged samples (${eligibleSamples.length} eligible / ${recentSamples.length} total)`,
|
||||
}
|
||||
}
|
||||
|
||||
return {
|
||||
label: "N/A",
|
||||
value: null,
|
||||
detail: `Need ${QUALITY_MIN_SAMPLES} demand samples (${eligibleSamples.length} demand / ${recentSamples.length} total)`,
|
||||
}
|
||||
}
|
||||
|
||||
const errors = eligibleSamples
|
||||
.map((sample) => Math.abs(toNumber(sample?.[config.desiredKey], 0) - toNumber(sample?.[config.actualKey], 0)))
|
||||
.sort((a, b) => a - b)
|
||||
|
||||
if (!errors.length) {
|
||||
return { label: "N/A", value: null, detail: "No eligible samples" }
|
||||
}
|
||||
|
||||
const p50 = percentile(errors, 0.50)
|
||||
const p90 = percentile(errors, 0.90)
|
||||
const robustError = (0.7 * p50) + (0.3 * p90)
|
||||
|
||||
const sampleSummary = `${eligibleSamples.length} samples / ${QUALITY_WINDOW_SECONDS}s${usedLowDemandFallback ? ", low-demand fallback" : ""}`
|
||||
|
||||
if (config.applyPersistenceRules) {
|
||||
const warnThreshold = Math.max(config.warnError || 0.35, config.good || 0.35)
|
||||
const severeThreshold = Math.max(config.severeError || 0.70, config.fair || 0.55)
|
||||
const warnFrac = errors.filter((value) => value > warnThreshold).length / errors.length
|
||||
const severeFrac = errors.filter((value) => value > severeThreshold).length / errors.length
|
||||
|
||||
let label = "Poor"
|
||||
if (robustError <= config.great && warnFrac <= 0.10 && severeFrac <= 0.03) label = "Great"
|
||||
else if (robustError <= config.good && warnFrac <= 0.22 && severeFrac <= 0.08) label = "Good"
|
||||
else if (robustError <= config.fair && warnFrac <= 0.40 && severeFrac <= 0.18) label = "Fair"
|
||||
|
||||
const warnPct = Math.round(warnFrac * 100)
|
||||
const severePct = Math.round(severeFrac * 100)
|
||||
return {
|
||||
label,
|
||||
value: robustError,
|
||||
detail: `${sampleSummary}, ${warnPct}% > ${formatValue(warnThreshold)} and ${severePct}% > ${formatValue(severeThreshold)}`,
|
||||
}
|
||||
}
|
||||
|
||||
let label = "Poor"
|
||||
if (robustError <= config.great) label = "Great"
|
||||
else if (robustError <= config.good) label = "Good"
|
||||
else if (robustError <= config.fair) label = "Fair"
|
||||
|
||||
return {
|
||||
label,
|
||||
value: robustError,
|
||||
detail: sampleSummary,
|
||||
}
|
||||
}
|
||||
|
||||
function qualityToneClass(label) {
|
||||
const normalized = String(label || "").toLowerCase()
|
||||
if (normalized === "great") return "qualityTone-great"
|
||||
if (normalized === "good") return "qualityTone-good"
|
||||
if (normalized === "fair") return "qualityTone-fair"
|
||||
if (normalized === "poor") return "qualityTone-poor"
|
||||
return "qualityTone-na"
|
||||
}
|
||||
|
||||
function stopPolling() {
|
||||
if (!pollHandle) return
|
||||
clearTimeout(pollHandle)
|
||||
@@ -77,6 +241,9 @@ function pushSample(payload) {
|
||||
lastTimestamp = timestamp
|
||||
state.samples.push({
|
||||
timestamp,
|
||||
speed: toNumber(payload.speed),
|
||||
controlsActive: !!payload.controlsActive,
|
||||
longitudinalControlActive: !!payload.longitudinalControlActive,
|
||||
desiredLateralAccel: toNumber(payload.desiredLateralAccel),
|
||||
actualLateralAccel: toNumber(payload.actualLateralAccel),
|
||||
desiredLongitudinalAccel: toNumber(payload.desiredLongitudinalAccel),
|
||||
@@ -121,6 +288,11 @@ function ensurePolling() {
|
||||
return
|
||||
}
|
||||
|
||||
if (document.visibilityState !== "visible") {
|
||||
pollHandle = setTimeout(poll, POLL_INTERVAL_MS)
|
||||
return
|
||||
}
|
||||
|
||||
if (!state.paused) {
|
||||
try {
|
||||
await fetchLiveData()
|
||||
@@ -367,6 +539,24 @@ async function initialize() {
|
||||
} finally {
|
||||
ensurePolling()
|
||||
}
|
||||
|
||||
if (!visibilityListenerAttached) {
|
||||
visibilityListenerAttached = true
|
||||
document.addEventListener("visibilitychange", () => {
|
||||
if (document.visibilityState !== "visible") {
|
||||
stopPolling()
|
||||
return
|
||||
}
|
||||
|
||||
if (isPlotsRouteActive() && !state.paused) {
|
||||
fetchLiveData().catch((error) => {
|
||||
state.error = error?.message || String(error)
|
||||
state.loading = false
|
||||
})
|
||||
ensurePolling()
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
export function LivePlots() {
|
||||
@@ -384,7 +574,10 @@ export function LivePlots() {
|
||||
|
||||
<section class="plotCard plotStatusCard">
|
||||
<p class="plotDescription">
|
||||
Live comparison view for tuning diagnostics. If desired tracks actual closely, tuning is likely good. Large divergence often points to model mismatch or limits.
|
||||
Live comparison view for tuning diagnostics. These scores are a quick health check, not a final verdict. Short spikes from bumps, lane changes, traffic transitions, and manual inputs can temporarily lower a score.
|
||||
</p>
|
||||
<p class="qualityUserGuidance">
|
||||
Work in progress: use this as trend guidance over time. If you see a brief "Poor," keep driving and look for repeated behavior across multiple situations before changing tune values.
|
||||
</p>
|
||||
|
||||
<div class="plotStatusGrid">
|
||||
@@ -394,6 +587,34 @@ export function LivePlots() {
|
||||
<p><strong>Samples:</strong> ${state.samples.length}</p>
|
||||
</div>
|
||||
|
||||
${() => {
|
||||
const lateralQuality = computeMatchQuality(state.samples, LATERAL_QUALITY_CONFIG)
|
||||
const longQuality = computeMatchQuality(state.samples, LONGITUDINAL_QUALITY_CONFIG)
|
||||
|
||||
return html`
|
||||
<div class="qualitySummaryGrid">
|
||||
<div class="qualitySummaryRow">
|
||||
<p class="qualitySentence">
|
||||
Your lateral tuning is
|
||||
<span class="qualityTone ${qualityToneClass(lateralQuality.label)}">${formatQualityLabel(lateralQuality.label)}</span>
|
||||
</p>
|
||||
<p class="qualityDetail">
|
||||
${lateralQuality.value === null ? lateralQuality.detail : `${formatValue(lateralQuality.value)} m/s² error (${lateralQuality.detail})`}
|
||||
</p>
|
||||
</div>
|
||||
<div class="qualitySummaryRow">
|
||||
<p class="qualitySentence">
|
||||
Your longitudinal tuning is
|
||||
<span class="qualityTone ${qualityToneClass(longQuality.label)}">${formatQualityLabel(longQuality.label)}</span>
|
||||
</p>
|
||||
<p class="qualityDetail">
|
||||
${longQuality.value === null ? longQuality.detail : `${formatValue(longQuality.value)} m/s² error (${longQuality.detail})`}
|
||||
</p>
|
||||
</div>
|
||||
</div>
|
||||
`
|
||||
}}
|
||||
|
||||
<div class="plotActions">
|
||||
<button class="plotButton" @click="${togglePaused}">
|
||||
${state.paused ? "Resume Live" : "Pause Live"}
|
||||
@@ -405,6 +626,9 @@ export function LivePlots() {
|
||||
|
||||
${state.error ? html`<p class="plotError"><strong>Error:</strong> ${state.error}</p>` : ""}
|
||||
${state.live?.lastError ? html`<p class="plotError"><strong>Source Error:</strong> ${state.live.lastError}</p>` : ""}
|
||||
<p class="qualityMethodNote">
|
||||
Match rating uses a 30-second rolling window. Lateral prefers true steering-demand moments, and longitudinal also checks how much of the window stays above error limits so brief spikes are less likely to mark "Poor."
|
||||
</p>
|
||||
</section>
|
||||
|
||||
<div class="plotCharts">
|
||||
|
||||
@@ -24,6 +24,10 @@ let rebootNoticeShown = false
|
||||
const POLL_INTERVAL_MS = 1000
|
||||
const ADVANCED_OPTIONS_KEY = "softwareShowAdvancedOptions"
|
||||
|
||||
function isUpdateRouteActive() {
|
||||
return window.location.pathname === "/manage_updates"
|
||||
}
|
||||
|
||||
function shortHash(value) {
|
||||
const text = String(value || "").trim()
|
||||
return text ? text.slice(0, 10) : "Unknown"
|
||||
@@ -167,6 +171,16 @@ function ensurePolling() {
|
||||
if (pollHandle) return
|
||||
|
||||
const poll = async () => {
|
||||
if (!isUpdateRouteActive()) {
|
||||
pollHandle = null
|
||||
return
|
||||
}
|
||||
|
||||
if (document.visibilityState !== "visible") {
|
||||
pollHandle = setTimeout(poll, POLL_INTERVAL_MS)
|
||||
return
|
||||
}
|
||||
|
||||
await fetchStatus(false)
|
||||
if (shouldContinuePolling()) {
|
||||
pollHandle = setTimeout(poll, POLL_INTERVAL_MS)
|
||||
|
||||
@@ -182,10 +182,10 @@ _fast_update_state = {
|
||||
"progressDetail": "",
|
||||
}
|
||||
|
||||
_PLOTS_POLL_INTERVAL_S = 0.5
|
||||
_PLOTS_POLL_INTERVAL_S = 0.75
|
||||
_PLOTS_BOOT_STABILIZATION_WINDOW_S = 45.0
|
||||
_PLOTS_BOOT_POLL_INTERVAL_S = 1.0
|
||||
_PLOTS_CLIENT_IDLE_TIMEOUT_S = 15.0
|
||||
_PLOTS_CLIENT_IDLE_TIMEOUT_S = 6.0
|
||||
_PLOTS_SAMPLE_STALE_AFTER_S = 1.5
|
||||
|
||||
_plots_lock = threading.Lock()
|
||||
@@ -197,6 +197,8 @@ _plots_state = {
|
||||
"actualLateralAccel": 0.0,
|
||||
"desiredLongitudinalAccel": 0.0,
|
||||
"actualLongitudinalAccel": 0.0,
|
||||
"controlsActive": False,
|
||||
"longitudinalControlActive": False,
|
||||
"lateralP": 0.0,
|
||||
"lateralI": 0.0,
|
||||
"lateralD": 0.0,
|
||||
@@ -369,12 +371,10 @@ def _extract_lateral_accel_values(controls_state, speed_mps):
|
||||
return desired_curvature * speed_sq, actual_curvature * speed_sq, "curvature"
|
||||
|
||||
def _extract_longitudinal_accel_values(controls_state, live_location_kalman):
|
||||
desired = _safe_float(getattr(controls_state, "upAccelCmd", 0.0)) + \
|
||||
_safe_float(getattr(controls_state, "uiAccelCmd", 0.0)) + \
|
||||
_safe_float(getattr(controls_state, "ufAccelCmd", 0.0))
|
||||
desired = _safe_float(getattr(controls_state, "aTarget", 0.0))
|
||||
source = "controlsState.aTarget + liveLocationKalman"
|
||||
|
||||
actual = 0.0
|
||||
source = "controlsState + liveLocationKalman"
|
||||
try:
|
||||
accel_calibrated = getattr(live_location_kalman, "accelerationCalibrated", None)
|
||||
if accel_calibrated and getattr(accel_calibrated, "valid", False):
|
||||
@@ -382,7 +382,17 @@ def _extract_longitudinal_accel_values(controls_state, live_location_kalman):
|
||||
if len(accel_values) > 0:
|
||||
actual = _safe_float(accel_values[0], 0.0)
|
||||
except Exception:
|
||||
source = "controlsState"
|
||||
source = "controlsState.aTarget"
|
||||
|
||||
# Fallback only if aTarget is unavailable/legacy-zero while PID terms are present.
|
||||
if abs(desired) < 1e-6:
|
||||
up = _safe_float(getattr(controls_state, "upAccelCmd", 0.0))
|
||||
ui = _safe_float(getattr(controls_state, "uiAccelCmd", 0.0))
|
||||
uf = _safe_float(getattr(controls_state, "ufAccelCmd", 0.0))
|
||||
pid_sum = up + ui + uf
|
||||
if abs(pid_sum) > 1e-6:
|
||||
desired = pid_sum
|
||||
source = "controlsState PID sum + liveLocationKalman"
|
||||
|
||||
return desired, actual, source
|
||||
|
||||
@@ -450,6 +460,9 @@ def _plots_worker():
|
||||
controls_state = sm["controlsState"]
|
||||
live_location_kalman = sm["liveLocationKalman"]
|
||||
speed = _safe_float(getattr(controls_state, "vPid", 0.0))
|
||||
controls_active = bool(getattr(controls_state, "active", False))
|
||||
long_control_state = int(_safe_float(getattr(controls_state, "longControlState", 0)))
|
||||
longitudinal_control_active = controls_active and long_control_state != 0
|
||||
|
||||
desired_lateral, actual_lateral, lateral_source = _extract_lateral_accel_values(controls_state, speed)
|
||||
desired_longitudinal, actual_longitudinal, longitudinal_source = _extract_longitudinal_accel_values(controls_state, live_location_kalman)
|
||||
@@ -463,6 +476,8 @@ def _plots_worker():
|
||||
"actualLateralAccel": round(actual_lateral, 4),
|
||||
"desiredLongitudinalAccel": round(desired_longitudinal, 4),
|
||||
"actualLongitudinalAccel": round(actual_longitudinal, 4),
|
||||
"controlsActive": controls_active,
|
||||
"longitudinalControlActive": longitudinal_control_active,
|
||||
"lateralP": round(lateral_terms["lateralP"], 4),
|
||||
"lateralI": round(lateral_terms["lateralI"], 4),
|
||||
"lateralD": round(lateral_terms["lateralD"], 4),
|
||||
|
||||
@@ -103,67 +103,83 @@ class CarController(CarControllerBase):
|
||||
self.pedal_steady = 0.0
|
||||
return 0., False
|
||||
|
||||
# Regen paddle state machine: speed-aware thresholds + min on/off duration for robust paddle transitions.
|
||||
press_cmd_threshold = interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [-0.90, -0.82, -0.72, -0.65])
|
||||
release_cmd_threshold = interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [-0.10, -0.17, -0.24, -0.30])
|
||||
press_aego_threshold = interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [-0.95, -0.86, -0.76, -0.70])
|
||||
release_aego_threshold = interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [-0.16, -0.23, -0.30, -0.36])
|
||||
|
||||
press_confirm_frames = int(round(interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [8.0, 6.0, 5.0, 4.0])))
|
||||
release_confirm_frames = int(round(interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [18.0, 15.0, 12.0, 10.0])))
|
||||
min_on_frames = int(round(interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [34.0, 27.0, 20.0, 16.0])))
|
||||
min_off_frames = int(round(interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [16.0, 14.0, 12.0, 10.0])))
|
||||
|
||||
want_press = self.planner_regen_hold or accel <= press_cmd_threshold or self.aego <= press_aego_threshold
|
||||
want_release = (not self.planner_regen_hold) and accel >= release_cmd_threshold and self.aego >= release_aego_threshold
|
||||
|
||||
if want_press:
|
||||
self.regen_press_counter += 1
|
||||
else:
|
||||
self.regen_press_counter = max(self.regen_press_counter - 1, 0)
|
||||
|
||||
if want_release:
|
||||
self.regen_release_counter += 1
|
||||
else:
|
||||
self.regen_release_counter = max(self.regen_release_counter - 1, 0)
|
||||
|
||||
# Strong planner request can skip most of debounce delay.
|
||||
if self.planner_regen_hold and accel <= (press_cmd_threshold - 0.30):
|
||||
self.regen_press_counter = max(self.regen_press_counter, press_confirm_frames)
|
||||
|
||||
if self.regen_min_on_frames > 0:
|
||||
self.regen_min_on_frames -= 1
|
||||
if self.regen_min_off_frames > 0:
|
||||
self.regen_min_off_frames -= 1
|
||||
|
||||
supports_regen_paddle = self.CP.carFingerprint in CC_REGEN_PADDLE_CAR
|
||||
switched_state = False
|
||||
if self.regen_paddle_pressed:
|
||||
if self.regen_min_on_frames == 0 and self.regen_release_counter >= release_confirm_frames:
|
||||
press_regen_paddle = False
|
||||
if supports_regen_paddle:
|
||||
# Regen paddle state machine: speed-aware thresholds + min on/off duration for robust paddle transitions.
|
||||
press_cmd_threshold = interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [-0.90, -0.82, -0.72, -0.65])
|
||||
release_cmd_threshold = interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [-0.10, -0.17, -0.24, -0.30])
|
||||
press_aego_threshold = interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [-0.95, -0.86, -0.76, -0.70])
|
||||
release_aego_threshold = interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [-0.16, -0.23, -0.30, -0.36])
|
||||
|
||||
press_confirm_frames = int(round(interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [8.0, 6.0, 5.0, 4.0])))
|
||||
release_confirm_frames = int(round(interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [18.0, 15.0, 12.0, 10.0])))
|
||||
min_on_frames = int(round(interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [34.0, 27.0, 20.0, 16.0])))
|
||||
min_off_frames = int(round(interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [16.0, 14.0, 12.0, 10.0])))
|
||||
# Extra hysteresis in the 8-20 mph band to reduce paddle edge pulsing.
|
||||
confirm_boost = int(round(interp(car_velocity, [0.0, 6.0, 8.0, 20.0, 25.0], [0.0, 0.0, 3.0, 3.0, 0.0])))
|
||||
press_confirm_frames += confirm_boost
|
||||
release_confirm_frames += confirm_boost
|
||||
|
||||
want_press = self.planner_regen_hold or accel <= press_cmd_threshold or self.aego <= press_aego_threshold
|
||||
want_release = (not self.planner_regen_hold) and accel >= release_cmd_threshold and self.aego >= release_aego_threshold
|
||||
|
||||
if want_press:
|
||||
self.regen_press_counter += 1
|
||||
else:
|
||||
self.regen_press_counter = max(self.regen_press_counter - 1, 0)
|
||||
|
||||
if want_release:
|
||||
self.regen_release_counter += 1
|
||||
else:
|
||||
self.regen_release_counter = max(self.regen_release_counter - 1, 0)
|
||||
|
||||
# Strong planner request can skip most of debounce delay.
|
||||
if self.planner_regen_hold and accel <= (press_cmd_threshold - 0.30):
|
||||
self.regen_press_counter = max(self.regen_press_counter, press_confirm_frames)
|
||||
|
||||
if self.regen_min_on_frames > 0:
|
||||
self.regen_min_on_frames -= 1
|
||||
if self.regen_min_off_frames > 0:
|
||||
self.regen_min_off_frames -= 1
|
||||
|
||||
if self.regen_paddle_pressed:
|
||||
if self.regen_min_on_frames == 0 and self.regen_release_counter >= release_confirm_frames:
|
||||
self.regen_paddle_pressed = False
|
||||
self.regen_min_off_frames = min_off_frames
|
||||
self.regen_release_counter = 0
|
||||
switched_state = True
|
||||
else:
|
||||
if self.regen_min_off_frames == 0 and self.regen_press_counter >= press_confirm_frames:
|
||||
self.regen_paddle_pressed = True
|
||||
self.regen_min_on_frames = min_on_frames
|
||||
self.regen_press_counter = 0
|
||||
switched_state = True
|
||||
|
||||
self.regen_paddle_timer = self.regen_press_counter
|
||||
press_regen_paddle = self.regen_paddle_pressed
|
||||
|
||||
if self.maneuver_paddle_mode == "off":
|
||||
self.regen_paddle_pressed = False
|
||||
self.regen_min_off_frames = min_off_frames
|
||||
self.regen_release_counter = 0
|
||||
switched_state = True
|
||||
else:
|
||||
if self.regen_min_off_frames == 0 and self.regen_press_counter >= press_confirm_frames:
|
||||
self.regen_paddle_pressed = True
|
||||
self.regen_min_on_frames = min_on_frames
|
||||
self.regen_press_counter = 0
|
||||
switched_state = True
|
||||
|
||||
self.regen_paddle_timer = self.regen_press_counter
|
||||
press_regen_paddle = self.regen_paddle_pressed
|
||||
|
||||
if self.maneuver_paddle_mode == "off":
|
||||
self.regen_release_counter = 0
|
||||
self.regen_min_on_frames = 0
|
||||
self.regen_mode_blend = 0.0
|
||||
press_regen_paddle = False
|
||||
elif self.maneuver_paddle_mode == "force":
|
||||
forced_press = accel < -0.02
|
||||
self.regen_paddle_pressed = forced_press
|
||||
press_regen_paddle = forced_press
|
||||
else:
|
||||
self.planner_regen_hold = False
|
||||
self.regen_paddle_pressed = False
|
||||
self.regen_paddle_timer = 0
|
||||
self.regen_press_counter = 0
|
||||
self.regen_release_counter = 0
|
||||
self.regen_min_on_frames = 0
|
||||
self.regen_min_off_frames = 0
|
||||
self.regen_mode_blend = 0.0
|
||||
press_regen_paddle = False
|
||||
elif self.maneuver_paddle_mode == "force":
|
||||
forced_press = accel < -0.02
|
||||
self.regen_paddle_pressed = forced_press
|
||||
press_regen_paddle = forced_press
|
||||
|
||||
# Regen gain ratios from bin-averaged 60–0 deceleration sweep; Calculates stronger decel from paddle
|
||||
speed_mps = [0.559, 1.678, 2.797, 3.916, 5.035, 6.154, 7.273, 8.392, 9.511, 10.63,
|
||||
@@ -175,6 +191,8 @@ class CarController(CarControllerBase):
|
||||
1.44, 1.45, 1.45]
|
||||
|
||||
gain = interp(car_velocity, speed_mps, regen_gain_ratio)
|
||||
# Terminal-stop authority boost for paddle+pedal mode: lower gain at low speed => stronger net decel.
|
||||
gain *= interp(car_velocity, [0.0, 2.0, 4.0, 5.5, 8.0, 12.0], [0.92, 0.92, 0.93, 0.94, 0.96, 1.0])
|
||||
accel_gain = interp(car_velocity, [0.0, 3.0, 8.0, 20.0], [0.47, 0.52, 0.57, 0.61])
|
||||
pedaloffset = interp(car_velocity, [0.0, 1.0, 3.0, 6.0, 15.0, 30.0], [0.085, 0.11, 0.17, 0.23, 0.235, 0.23])
|
||||
|
||||
|
||||
@@ -90,7 +90,7 @@ class CarInterface(CarInterfaceBase):
|
||||
if CP.enableGasInterceptor and bool(CP.flags & GMFlags.PEDAL_LONG.value):
|
||||
if CP.carFingerprint in BOLT_PEDAL_LONG_CARS:
|
||||
accel_min = interp(current_speed, [0.0, 1.5, 4.0, 8.0, 15.0, 30.0],
|
||||
[-0.85, -1.2, -1.90, -2.50, -2.82, -2.95])
|
||||
[-0.93, -1.28, -1.98, -2.58, -2.86, -2.95])
|
||||
accel_max = interp(current_speed, [0.0, 1.5, 4.0, 8.0, 15.0],
|
||||
[0.54, 0.74, 1.03, 1.46, CarControllerParams.ACCEL_MAX])
|
||||
else:
|
||||
|
||||
@@ -917,6 +917,8 @@ class Controls:
|
||||
controlsState.vPid = float(self.LoC.v_pid)
|
||||
controlsState.vCruise = float(self.v_cruise_helper.v_cruise_kph)
|
||||
controlsState.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph)
|
||||
# Publish planner intent explicitly so tools can compare "model wants" vs measured accel.
|
||||
controlsState.aTarget = float(self.sm['longitudinalPlan'].aTarget)
|
||||
controlsState.upAccelCmd = float(self.LoC.pid.p)
|
||||
controlsState.uiAccelCmd = float(self.LoC.pid.i)
|
||||
controlsState.ufAccelCmd = float(self.LoC.pid.f)
|
||||
|
||||
@@ -24,13 +24,17 @@ Test your vehicle's longitudinal control tuning with this tool. The tool will te
|
||||
- `LONG_MANEUVER_PHASE|name=pedal_only|paddleMode=off`
|
||||
- `LONG_MANEUVER_PHASE|name=pedal_plus_paddle|paddleMode=force`
|
||||
|
||||
5. Ensure the road ahead is clear, as openpilot will not brake for any obstructions in this mode. Once you are ready, press "Set" on your steering wheel to start the tests. The tests will run for about 4 minutes. If you need to pause the tests, press "Cancel" on your steering wheel. You can resume the tests by pressing "Resume" on your steering wheel.
|
||||
5. Ensure the road ahead is clear, as openpilot will not brake for any obstructions in this mode. Once you are ready, press "Set" on your steering wheel to start the tests. Typical runtime is about 4 minutes for the base suite and can be ~8-10 minutes for GM pedal-long A/B runs. If you need to pause the tests, press "Cancel" on your steering wheel. You can resume the tests by pressing "Resume" on your steering wheel.
|
||||
|
||||
**Note:** For GM cars, it is recommended to hold down the resume button for all low-speed tests (starting, stopping and creep) to avoid the car entering standstill.
|
||||
**Note:** For GM pedal-long cars (pedal interceptor + regen paddle), the maneuver suite automatically runs A/B phases:
|
||||
- `pedal-only` (`LongitudinalManeuverPaddleMode=off`)
|
||||
- `pedal+paddle` (`LongitudinalManeuverPaddleMode=force`)
|
||||
It also uses additional `-2.0` and `-2.5 m/s^2` brake steps as required checks. The `-4.0 m/s^2` step is logged as informational since pedal/regen authority is physically limited without friction brakes.
|
||||
It also includes:
|
||||
- Additional `-2.0` and `-2.5 m/s^2` brake steps as required checks (`-4.0` is informational due to pedal/regen limits).
|
||||
- Low-speed stop-envelope sweeps (8/12/16/20 mph starts) to quantify terminal stop authority.
|
||||
- Terminal stop probes (ramped/late strong regen request) to measure end-of-stop behavior.
|
||||
- Paddle blend probes to measure on/off transition smoothness and jerk around paddle edges.
|
||||
|
||||

|
||||
|
||||
@@ -53,4 +57,24 @@ Test your vehicle's longitudinal control tuning with this tool. The tool will te
|
||||
Report written to /home/batman/openpilot/tools/longitudinal_maneuvers/longitudinal_reports/LEXUS_ES_TSS2_57048cfce01d9625_0000010e--5b26bc3be7.html
|
||||
```
|
||||
|
||||
For GM pedal-long routes, the report now adds concrete tuning tables:
|
||||
- **Low-Speed Stop Envelope**:
|
||||
- achieved/target decel ratio
|
||||
- terminal ratio in 0.2–2.0 m/s
|
||||
- linger time in 0.2–1.5 m/s
|
||||
- rollout distance after `shouldStop`
|
||||
- numeric recommendations for:
|
||||
- low-speed `regen_gain_ratio` percent increase
|
||||
- low-speed `accel_min` delta
|
||||
- stop-margin increase (`IncreasedStoppingDistance` / planner stop margin)
|
||||
- per-run **tuning hint** for what to adjust next
|
||||
- **Paddle Blend Transition Probe**:
|
||||
- regen edge count
|
||||
- max jerk near edges
|
||||
- max command/output step at edges
|
||||
- numeric recommendations for:
|
||||
- blend-step reduction %
|
||||
- press/release confirm-frame increase
|
||||
- per-run **tuning hint** for blend/debounce/rate-limit changes
|
||||
|
||||
You can reach out on [Discord](https://discord.comma.ai) if you have any questions about these instructions or the tool itself.
|
||||
|
||||
@@ -92,11 +92,133 @@ def is_informational_maneuver(description: str) -> bool:
|
||||
return "informational" in description.lower()
|
||||
|
||||
|
||||
def split_phase_suffix(description: str) -> tuple[str, str]:
|
||||
desc = description.strip()
|
||||
if desc.endswith("]") and " [" in desc:
|
||||
idx = desc.rfind(" [")
|
||||
phase = desc[idx + 2:-1].strip().lower()
|
||||
if phase in {"pedal-only", "pedal+paddle"}:
|
||||
return desc[:idx], phase
|
||||
return desc, "standard"
|
||||
|
||||
|
||||
def is_low_speed_stop_maneuver(description: str) -> bool:
|
||||
base_desc, _ = split_phase_suffix(description)
|
||||
return base_desc.lower().startswith("low-speed stop envelope:")
|
||||
|
||||
|
||||
def is_paddle_blend_probe(description: str) -> bool:
|
||||
base_desc, _ = split_phase_suffix(description)
|
||||
return base_desc.lower().startswith("paddle blend probe:")
|
||||
|
||||
|
||||
def is_terminal_stop_probe(description: str) -> bool:
|
||||
base_desc, _ = split_phase_suffix(description)
|
||||
return base_desc.lower().startswith("terminal stop probe:")
|
||||
|
||||
|
||||
def interp_scalar(x: float, xp: list[float], fp: list[float]) -> float:
|
||||
if x <= xp[0]:
|
||||
return fp[0]
|
||||
for i in range(1, len(xp)):
|
||||
if x <= xp[i]:
|
||||
x0, x1 = xp[i - 1], xp[i]
|
||||
y0, y1 = fp[i - 1], fp[i]
|
||||
if x1 == x0:
|
||||
return y1
|
||||
return y0 + (y1 - y0) * ((x - x0) / (x1 - x0))
|
||||
return fp[-1]
|
||||
|
||||
|
||||
def clamp(x: float, lo: float, hi: float) -> float:
|
||||
return max(lo, min(hi, x))
|
||||
|
||||
|
||||
def first_true_time(times: list[float], values: list[bool]) -> float | None:
|
||||
for t, v in zip(times, values, strict=True):
|
||||
if v:
|
||||
return t
|
||||
return None
|
||||
|
||||
|
||||
def sample_nearest(times: list[float], values: list[float], t_target: float) -> float:
|
||||
if not times:
|
||||
return 0.0
|
||||
idx = min(range(len(times)), key=lambda i: abs(times[i] - t_target))
|
||||
return values[idx]
|
||||
|
||||
|
||||
def integrate_distance(times: list[float], speeds: list[float], start_t: float, end_t: float | None = None) -> float:
|
||||
if len(times) < 2:
|
||||
return 0.0
|
||||
|
||||
total = 0.0
|
||||
end_val = times[-1] if end_t is None else end_t
|
||||
if end_val <= start_t:
|
||||
return 0.0
|
||||
|
||||
for i in range(1, len(times)):
|
||||
t0, t1 = times[i - 1], times[i]
|
||||
v0, v1 = speeds[i - 1], speeds[i]
|
||||
if t1 <= start_t:
|
||||
continue
|
||||
if t0 >= end_val:
|
||||
break
|
||||
|
||||
seg_start = max(t0, start_t)
|
||||
seg_end = min(t1, end_val)
|
||||
if seg_end <= seg_start:
|
||||
continue
|
||||
|
||||
dt = max(t1 - t0, 1e-3)
|
||||
alpha_start = (seg_start - t0) / dt
|
||||
alpha_end = (seg_end - t0) / dt
|
||||
vs = v0 + (v1 - v0) * alpha_start
|
||||
ve = v0 + (v1 - v0) * alpha_end
|
||||
total += 0.5 * (vs + ve) * (seg_end - seg_start)
|
||||
|
||||
return total
|
||||
|
||||
|
||||
def extract_regen_paddle_state_from_can(msgs, t_ref_nanos: int) -> tuple[list[float], list[bool], str]:
|
||||
by_src: dict[int, list[tuple[float, bool]]] = defaultdict(list)
|
||||
for m in msgs:
|
||||
if m.which() != "can":
|
||||
continue
|
||||
t = (m.logMonoTime - t_ref_nanos) / 1e9
|
||||
for c in m.can:
|
||||
if int(c.address) != 189:
|
||||
continue
|
||||
dat = bytes(c.dat)
|
||||
if len(dat) == 0:
|
||||
continue
|
||||
# DBC: EBCMRegenPaddle.RegenPaddle is 4-bit signal at start bit 7 (motorola/big-endian), i.e. high nibble of byte 0.
|
||||
paddle_raw = (dat[0] >> 4) & 0xF
|
||||
by_src[int(c.src)].append((t, paddle_raw != 0))
|
||||
|
||||
if by_src.get(128):
|
||||
samples = by_src[128]
|
||||
times = [s[0] for s in samples]
|
||||
states = [s[1] for s in samples]
|
||||
return times, states, "can src 128"
|
||||
|
||||
if by_src:
|
||||
best_src = max(by_src.keys(), key=lambda s: len(by_src[s]))
|
||||
samples = by_src[best_src]
|
||||
times = [s[0] for s in samples]
|
||||
states = [s[1] for s in samples]
|
||||
return times, states, f"can src {best_src}"
|
||||
|
||||
return [], [], "unavailable"
|
||||
|
||||
|
||||
def report(platform, route, _description, CP, ID, maneuvers):
|
||||
output_path = Path(__file__).resolve().parent / "longitudinal_reports"
|
||||
output_fn = output_path / f"{platform}_{route.replace('/', '_')}.html"
|
||||
output_path.mkdir(exist_ok=True)
|
||||
target_cross_times = defaultdict(list)
|
||||
low_speed_stop_rows = []
|
||||
paddle_blend_rows = []
|
||||
|
||||
builder = [
|
||||
"<style>summary { cursor: pointer; }\n td, th { padding: 8px; } </style>\n",
|
||||
@@ -194,6 +316,241 @@ def report(platform, route, _description, CP, ID, maneuvers):
|
||||
if info_only:
|
||||
builder.append('<h3 style="font-weight: normal">Result type: <strong>informational</strong> (pedal/regen authority check)</h3>')
|
||||
|
||||
cc_accels = [m.actuators.accel for m in carControl]
|
||||
co_accels = [m.actuatorsOutput.accel for m in carOutput]
|
||||
cs_accels = [m.aEgo for m in carState]
|
||||
cs_speeds = [m.vEgo for m in carState]
|
||||
plan_a_targets = [m.aTarget for m in longitudinalPlan]
|
||||
|
||||
if is_low_speed_stop_maneuver(description) or is_terminal_stop_probe(description):
|
||||
should_stop_t = first_true_time(t_longitudinalPlan, [m.shouldStop for m in longitudinalPlan])
|
||||
stop_threshold = 0.20
|
||||
stop_t = first_true_time(t_carState, [v <= stop_threshold for v in cs_speeds])
|
||||
stop_time = stop_t if stop_t is not None else float("nan")
|
||||
target_decel = min(plan_a_targets)
|
||||
achieved_decel = min(cs_accels)
|
||||
ratio = (abs(achieved_decel) / max(abs(target_decel), 1e-3)) if target_decel < -1e-3 else 0.0
|
||||
min_speed = min(cs_speeds) if cs_speeds else float("nan")
|
||||
final_speed = cs_speeds[-1] if cs_speeds else float("nan")
|
||||
stop_reached = bool(cs_speeds) and (min_speed <= stop_threshold)
|
||||
|
||||
rollout_m = float("nan")
|
||||
if should_stop_t is not None:
|
||||
rollout_end = stop_t if stop_t is not None else t_carState[-1]
|
||||
rollout_m = integrate_distance(t_carState, cs_speeds, should_stop_t, rollout_end)
|
||||
|
||||
terminal_zone = [i for i, v in enumerate(cs_speeds) if 0.20 <= v <= 2.00]
|
||||
terminal_target = float("nan")
|
||||
terminal_achieved = float("nan")
|
||||
terminal_ratio = float("nan")
|
||||
if terminal_zone:
|
||||
terminal_achieved = min(cs_accels[i] for i in terminal_zone)
|
||||
terminal_targets = [sample_nearest(t_longitudinalPlan, plan_a_targets, t_carState[i]) for i in terminal_zone]
|
||||
terminal_target = min(terminal_targets) if terminal_targets else float("nan")
|
||||
if not math.isnan(terminal_target) and terminal_target < -1e-3:
|
||||
terminal_ratio = abs(terminal_achieved) / max(abs(terminal_target), 1e-3)
|
||||
|
||||
linger_s = 0.0
|
||||
for i in range(1, len(t_carState)):
|
||||
if 0.20 <= cs_speeds[i - 1] <= 1.50 and 0.20 <= cs_speeds[i] <= 1.50:
|
||||
linger_s += (t_carState[i] - t_carState[i - 1])
|
||||
|
||||
init_mph = initial_speed * 2.23694
|
||||
expected_ratio_floor = interp_scalar(init_mph, [6.0, 8.0, 12.0, 16.0, 20.0], [0.38, 0.45, 0.60, 0.72, 0.82])
|
||||
ratio_deficit = max(expected_ratio_floor - ratio, 0.0)
|
||||
terminal_ratio_goal = 0.95
|
||||
terminal_deficit = 0.0 if math.isnan(terminal_ratio) else max(terminal_ratio_goal - terminal_ratio, 0.0)
|
||||
_, phase_tag = split_phase_suffix(description)
|
||||
|
||||
# In paddle mode, lower regen_gain_ratio => stronger commanded decel (since accel_term_scale uses 1/gain).
|
||||
# Delta is relative to goal tracking in the terminal zone.
|
||||
suggested_paddle_gain_delta_pct = 0.0
|
||||
if phase_tag == "pedal+paddle" and (not math.isnan(terminal_ratio)):
|
||||
suggested_paddle_gain_delta_pct = clamp(100.0 * ((terminal_ratio / max(terminal_ratio_goal, 1e-3)) - 1.0), -20.0, 10.0)
|
||||
|
||||
suggested_accel_min_delta = 0.0
|
||||
if (not stop_reached) or ratio_deficit > 0.03 or terminal_deficit > 0.05:
|
||||
suggested_accel_min_delta = -clamp(0.05 + 0.40 * max(ratio_deficit, terminal_deficit), 0.05, 0.35)
|
||||
|
||||
rollout_excess = 0.0 if math.isnan(rollout_m) else max(rollout_m - 2.0, 0.0)
|
||||
linger_excess = max(linger_s - 2.0, 0.0)
|
||||
suggested_stop_margin_m = clamp((0.60 * rollout_excess) + (0.40 * linger_excess), 0.0, 3.0)
|
||||
suggested_stop_margin_ft = suggested_stop_margin_m * 3.28084
|
||||
|
||||
if not stop_reached:
|
||||
stop_grade = "NO_STOP"
|
||||
elif ratio >= expected_ratio_floor and (math.isnan(terminal_ratio) or terminal_ratio >= terminal_ratio_goal) and (math.isnan(rollout_m) or rollout_m <= 2.0) and linger_s <= 2.0:
|
||||
stop_grade = "OK"
|
||||
elif ratio >= 0.88 * expected_ratio_floor and min_speed <= 0.35 and (math.isnan(rollout_m) or rollout_m <= 3.5) and linger_s <= 3.5:
|
||||
stop_grade = "MARGINAL"
|
||||
else:
|
||||
stop_grade = "LIMITED"
|
||||
|
||||
if not stop_reached:
|
||||
if phase_tag == "pedal+paddle" and abs(suggested_paddle_gain_delta_pct) >= 1.0:
|
||||
gain_text = f" and adjust low-speed `regen_gain_ratio` by {suggested_paddle_gain_delta_pct:+.0f}%"
|
||||
else:
|
||||
gain_text = ""
|
||||
tuning_hint = (
|
||||
f"No true stop: lower low-speed `accel_min` by {abs(suggested_accel_min_delta):.2f} m/s^2"
|
||||
f"{gain_text}."
|
||||
)
|
||||
elif ratio_deficit > 0.03 or terminal_deficit > 0.05:
|
||||
if phase_tag == "pedal+paddle" and abs(suggested_paddle_gain_delta_pct) >= 1.0:
|
||||
tuning_hint = (
|
||||
f"Terminal decel deficit: adjust low-speed `regen_gain_ratio` by {suggested_paddle_gain_delta_pct:+.0f}% "
|
||||
f"(negative = stronger paddle+pedal decel) and lower low-speed `accel_min` by {abs(suggested_accel_min_delta):.2f} m/s^2."
|
||||
)
|
||||
else:
|
||||
tuning_hint = (
|
||||
f"Terminal decel deficit: lower low-speed `accel_min` by {abs(suggested_accel_min_delta):.2f} m/s^2 "
|
||||
f"(pedal-only authority path)."
|
||||
)
|
||||
elif rollout_excess > 0.0 or linger_excess > 0.0:
|
||||
tuning_hint = (
|
||||
f"Late-stop rollout/linger: increase `IncreasedStoppingDistance` by ~{max(suggested_stop_margin_ft, 0.5):.1f} ft "
|
||||
f"(or add equivalent planner shouldStop margin)."
|
||||
)
|
||||
else:
|
||||
tuning_hint = "Low-speed stop authority is within target; focus next on paddle-transition smoothness."
|
||||
|
||||
builder.append(
|
||||
f'<h3 style="font-weight: normal">Low-speed stop metrics: '
|
||||
f'expected ratio floor <strong>{expected_ratio_floor:.2f}</strong>, '
|
||||
f'achieved ratio <strong>{ratio:.2f}</strong>, '
|
||||
f'terminal ratio (0.2-2.0 m/s) <strong>{"n/a" if math.isnan(terminal_ratio) else f"{terminal_ratio:.2f}"}</strong>, '
|
||||
f'linger in 0.2-1.5 m/s <strong>{linger_s:.2f}s</strong>, '
|
||||
f'stop time <strong>{"n/a" if math.isnan(stop_time) else f"{stop_time:.2f}s"}</strong>, '
|
||||
f'rollout after shouldStop <strong>{"n/a" if math.isnan(rollout_m) else f"{rollout_m:.2f}m"}</strong>, '
|
||||
f'min speed <strong>{"n/a" if math.isnan(min_speed) else f"{min_speed:.2f} m/s"}</strong>, '
|
||||
f'suggested paddle gain delta <strong>{suggested_paddle_gain_delta_pct:+.0f}%</strong>, '
|
||||
f'suggested accel_min delta <strong>{suggested_accel_min_delta:.2f} m/s^2</strong>, '
|
||||
f'suggested stop margin <strong>{suggested_stop_margin_ft:.1f} ft</strong>, '
|
||||
f'grade <strong>{stop_grade}</strong></h3>'
|
||||
)
|
||||
builder.append(f'<h3 style="font-weight: normal">Tuning hint: <strong>{tuning_hint}</strong></h3>')
|
||||
|
||||
low_speed_stop_rows.append([
|
||||
description,
|
||||
phase_tag,
|
||||
int(run) + 1,
|
||||
round(init_mph, 1),
|
||||
round(target_decel, 2),
|
||||
round(achieved_decel, 2),
|
||||
round(expected_ratio_floor, 2),
|
||||
round(ratio, 2),
|
||||
"n/a" if math.isnan(stop_time) else round(stop_time, 2),
|
||||
"n/a" if math.isnan(rollout_m) else round(rollout_m, 2),
|
||||
"n/a" if math.isnan(terminal_ratio) else round(terminal_ratio, 2),
|
||||
round(linger_s, 2),
|
||||
"n/a" if math.isnan(min_speed) else round(min_speed, 2),
|
||||
round(final_speed, 2),
|
||||
round(suggested_paddle_gain_delta_pct, 0),
|
||||
round(suggested_accel_min_delta, 2),
|
||||
round(suggested_stop_margin_ft, 1),
|
||||
stop_grade,
|
||||
tuning_hint,
|
||||
])
|
||||
|
||||
if is_paddle_blend_probe(description):
|
||||
regen_times, regen_states, regen_source = extract_regen_paddle_state_from_can(msgs, cs_pairs[0][0])
|
||||
if not regen_states:
|
||||
regen_times = list(t_carState)
|
||||
regen_states = [bool(getattr(m, "regenBraking", False)) for m in carState]
|
||||
regen_source = "carState.regenBraking"
|
||||
edge_times = [regen_times[i] for i in range(1, len(regen_states)) if regen_states[i] != regen_states[i - 1]]
|
||||
|
||||
jerks = []
|
||||
jerk_times = []
|
||||
for i in range(1, len(livePose)):
|
||||
dt = max(t_livePose[i] - t_livePose[i - 1], 1e-3)
|
||||
da = livePose[i].accelerationDevice.x - livePose[i - 1].accelerationDevice.x
|
||||
jerks.append(da / dt)
|
||||
jerk_times.append(t_livePose[i])
|
||||
|
||||
edge_jerk_peaks = []
|
||||
edge_cmd_steps = []
|
||||
edge_out_steps = []
|
||||
for edge_t in edge_times:
|
||||
local_jerks = [abs(j) for tj, j in zip(jerk_times, jerks, strict=True) if abs(tj - edge_t) <= 0.5]
|
||||
if local_jerks:
|
||||
edge_jerk_peaks.append(max(local_jerks))
|
||||
|
||||
cc_pre = sample_nearest(t_carControl, cc_accels, edge_t - 0.25)
|
||||
cc_post = sample_nearest(t_carControl, cc_accels, edge_t + 0.25)
|
||||
edge_cmd_steps.append(abs(cc_post - cc_pre))
|
||||
|
||||
co_pre = sample_nearest(t_carOutput, co_accels, edge_t - 0.25)
|
||||
co_post = sample_nearest(t_carOutput, co_accels, edge_t + 0.25)
|
||||
edge_out_steps.append(abs(co_post - co_pre))
|
||||
|
||||
max_edge_jerk = max(edge_jerk_peaks) if edge_jerk_peaks else float("nan")
|
||||
max_cmd_step = max(edge_cmd_steps) if edge_cmd_steps else float("nan")
|
||||
max_out_step = max(edge_out_steps) if edge_out_steps else float("nan")
|
||||
suggested_blend_step_reduction = 0.0
|
||||
suggested_confirm_frames_add = 0
|
||||
if not math.isnan(max_cmd_step) and max_cmd_step > 0.45:
|
||||
suggested_blend_step_reduction = clamp((1.0 - (0.45 / max_cmd_step)) * 100.0, 0.0, 50.0)
|
||||
if not math.isnan(max_edge_jerk) and max_edge_jerk > 2.5:
|
||||
suggested_confirm_frames_add = int(clamp(math.ceil((max_edge_jerk - 2.5) / 0.8), 1, 3))
|
||||
|
||||
if len(edge_times) == 0:
|
||||
blend_grade = "NO_EDGES"
|
||||
elif not math.isnan(max_edge_jerk) and max_edge_jerk <= 2.5 and (math.isnan(max_cmd_step) or max_cmd_step <= 0.45):
|
||||
blend_grade = "SMOOTH"
|
||||
elif not math.isnan(max_edge_jerk) and max_edge_jerk <= 3.5:
|
||||
blend_grade = "OK"
|
||||
else:
|
||||
blend_grade = "HARSH"
|
||||
|
||||
if blend_grade == "NO_EDGES":
|
||||
blend_hint = "Probe did not toggle regen paddle; verify pedal+paddle phase was active."
|
||||
elif blend_grade == "HARSH":
|
||||
adjustments = []
|
||||
if suggested_blend_step_reduction > 0.0:
|
||||
adjustments.append(f"reduce blend step rates by ~{suggested_blend_step_reduction:.0f}%")
|
||||
if suggested_confirm_frames_add > 0:
|
||||
adjustments.append(f"increase press/release confirm frames by +{suggested_confirm_frames_add}")
|
||||
if not adjustments:
|
||||
adjustments.append("increase paddle min on/off hold by +1 frame")
|
||||
blend_hint = f"Harsh paddle transitions: {', '.join(adjustments)}."
|
||||
elif blend_grade == "OK":
|
||||
if suggested_confirm_frames_add > 0:
|
||||
blend_hint = f"Mostly stable; +{suggested_confirm_frames_add} confirm frame may remove remaining edge pulse."
|
||||
else:
|
||||
blend_hint = "Mostly stable; transition behavior is acceptable."
|
||||
else:
|
||||
blend_hint = "Transition behavior is already smooth."
|
||||
|
||||
builder.append(
|
||||
f'<h3 style="font-weight: normal">Paddle blend metrics: '
|
||||
f'edge source <strong>{regen_source}</strong>, '
|
||||
f'edges <strong>{len(edge_times)}</strong>, '
|
||||
f'max |jerk| near edges <strong>{"n/a" if math.isnan(max_edge_jerk) else f"{max_edge_jerk:.2f} m/s^3"}</strong>, '
|
||||
f'max command step <strong>{"n/a" if math.isnan(max_cmd_step) else f"{max_cmd_step:.2f} m/s^2"}</strong>, '
|
||||
f'max output step <strong>{"n/a" if math.isnan(max_out_step) else f"{max_out_step:.2f} m/s^2"}</strong>, '
|
||||
f'suggested blend-step reduction <strong>{suggested_blend_step_reduction:.0f}%</strong>, '
|
||||
f'suggested confirm-frame add <strong>+{suggested_confirm_frames_add}</strong>, '
|
||||
f'grade <strong>{blend_grade}</strong></h3>'
|
||||
)
|
||||
builder.append(f'<h3 style="font-weight: normal">Tuning hint: <strong>{blend_hint}</strong></h3>')
|
||||
|
||||
_, phase_tag = split_phase_suffix(description)
|
||||
paddle_blend_rows.append([
|
||||
description,
|
||||
phase_tag,
|
||||
int(run) + 1,
|
||||
regen_source,
|
||||
len(edge_times),
|
||||
"n/a" if math.isnan(max_edge_jerk) else round(max_edge_jerk, 2),
|
||||
"n/a" if math.isnan(max_cmd_step) else round(max_cmd_step, 2),
|
||||
"n/a" if math.isnan(max_out_step) else round(max_out_step, 2),
|
||||
round(suggested_blend_step_reduction, 0),
|
||||
suggested_confirm_frames_add,
|
||||
blend_grade,
|
||||
blend_hint,
|
||||
])
|
||||
|
||||
pitches = [math.degrees(m.orientationNED[1]) for m in carControl]
|
||||
builder.append(f'<h3 style="font-weight: normal">Average pitch: <strong>{sum(pitches) / len(pitches):0.2f} degrees</strong></h3>')
|
||||
|
||||
@@ -250,6 +607,57 @@ def report(platform, route, _description, CP, ID, maneuvers):
|
||||
table.append(l)
|
||||
summary.append(tabulate_html(table, cols) + '\n')
|
||||
|
||||
if low_speed_stop_rows:
|
||||
summary.append("<h3>Low-Speed Stop Envelope</h3>\n")
|
||||
summary.append(
|
||||
tabulate_html(
|
||||
low_speed_stop_rows,
|
||||
[
|
||||
"maneuver",
|
||||
"phase",
|
||||
"run",
|
||||
"initial mph",
|
||||
"target decel",
|
||||
"achieved decel",
|
||||
"expected ratio floor",
|
||||
"achieved ratio",
|
||||
"time to stop (s)",
|
||||
"rollout after shouldStop (m)",
|
||||
"terminal ratio (0.2-2.0m/s)",
|
||||
"linger 0.2-1.5m/s (s)",
|
||||
"min speed (m/s)",
|
||||
"final speed (m/s)",
|
||||
"suggested paddle gain delta (%)",
|
||||
"suggested accel_min delta",
|
||||
"suggested stop margin (ft)",
|
||||
"grade",
|
||||
"tuning hint",
|
||||
],
|
||||
) + '\n'
|
||||
)
|
||||
|
||||
if paddle_blend_rows:
|
||||
summary.append("<h3>Paddle Blend Transition Probe</h3>\n")
|
||||
summary.append(
|
||||
tabulate_html(
|
||||
paddle_blend_rows,
|
||||
[
|
||||
"maneuver",
|
||||
"phase",
|
||||
"run",
|
||||
"edge source",
|
||||
"regen edges",
|
||||
"max |jerk| near edges (m/s^3)",
|
||||
"max command step (m/s^2)",
|
||||
"max output step (m/s^2)",
|
||||
"suggested blend-step reduction (%)",
|
||||
"suggested confirm-frame add",
|
||||
"grade",
|
||||
"tuning hint",
|
||||
],
|
||||
) + '\n'
|
||||
)
|
||||
|
||||
sum_idx = builder.index('{ summary }')
|
||||
builder[sum_idx:sum_idx + 1] = summary
|
||||
|
||||
|
||||
@@ -156,6 +156,85 @@ PEDAL_LONG_BRAKE_STEPS = [
|
||||
),
|
||||
]
|
||||
|
||||
PEDAL_LONG_LOW_SPEED_STOP_SWEEP = [
|
||||
Maneuver(
|
||||
"low-speed stop envelope: -0.8m/s^2 from 8mph",
|
||||
[Action([-0.8], [7.0])],
|
||||
repeat=0,
|
||||
initial_speed=8. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"low-speed stop envelope: -1.2m/s^2 from 12mph",
|
||||
[Action([-1.2], [7.0])],
|
||||
repeat=0,
|
||||
initial_speed=12. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"low-speed stop envelope: -1.6m/s^2 from 16mph",
|
||||
[Action([-1.6], [7.0])],
|
||||
repeat=0,
|
||||
initial_speed=16. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"low-speed stop envelope: -2.0m/s^2 from 20mph",
|
||||
[Action([-2.0], [7.0])],
|
||||
repeat=0,
|
||||
initial_speed=20. * CV.MPH_TO_MS,
|
||||
),
|
||||
]
|
||||
|
||||
PEDAL_LONG_PADDLE_BLEND_PROBES = [
|
||||
Maneuver(
|
||||
"paddle blend probe: accel/decel toggle around zero from 12mph",
|
||||
[
|
||||
Action([-0.20], [1.5]),
|
||||
Action([0.20], [1.5]),
|
||||
Action([-0.20], [1.5]),
|
||||
Action([0.20], [1.5]),
|
||||
Action([-0.35], [2.5]),
|
||||
],
|
||||
repeat=0,
|
||||
initial_speed=12. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"paddle blend probe: regen threshold sweep from 18mph",
|
||||
[
|
||||
Action([-0.45], [2.0]),
|
||||
Action([-0.75], [2.0]),
|
||||
Action([-0.45], [2.0]),
|
||||
Action([-0.90], [2.0]),
|
||||
Action([-0.10], [1.5]),
|
||||
],
|
||||
repeat=0,
|
||||
initial_speed=18. * CV.MPH_TO_MS,
|
||||
),
|
||||
]
|
||||
|
||||
PEDAL_LONG_TERMINAL_STOP_PROBES = [
|
||||
Maneuver(
|
||||
"terminal stop probe: ramped decel catch from 14mph",
|
||||
[
|
||||
Action([-0.45], [2.0]),
|
||||
Action([-1.20], [2.0]),
|
||||
Action([-2.20], [2.5]),
|
||||
Action([-0.35], [2.0]),
|
||||
],
|
||||
repeat=0,
|
||||
initial_speed=14. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"terminal stop probe: late strong regen request from 18mph",
|
||||
[
|
||||
Action([-0.55], [2.0]),
|
||||
Action([-1.45], [2.0]),
|
||||
Action([-2.40], [2.2]),
|
||||
Action([-0.45], [2.0]),
|
||||
],
|
||||
repeat=0,
|
||||
initial_speed=18. * CV.MPH_TO_MS,
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
def clone_maneuver(m: Maneuver, label_suffix: str = "") -> Maneuver:
|
||||
actions = [Action(list(a.accel_bp), list(a.time_bp)) for a in m.actions]
|
||||
@@ -170,6 +249,14 @@ def build_maneuvers(CP, label_suffix: str = "") -> list[Maneuver]:
|
||||
|
||||
# Keep brake step maneuvers grouped with other step responses.
|
||||
insert_idx = next((i for i, m in enumerate(maneuvers) if m.description.startswith("gas step response")), len(maneuvers))
|
||||
if is_gm_pedal_long:
|
||||
for probe in reversed(PEDAL_LONG_PADDLE_BLEND_PROBES):
|
||||
maneuvers.insert(insert_idx, clone_maneuver(probe, label_suffix))
|
||||
for terminal_probe in reversed(PEDAL_LONG_TERMINAL_STOP_PROBES):
|
||||
maneuvers.insert(insert_idx, clone_maneuver(terminal_probe, label_suffix))
|
||||
for stop_sweep in reversed(PEDAL_LONG_LOW_SPEED_STOP_SWEEP):
|
||||
maneuvers.insert(insert_idx, clone_maneuver(stop_sweep, label_suffix))
|
||||
|
||||
for step in reversed(brake_steps):
|
||||
maneuvers.insert(insert_idx, clone_maneuver(step, label_suffix))
|
||||
|
||||
|
||||
Reference in New Issue
Block a user