mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-23 02:02:08 +08:00
whoisdomi
766 lines
43 KiB
Python
766 lines
43 KiB
Python
#!/usr/bin/env python3
|
|
import math
|
|
from numbers import Number
|
|
|
|
from cereal import car, custom, log
|
|
import cereal.messaging as messaging
|
|
from openpilot.common.constants import CV
|
|
from openpilot.common.params import Params
|
|
from openpilot.common.realtime import config_realtime_process, DT_CTRL, Priority, Ratekeeper
|
|
from openpilot.common.swaglog import cloudlog
|
|
|
|
from opendbc.car.car_helpers import interfaces
|
|
from opendbc.car.chrysler.values import pacifica_hybrid_aol_stock_acc_mode
|
|
from opendbc.car.gm.values import CAR as GM_CAR
|
|
from opendbc.car.vehicle_model import VehicleModel
|
|
from openpilot.selfdrive.controls.lib.drive_helpers import MAX_LATERAL_JERK, clip_curvature, get_lateral_active
|
|
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
|
|
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
|
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
|
|
from openpilot.selfdrive.controls.lib.latcontrol_torque import (
|
|
BOLT_2018_2021_STEER_RATIO_TEST_SCALE,
|
|
LatControlTorque,
|
|
get_bolt_2017_steer_ratio_scale,
|
|
)
|
|
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
|
|
from openpilot.selfdrive.car.cruise_state import should_cancel_stock_cruise
|
|
from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
|
|
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
|
|
|
|
from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles
|
|
from openpilot.starpilot.controls.lib.neural_network_feedforward import LatControlNNFF
|
|
|
|
State = log.SelfdriveState.OpenpilotState
|
|
LaneChangeState = log.LaneChangeState
|
|
LaneChangeDirection = log.LaneChangeDirection
|
|
|
|
ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
|
|
|
|
# After a smoothed lane change ends, ramp the curvature limits back to stock over this
|
|
# time so the final recenter correction is shaped instead of stepping through unclamped.
|
|
LANE_CHANGE_SMOOTH_RELEASE_T = 2.0
|
|
|
|
# Cap on the extra jerk factor granted while the model is unwinding lane-change curvature
|
|
# (the arrest and any correction back toward center). Entry gentleness is comfort, but arrest
|
|
# speed is a correctness constraint: a slow symmetric cap lets the car glide past the new lane
|
|
# center. 0.6 (≈ pace-5 rate) fully tracks the arrest demand seen in logs, 40% below stock.
|
|
LANE_CHANGE_ARREST_JERK_FLOOR = 0.6
|
|
# The extra unwind authority is proportional to how far the command lags the model
|
|
# (rate = lag/tau, a P-pursuit), NOT a fixed fast rate: a boolean-gated floor engages as a
|
|
# bang-bang switch right at the maneuver crest — where the slow entry command meets a model
|
|
# that already peaked and is diving — snapping the wheel ~2 deg in 0.2 s (lanechange1/2
|
|
# rlogs 2026-07-17). With pursuit, lag ~0 at the crest so the rate crosses zero smoothly,
|
|
# and noise-scale lag inside the deadband gets no boost (kills the fast-down/slow-up sawtooth).
|
|
LANE_CHANGE_ARREST_PURSUIT_TAU = 0.2 # s
|
|
LANE_CHANGE_ARREST_GAP_DEADBAND = 5e-5 # 1/m
|
|
|
|
# Low-speed turn-intent curvature hold. Approaching a turn with the blinker on, the
|
|
# model's time-based plan collapses as the car slows to a stop: desiredCurvature decays
|
|
# to zero, the controller actively unwinds the wheel at the intersection, and on
|
|
# pull-away it re-winds too late — the car goes wide (pauseturn rlog 2026-07-13).
|
|
# The hold ratchets up on the blinker-matching model command below the release speed
|
|
# and floors the command magnitude afterwards. Below the hard speed the floor is firm;
|
|
# between hard and release speed it decays toward the model's sustained demand, so a
|
|
# transient model dip barely sags it while a genuine end-of-turn unwind or an aborted
|
|
# turn still drains it in a few seconds. Retention deliberately does NOT depend on the
|
|
# blinker (the stalk auto-cancels during the stop in the log), on latActive (lateral
|
|
# goes inactive at standstill on torque cars; the wheel parks on rack friction), or on
|
|
# steeringPressed (the driver's instinctive grip during the unwind is what let the
|
|
# collapse through, and the driver physically overpowers a torque command regardless).
|
|
CURVATURE_HOLD_HARD_SPEED = 4.5 * CV.MPH_TO_MS
|
|
# Release must sit ABOVE the speed where the model's action wakes up mid-turn. Left
|
|
# turns cross the intersection before arcing, so the car reaches ~3.5-4 m/s while the
|
|
# action still reads ~0 (left1/left2 rlogs 2026-07-15: a 6 mph release dropped the
|
|
# floor mid-turn and visibly unwound the wheel 50 deg before the action woke; rights
|
|
# wake at ~2.2 m/s and never showed it). Hold authority above creep speed is bounded
|
|
# by the opposite-command release and the decay band, not by this ceiling.
|
|
CURVATURE_HOLD_RELEASE_SPEED = 10.0 * CV.MPH_TO_MS
|
|
# Pre-wind is a NEAR-STANDSTILL device: winding the wheel is only free when the car
|
|
# isn't moving. On rolling slow turns a plan-sourced floor applies the turn's final
|
|
# curvature at the entry, starting the arc 4-7 m early — the "turning too much"
|
|
# corrections in the stickyright1/2 and left-crossing rlogs (2026-07-16) all trace to
|
|
# plan capture while moving. Above this speed only the model's own action can raise
|
|
# the hold, rate-limited so a single-frame action spike (left1 rlog 21.9s: -0.157 for
|
|
# one model frame) can't get captured and floored for seconds.
|
|
CURVATURE_HOLD_PLAN_SOURCE_SPEED = 2.0 * CV.MPH_TO_MS
|
|
CURVATURE_HOLD_RATCHET_RATE = 0.04 # 1/m per s, hold growth limit above the plan-source speed
|
|
# Once the model's action has sustainably taken over the turn, hand off COMPLETELY:
|
|
# clear the hold and don't re-engage until the blinker cycle ends. A floor that chases
|
|
# the awake action only distorts the model's entry spiral, mid-turn shape, and exit
|
|
# unwind — the sticky right-turn exits were the floor trailing the model's unwind by
|
|
# 0.03-0.05 even at the fast decay (stickyright1 41.15s: floor 0.064 vs action 0.014,
|
|
# driver correcting +394). The bridge job is done the moment the action is awake.
|
|
CURVATURE_HOLD_HANDOFF_FRAC = 0.75
|
|
CURVATURE_HOLD_HANDOFF_TIME = 0.3 # s of sustained action >= frac*hold before handoff
|
|
CURVATURE_HOLD_DECAY_TAU = 2.0 # s; hold tracks a sustained lower model demand with this time constant
|
|
# At a turn exit the model unwinds through small SAME-sign commands (curvature only
|
|
# flips negative for the final counter-steer), so the opposite-release fires late and
|
|
# the tau-2 decay melts the floor slower than the model's exit ramp — the car keeps
|
|
# arcing while the driver hauls the wheel back (tightright3/4 rlogs 2026-07-15, drv
|
|
# +500). Turn progress is the discriminator between a mid-turn dip (protect the floor;
|
|
# left-turn sags happened at ~20 deg of swept heading) and an exit (drop it; the
|
|
# sticks happened past ~80 deg): once the swept heading passes the threshold, the
|
|
# decay switches to the fast tau and runs at ANY speed, including below the hard-hold
|
|
# speed. Swept resets whenever the hold disengages.
|
|
CURVATURE_HOLD_SWEPT_EXIT = 0.9 # rad of heading actually turned (~52 deg)
|
|
CURVATURE_HOLD_EXIT_DECAY_TAU = 0.5 # s
|
|
CURVATURE_HOLD_STANDSTILL_TIMEOUT = 30.0 # s stopped before the held turn intent is dropped
|
|
|
|
# The model's time-domain action.desiredCurvature is blind below ~2.5 m/s (0.3 s ahead
|
|
# at creep speed is centimeters of road), but the plan's spatial geometry already shows
|
|
# the turn at standstill: turn3/turn4 rlogs 2026-07-14 read plan curvature 0.13-0.16
|
|
# while the action output sat at 0.005, and the plan value matched the demand the
|
|
# action produced once rolling. Feeding it into the turn-hold ratchet lets the wheel
|
|
# pre-wind toward the real turn before the car moves. Scaled and capped conservatively:
|
|
# a too-high floor turns in tighter than the path (mild at creep lat accel), while a
|
|
# too-low one just reduces the head start. The plan flickers straight for ~1-2 s right
|
|
# at the standstill->motion transition; the ratchet holds through it by design.
|
|
CURVATURE_HOLD_PLAN_LOOKAHEAD_NEAR = 4.0 # m; reads whether the turn starts NOW
|
|
CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR = 7.0 # m; reads the turn's curvature
|
|
CURVATURE_HOLD_PLAN_SCALE = 0.85
|
|
CURVATURE_HOLD_PLAN_CAP = 0.12 # 1/m
|
|
# Proximity gate on the pre-wind: the 4/7 m probe reads the corner's full curvature the
|
|
# instant the plan bends toward it, which at a stop-line turn is several meters before
|
|
# the car reaches the line — so it wound the wheel hard while still rolling and cut the
|
|
# inside curb (both directions), and on an early blinker it turned in before the corner
|
|
# (RL2 seg1 t=42: corner 2 m ahead, car at 1.2 m/s, pre-wind already at 0.13). The plan
|
|
# itself carries the distance: get_plan_turn_onset_dist is where the path bends. Scale
|
|
# the pre-wind from full at ONSET_NEAR to zero at ONSET_FAR_GATE so it winds only as the
|
|
# corner closes. At a real stop the onset collapses to ~0 m once stopped (model: "turn
|
|
# is here"), so the stop-line pre-wind still reaches full strength — just later, at the
|
|
# line instead of 3 m short of it.
|
|
CURVATURE_HOLD_ONSET_HEADING = 10.0 # deg of plan heading change that marks the corner
|
|
CURVATURE_HOLD_ONSET_NEAR = 1.5 # m; corner this close -> full pre-wind
|
|
CURVATURE_HOLD_ONSET_FAR_GATE = 5.0 # m; corner past this -> no pre-wind yet
|
|
CURVATURE_HOLD_ONSET_FAR = 100.0 # m; sentinel "no corner found"
|
|
# The model counter-steers at every turn exit; an opposite-direction command is the
|
|
# "turn is over" signal at any speed. Without this the floor converted the exit unwind
|
|
# (-0.076) into a stuck +0.012 for 1.4 s and the driver had to unwind by hand
|
|
# (rightturnfail rlog 33.2-34.4s). Deadband rejects the ~0.002 pull-away flickers.
|
|
CURVATURE_HOLD_OPPOSITE_RELEASE = 0.01 # 1/m
|
|
# Nudge-to-commit: creeping toward a rolling left below the release speed, the model
|
|
# often does not commit until the geometry is entered — the driver hauled the wheel
|
|
# 144-260 deg alone with the action at zero (0000087c segs 7/8, +377..+411 driver
|
|
# torque) before the model took over. Forcing the plan probe here instead is what
|
|
# caused the 2026-07-19 fights, and the driver's own torque separates those cases
|
|
# perfectly: they RESISTED the premature machine wind (-117, bracing) but PUSH when
|
|
# the gap is theirs. So the driver's matching push is the "go" signal: capture the
|
|
# curvature they have physically wound into the hold, un-rate-limited (the wheel is
|
|
# already there; holding adds no motion), so the car grabs the turn at a nudge instead
|
|
# of making them wind it all by hand. Allowed anywhere the hold exists (< release
|
|
# speed): a 3 m/s ceiling left the 0000087f seg 1 gap-taken-at-3.5-4.2 m/s haul
|
|
# (40->220 deg at +418) unassisted; the decay/handoff/opposite-release machinery
|
|
# already bounds the captured floor in that band.
|
|
CURVATURE_HOLD_CONFIRM_MIN = 0.003 # 1/m (~7 deg) of wound curvature before capture
|
|
CURVATURE_HOLD_CONFIRM_SWEPT = 0.6 # rad of heading swept this blinker cycle; past this the push is exit-shaping, not initiation
|
|
|
|
|
|
def _plan_circle_curvature(xs, ys, lookahead: float) -> float:
|
|
# curvature of the circle through the origin, tangent to the car's heading, passing
|
|
# through the plan point ~lookahead meters ahead: kappa = 2y / (x^2 + y^2)
|
|
px, py = 0.0, 0.0
|
|
for x, y in zip(xs, ys):
|
|
px, py = x, y
|
|
if math.hypot(x, y) >= lookahead:
|
|
break
|
|
d2 = px * px + py * py
|
|
if d2 < 1.0:
|
|
return 0.0
|
|
return 2.0 * py / d2
|
|
|
|
|
|
def _plan_dual_probe(model_v2, d_near: float, d_far: float) -> float:
|
|
# Min-magnitude of a near and a far circle fit. The far probe alone assumes the turn
|
|
# starts immediately, which over-winds wide turns whose arc begins several meters out
|
|
# (wide multi-lane lefts): the near probe reads ~straight there and only grows as the
|
|
# car approaches the arc, so the readout self-scales to the turn geometry. Sign
|
|
# disagreement means no coherent turn ahead: contribute nothing.
|
|
xs, ys = model_v2.position.x, model_v2.position.y
|
|
near = _plan_circle_curvature(xs, ys, d_near)
|
|
far = _plan_circle_curvature(xs, ys, d_far)
|
|
if near * far <= 0.0:
|
|
return 0.0
|
|
return near if abs(near) < abs(far) else far
|
|
|
|
|
|
def get_plan_spatial_curvature(model_v2) -> float:
|
|
return _plan_dual_probe(model_v2, CURVATURE_HOLD_PLAN_LOOKAHEAD_NEAR, CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR)
|
|
|
|
|
|
def get_plan_turn_onset_dist(model_v2) -> float:
|
|
# Distance along the plan at which the path first bends past ONSET_HEADING from the
|
|
# car's current heading — i.e. how far ahead the corner actually starts. The pre-wind
|
|
# magnitude scales on this so a stop-line turn winds only once the corner is close,
|
|
# not the instant the plan first shows a turn several meters out (which cut the inside
|
|
# curb at a stop and turned in early on an over-eager blinker). Returns a large
|
|
# sentinel when no bend is found, so distant/straight plans read "far" and don't wind.
|
|
xs, ys = model_v2.position.x, model_v2.position.y
|
|
n = min(len(xs), len(ys))
|
|
for i in range(2, n):
|
|
dx = xs[i] - xs[i - 1]
|
|
dy = ys[i] - ys[i - 1]
|
|
if abs(dx) < 1e-3 and abs(dy) < 1e-3:
|
|
continue
|
|
if abs(math.degrees(math.atan2(dy, dx))) > CURVATURE_HOLD_ONSET_HEADING:
|
|
return math.hypot(xs[i], ys[i])
|
|
return CURVATURE_HOLD_ONSET_FAR
|
|
|
|
|
|
# Turn-initiation lead. The model's action and the fixed 4/7 m probes are anchored in
|
|
# METERS, so the seconds of warning they give shrinks with speed — at 12 mph a corner
|
|
# enters the 7 m window only ~1.3 s out, too late to wind the wheel, which is why every
|
|
# turn in the Desktop/new drive initiated only below ~5.5 m/s regardless of approach
|
|
# speed. This lead probes the plan at a constant-TIME distance instead and max-mag
|
|
# blends into the command, so initiation can start while still rolling at 9-12 mph.
|
|
# The engagement fade (authority ramps to zero as measured curvature approaches the
|
|
# lead) limits it to the initiation phase: once the car is tracking the arc, the model
|
|
# owns the turn shape — without it, the blend re-creates the 2026-07-16 rolling-turn
|
|
# front-load (stickyright2 entry: 0.084 commanded vs the model's own 0.041 spiral). A
|
|
# binary gate here limit-cycles: cutting drops demand to a still-small action, the
|
|
# wheel unwinds below the threshold, the lead re-fires — a 5 Hz demand sawtooth felt
|
|
# as wiggle (2026-07-19 drive seg 7 t=49-50). The fade instead settles at a stable
|
|
# ~2/3-of-lead equilibrium until the action takes over via the max-mag blend.
|
|
# Stop-sign approaches stay quiet because the stopping plan compresses to the stop
|
|
# line and reads ~straight until the car is nearly there (probes ~0 for 7 s of
|
|
# blinker-on coasting at 9-12 m/s). Below TURN_LEAD_MIN_SPEED the lead must stay OFF,
|
|
# not just for standstill plan flicker: at creep speed the probe's 4 m distance floor
|
|
# chord-fits a left turn's straight-then-arc entry as "arc now", demanding 3-5x the
|
|
# model's intent. The model fights back — its plan flips to correct the premature yaw,
|
|
# the wheel swings back through center, and the swing trips the stalk auto-cancel,
|
|
# killing the turn (2026-07-19 segs 6/7/8/10, all at 1.5-2.4 m/s; the same drive's
|
|
# completed turns show the model committing on its own below 3 m/s). The model's
|
|
# meter-anchored horizon gives it 2+ s of warning at creep speed — the lead's whole
|
|
# reason to exist (time-warning shrinking with speed) does not apply there.
|
|
TURN_LEAD_T = 1.3 # s of travel the probe looks ahead (~wind-up time + lat delay)
|
|
TURN_LEAD_MIN_M = 4.0
|
|
TURN_LEAD_MAX_M = 14.0
|
|
TURN_LEAD_MIN_SPEED = 3.0 # m/s: authority 0 here, ramps to full at FULL_SPEED
|
|
TURN_LEAD_FULL_SPEED = 4.0 # m/s
|
|
TURN_LEAD_MAX_SPEED = 7.0 # m/s (~15.7 mph)
|
|
TURN_LEAD_SCALE = 0.85
|
|
TURN_LEAD_CAP = 0.12 # 1/m
|
|
TURN_LEAD_ENGAGED_FRAC = 0.5 # engagement fade starts here, zero authority at 1.0
|
|
TURN_LEAD_MODEL_OPPOSE = 0.003 # 1/m: model steering this hard against the blinker vetoes the lead
|
|
# Braking-to-a-stop veto: if sustaining the current decel parks the car within this
|
|
# factor of the probe distance, the driver intends to stop short of the arc — demanding
|
|
# that arc's curvature NOW winds the wheel at a stop approach the model didn't plan
|
|
# (0000087c seg 1: lead wound 30 deg at 6 m/s while braking for a 13 s stop; driver
|
|
# yanked it back with -509 torque). Turn-approach braking releases before this trips;
|
|
# a held brake to standstill keeps it tripped, deferring the turn to the pre-wind.
|
|
TURN_LEAD_STOP_MARGIN = 1.5
|
|
TURN_LEAD_DECEL_GATE = -0.5 # m/s^2: only project a stop when genuinely braking
|
|
|
|
|
|
def get_gm_hud_set_speed(set_speed_ms: float, starpilot_toggles) -> float:
|
|
spoofed_speed = set_speed_ms
|
|
|
|
set_speed_offset = float(getattr(starpilot_toggles, "set_speed_offset", 0.0) or 0.0)
|
|
if spoofed_speed > 0 and set_speed_offset > 0:
|
|
spoofed_speed += set_speed_offset * CV.KPH_TO_MS
|
|
|
|
return spoofed_speed
|
|
|
|
|
|
def get_torque_control_params(CP, torque_params, starpilot_toggles, use_live_params: bool) -> tuple[float, float, float]:
|
|
torque_tune = CP.lateralTuning.torque
|
|
lat_accel_factor = torque_tune.latAccelFactor
|
|
lat_accel_offset = torque_tune.latAccelOffset
|
|
friction = torque_tune.friction
|
|
|
|
use_custom_lat_accel = getattr(starpilot_toggles, "use_custom_latAccelFactor", False)
|
|
use_custom_friction = getattr(starpilot_toggles, "use_custom_friction", False)
|
|
|
|
if use_live_params:
|
|
if not use_custom_lat_accel:
|
|
lat_accel_factor = torque_params.latAccelFactorFiltered
|
|
lat_accel_offset = torque_params.latAccelOffsetFiltered
|
|
if not use_custom_friction:
|
|
friction = torque_params.frictionCoefficientFiltered
|
|
|
|
if use_custom_lat_accel:
|
|
lat_accel_factor = starpilot_toggles.latAccelFactor
|
|
if use_custom_friction:
|
|
friction = starpilot_toggles.friction
|
|
|
|
return lat_accel_factor, lat_accel_offset, friction
|
|
|
|
|
|
class Controls:
|
|
def __init__(self) -> None:
|
|
self.params = Params()
|
|
cloudlog.info("controlsd is waiting for CarParams")
|
|
self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
|
|
self.FPCP = messaging.log_from_bytes(self.params.get("StarPilotCarParams", block=True), custom.StarPilotCarParams)
|
|
cloudlog.info("controlsd got CarParams")
|
|
|
|
self.CI = interfaces[self.CP.carFingerprint](self.CP, self.FPCP)
|
|
|
|
self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
|
|
'liveCalibration', 'livePose', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput',
|
|
'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
|
|
self.pm = messaging.PubMaster(['carControl', 'controlsState'])
|
|
|
|
self.steer_limited_by_safety = False
|
|
self.curvature = 0.0
|
|
self.desired_curvature = 0.0
|
|
self.lc_smooth_release = 0.0
|
|
self.turn_hold_curvature = 0.0
|
|
self.turn_hold_standstill_t = 0.0
|
|
self.turn_hold_swept = 0.0
|
|
self.turn_hold_handoff_t = 0.0
|
|
self.turn_hold_done = False
|
|
self.turn_blinker_swept = 0.0
|
|
|
|
self.pose_calibrator = PoseCalibrator()
|
|
self.calibrated_pose: Pose | None = None
|
|
|
|
self.LoC = LongControl(self.CP)
|
|
self.VM = VehicleModel(self.CP)
|
|
self.LaC: LatControl
|
|
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
|
self.LaC = LatControlAngle(self.CP, self.CI, DT_CTRL)
|
|
elif self.CP.lateralTuning.which() == 'pid':
|
|
self.LaC = LatControlPID(self.CP, self.CI, DT_CTRL)
|
|
elif self.CP.lateralTuning.which() == 'torque':
|
|
self.LaC = LatControlTorque(self.CP, self.CI, DT_CTRL)
|
|
|
|
self.sm = self.sm.extend(['liveDelay', 'starpilotCarState', 'starpilotPlan'])
|
|
|
|
self.starpilot_toggles = get_starpilot_toggles()
|
|
self.ecu_disable_failed = False
|
|
self.ecu_disable_failed_checked = not self.CP.openpilotLongitudinalControl
|
|
|
|
if self.CP.lateralTuning.which() == "torque" and (self.starpilot_toggles.nnff or self.starpilot_toggles.nnff_lite):
|
|
self.LaC = LatControlNNFF(self.CP, self.CI, DT_CTRL)
|
|
|
|
def update(self):
|
|
self.sm.update(15)
|
|
if self.sm.updated["liveCalibration"]:
|
|
self.pose_calibrator.feed_live_calib(self.sm['liveCalibration'])
|
|
if self.sm.updated["livePose"]:
|
|
device_pose = Pose.from_live_pose(self.sm['livePose'])
|
|
self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose)
|
|
|
|
if hasattr(self.LaC, "pid") and self.CP.lateralTuning.which() != "pid":
|
|
self.LaC.pid._k_p = self.starpilot_toggles.steerKp
|
|
|
|
if self.sm.updated['liveDelay'] and hasattr(self.LaC, "update_live_delay"):
|
|
self.LaC.update_live_delay(self.sm['liveDelay'].lateralDelay)
|
|
|
|
self.starpilot_toggles = get_starpilot_toggles(self.sm)
|
|
|
|
def update_ecu_disable_failed(self):
|
|
if self.ecu_disable_failed_checked:
|
|
return
|
|
|
|
# ControlsReady is set after CarInterface.init(), where Hyundai ECU disable
|
|
# writes EcuDisableFailed. Once init has completed, the value is stable.
|
|
if self.params.get_bool("ControlsReady"):
|
|
self.ecu_disable_failed = self.params.get_bool("EcuDisableFailed")
|
|
self.ecu_disable_failed_checked = True
|
|
|
|
def state_control(self):
|
|
CS = self.sm['carState']
|
|
|
|
# Update VehicleModel
|
|
lp = self.sm['liveParameters']
|
|
x = max(lp.stiffnessFactor, 0.1)
|
|
sr = max(lp.steerRatio, 0.1)
|
|
if self.CP.carFingerprint == GM_CAR.CHEVROLET_BOLT_CC_2017:
|
|
sr *= get_bolt_2017_steer_ratio_scale(CS.vEgo)
|
|
elif self.CP.carFingerprint == GM_CAR.CHEVROLET_BOLT_CC_2018_2021:
|
|
sr *= BOLT_2018_2021_STEER_RATIO_TEST_SCALE
|
|
self.VM.update_params(x, sr)
|
|
|
|
steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
|
|
self.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll)
|
|
|
|
# Update Torque Params
|
|
if self.CP.lateralTuning.which() == 'torque':
|
|
torque_params = self.sm['liveTorqueParameters']
|
|
force_auto_tune = getattr(self.starpilot_toggles, "force_auto_tune", False)
|
|
use_live_params = self.sm.all_checks(['liveTorqueParameters']) and (torque_params.useParams or force_auto_tune)
|
|
use_custom_torque_params = (
|
|
getattr(self.starpilot_toggles, "use_custom_latAccelFactor", False) or
|
|
getattr(self.starpilot_toggles, "use_custom_friction", False)
|
|
)
|
|
if use_live_params or use_custom_torque_params:
|
|
lat_accel_factor, lat_accel_offset, friction = get_torque_control_params(self.CP, torque_params, self.starpilot_toggles, use_live_params)
|
|
self.LaC.update_live_torque_params(lat_accel_factor, lat_accel_offset, friction)
|
|
|
|
long_plan = self.sm['longitudinalPlan']
|
|
model_v2 = self.sm['modelV2']
|
|
|
|
CC = car.CarControl.new_message()
|
|
CC.enabled = self.sm['selfdriveState'].enabled
|
|
|
|
# Check which actuators can be enabled
|
|
standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill
|
|
CC.latActive = get_lateral_active(CC.enabled, self.sm['selfdriveState'].active,
|
|
self.sm['starpilotCarState'].alwaysOnLateralEnabled,
|
|
CS.steerFaultTemporary, CS.steerFaultPermanent,
|
|
standstill, self.CP.steerAtStandstill,
|
|
self.sm['starpilotPlan'].lateralCheck)
|
|
# EcuDisableFailed is set when car started in READY mode (ECU disable was rejected)
|
|
# Disable longitudinal so stock ACC works instead
|
|
self.update_ecu_disable_failed()
|
|
CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and not self.sm['starpilotCarState'].pauseLongitudinal and self.CP.openpilotLongitudinalControl and not self.ecu_disable_failed
|
|
|
|
actuators = CC.actuators
|
|
actuators.longControlState = self.LoC.long_control_state
|
|
|
|
# Enable blinkers while lane changing
|
|
if model_v2.meta.laneChangeState != LaneChangeState.off:
|
|
CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left
|
|
CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right
|
|
|
|
if not CC.latActive:
|
|
self.LaC.reset()
|
|
if not CC.longActive:
|
|
self.LoC.reset()
|
|
|
|
# accel PID loop
|
|
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
|
|
self.LoC.experimental_mode = bool(self.sm['selfdriveState'].experimentalMode)
|
|
actuators.accel = float(min(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits,
|
|
self.starpilot_toggles, has_lead=long_plan.hasLead,
|
|
traffic_mode_enabled=self.sm['starpilotCarState'].trafficModeEnabled,
|
|
profile_max_accel=self.sm['starpilotPlan'].maxAcceleration),
|
|
self.starpilot_toggles.max_desired_acceleration))
|
|
|
|
# Steering PID loop and lateral MPC
|
|
# Reset desired curvature to current to avoid violating the limits on engage
|
|
if self.sm.valid['lateralManeuverPlan']:
|
|
new_desired_curvature = self.sm['lateralManeuverPlan'].desiredCurvature if CC.latActive else self.curvature
|
|
else:
|
|
new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature
|
|
|
|
# Low-speed turn-intent hold (see CURVATURE_HOLD_* above). Curvature sign convention
|
|
# here is positive for RIGHT turns (pauseturn log: left turn at +148 deg steering
|
|
# angle logs desiredCurvature -0.07), so the blinker maps right=+1, left=-1.
|
|
blinker_dir = float(CS.rightBlinker) - float(CS.leftBlinker)
|
|
# heading swept in the blinker's direction over the whole blinker cycle (any speed):
|
|
# discriminates a turn not yet made from one being exited (see the re-arm below)
|
|
if blinker_dir == 0.0:
|
|
self.turn_blinker_swept = 0.0
|
|
else:
|
|
self.turn_blinker_swept += max(CS.vEgo * self.curvature * blinker_dir, 0.0) * DT_CTRL
|
|
if CS.vEgo >= CURVATURE_HOLD_RELEASE_SPEED:
|
|
self.turn_hold_curvature = 0.0
|
|
self.turn_hold_standstill_t = 0.0
|
|
self.turn_hold_swept = 0.0
|
|
self.turn_hold_handoff_t = 0.0
|
|
self.turn_hold_done = False
|
|
else:
|
|
if self.turn_hold_curvature == 0.0:
|
|
self.turn_hold_swept = 0.0
|
|
else:
|
|
# heading actually swept in the hold's direction: the measure of turn progress
|
|
self.turn_hold_swept += max(CS.vEgo * self.curvature * math.copysign(1.0, self.turn_hold_curvature), 0.0) * DT_CTRL
|
|
turn_exiting = self.turn_hold_swept > CURVATURE_HOLD_SWEPT_EXIT
|
|
if (CS.vEgo > CURVATURE_HOLD_HARD_SPEED or turn_exiting) and CC.latActive and self.turn_hold_curvature != 0.0:
|
|
# Decay toward the model's sustained same-direction demand instead of leaking on
|
|
# wall-clock time: a wall-clock leak drained the floor mid-turn while the model
|
|
# dipped transiently (turnn rlog 40.0-40.5s), while sustained low demand (end of
|
|
# turn, abort) still drains the hold within a couple of time constants.
|
|
hold_dir = math.copysign(1.0, self.turn_hold_curvature)
|
|
model_mag = max(new_desired_curvature * hold_dir, 0.0)
|
|
if model_mag < abs(self.turn_hold_curvature):
|
|
decay_tau = CURVATURE_HOLD_EXIT_DECAY_TAU if turn_exiting else CURVATURE_HOLD_DECAY_TAU
|
|
decayed = abs(self.turn_hold_curvature) + (model_mag - abs(self.turn_hold_curvature)) * (DT_CTRL / decay_tau)
|
|
self.turn_hold_curvature = math.copysign(decayed, self.turn_hold_curvature)
|
|
if CS.vEgo < 0.5:
|
|
self.turn_hold_standstill_t += DT_CTRL
|
|
if self.turn_hold_standstill_t > CURVATURE_HOLD_STANDSTILL_TIMEOUT:
|
|
self.turn_hold_curvature = 0.0
|
|
# a stop resets the turn cycle: the model goes blind again, so a prior handoff
|
|
# must not block the standstill pre-wind (turn4 regression in v9 replay)
|
|
self.turn_hold_done = False
|
|
else:
|
|
self.turn_hold_standstill_t = 0.0
|
|
if CC.latActive and self.turn_hold_curvature != 0.0 and \
|
|
new_desired_curvature * math.copysign(1.0, self.turn_hold_curvature) < -CURVATURE_HOLD_OPPOSITE_RELEASE:
|
|
# model is actively counter-steering: the turn is over, release at any speed
|
|
self.turn_hold_curvature = 0.0
|
|
self.turn_hold_done = True
|
|
if CC.latActive and self.turn_hold_curvature != 0.0 and \
|
|
new_desired_curvature * math.copysign(1.0, self.turn_hold_curvature) >= CURVATURE_HOLD_HANDOFF_FRAC * abs(self.turn_hold_curvature):
|
|
self.turn_hold_handoff_t += DT_CTRL
|
|
if self.turn_hold_handoff_t > CURVATURE_HOLD_HANDOFF_TIME:
|
|
# action has sustainably taken over: hand off completely (see HANDOFF consts)
|
|
self.turn_hold_curvature = 0.0
|
|
self.turn_hold_done = True
|
|
else:
|
|
self.turn_hold_handoff_t = 0.0
|
|
if blinker_dir == 0.0:
|
|
# blinker cycle over: a fresh turn may engage a fresh hold
|
|
self.turn_hold_done = False
|
|
elif CC.latActive and CS.steeringPressed and CS.steeringTorque * blinker_dir < 0.0 and \
|
|
self.curvature * blinker_dir > CURVATURE_HOLD_CONFIRM_MIN and \
|
|
self.turn_blinker_swept < CURVATURE_HOLD_CONFIRM_SWEPT:
|
|
# an active driver push into the signaled turn BEFORE the turn is made is fresh
|
|
# turn intent: re-arm the cycle even after a prior handoff. A long blinker-on
|
|
# approach can latch done on a trivial micro-handoff and lock out
|
|
# nudge-to-commit ten seconds later at the real turn (0000087f seg 1: +418 haul
|
|
# unassisted). The swept gate keeps a light same-direction touch during the
|
|
# EXIT unwind from re-latching a large hold against the model's recentering
|
|
# (0000087f seg 4 t=14.5 in replay: floor trailed the exit by 0.07 for 1.5 s).
|
|
self.turn_hold_done = False
|
|
if blinker_dir != 0.0 and not self.turn_hold_done:
|
|
# Ratchet up on the raw model command, never on the floored/measured value, so
|
|
# the hold can't feed itself and defeat the decay. Below the release speed the
|
|
# plan's spatial curvature (see get_plan_spatial_curvature) is the second,
|
|
# earlier-seeing source: it shows the turn at standstill while the action is
|
|
# still blind, letting the pre-wind start before the car moves.
|
|
turn_candidate = new_desired_curvature if CC.latActive else 0.0
|
|
if CC.latActive and CS.vEgo < CURVATURE_HOLD_PLAN_SOURCE_SPEED:
|
|
plan_curvature = get_plan_spatial_curvature(model_v2) * CURVATURE_HOLD_PLAN_SCALE
|
|
plan_curvature = max(min(plan_curvature, CURVATURE_HOLD_PLAN_CAP), -CURVATURE_HOLD_PLAN_CAP)
|
|
# Proximity gate (see CURVATURE_HOLD_ONSET_*): wind only as the corner closes,
|
|
# so a stop-line turn winds at the line and an early blinker doesn't turn in
|
|
# early. Full at ONSET_NEAR, zero at ONSET_FAR_GATE.
|
|
onset = get_plan_turn_onset_dist(model_v2)
|
|
onset_w = min(max((CURVATURE_HOLD_ONSET_FAR_GATE - onset) /
|
|
(CURVATURE_HOLD_ONSET_FAR_GATE - CURVATURE_HOLD_ONSET_NEAR), 0.0), 1.0)
|
|
plan_curvature *= onset_w
|
|
if plan_curvature * blinker_dir > turn_candidate * blinker_dir:
|
|
turn_candidate = plan_curvature
|
|
# Nudge-to-commit (see CURVATURE_HOLD_CONFIRM_*): the driver actively pushing in
|
|
# the blinker direction at creep speed captures what they have wound. Positive
|
|
# steeringTorque is a LEFT push (negative curvature), so agreement is a negative
|
|
# product with blinker_dir. Exempt from the ratchet rate limit: latching the
|
|
# wheel's current position commands no motion, only keeps the driver's progress.
|
|
driver_confirmed = False
|
|
if CC.latActive and CS.steeringPressed and \
|
|
CS.steeringTorque * blinker_dir < 0.0 and self.curvature * blinker_dir > CURVATURE_HOLD_CONFIRM_MIN:
|
|
wound_curvature = max(min(self.curvature, CURVATURE_HOLD_PLAN_CAP), -CURVATURE_HOLD_PLAN_CAP)
|
|
if wound_curvature * blinker_dir > turn_candidate * blinker_dir:
|
|
turn_candidate = wound_curvature
|
|
driver_confirmed = True
|
|
if turn_candidate * blinker_dir > abs(self.turn_hold_curvature):
|
|
new_mag = turn_candidate * blinker_dir
|
|
if CS.vEgo > CURVATURE_HOLD_PLAN_SOURCE_SPEED and not driver_confirmed:
|
|
new_mag = min(new_mag, abs(self.turn_hold_curvature) + CURVATURE_HOLD_RATCHET_RATE * DT_CTRL)
|
|
self.turn_hold_curvature = math.copysign(new_mag, turn_candidate)
|
|
elif self.turn_hold_curvature * blinker_dir < 0.0:
|
|
# blinker flipped to the other side: turn intent changed
|
|
self.turn_hold_curvature = 0.0
|
|
if CC.latActive and self.turn_hold_curvature != 0.0:
|
|
hold_dir = math.copysign(1.0, self.turn_hold_curvature)
|
|
if new_desired_curvature * hold_dir < abs(self.turn_hold_curvature):
|
|
new_desired_curvature = self.turn_hold_curvature
|
|
|
|
# Turn-initiation lead (see TURN_LEAD_*). Applied AFTER the hold block so the
|
|
# ratchet/handoff only ever see the raw model action; pure max-magnitude, so it can
|
|
# never reduce or oppose the model. Lane changes are excluded: that blinker's plan
|
|
# bend is not a turn. The model-oppose veto is defense-in-depth for the fade-in
|
|
# edge: a model actively steering against the blinker is correcting something the
|
|
# lead must not fight (see the constants comment for the 2026-07-19 failures).
|
|
if (CC.latActive and blinker_dir != 0.0 and
|
|
model_v2.meta.laneChangeState == LaneChangeState.off and
|
|
TURN_LEAD_MIN_SPEED <= CS.vEgo < TURN_LEAD_MAX_SPEED and
|
|
new_desired_curvature * blinker_dir > -TURN_LEAD_MODEL_OPPOSE):
|
|
d_near = max(min(TURN_LEAD_T * CS.vEgo, TURN_LEAD_MAX_M), TURN_LEAD_MIN_M)
|
|
stopping_short = CS.aEgo < TURN_LEAD_DECEL_GATE and \
|
|
CS.vEgo ** 2 / (2.0 * -CS.aEgo) < TURN_LEAD_STOP_MARGIN * d_near
|
|
lead_curvature = 0.0 if stopping_short else _plan_dual_probe(model_v2, d_near, d_near + 3.0) * TURN_LEAD_SCALE
|
|
lead_curvature = max(min(lead_curvature, TURN_LEAD_CAP), -TURN_LEAD_CAP)
|
|
if lead_curvature * blinker_dir > 0.0:
|
|
speed_w = min(max((CS.vEgo - TURN_LEAD_MIN_SPEED) / (TURN_LEAD_FULL_SPEED - TURN_LEAD_MIN_SPEED), 0.0), 1.0)
|
|
engaged_ratio = abs(self.curvature) / abs(lead_curvature)
|
|
engage_w = min(max((1.0 - engaged_ratio) / (1.0 - TURN_LEAD_ENGAGED_FRAC), 0.0), 1.0)
|
|
lead_curvature *= speed_w * engage_w
|
|
if lead_curvature * blinker_dir > max(new_desired_curvature * blinker_dir, 0.0):
|
|
new_desired_curvature = lead_curvature
|
|
# Capture the applied lead into the hold (rate-limited like any moving-speed
|
|
# ratchet) so decelerating through the fade floor keeps the initiation
|
|
# progress: without this, braking mid-wind dumped the lead's demand back to
|
|
# the still-small action and visibly unwound the wheel 50->15 deg before the
|
|
# standstill pre-wind had to redo the work (0000087c seg 6 first left).
|
|
if CS.vEgo < CURVATURE_HOLD_RELEASE_SPEED and not self.turn_hold_done and \
|
|
lead_curvature * blinker_dir > abs(self.turn_hold_curvature):
|
|
held_mag = min(lead_curvature * blinker_dir, abs(self.turn_hold_curvature) + CURVATURE_HOLD_RATCHET_RATE * DT_CTRL)
|
|
self.turn_hold_curvature = math.copysign(held_mag, lead_curvature)
|
|
|
|
jerk_factor = 1.0
|
|
if self.starpilot_toggles.lane_change_pace < 10:
|
|
set_jerk = self.starpilot_toggles.lane_change_jerk_factor
|
|
in_lane_change = model_v2.meta.laneChangeState in (LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing) \
|
|
and CS.vEgo >= self.starpilot_toggles.minimum_lane_change_speed
|
|
# Hold the tight jerk limit for the whole maneuver, then taper back to stock so the
|
|
# model's recenter step and mid-change corrections stay shaped instead of passing
|
|
# through a mostly-relaxed clamp. Only the jerk (curvature rate) is tightened: capping
|
|
# lat accel strangles the end-of-maneuver arrest and lets the car glide past the new
|
|
# lane center before it can build enough counter-curvature.
|
|
if in_lane_change:
|
|
self.lc_smooth_release = LANE_CHANGE_SMOOTH_RELEASE_T
|
|
else:
|
|
self.lc_smooth_release = max(self.lc_smooth_release - DT_CTRL, 0.0)
|
|
if self.lc_smooth_release > 0.0:
|
|
release = 1.0 - self.lc_smooth_release / LANE_CHANGE_SMOOTH_RELEASE_T # 0 in maneuver → 1 after
|
|
jerk_factor = set_jerk + (1.0 - set_jerk) * release
|
|
# When the model is unwinding curvature (reducing the lane-change curvature magnitude)
|
|
# and the entry cap would make the command lag it, grant extra rate proportional to the
|
|
# lag so the car can stop on the new lane center. Applies only to the unwind direction;
|
|
# the entry ramp keeps the full pace smoothness. Robust to double lane changes (no
|
|
# baseline). Pursuit (lag/tau) rather than a fixed floor so the crest stays smooth.
|
|
model_unwinding = abs(new_desired_curvature) < abs(self.desired_curvature) and \
|
|
math.copysign(1.0, new_desired_curvature - self.desired_curvature) == -math.copysign(1.0, self.desired_curvature) and \
|
|
abs(self.desired_curvature) > 1e-4
|
|
if model_unwinding:
|
|
v_lim = max(CS.vEgo, 1.0)
|
|
gap = max(abs(new_desired_curvature - self.desired_curvature) - LANE_CHANGE_ARREST_GAP_DEADBAND, 0.0)
|
|
jf_gap = (gap / LANE_CHANGE_ARREST_PURSUIT_TAU) * v_lim ** 2 / MAX_LATERAL_JERK
|
|
arrest_cap = LANE_CHANGE_ARREST_JERK_FLOOR + (1.0 - LANE_CHANGE_ARREST_JERK_FLOOR) * release
|
|
jerk_factor = max(jerk_factor, min(arrest_cap, jerk_factor + jf_gap))
|
|
|
|
self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll,
|
|
jerk_factor)
|
|
lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
|
|
|
|
actuators.curvature = self.desired_curvature
|
|
steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
|
|
self.steer_limited_by_safety, self.desired_curvature,
|
|
curvature_limited, lat_delay,
|
|
self.calibrated_pose,
|
|
self.sm['modelV2'],
|
|
self.starpilot_toggles)
|
|
actuators.torque = float(steer)
|
|
actuators.steeringAngleDeg = float(steeringAngleDeg)
|
|
|
|
if len(long_plan.speeds):
|
|
actuators.speed = long_plan.speeds[-1]
|
|
|
|
# Ensure no NaNs/Infs
|
|
for p in ACTUATOR_FIELDS:
|
|
attr = getattr(actuators, p)
|
|
if not isinstance(attr, Number):
|
|
continue
|
|
|
|
if not math.isfinite(attr):
|
|
cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
|
|
setattr(actuators, p, 0.0)
|
|
|
|
return CC, lac_log
|
|
|
|
def publish(self, CC, lac_log):
|
|
CS = self.sm['carState']
|
|
long_plan = self.sm['longitudinalPlan']
|
|
|
|
# Orientation and angle rates can be useful for carcontroller
|
|
# Only calibrated (car) frame is relevant for the carcontroller
|
|
CC.currentCurvature = self.curvature
|
|
if self.calibrated_pose is not None:
|
|
CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
|
|
CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()
|
|
|
|
CC.cruiseControl.override = CC.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl
|
|
pacifica_hybrid_aol = pacifica_hybrid_aol_stock_acc_mode(
|
|
self.CP.carFingerprint,
|
|
self.CP.pcmCruise,
|
|
CC.enabled,
|
|
self.sm['starpilotCarState'].alwaysOnLateralEnabled,
|
|
)
|
|
cancel_requested = should_cancel_stock_cruise(self.CP, CS.cruiseState.enabled, CC.enabled)
|
|
CC.cruiseControl.cancel = cancel_requested and not pacifica_hybrid_aol
|
|
|
|
legacy_resume_hack = False
|
|
if len(long_plan.speeds):
|
|
planned_resume = long_plan.speeds[-1] > 0.1
|
|
# Some stock-ACC SNG hacks still rely on the legacy "planner wants speed"
|
|
# signal rather than longitudinalPlan.shouldStop going false first.
|
|
legacy_resume_hack = planned_resume and (
|
|
(self.CP.brand == "toyota" and self.CP.openpilotLongitudinalControl and not self.CP.autoResumeSng) or
|
|
(self.CP.brand == "gm" and getattr(self.starpilot_toggles, "volt_sng", False))
|
|
)
|
|
|
|
CC.cruiseControl.resume = CC.enabled and CS.cruiseState.standstill and (
|
|
not self.sm['longitudinalPlan'].shouldStop or legacy_resume_hack
|
|
)
|
|
|
|
hudControl = CC.hudControl
|
|
hud_set_speed = float(CS.vCruiseCluster * CV.KPH_TO_MS)
|
|
gm_dash_spoof_offsets_enabled = (
|
|
self.CP.brand == "gm" and
|
|
self.CP.openpilotLongitudinalControl and
|
|
self.CP.enableGasInterceptorDEPRECATED and
|
|
getattr(self.starpilot_toggles, "gm_pedal_longitudinal", True) and
|
|
not getattr(self.starpilot_toggles, "disable_openpilot_long", False) and
|
|
getattr(self.starpilot_toggles, "gm_dash_spoof_offsets", False)
|
|
)
|
|
if gm_dash_spoof_offsets_enabled:
|
|
hud_set_speed = get_gm_hud_set_speed(hud_set_speed, self.starpilot_toggles)
|
|
hudControl.setSpeed = hud_set_speed
|
|
hudControl.speedVisible = CC.enabled
|
|
hudControl.lanesVisible = CC.enabled
|
|
hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
|
|
hudControl.leadDistanceBars = self.sm['selfdriveState'].personality.raw + 1
|
|
hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual
|
|
|
|
hudControl.rightLaneVisible = True
|
|
hudControl.leftLaneVisible = True
|
|
if self.sm.valid['driverAssistance']:
|
|
hudControl.leftLaneDepart = self.sm['driverAssistance'].leftLaneDeparture
|
|
hudControl.rightLaneDepart = self.sm['driverAssistance'].rightLaneDeparture
|
|
|
|
if self.sm['selfdriveState'].active:
|
|
CO = self.sm['carOutput']
|
|
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
|
self.steer_limited_by_safety = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \
|
|
STEER_ANGLE_SATURATION_THRESHOLD
|
|
else:
|
|
self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2
|
|
|
|
# TODO: both controlsState and carControl valids should be set by
|
|
# sm.all_checks(), but this creates a circular dependency
|
|
|
|
# controlsState
|
|
dat = messaging.new_message('controlsState')
|
|
dat.valid = CS.canValid
|
|
cs = dat.controlsState
|
|
|
|
cs.curvature = self.curvature
|
|
cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
|
|
cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
|
|
cs.desiredCurvature = self.desired_curvature
|
|
cs.longControlState = self.LoC.long_control_state
|
|
cs.upAccelCmd = float(self.LoC.pid.p)
|
|
cs.uiAccelCmd = float(self.LoC.pid.i)
|
|
cs.ufAccelCmd = float(self.LoC.pid.f)
|
|
cs.forceDecel = bool(self.sm['driverMonitoringState'].noResponseForceDecel or
|
|
(self.sm['selfdriveState'].state == State.softDisabling) or self.sm["starpilotCarState"].forceCoast)
|
|
|
|
lat_tuning = self.CP.lateralTuning.which()
|
|
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
|
cs.lateralControlState.angleState = lac_log
|
|
elif lat_tuning == 'pid':
|
|
cs.lateralControlState.pidState = lac_log
|
|
elif lat_tuning == 'torque':
|
|
cs.lateralControlState.torqueState = lac_log
|
|
|
|
self.pm.send('controlsState', dat)
|
|
|
|
# carControl
|
|
cc_send = messaging.new_message('carControl')
|
|
cc_send.valid = CS.canValid
|
|
cc_send.carControl = CC
|
|
self.pm.send('carControl', cc_send)
|
|
|
|
def run(self):
|
|
rk = Ratekeeper(100, print_delay_threshold=None)
|
|
while True:
|
|
self.update()
|
|
CC, lac_log = self.state_control()
|
|
self.publish(CC, lac_log)
|
|
rk.monitor_time()
|
|
|
|
|
|
def main():
|
|
config_realtime_process(4, Priority.CTRL_HIGH)
|
|
controls = Controls()
|
|
controls.run()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|