"""Post-intake data corrections for known CAN log issues."""
from __future__ import annotations
import numpy as np
import pandas as pd
MPH_TO_M_PER_S = 0.44704
IN_TO_M = 0.0254
[docs]
def patch_ned_velocity(
df: pd.DataFrame,
vel_x_col: str = "pcm.vnav.velocityBody.x",
vel_y_col: str = "pcm.vnav.velocityBody.y",
vel_z_col: str = "pcm.vnav.velocityBody.z",
yaw_col: str = "pcm.vnav.yawPitchRoll.yaw",
ned_cols: tuple[str, str, str] = ("velN", "velE", "velD"),
) -> pd.DataFrame:
"""Correct mislabeled NED velocities that were logged as body-frame.
Due to a VectorNav configuration issue, ``velocityBody.x/y/z``
actually contains NED (North/East/Down) velocities rather than
body-frame (Forward/Right/Down) velocities. This function:
1. Copies the raw NED data into new columns (*ned_cols*).
2. Rotates NED → FRD body frame using yaw and overwrites the
original ``velocityBody`` columns with corrected values.
The rotation (yaw-only, ignoring pitch/roll for ground vehicles)::
v_fwd = velN * cos(yaw) + velE * sin(yaw)
v_right = -velN * sin(yaw) + velE * cos(yaw)
v_down = velD (unchanged)
This is a **temporary workaround**. When the VectorNav is
reconfigured to output body-frame velocities natively, skip this
step entirely.
Parameters
----------
df : pd.DataFrame
DataFrame with velocity and yaw columns (typically from
``perda_to_dataframe``).
vel_x_col, vel_y_col, vel_z_col : str
The mislabeled velocity columns (actually NED).
yaw_col : str
Heading column in degrees (0 = North, 90 = East, CW positive).
ned_cols : tuple[str, str, str]
Names for the preserved NED copy columns.
Returns
-------
pd.DataFrame
The same DataFrame, modified in-place and returned for chaining.
"""
print(
"""
DEPRECATION WARNING: patch_ned_velocity is deprecated and will be removed in a future release.
Please reinstall PERDA. This functionality is achieved by the Analyzer class's preprocessing pipeline.
Reference documentation for perda.analyzer and perda.utils.preprocessing to learn how to setup an Analyzer
with patch_ned_velocity as a preprocessing step.
"""
)
vel_n = df[vel_x_col].values.astype(np.float64)
vel_e = df[vel_y_col].values.astype(np.float64)
vel_d = df[vel_z_col].values.astype(np.float64)
yaw_rad = np.radians(df[yaw_col].values.astype(np.float64))
# Sanity check: if the raw "body x" never drops below 5 m/s,
# it may already be true body-frame forward velocity (always positive
# when driving forward). NED North velocity should swing through
# low/negative values as heading changes.
if not np.any(vel_n < 5.0):
print(
f" [patch] WARNING: {vel_x_col} has no values < 5 m/s. "
f"The data may already be in body frame — "
f"the NED correction might not be needed."
)
# ZOH duplicate warning
for col_name, vals in [(vel_x_col, vel_n), (vel_y_col, vel_e)]:
dupes = np.sum(vals[1:] == vals[:-1])
ratio = dupes / max(len(vals) - 1, 1)
if ratio > 0.5:
print(
f" [patch] WARNING: {col_name} has {ratio:.0%} consecutive "
f"duplicates — likely ZOH at GPS rate. Consider adding "
f"velocity vars to deduplicate_vars at intake."
)
# Step 1: preserve raw NED
df[ned_cols[0]] = vel_n
df[ned_cols[1]] = vel_e
df[ned_cols[2]] = vel_d
# Step 2: rotate NED → body (FRD)
cos_y = np.cos(yaw_rad)
sin_y = np.sin(yaw_rad)
df[vel_x_col] = vel_n * cos_y + vel_e * sin_y # forward
df[vel_y_col] = -vel_n * sin_y + vel_e * cos_y # right
# vel_z (down) is identical in NED and FRD — no change needed
print(
f"[corrections] patch_ned_velocity: "
f"copied NED → {ned_cols}, "
f"rotated → body frame in {vel_x_col}, {vel_y_col} "
f"({len(df)} rows)"
)
return df
[docs]
def compute_ned_from_body(
df: pd.DataFrame,
vel_x_col: str = "pcm.vnav.velocityBody.x",
vel_y_col: str = "pcm.vnav.velocityBody.y",
vel_z_col: str = "pcm.vnav.velocityBody.z",
yaw_col: str = "pcm.vnav.yawPitchRoll.yaw",
ned_cols: tuple[str, str, str] = ("velN", "velE", "velD"),
) -> pd.DataFrame:
"""Compute NED velocities from body-frame (FRD) using yaw rotation.
Used when the VectorNav is correctly configured and velocityBody
columns contain true body-frame data. The inverse of the rotation
in ``patch_ned_velocity()``::
velN = v_fwd * cos(yaw) - v_right * sin(yaw)
velE = v_fwd * sin(yaw) + v_right * cos(yaw)
velD = v_down (unchanged)
"""
print(
"""
DEPRECATION WARNING: compute_ned_from_body is deprecated and will be removed in a future release.
Please use suboptimumg.log_analysis.kinematics.add_ned_velocities instead, which has the same
functionality and avoids the need to convert to pandas DataFrames.
"""
)
vel_fwd = df[vel_x_col].values.astype(np.float64)
vel_right = df[vel_y_col].values.astype(np.float64)
vel_down = df[vel_z_col].values.astype(np.float64)
yaw_rad = np.radians(df[yaw_col].values.astype(np.float64))
cos_y = np.cos(yaw_rad)
sin_y = np.sin(yaw_rad)
df[ned_cols[0]] = vel_fwd * cos_y - vel_right * sin_y # North
df[ned_cols[1]] = vel_fwd * sin_y + vel_right * cos_y # East
df[ned_cols[2]] = vel_down # Down
print(
f"[corrections] compute_ned_from_body: "
f"rotated body → NED in {ned_cols} ({len(df)} rows)"
)
return df
[docs]
def convert_wheelspeeds_to_m_per_s(
df: pd.DataFrame,
cols: list[str],
) -> pd.DataFrame:
"""Convert wheel speed columns from mph to m/s.
Backs up original mph values into ``{col}_mph`` columns before
overwriting with the converted values.
Parameters
----------
df : pd.DataFrame
cols : list[str]
Column names to convert.
Returns
-------
pd.DataFrame
The same DataFrame, modified in-place and returned for chaining.
"""
print(
"""
DEPRECATION WARNING: convert_wheelspeeds_to_m_per_s is deprecated and will be removed in a future release.
Please reinstall PERDA. This functionality is achieved by the Analyzer class's preprocessing pipeline.
Reference documentation for perda.analyzer and perda.utils.preprocessing to learn how to setup an Analyzer
with convert_wheelspeeds_to_m_per_s as a preprocessing step.
"""
)
for col in cols:
backup = col + "_mph"
if backup not in df.columns:
df[backup] = df[col].copy()
df[col] = df[col] * MPH_TO_M_PER_S
print(
f"[corrections] convert_wheelspeeds_to_m_per_s: "
f"converted {len(cols)} columns (mph → m/s), "
f"backups in *_mph"
)
for col in cols:
print(f" {col}")
return df
[docs]
def correct_motor_data(
df: pd.DataFrame,
rpm_col: str = "pcm.moc.motor.angularSpeed",
out_col: str = "pcm.moc.motor.wheelSpeed",
gear_ratio: float | None = None,
tire_radius_in: float | None = None,
car_yaml: str = "parameters/car.yaml",
) -> pd.DataFrame:
"""Flip motor RPM sign and compute driven wheel speed.
The inverter logs motor RPM as negative for forward motion.
This function negates the RPM (so positive = forward) and
computes the corresponding driven wheel linear speed in m/s.
Parameters
----------
df : pd.DataFrame
rpm_col : str
Motor angular speed column (RPM, raw sign: negative = forward).
out_col : str
Column name for the computed wheel speed (m/s).
gear_ratio : float or None
Overall gear ratio (motor RPM / wheel RPM). If ``None``,
loaded from *car_yaml* (``pwrtn.ratio``).
tire_radius_in : float or None
Loaded tire radius in inches. If ``None``, loaded from
*car_yaml* (``tires.tire_radius``).
car_yaml : str
Path to car YAML file, used when *gear_ratio* or
*tire_radius_in* are not provided.
Returns
-------
pd.DataFrame
Modified in-place and returned for chaining.
"""
print(
"""
DEPRECATION WARNING: correct_motor_data is deprecated and will be removed in a future release.
Please reinstall PERDA. This functionality is achieved by the Analyzer class's preprocessing pipeline.
Reference documentation for perda.analyzer and perda.utils.preprocessing to learn how to setup an Analyzer
with correct_motor_data as a preprocessing step.
"""
)
if gear_ratio is None or tire_radius_in is None:
import yaml
with open(car_yaml, "r") as f:
car_cfg = yaml.safe_load(f)
if gear_ratio is None:
gear_ratio = car_cfg["pwrtn"]["ratio"]
if tire_radius_in is None:
tire_radius_in = car_cfg["tires"]["tire_radius"]
print(
f"[corrections] correct_motor_data: loaded from {car_yaml} "
f"(ratio={gear_ratio}, tire_radius={tire_radius_in} in)"
)
backup = rpm_col + "_raw"
if backup not in df.columns:
df[backup] = df[rpm_col].copy()
df[rpm_col] = -df[rpm_col]
tire_radius_m = tire_radius_in * IN_TO_M
df[out_col] = df[rpm_col] * 2.0 * np.pi * tire_radius_m / (60.0 * gear_ratio)
print(
f"[corrections] correct_motor_data: "
f"flipped {rpm_col} sign (backup → {backup}), "
f"computed {out_col} "
f"(ratio={gear_ratio}, r={tire_radius_in} in)"
)
return df
# Default 3-point voltage→angle calibration for the Ludwig steering pot.
# Two-segment piecewise linear so each calibration point is hit exactly,
# which matters because the sensor is slightly asymmetric about center.
DEFAULT_STEERING_CALIBRATION: tuple[tuple[float, float], ...] = (
(1.86, -97.0), # max left
(2.93, 0.0), # zero
(3.96, 97.0), # max right
)
[docs]
def recompute_steering_angle(
df: pd.DataFrame,
raw_col: str = "ludwig.steeringWheel.raw",
angle_col: str = "ludwig.steeringWheel.angle",
calibration: tuple[tuple[float, float], ...] = DEFAULT_STEERING_CALIBRATION,
) -> pd.DataFrame:
"""Regenerate ``ludwig.steeringWheel.angle`` from the raw analog voltage.
The on-vehicle DSP that produces ``steeringWheel.angle`` has been
observed to drift relative to the raw pot signal. This function
rebuilds the angle column from ``steeringWheel.raw`` using a
least-squares polynomial fit through the calibration points
(quadratic by default — exact fit for the standard 3-point
left/center/right calibration, capturing the small nonlinearity
of the pot).
Any existing ``angle_col`` is preserved as ``{angle_col}_original``.
Parameters
----------
df : pd.DataFrame
Must contain *raw_col*.
raw_col : str
Raw analog steering voltage column (volts).
angle_col : str
Output / overwritten angle column (degrees).
calibration : tuple of (voltage, angle) pairs
Calibration points. With three points a quadratic fits all of
them exactly; with more points it's a least-squares quadratic.
With two points it falls back to a linear fit.
Returns
-------
pd.DataFrame
Modified in-place and returned for chaining.
"""
print(
"""
DEPRECATION WARNING: recompute_steering_angle is deprecated and will be removed in a future release.
Please reinstall PERDA. This functionality is achieved by the Analyzer class's preprocessing pipeline.
Reference documentation for perda.analyzer and perda.utils.preprocessing to learn how to setup an Analyzer
with correct_steering_angle as a preprocessing step.
"""
)
if raw_col not in df.columns:
raise KeyError(
f"recompute_steering_angle: '{raw_col}' not in DataFrame. "
f"Add it to your intake variable list."
)
pts = sorted(calibration, key=lambda p: p[0])
if len(pts) < 2:
raise ValueError("calibration needs at least 2 (voltage, angle) points.")
volts = np.array([p[0] for p in pts], dtype=np.float64)
angles = np.array([p[1] for p in pts], dtype=np.float64)
deg = 2 if len(pts) >= 3 else 1
coeffs = np.polyfit(volts, angles, deg)
raw = df[raw_col].values.astype(np.float64)
angle = np.polyval(coeffs, raw)
if angle_col in df.columns:
backup = angle_col + "_original"
if backup not in df.columns:
df[backup] = df[angle_col].copy()
df[angle_col] = angle
cal_str = ", ".join(f"{v:.2f}V→{a:+.1f}°" for v, a in pts)
coef_str = " + ".join(
f"{c:+.4g}*V^{deg - i}" if (deg - i) > 0 else f"{c:+.4g}"
for i, c in enumerate(coeffs)
)
print(
f"[corrections] recompute_steering_angle: rebuilt {angle_col} from "
f"{raw_col} using deg-{deg} fit ({cal_str})\n"
f" angle(V) = {coef_str}"
)
return df