mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-19 05:53:57 +08:00
whoisdomi
435 lines
15 KiB
Python
435 lines
15 KiB
Python
#!/usr/bin/env python3
|
|
import math
|
|
|
|
import numpy as np
|
|
|
|
from openpilot.common.constants import CV
|
|
from openpilot.common.filter_simple import FirstOrderFilter
|
|
from openpilot.common.realtime import DT_MDL
|
|
|
|
from openpilot.starpilot.common.starpilot_variables import (
|
|
CITY_SPEED_LIMIT,
|
|
CRUISING_SPEED,
|
|
DEFAULT_LATERAL_ACCELERATION,
|
|
PLANNER_TIME,
|
|
)
|
|
|
|
CALIBRATION_PROGRESS_THRESHOLD = 10 / DT_MDL
|
|
CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS
|
|
|
|
CSC_APPROACH_DECEL = 0.3
|
|
CSC_TARGET_UP_RATE = 3.0
|
|
CSC_TARGET_DOWN_RATE = 2.5
|
|
CSC_TARGET_FILTER_RC = 0.4
|
|
CSC_EGO_HEADROOM = 2.0
|
|
CSC_RELEASE_DEBOUNCE = 0.25
|
|
CSC_ACTIVE_ON_DELTA = 0.5
|
|
CSC_ACTIVE_OFF_DELTA = 0.25
|
|
CSC_GLOW_ON_DELTA = 1.0
|
|
CSC_GLOW_HOLD_TIME = 3.0
|
|
|
|
CSC_COUNT_CAP = 600
|
|
CSC_PRIOR_COUNT = 100
|
|
CSC_LAT_ACCEL_MIN = 1.2
|
|
CSC_LAT_ACCEL_MAX = 3.2
|
|
CSC_MAX_LATERAL_ACCEL = 4.0
|
|
CSC_NUDGE = 0.15
|
|
CSC_NUDGE_WEIGHT = 20
|
|
CSC_OVERRIDE_WATCH_TIME = 6.0
|
|
CSC_TRAINING_QUIET_TIME = 5.0
|
|
CSC_TRAINING_SETTLE_TIME = 2.0
|
|
CSC_COMFORT_MARGIN = 1.0
|
|
|
|
CSC_FARFIELD_MIN_CURVATURE = 0.004
|
|
CSC_FARFIELD_MIN_DISTANCE = 30.0
|
|
CSC_FARFIELD_GAIN = 1.23
|
|
|
|
MIN_CURVATURE = 0.0005
|
|
MAX_CURVATURE = 0.02
|
|
CURVATURE_BUCKETS = 24
|
|
ROUNDING_PRECISION = 6
|
|
CURVATURE_GRID = MIN_CURVATURE * np.power(MAX_CURVATURE / MIN_CURVATURE,
|
|
np.arange(CURVATURE_BUCKETS) / (CURVATURE_BUCKETS - 1))
|
|
LOG_CURVATURE_GRID = np.log(CURVATURE_GRID)
|
|
|
|
|
|
PRIOR_CURVATURE_BP = [0.001, 0.003, 0.01, 0.03, 0.1]
|
|
PRIOR_LAT_ACCEL_V = [1.5, 1.8, 2.2, 2.6, 2.9]
|
|
|
|
|
|
def weighted_isotonic(values, weights):
|
|
"""Weighted non-decreasing fit (pool adjacent violators).
|
|
|
|
Keeps comfort from falling as curves tighten, without letting a sparse bucket
|
|
overrule a well-sampled neighbour the way a running maximum would.
|
|
"""
|
|
block_values: list[float] = []
|
|
block_weights: list[float] = []
|
|
block_sizes: list[int] = []
|
|
|
|
for value, weight in zip(values, weights, strict=True):
|
|
block_values.append(float(value))
|
|
block_weights.append(float(weight))
|
|
block_sizes.append(1)
|
|
|
|
while len(block_values) > 1 and block_values[-2] > block_values[-1]:
|
|
merged_weight = block_weights[-2] + block_weights[-1]
|
|
merged_value = ((block_values[-2] * block_weights[-2]) + (block_values[-1] * block_weights[-1])) / merged_weight
|
|
block_values.pop()
|
|
block_weights.pop()
|
|
merged_size = block_sizes.pop()
|
|
block_values[-1] = merged_value
|
|
block_weights[-1] = merged_weight
|
|
block_sizes[-1] += merged_size
|
|
|
|
fitted = np.empty(len(values))
|
|
index = 0
|
|
for value, size in zip(block_values, block_sizes, strict=True):
|
|
fitted[index:index + size] = value
|
|
index += size
|
|
return fitted
|
|
|
|
|
|
def is_user_overriding_longitudinal(sm):
|
|
try:
|
|
if any(getattr(event, "overrideLongitudinal", False) for event in sm["onroadEvents"]):
|
|
return True
|
|
except (KeyError, TypeError):
|
|
pass
|
|
|
|
car_state = sm["carState"]
|
|
starpilot_car_state = sm["starpilotCarState"]
|
|
return bool(
|
|
getattr(car_state, "gasPressed", False) or
|
|
getattr(car_state, "brakePressed", False) or
|
|
getattr(starpilot_car_state, "accelPressed", False)
|
|
)
|
|
|
|
|
|
def is_manual_speed_control(sm):
|
|
"""Return whether the driver, rather than longitudinal control, owns speed."""
|
|
return not bool(sm["carControl"].longActive) or is_user_overriding_longitudinal(sm)
|
|
|
|
|
|
class CurveSpeedController:
|
|
def __init__(self, StarPilotVCruise):
|
|
self.starpilot_planner = StarPilotVCruise.starpilot_planner
|
|
|
|
self.enable_training = False
|
|
self.nudge_applied = False
|
|
|
|
self.override_watch_key = None
|
|
self.override_watch_peak = 0.0
|
|
self.override_watch_timer = 0.0
|
|
|
|
self.training_timer = 0.0
|
|
self.persistence_timer = 0.0
|
|
self.training_quiet_timer = 0.0
|
|
self.data_dirty = False
|
|
|
|
self.target = 0.0
|
|
self.binding_distance = 0.0
|
|
self.release_timer = 0.0
|
|
self.target_filter = FirstOrderFilter(0.0, CSC_TARGET_FILTER_RC, DT_MDL, initialized=False)
|
|
self.seed_pending = True
|
|
|
|
self._long_active_prev = False
|
|
|
|
curvature_data = self.starpilot_planner.params.get("CurvatureData")
|
|
self.curvature_data = self._normalize_curvature_data(curvature_data)
|
|
|
|
self.required_curvatures = [self._bucket_curvature(curvature) for curvature in CURVATURE_GRID]
|
|
|
|
self.rebuild_lat_accel_curve()
|
|
|
|
self.data_dirty = True
|
|
|
|
self._publish_live_values()
|
|
|
|
@staticmethod
|
|
def _bucket_curvature(road_curvature):
|
|
clipped_curvature = float(np.clip(abs(road_curvature), MIN_CURVATURE, MAX_CURVATURE))
|
|
|
|
bucket_index = int(np.argmin(np.abs(LOG_CURVATURE_GRID - np.log(clipped_curvature))))
|
|
return str(round(float(CURVATURE_GRID[bucket_index]), ROUNDING_PRECISION))
|
|
|
|
@classmethod
|
|
def _normalize_curvature_data(cls, curvature_data):
|
|
if not isinstance(curvature_data, dict):
|
|
return {}
|
|
|
|
normalized = {}
|
|
for key, value in curvature_data.items():
|
|
if not isinstance(value, dict):
|
|
continue
|
|
|
|
try:
|
|
raw_curvature = abs(float(key))
|
|
average = float(value["average"])
|
|
count = int(value["count"])
|
|
except (KeyError, TypeError, ValueError):
|
|
continue
|
|
|
|
if count <= 0 or not math.isfinite(raw_curvature) or not math.isfinite(average):
|
|
continue
|
|
|
|
bucket = cls._bucket_curvature(raw_curvature)
|
|
if bucket in normalized:
|
|
existing = normalized[bucket]
|
|
total_count = existing["count"] + count
|
|
normalized[bucket] = {
|
|
"average": ((existing["average"] * existing["count"]) + (average * count)) / total_count,
|
|
"count": total_count,
|
|
}
|
|
else:
|
|
normalized[bucket] = {
|
|
"average": average,
|
|
"count": count,
|
|
}
|
|
|
|
return normalized
|
|
|
|
def _calibration_progress(self):
|
|
progress = 0.0
|
|
for key in self.required_curvatures:
|
|
if key in self.curvature_data:
|
|
progress += min(self.curvature_data[key]["count"] / CALIBRATION_PROGRESS_THRESHOLD, 1.0)
|
|
return (progress / len(self.required_curvatures)) * 100
|
|
|
|
def _publish_live_values(self, progress=None):
|
|
params_memory = getattr(self.starpilot_planner, "params_memory", None)
|
|
if params_memory is None:
|
|
return
|
|
if progress is None:
|
|
progress = self._calibration_progress()
|
|
params_memory.put_nonblocking("CalibratedLateralAcceleration", self.lateral_acceleration)
|
|
params_memory.put_nonblocking("CalibrationProgress", progress)
|
|
|
|
def _persist_data(self):
|
|
if not self.data_dirty:
|
|
return
|
|
|
|
progress = self._calibration_progress()
|
|
self.starpilot_planner.params.put_nonblocking("CalibratedLateralAcceleration", self.lateral_acceleration)
|
|
self.starpilot_planner.params.put_nonblocking("CalibrationProgress", progress)
|
|
self.starpilot_planner.params.put_nonblocking("CurvatureData", self.curvature_data)
|
|
self._publish_live_values(progress)
|
|
self.data_dirty = False
|
|
self.persistence_timer = 0.0
|
|
|
|
def flush_data(self):
|
|
self._persist_data()
|
|
|
|
def log_data(self, v_ego, sm):
|
|
self.training_quiet_timer = max(self.training_quiet_timer - DT_MDL, 0.0)
|
|
|
|
eligible = (
|
|
v_ego > CRUISING_SPEED and
|
|
not self.starpilot_planner.tracking_lead and
|
|
is_manual_speed_control(sm) and
|
|
self.training_quiet_timer <= 0.0
|
|
)
|
|
self.enable_training = False
|
|
|
|
if not eligible:
|
|
self.flush_data()
|
|
|
|
self.training_timer = max(self.training_timer - DT_MDL, 0.0)
|
|
self.persistence_timer = 0.0
|
|
return
|
|
|
|
self.training_timer += DT_MDL
|
|
if self.data_dirty:
|
|
self.persistence_timer += DT_MDL
|
|
|
|
in_curve = (
|
|
self.training_timer >= CSC_TRAINING_SETTLE_TIME and
|
|
self.starpilot_planner.driving_in_curve and
|
|
not (sm["carState"].leftBlinker or sm["carState"].rightBlinker)
|
|
)
|
|
if in_curve:
|
|
lateral_acceleration = abs(self.starpilot_planner.lateral_acceleration)
|
|
road_curvature = self._bucket_curvature(abs(self.starpilot_planner.road_curvature))
|
|
|
|
if road_curvature in self.curvature_data:
|
|
data = self.curvature_data[road_curvature]
|
|
|
|
effective_count = min(data["count"], CSC_COUNT_CAP)
|
|
self.curvature_data[road_curvature] = {
|
|
"average": ((data["average"] * effective_count) + lateral_acceleration) / (effective_count + 1),
|
|
"count": data["count"] + 1
|
|
}
|
|
else:
|
|
self.curvature_data[road_curvature] = {
|
|
"average": lateral_acceleration,
|
|
"count": 1
|
|
}
|
|
|
|
self.data_dirty = True
|
|
self.rebuild_lat_accel_curve()
|
|
self._publish_live_values()
|
|
self.enable_training = True
|
|
|
|
if self.persistence_timer >= PLANNER_TIME:
|
|
self.flush_data()
|
|
elif self.data_dirty:
|
|
self.flush_data()
|
|
|
|
def handle_override(self, v_ego, was_controlling, sm, accel_button=False):
|
|
long_active = bool(sm["carControl"].longActive)
|
|
long_dropped = self._long_active_prev and not long_active
|
|
self._long_active_prev = long_active
|
|
|
|
self._update_override_watch(sm)
|
|
|
|
if not was_controlling:
|
|
self.nudge_applied = False
|
|
return
|
|
|
|
if self.nudge_applied:
|
|
return
|
|
|
|
if accel_button or (sm["carState"].gasPressed and self.target < v_ego - 0.5):
|
|
self.override_watch_key = self._bucket_curvature(abs(self.starpilot_planner.road_curvature))
|
|
self.override_watch_peak = abs(self.starpilot_planner.lateral_acceleration)
|
|
self.override_watch_timer = CSC_OVERRIDE_WATCH_TIME
|
|
self.nudge_applied = True
|
|
elif (getattr(sm["carState"], "brakePressed", False) or long_dropped) and self.starpilot_planner.driving_in_curve:
|
|
self._apply_nudge(-CSC_NUDGE)
|
|
|
|
def _update_override_watch(self, sm):
|
|
if self.override_watch_key is None:
|
|
return
|
|
|
|
lateral_acceleration = abs(self.starpilot_planner.lateral_acceleration)
|
|
if lateral_acceleration > self.override_watch_peak:
|
|
|
|
self.override_watch_peak = lateral_acceleration
|
|
self.override_watch_key = self._bucket_curvature(abs(self.starpilot_planner.road_curvature))
|
|
|
|
self.override_watch_timer -= DT_MDL
|
|
if self.override_watch_timer > 0.0 and (is_user_overriding_longitudinal(sm) or
|
|
self.starpilot_planner.driving_in_curve):
|
|
return
|
|
|
|
key = self.override_watch_key
|
|
self.override_watch_key = None
|
|
|
|
self._record_pseudo_sample(key, max(self.override_watch_peak,
|
|
self.learned_lat_accel(float(key)) + CSC_NUDGE))
|
|
|
|
def _apply_nudge(self, offset):
|
|
key = self._bucket_curvature(abs(self.starpilot_planner.road_curvature))
|
|
|
|
self._record_pseudo_sample(key, self.learned_lat_accel(float(key)) + offset)
|
|
self.nudge_applied = True
|
|
|
|
def _record_pseudo_sample(self, key, sample):
|
|
sample = float(np.clip(sample, CSC_LAT_ACCEL_MIN, CSC_LAT_ACCEL_MAX))
|
|
|
|
data = self.curvature_data.get(key, {"average": sample, "count": 0})
|
|
effective_count = min(data["count"], CSC_COUNT_CAP)
|
|
total = effective_count + CSC_NUDGE_WEIGHT
|
|
self.curvature_data[key] = {
|
|
"average": ((data["average"] * effective_count) + (sample * CSC_NUDGE_WEIGHT)) / total,
|
|
"count": data["count"] + CSC_NUDGE_WEIGHT,
|
|
}
|
|
|
|
self.rebuild_lat_accel_curve()
|
|
self.data_dirty = True
|
|
self.flush_data()
|
|
|
|
def rebuild_lat_accel_curve(self):
|
|
grid_k = np.array([float(key) for key in self.required_curvatures])
|
|
prior = np.interp(grid_k, PRIOR_CURVATURE_BP, PRIOR_LAT_ACCEL_V)
|
|
|
|
blended = prior.copy()
|
|
counts = np.zeros(len(grid_k))
|
|
for i, key in enumerate(self.required_curvatures):
|
|
data = self.curvature_data.get(key)
|
|
if data:
|
|
confidence = data["count"] / (data["count"] + CSC_PRIOR_COUNT)
|
|
blended[i] = confidence * data["average"] + (1.0 - confidence) * prior[i]
|
|
counts[i] = data["count"]
|
|
|
|
blended = np.clip(blended, CSC_LAT_ACCEL_MIN, CSC_LAT_ACCEL_MAX)
|
|
blended = weighted_isotonic(blended, counts + CSC_PRIOR_COUNT)
|
|
|
|
self._curve_k = grid_k
|
|
self._curve_a = blended
|
|
|
|
if counts.sum() > 0:
|
|
self.lateral_acceleration = float(np.average(blended, weights=counts))
|
|
else:
|
|
self.lateral_acceleration = DEFAULT_LATERAL_ACCELERATION
|
|
|
|
def learned_lat_accel(self, curvature):
|
|
"""Comfort level learned for this curvature, before any control margin."""
|
|
return float(np.interp(abs(curvature), self._curve_k, self._curve_a))
|
|
|
|
def lat_accel_for_curvature(self, curvature):
|
|
lat_accel = np.interp(np.abs(curvature), self._curve_k, self._curve_a) * CSC_COMFORT_MARGIN
|
|
|
|
weather = self.starpilot_planner.starpilot_weather
|
|
if weather.weather_id != 0:
|
|
lat_accel = lat_accel * (1.0 - weather.reduce_lateral_acceleration)
|
|
|
|
return lat_accel
|
|
|
|
@staticmethod
|
|
def _correct_far_field(curvatures, distances):
|
|
"""Undo the model's known under-read of distant curvature, where the reading is firm."""
|
|
firm = (curvatures >= CSC_FARFIELD_MIN_CURVATURE) & (distances >= CSC_FARFIELD_MIN_DISTANCE)
|
|
return np.minimum(np.where(firm, curvatures * CSC_FARFIELD_GAIN, curvatures), MAX_CURVATURE)
|
|
|
|
def reset(self, v_cruise):
|
|
self.target = float(v_cruise)
|
|
self.release_timer = 0.0
|
|
self.target_filter.x = float(v_cruise)
|
|
self.target_filter.initialized = True
|
|
self.seed_pending = True
|
|
|
|
def update_target(self, v_ego, v_cruise):
|
|
if not self.target_filter.initialized:
|
|
self.reset(v_cruise)
|
|
|
|
curvatures, distances = self.starpilot_planner.curve_profile
|
|
if len(curvatures) == 0:
|
|
raw_target = float(v_cruise)
|
|
self.binding_distance = 0.0
|
|
else:
|
|
curvatures = self._correct_far_field(curvatures, distances)
|
|
lat_accel = self.lat_accel_for_curvature(curvatures)
|
|
curvature_floor = np.maximum(curvatures, 1e-4)
|
|
point_speeds = np.sqrt(lat_accel / curvature_floor)
|
|
point_speeds = np.minimum(
|
|
np.maximum(point_speeds, CSC_MIN_SPEED),
|
|
np.sqrt(CSC_MAX_LATERAL_ACCEL / curvature_floor),
|
|
)
|
|
allowed_speeds = np.sqrt(point_speeds**2 + 2.0 * CSC_APPROACH_DECEL * np.maximum(distances, 0.0))
|
|
binding_index = int(np.argmin(allowed_speeds))
|
|
raw_target = min(float(allowed_speeds[binding_index]), float(v_cruise))
|
|
self.binding_distance = float(distances[binding_index]) if raw_target < v_cruise else 0.0
|
|
|
|
if self.seed_pending:
|
|
seed = min(float(v_cruise), max(raw_target, v_ego + CSC_EGO_HEADROOM))
|
|
self.target = seed
|
|
self.target_filter.x = seed
|
|
self.seed_pending = False
|
|
|
|
if raw_target >= v_ego:
|
|
self.release_timer += DT_MDL
|
|
else:
|
|
self.release_timer = 0.0
|
|
|
|
filtered = self.target_filter.update(raw_target)
|
|
self.target = float(np.clip(max(filtered, min(raw_target, v_ego + CSC_EGO_HEADROOM)),
|
|
self.target - CSC_TARGET_DOWN_RATE * DT_MDL,
|
|
self.target + CSC_TARGET_UP_RATE * DT_MDL))
|
|
|
|
if self.release_timer >= CSC_RELEASE_DEBOUNCE:
|
|
self.target = max(self.target, min(raw_target, v_ego))
|
|
|
|
if self.target < v_cruise - CSC_ACTIVE_ON_DELTA:
|
|
self.training_quiet_timer = CSC_TRAINING_QUIET_TIME
|