"""High-level convenience macros that chain multiple loganalysis steps."""
from __future__ import annotations
from typing import TYPE_CHECKING
import pandas as pd
from .corrections import (
compute_ned_from_body,
convert_wheelspeeds_to_m_per_s,
correct_motor_data,
patch_ned_velocity,
)
from .derived import compute_groundspeed
from .filtering import lowpass_filter, plot_fft, zscore_filter
from .gps import (
compute_elapsed_distance,
prepare_gps_data,
resolve_gps_columns,
)
from .intake import perda_to_dataframe
from .plotting import plot_log
if TYPE_CHECKING:
import plotly.graph_objects as go
from perda.analyzer import Analyzer
# ---------------------------------------------------------------------------
# Default intake variable list for vehicle dynamics analysis
# ---------------------------------------------------------------------------
VD_INTAKE_VARS: list[str] = [
# VectorNav GPS/INS
"pcm.vnav.posLla.latitude",
"pcm.vnav.posLla.longitude",
"pcm.vnav.posLla.altitude",
"pcm.vnav.gpsFix",
"pcm.vnav.yawPitchRoll.yaw",
"pcm.vnav.yawPitchRoll.pitch",
"pcm.vnav.yawPitchRoll.roll",
"pcm.vnav.velocityBody.x",
"pcm.vnav.velocityBody.y",
"pcm.vnav.velocityBody.z",
"pcm.vnav.compensatedAccel.x",
"pcm.vnav.compensatedAccel.y",
"pcm.vnav.compensatedAccel.z",
"pcm.vnav.linearAccelBody.x",
"pcm.vnav.linearAccelBody.y",
"pcm.vnav.linearAccelBody.z",
"pcm.vnav.compensatedAngularRate.x",
"pcm.vnav.compensatedAngularRate.y",
"pcm.vnav.compensatedAngularRate.z",
# Wheel speeds
"pcm.wheelSpeeds.frontLeft",
"pcm.wheelSpeeds.frontRight",
"pcm.wheelSpeeds.backLeft",
"pcm.wheelSpeeds.backRight",
# Shock pots
"ludwig.shockpot.frontLeft",
"ludwig.shockpot.frontRight",
"ludwig.shockpot.rearLeft",
"ludwig.shockpot.rearRight",
# Motor / inverter
"pcm.moc.motor.angularSpeed",
"pcm.moc.motor.torqueFeedback",
"pcm.moc.motor.requestedTorque",
"pcm.moc.motor.driverTorque",
"pcm.moc.temp",
"pcm.moc.motor.temp",
# Brakes
"pcm.pedals.brakePressure.front",
"pcm.pedals.brakePressure.rear",
# Traction control
"pcm.tractionControl.slipRatio",
# Cooling
"pcm.coolingLoop.temp",
# Steering
"ludwig.steeringWheel.angle",
"ludwig.steeringWheel.raw",
# Tire temperatures
"ludwig.tireTemps.sensorTemp[0]",
"ludwig.tireTemps.sensorTemp[1]",
"ludwig.tireTemps.sensorTemp[2]",
"ludwig.tireTemps.sensorTemp[3]",
# BMS
"bms.pack.voltage",
"bms.pack.current",
"bms.pack.power",
"bms.stack.mma.temp.min",
"bms.stack.mma.temp.max",
"bms.stack.mma.temp.avg",
]
_VD_DEDUP_VARS: list[str] = [
"pcm.vnav.posLla.latitude",
"pcm.vnav.posLla.longitude",
"pcm.vnav.velocityBody.x",
"pcm.vnav.velocityBody.y",
"pcm.vnav.velocityBody.z",
]
# VectorNav writes lat/lon = 0.0 as a "no INS solution" sentinel, sometimes
# even when gpsFix == 3. These samples must be removed from the source
# DataInstance *before* dedup/resampling — otherwise dedup collapses long
# zero runs to a single (t, 0.0) marker and linear interpolation then ramps
# from 0 up to the next real fix, smearing garbage across the whole gap.
_GPS_SENTINEL_VARS: tuple[str, ...] = (
"pcm.vnav.posLla.latitude",
"pcm.vnav.posLla.longitude",
)
def _strip_gps_sentinel_zeros(analyzer: Analyzer) -> None:
"""Drop lat/lon samples whose value is exactly 0.0 from the analyzer.
Mutates ``analyzer.data.id_to_instance`` in place, replacing each
sentinel-affected ``DataInstance`` with a copy whose timestamp/value
arrays have the zero samples removed. Variables that aren't present
or have no zero samples are left untouched.
"""
from perda.analyzer import DataInstance
for name in _GPS_SENTINEL_VARS:
if name not in analyzer.data:
continue
di = analyzer.data[name]
vals = di.value_np
keep = vals != 0.0
n_drop = int((~keep).sum())
if n_drop == 0:
continue
analyzer.data.id_to_instance[di.var_id] = DataInstance(
timestamp_np=di.timestamp_np[keep],
value_np=vals[keep],
label=di.label,
var_id=di.var_id,
)
print(
f"[macro] strip_gps_sentinel_zeros: {name}: "
f"dropped {n_drop}/{len(vals)} (0.0) samples"
)
# ---------------------------------------------------------------------------
# Column matching helper
# ---------------------------------------------------------------------------
_BACKUP_SUFFIXES = ("_pre_lp", "_raw", "_mph", "_original")
_NEVER_FILTER = {
"pcm.vnav.yawPitchRoll.yaw", # wrapping discontinuities; filter after unwrap
}
def _match_columns(
df: pd.DataFrame,
partial_path: str,
exclude: set[str] | None = None,
) -> list[str]:
"""Return columns whose name starts with *partial_path* + '.'.
Excludes backup columns (``_pre_lp``, ``_raw``, ``_mph``,
``_original``). Pass *exclude* to additionally skip specific
column names (used by filter macros to protect yaw etc.).
"""
prefix = partial_path if partial_path.endswith(".") else partial_path + "."
matches = []
for c in df.columns:
if not c.startswith(prefix):
continue
if any(c.endswith(s) for s in _BACKUP_SUFFIXES):
continue
if exclude and c in exclude:
print(f" [macro] Skipping {c} (protected from filtering)")
continue
matches.append(c)
if not matches:
raise KeyError(
f"No columns match prefix '{prefix}'. "
f"Available: {[c for c in df.columns if partial_path.split('.')[0] in c]}"
)
return sorted(matches)
# ---------------------------------------------------------------------------
# Intake
# ---------------------------------------------------------------------------
[docs]
def intake_vd_starter_kit(
analyzer: Analyzer,
deduplicate: bool = True,
resample_method: str | dict[str, str] = "linear",
) -> pd.DataFrame:
"""Load the standard vehicle-dynamics variable set into a DataFrame.
Wraps ``perda_to_dataframe`` with the default VD variable list and
ZOH deduplication on VectorNav position and velocity channels.
``pcm.vnav.gpsFix`` is always resampled with zero-order hold (it is
a discrete state, not a continuous signal — linear interpolation
between ``0`` and ``3`` would produce nonsensical fractional fix
values that defeat downstream gpsFix-based masking).
Sentinel ``0.0`` samples in ``posLla.latitude``/``.longitude``
(VectorNav's "no INS solution" marker) are stripped from the source
``DataInstance`` before resampling so dedup + linear interpolation
don't smear them into ramps across no-fix gaps.
"""
_strip_gps_sentinel_zeros(analyzer)
if isinstance(resample_method, str):
method_arg: dict[str, str] | str = {"pcm.vnav.gpsFix": "zoh"}
# All other vars fall back to the user-requested default via the
# per-name dict lookup in perda_to_dataframe (defaults to "linear"
# for missing keys), so we preserve `resample_method` only when
# it differs from the dict default.
if resample_method != "linear":
for v in VD_INTAKE_VARS:
if v != "pcm.vnav.gpsFix":
method_arg[v] = resample_method
else:
method_arg = {"pcm.vnav.gpsFix": "zoh", **resample_method}
dedup = _VD_DEDUP_VARS if deduplicate else None
return perda_to_dataframe(
analyzer,
VD_INTAKE_VARS,
resample_method=method_arg,
deduplicate_vars=dedup,
)
# ---------------------------------------------------------------------------
# Speed preprocessing
# ---------------------------------------------------------------------------
_WHEELSPEED_COLS = [
"pcm.wheelSpeeds.frontLeft",
"pcm.wheelSpeeds.frontRight",
"pcm.wheelSpeeds.backLeft",
"pcm.wheelSpeeds.backRight",
]
[docs]
def preprocess_speeds(
df: pd.DataFrame,
patch_ned: bool = True,
car_yaml: str = "parameters/car.yaml",
) -> pd.DataFrame:
"""NED velocity patch, wheelspeed unit conversion, motor correction, ground speed."""
if patch_ned:
df = patch_ned_velocity(df)
else:
df = compute_ned_from_body(df)
df = convert_wheelspeeds_to_m_per_s(df, _WHEELSPEED_COLS)
df = correct_motor_data(df, car_yaml=car_yaml)
df = compute_groundspeed(df)
return df
# ---------------------------------------------------------------------------
# GPS preprocessing
# ---------------------------------------------------------------------------
[docs]
def preprocess_gps(df: pd.DataFrame) -> pd.DataFrame:
"""Resolve GPS columns, clean/project, and compute elapsed distance."""
df = resolve_gps_columns(df)
df = prepare_gps_data(df)
df = compute_elapsed_distance(df)
return df
# ---------------------------------------------------------------------------
# Group operations (column prefix matching)
# ---------------------------------------------------------------------------
[docs]
def fft_group(
df: pd.DataFrame,
partial_path: str,
log_y: bool = False,
stacked: bool = False,
**kwargs,
) -> go.Figure:
"""Plot FFT for all columns matching *partial_path*.*.
Default is linear y-axis and overlaid traces.
"""
cols = _match_columns(df, partial_path)
print(f"[macro] fft_group: {partial_path} → {cols}")
return plot_fft(df, cols, stacked=stacked, log_y=log_y, **kwargs)
[docs]
def lowpass_group(
df: pd.DataFrame,
partial_path: str,
cutoff_hz: float,
**kwargs,
) -> pd.DataFrame:
"""Lowpass filter all columns matching *partial_path*.*."""
cols = _match_columns(df, partial_path, exclude=_NEVER_FILTER)
print(f"[macro] lowpass_group: {partial_path} → {cols}")
return lowpass_filter(df, cols, cutoff_hz, **kwargs)
[docs]
def zfilter_group(
df: pd.DataFrame,
partial_path: str,
window_s: float = 2.0,
threshold: float = 4.8,
**kwargs,
) -> pd.DataFrame:
"""Z-score filter all columns matching *partial_path*.*."""
cols = _match_columns(df, partial_path, exclude=_NEVER_FILTER)
print(f"[macro] zfilter_group: {partial_path} → {cols}")
return zscore_filter(df, cols, window_s=window_s, threshold=threshold, **kwargs)
[docs]
def plot_group(
df: pd.DataFrame,
partial_path: str,
stacked: bool = False,
**kwargs,
) -> go.Figure:
"""Plot all columns matching *partial_path*.*.
Default is overlaid traces on a single axis.
"""
cols = _match_columns(df, partial_path)
print(f"[macro] plot_group: {partial_path} → {cols}")
return plot_log(df, cols, stacked=stacked, **kwargs)
# ---------------------------------------------------------------------------
# Speed filtering
# ---------------------------------------------------------------------------
_SPEED_ZSCORE_COLS = [
"pcm.wheelSpeeds.frontLeft",
"pcm.wheelSpeeds.frontRight",
"pcm.wheelSpeeds.backLeft",
"pcm.wheelSpeeds.backRight",
]
_SPEED_LP_COLS = [
"pcm.wheelSpeeds.frontLeft",
"pcm.wheelSpeeds.frontRight",
"pcm.wheelSpeeds.backLeft",
"pcm.wheelSpeeds.backRight",
"pcm.moc.motor.wheelSpeed",
"groundSpeed",
]
[docs]
def filter_speeds(
df: pd.DataFrame,
zscore_window: float = 8.0,
zscore_thresh: float = 2.5,
lowpass_hz: float = 3.0,
) -> pd.DataFrame:
"""Z-score + lowpass pipeline for wheel speed signals.
1. Z-score filter the four raw wheelspeed sensors.
2. Recompute groundSpeed from the cleaned front wheelspeeds.
3. Lowpass all wheelspeeds, motor wheelSpeed, and groundSpeed.
"""
df = zscore_filter(
df, _SPEED_ZSCORE_COLS, window_s=zscore_window, threshold=zscore_thresh
)
df = compute_groundspeed(df)
df = lowpass_filter(df, _SPEED_LP_COLS, cutoff_hz=lowpass_hz)
return df
# ---------------------------------------------------------------------------
# Derived quantities (stub)
# ---------------------------------------------------------------------------
[docs]
def derive_quantities(df: pd.DataFrame) -> pd.DataFrame:
"""Compute all standard derived quantities.
TODO: Wire up the full set of derived.py calls.
"""
pass
# ---------------------------------------------------------------------------
# QSS simulation
# ---------------------------------------------------------------------------
[docs]
def simulate_on_track(
lap_df: pd.DataFrame,
track,
car_yaml: str = "../parameters/car.yaml",
velocity_col: str = "groundSpeed",
):
"""Load a car, simulate on a Track, and return a QSS DataFrame.
Parameters
----------
lap_df : DataFrame
The trimmed single-lap DataFrame used to build the track.
track : Track
A QSS-compatible Track (from :func:`build_qss_track`).
car_yaml : str
Path to the car YAML config file.
velocity_col : str
Column to read IRL initial speed from.
Returns
-------
(sim, qss_df)
*sim* is the :class:`CustomRun` instance (carries push/coast
results). *qss_df* is a distance-indexed DataFrame with
``qss.*`` columns.
"""
from suboptimumg.compsim import CustomRun
from suboptimumg.yaml import load_car_from_yaml
from .track_builder import qss_results_to_dataframe
initial_speed = float(lap_df[velocity_col].iloc[0])
car = load_car_from_yaml(car_yaml)
sim = CustomRun(mycar=car, track=track)
push, coast = sim.run(extract_internal_data=True, initial_velocity=initial_speed)
sim.print_results_summary()
qss_df = qss_results_to_dataframe(push, track)
return sim, qss_df