mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-21 08:14:00 +08:00
Updates
This commit is contained in:
@@ -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