mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-28 00:53:45 +08:00
firestar5683
551 lines
31 KiB
Python
Executable File
551 lines
31 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
import math
|
|
import os
|
|
import time
|
|
from numbers import Number
|
|
|
|
from openpilot.cereal import log
|
|
from opendbc.car.structs import car
|
|
import openpilot.cereal.messaging as messaging
|
|
from openpilot.common.constants import CV
|
|
from openpilot.common.hardware import HARDWARE
|
|
from openpilot.common.params import Params
|
|
from openpilot.common.realtime import config_realtime_process, DT_CTRL, Priority, Ratekeeper, set_core_affinity
|
|
from openpilot.common.swaglog import cloudlog
|
|
|
|
from opendbc.car.car_helpers import interfaces
|
|
from opendbc.car.vehicle_model import VehicleModel
|
|
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
|
|
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
|
|
from openpilot.selfdrive.controls.lib.latcontrol_angle import STEER_ANGLE_SATURATION_THRESHOLD
|
|
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
|
from openpilot.selfdrive.controls.lib.lateral_controller_factory import create_lateral_controller
|
|
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
|
|
from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
|
|
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
|
|
from openpilot.starpilot.car.hyundai.lateral_fault import LateralFaultLatch
|
|
from openpilot.starpilot.feature_runtime import enabled as feature_enabled
|
|
from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences
|
|
from openpilot.starpilot.lateral.pause import LateralPause, read_settings as read_lateral_pause
|
|
from openpilot.starpilot.lateral.model_turn_assist import ModelTurnAssist, update_twitch_guard
|
|
from openpilot.starpilot.aol.runtime import (current_axis, current_native, ordinary_lateral_requested, ordinary_axis_acknowledged,
|
|
refresh_expired_lateral_snapshot)
|
|
from openpilot.starpilot.aol.vehicle import policy_for as axis_policy_for, ordinary_axis_request_allowed, allow_lateral_onset
|
|
from openpilot.starpilot.lateral.lane_centering import (
|
|
LaneCenteringController, LaneCenteringInput, LaneCenteringRequest, LaneCenteringResult,
|
|
)
|
|
from openpilot.starpilot.lateral.lane_feedback import feedback_message
|
|
from openpilot.starpilot.lateral.lane_change_preferences import LiveLaneChangePolicy
|
|
from openpilot.starpilot.lateral.lane_change_smoothing import LaneChangeSmoother, limit_rate
|
|
from openpilot.starpilot.lateral.lane_runtime import LaneCenteringHost, runtime_supported as lane_runtime_supported
|
|
from openpilot.starpilot.lateral.torque_runtime import TorqueHost, runtime_enabled as torque_runtime_enabled
|
|
from openpilot.starpilot.lateral.controller_selection import learning_allowed, read_selection
|
|
from openpilot.starpilot.lateral.gain_runtime import create_gain_owner
|
|
from openpilot.starpilot.flm.global_runtime import GlobalRuntime
|
|
from openpilot.starpilot.flm.evidence_hooks import control_message
|
|
from openpilot.starpilot.flm.application_wire import write_application
|
|
from openpilot.starpilot.longitudinal.inputs import LongitudinalInputs
|
|
from openpilot.starpilot.longitudinal.output_max import final_output
|
|
|
|
State = log.SelfdriveState.OpenpilotState
|
|
LaneChangeState = log.LaneChangeState
|
|
LaneChangeDirection = log.LaneChangeDirection
|
|
|
|
ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
|
|
|
|
|
|
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)
|
|
cloudlog.info("controlsd got CarParams")
|
|
|
|
self.CI = interfaces[self.CP.carFingerprint](self.CP)
|
|
self.hyundai_lateral_fault = LateralFaultLatch(self.CP.carFingerprint)
|
|
self.lateral_controller_selection = read_selection(self.params, self.CP)
|
|
self.vehicle_startup_preferences = VehicleStartupPreferences.read(
|
|
self.params, enabled=self.params.get_bool("OpenpilotEnabledToggle"))
|
|
self.torque_learning_allowed = learning_allowed(self.params, self.CP, selection=self.lateral_controller_selection)
|
|
self.aol_replay = feature_enabled(self.params, self.CP, 'aol', os.environ)
|
|
self.ordinary_axis_ack_required = axis_policy_for(self.CP).ordinary_axis_ack_required
|
|
self.longitudinal_inputs = LongitudinalInputs(self.CP, self.params, lambda: self.sm)
|
|
|
|
services = (['lateralDelay', 'vehicleParameters', 'lateralTorqueParameters', 'modelV2', 'selfdriveState',
|
|
'extrinsicsCalibration', 'deviceMotion', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput',
|
|
'driverMonitoringState', 'onroadEvents', 'driverAssistance'] +
|
|
(['aolAxisState', 'aolSafetyWire'] if self.aol_replay or self.ordinary_axis_ack_required else []))
|
|
optional = list(dict.fromkeys(
|
|
self.longitudinal_inputs.optional_services + ['deviceState', 'autoSteeringLearning', 'starpilotSteeringTracking', 'pandaStates']
|
|
))
|
|
if optional:
|
|
ignore = set(optional)
|
|
self.sm = messaging.SubMaster(services + optional, poll='selfdriveState',
|
|
ignore_alive=ignore, ignore_valid=ignore, ignore_avg_freq=ignore)
|
|
else:
|
|
self.sm = messaging.SubMaster(services, poll='selfdriveState')
|
|
self.pm = messaging.PubMaster(['carControl', 'controlsState', 'starpilotLateralState'])
|
|
|
|
self.steer_limited_by_safety = False
|
|
self.curvature = 0.0
|
|
self.desired_curvature = 0.0
|
|
self.model_turn_assist = ModelTurnAssist()
|
|
self.lane_centering_controller = LaneCenteringController()
|
|
self.lateral_pause = LateralPause(read_lateral_pause(self.params), self.params)
|
|
self.lane_change_preferences = LiveLaneChangePolicy(self.params,
|
|
now_ns=0 if os.getenv('REPLAY') == '1' else time.monotonic_ns())
|
|
self.lane_change_policy = self.lane_change_preferences.applied
|
|
self.lane_change_smoother = LaneChangeSmoother()
|
|
self.lane_centering_host = LaneCenteringHost(self.params) if lane_runtime_supported(self.CP) else None
|
|
self.torque_host = (TorqueHost(self.params, self.CP, allow_learning=(self.torque_learning_allowed
|
|
if self.lateral_controller_selection.policy is not None else None))
|
|
if torque_runtime_enabled(self.CP, self.params) else None)
|
|
self.last_lane_centering_result: LaneCenteringResult | None = None
|
|
self.lane_centering_applied = 0.0
|
|
|
|
self.pose_calibrator = PoseCalibrator()
|
|
self.calibrated_pose: Pose | None = None
|
|
|
|
self.lateral_gain_owner = None
|
|
self.LoC = LongControl(self.CP, startup_preferences=self.vehicle_startup_preferences)
|
|
self.VM = VehicleModel(self.CP)
|
|
self.LaC: LatControl
|
|
self.LaC = create_lateral_controller(self.CP, self.CI, DT_CTRL,
|
|
mode=self.lateral_controller_selection.mode, turn_assist=self.vehicle_startup_preferences.turn_assist)
|
|
if isinstance(self.LaC, LatControlTorque):
|
|
self.lateral_gain_owner = create_gain_owner(self.params, self.CP, self.LaC)
|
|
cloudlog.info({"event": "torque controller selected", "controller": self.LaC.controller_mode.value,
|
|
"source": self.lateral_controller_selection.source,
|
|
"vehicle": self.CP.carFingerprint, "policy": self.LaC.controller_policy,
|
|
"automaticLearning": self.torque_learning_allowed, "startupParameterSource": "vehicle",
|
|
"startupTorque": {"factor": self.LaC.torque_params.latAccelFactor,
|
|
"offset": self.LaC.torque_params.latAccelOffset,
|
|
"friction": self.LaC.torque_params.friction},
|
|
"startupPidGain": [list(row) for row in self.LaC.pid._k_p],
|
|
"startupPidLimits": [self.LaC.pid.neg_limit, self.LaC.pid.pos_limit]})
|
|
|
|
from openpilot.starpilot.flm.live_trial_store import load_confirmed_history
|
|
self.global_learning = GlobalRuntime(self.CP, self.CI, self.LaC, confirmed_live_history=load_confirmed_history(self.params))
|
|
|
|
from openpilot.starpilot.lateral.controller_selection import model_turn_assist_supported
|
|
assist_enabled = self.vehicle_startup_preferences.turn_assist and model_turn_assist_supported(self.CP)
|
|
self.turn_assist_enabled = lambda: assist_enabled
|
|
|
|
def update(self):
|
|
self.sm.update(15)
|
|
if self.sm.updated["extrinsicsCalibration"]:
|
|
self.pose_calibrator.feed_extrinsics_calibration(self.sm['extrinsicsCalibration'])
|
|
if self.sm.updated["deviceMotion"]:
|
|
device_motion = Pose.from_device_motion(self.sm['deviceMotion'])
|
|
self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_motion)
|
|
|
|
def refresh_lane_change_policy(self, carstate, model_v2, now_ns):
|
|
self.lane_change_policy = self.lane_change_preferences.sample(now_ns,
|
|
signaling=bool(carstate.leftBlinker or carstate.rightBlinker),
|
|
maneuver_active=model_v2.meta.laneChangeState != LaneChangeState.off)
|
|
return self.lane_change_policy
|
|
|
|
def _discard_global_learning(self):
|
|
automatic = getattr(self, 'global_learning', None)
|
|
self.global_learning = None
|
|
if automatic is not None:
|
|
try:
|
|
automatic._neutral('optional_metadata_failed')
|
|
except Exception:
|
|
pass # Hooks are detached below even if optional cleanup failed.
|
|
# These are optional metadata fields on the real GlobalRuntime object.
|
|
automatic.outcome = automatic.frame = automatic.profile = automatic.validation = None
|
|
automatic.eligible = False
|
|
# Detach only optional hooks. Native gains/PID/filter state and every core
|
|
# controller/authority gate remain owned by their existing callers.
|
|
for name in ('automatic_flm_stages', 'automatic_flm_centre', 'automatic_flm_observation', 'automatic_flm_adapter'):
|
|
self.LaC.__dict__.pop(name, None)
|
|
|
|
def state_control(self, *, lane_centering: LaneCenteringRequest | None = None):
|
|
lateral_inputs_refreshed = False
|
|
if (self.aol_replay and os.getenv('REPLAY') != '1' and
|
|
getattr(self, 'aol_previous_lateral_active', False)):
|
|
lateral_inputs_refreshed = refresh_expired_lateral_snapshot(self.sm, now_ns=time.monotonic_ns())
|
|
CS = self.sm['carState']
|
|
self.flm_native_witness = None
|
|
if self.lateral_gain_owner is not None:
|
|
self.lateral_gain_owner.refresh()
|
|
|
|
# Update VehicleModel
|
|
lp = self.sm['vehicleParameters']
|
|
x = max(lp.stiffnessFactor, 0.1)
|
|
sr = max(lp.steerRatio, 0.1)
|
|
torque_extension = getattr(self.LaC, 'starpilot_extension', None)
|
|
if torque_extension is not None:
|
|
sr = torque_extension.vehicle_model_ratio(sr, CS.vEgo)
|
|
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 isinstance(self.LaC, LatControlTorque) and self.torque_host is None and self.torque_learning_allowed:
|
|
torque_params = self.sm['lateralTorqueParameters']
|
|
if self.sm.all_checks(['lateralTorqueParameters']) and torque_params.useParams:
|
|
from openpilot.starpilot.flm.native_baseline import apply_native_refinement
|
|
apply_native_refinement(self.LaC, self.CP, self.sm,
|
|
now_ns=int(self.sm.logMonoTime['carState']) if os.getenv('REPLAY') == '1' else time.monotonic_ns(),
|
|
apply=lambda: self.LaC.update_torque_parameters(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
|
|
torque_params.frictionCoefficientFiltered))
|
|
|
|
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
|
|
lateral_requested = self.sm['selfdriveState'].enabled and self.sm['selfdriveState'].active
|
|
CC.latActive = ordinary_lateral_requested(self.sm['selfdriveState'].active, CS, self.CP)
|
|
CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl
|
|
CC.longActive = self.longitudinal_inputs.qualify_active(CC.longActive)
|
|
if self.aol_replay:
|
|
now_ns = int(self.sm.logMonoTime['carState']) if os.getenv('REPLAY') == '1' else time.monotonic_ns()
|
|
axis = current_axis(self.sm, now_ns=now_ns)
|
|
native = current_native(self.sm, self.CP, now_ns=now_ns,
|
|
axis_session_id=str(axis.sessionId) if axis is not None else None)
|
|
self.flm_native_witness = native
|
|
acknowledged = axis is not None and native is not None and axis.nativeAcknowledged
|
|
lateral_requested = bool(axis.desiredLateral) if axis is not None else None
|
|
CC.latActive = bool(acknowledged and axis.lateralActive and
|
|
bool(axis.desiredLateral) == bool(native.requestedLateral) and native.lateralAllowed and
|
|
not CS.steerFaultTemporary and not CS.steerFaultPermanent and
|
|
(not standstill or self.CP.steerAtStandstill))
|
|
CC.longActive = bool(acknowledged and axis.longitudinalActive and
|
|
bool(axis.desiredLongitudinal) == bool(native.requestedLongitudinal) and native.longitudinalAllowed and
|
|
self.CP.openpilotLongitudinalControl and
|
|
not any(e.overrideLongitudinal for e in self.sm['onroadEvents']))
|
|
|
|
elif getattr(self, 'ordinary_axis_ack_required', False):
|
|
now_ns = int(self.sm.logMonoTime['carState']) if os.getenv('REPLAY') == '1' else time.monotonic_ns()
|
|
CC.latActive = bool(CC.latActive and CS.canValid and not CS.canTimeout and
|
|
ordinary_axis_request_allowed(self.CP, CS) and
|
|
ordinary_axis_acknowledged(self.sm, self.CP, now_ns=now_ns,
|
|
on_validated_native=lambda native: setattr(self, 'flm_native_witness', native)))
|
|
|
|
if lateral_inputs_refreshed:
|
|
# A newly received physical veto wins over an older admitted axis. This
|
|
# LAT-only refresh cannot enable ordinary LONG; its next normal cycle owns that transition.
|
|
CC.latActive = bool(CC.latActive and CS.canValid and not CS.canTimeout)
|
|
CC.longActive = False
|
|
|
|
fault_latch = getattr(self, 'hyundai_lateral_fault', None)
|
|
if fault_latch is not None:
|
|
latched = fault_latch.update(requested=lateral_requested, temporary_fault=bool(CS.steerFaultTemporary),
|
|
cruise_enabled=bool(CS.cruiseState.enabled))
|
|
CC.latActive = bool(CC.latActive and not latched)
|
|
|
|
CC.latActive = allow_lateral_onset(self.CP, requested=bool(CC.latActive), normal_enabled=bool(CC.enabled),
|
|
steering_pressed=bool(CS.steeringPressed),
|
|
previous_active=bool(getattr(self, 'aol_previous_lateral_active', False)))
|
|
|
|
pause_now_ns = int(self.sm.logMonoTime['carState']) if os.getenv('REPLAY') == '1' else time.monotonic_ns()
|
|
self.lateral_pause.refresh(pause_now_ns)
|
|
pause_allowed = True
|
|
if self.lateral_pause.settings.speed_mps > 0:
|
|
device = self.sm['deviceState']
|
|
device_ns = int(self.sm.logMonoTime['deviceState'])
|
|
pause_drive = int(device.startedMonoTime) if (self.sm.seen['deviceState'] and self.sm.alive['deviceState'] and
|
|
self.sm.valid['deviceState'] and device.started and 0 < int(device.startedMonoTime) < device_ns <= pause_now_ns and
|
|
pause_now_ns - device_ns <= 2_000_000_000) else 0
|
|
pause_source = int(self.sm.logMonoTime['carState']) if (self.sm.seen['carState'] and self.sm.alive['carState'] and
|
|
self.sm.valid['carState']) else 0
|
|
pause_allowed = self.lateral_pause.allowed(CS, now_ns=pause_now_ns, source_ns=pause_source, drive_id=pause_drive)
|
|
|
|
CC.latActive = bool(CC.latActive and pause_allowed)
|
|
|
|
if self.torque_host is not None:
|
|
# The optional source must not keep an active torque request through lost car state.
|
|
CC.latActive = bool(CC.latActive and CS.canValid and not CS.canTimeout)
|
|
now_ns = int(self.sm.logMonoTime['carState']) if os.getenv('REPLAY') == '1' else time.monotonic_ns()
|
|
tune = self.torque_host.sample(self.sm, now_ns=now_ns, lat_active=bool(CC.latActive))
|
|
from openpilot.starpilot.flm.native_baseline import apply_native_refinement
|
|
from openpilot.starpilot.lateral.torque_tuning import TorqueSource
|
|
native_applied = bool(self.torque_learning_allowed and tune.source == TorqueSource.LEARNED and
|
|
self.torque_host.settings.user_factor is None and self.torque_host.settings.user_friction is None and
|
|
apply_native_refinement(self.LaC, self.CP, self.sm, now_ns=now_ns, host=True,
|
|
apply=lambda: self.torque_host.apply(self.LaC, tune)))
|
|
if not native_applied:
|
|
self.torque_host.apply(self.LaC, tune)
|
|
if not CC.latActive:
|
|
self.LaC.pid.reset()
|
|
|
|
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
|
|
publish_takeoff_state = getattr(self.LoC, 'takeoff_starting', False)
|
|
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
|
|
from opendbc.car.tesla.preap.pedal_profile import accel_limits as preap_pedal_limits
|
|
pid_accel_limits = preap_pedal_limits(self.CP, CS.vEgo,
|
|
getattr(self.sm['selfdriveState'].personality, 'raw', 1), pid_accel_limits)
|
|
longitudinal_output = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits,
|
|
context=self.longitudinal_inputs.context(CC.longActive))
|
|
actuators.accel = final_output(longitudinal_output)
|
|
if self.longitudinal_inputs.publish_state or publish_takeoff_state:
|
|
# Publish the selected launch state to the vehicle controller in this same frame.
|
|
actuators.longControlState = self.LoC.long_control_state
|
|
|
|
# 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
|
|
assist = self.turn_assist_enabled()
|
|
rolling = math.isfinite(CS.vEgo) and CS.vEgo > max(.1 * CV.MPH_TO_MS, self.CP.minSteerSpeed)
|
|
if (assist and rolling and self.sm.all_checks(['modelV2']) and CC.latActive and not CS.standstill and not CS.steerFaultTemporary and
|
|
not CS.steerFaultPermanent and CS.gearShifter in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low)):
|
|
self.model_turn_assist.curvature = self.curvature
|
|
self.model_turn_assist.twitch_guard_remaining = update_twitch_guard(self.model_turn_assist.twitch_guard_remaining, CS.vEgo, CS.standstill)
|
|
new_desired_curvature = self.model_turn_assist.update(CS, model_v2, CC, new_desired_curvature)
|
|
elif getattr(self, 'model_turn_assist', None) is not None:
|
|
self.model_turn_assist.reset()
|
|
self.model_turn_assist.twitch_guard_remaining = update_twitch_guard(0., CS.vEgo, CS.standstill)
|
|
lane_base_curvature = new_desired_curvature
|
|
self.lane_centering_applied = 0.0
|
|
self.last_lane_centering_result = None
|
|
if lane_centering is None and self.lane_centering_host is not None:
|
|
now_ns = int(self.sm.logMonoTime['carState']) if os.getenv('REPLAY') == '1' else time.monotonic_ns()
|
|
lane_centering = self.lane_centering_host.sample(self.sm, now_ns=now_ns,
|
|
lateral_active=CC.latActive, longitudinal_active=CC.longActive)
|
|
if lane_centering is None:
|
|
self.lane_centering_controller.reset()
|
|
elif not isinstance(lane_centering, LaneCenteringRequest):
|
|
self.lane_centering_controller.reset()
|
|
self.last_lane_centering_result = LaneCenteringResult(None, 0.0, 0, "invalid_request")
|
|
else:
|
|
result = self.lane_centering_controller.update(LaneCenteringInput(
|
|
model=model_v2, model_sample_id=self.sm.logMonoTime['modelV2'],
|
|
model_valid=bool(self.sm.all_checks(['modelV2'])), base_curvature=new_desired_curvature,
|
|
speed_mps=CS.vEgo, mode=lane_centering.mode, lateral_active=CC.latActive,
|
|
settings=lane_centering.settings, elapsed_seconds=lane_centering.elapsed_seconds,
|
|
turn_signal_active=bool(CS.leftBlinker or CS.rightBlinker), driver_override=bool(CS.steeringPressed),
|
|
time_discontinuity=lane_centering.time_discontinuity,
|
|
))
|
|
self.last_lane_centering_result = result
|
|
if result.candidate_curvature is not None:
|
|
new_desired_curvature = result.candidate_curvature
|
|
previous_curvature = self.desired_curvature
|
|
lane_policy_now_ns = int(self.sm.logMonoTime['carState']) if os.getenv('REPLAY') == '1' else time.monotonic_ns()
|
|
self.refresh_lane_change_policy(CS, model_v2, lane_policy_now_ns)
|
|
factor = self.lane_change_smoother.factor(
|
|
active=bool(CC.latActive and CS.canValid and not CS.canTimeout and self.sm.all_checks(['modelV2']) and
|
|
self.lane_change_policy.enabled), lane_change_state=model_v2.meta.laneChangeState,
|
|
speed=CS.vEgo, minimum_speed=self.lane_change_policy.minimum_speed_mps,
|
|
duration=self.lane_change_policy.duration_s, previous=previous_curvature, desired=new_desired_curvature,
|
|
turn_assist=assist)
|
|
new_desired_curvature = limit_rate(CS.vEgo, previous_curvature, new_desired_curvature, factor)
|
|
self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, previous_curvature, new_desired_curvature, lp.roll)
|
|
if self.last_lane_centering_result is not None and self.last_lane_centering_result.correction != 0.0:
|
|
baseline_curvature, _ = clip_curvature(CS.vEgo, previous_curvature, limit_rate(CS.vEgo, previous_curvature, lane_base_curvature, factor), lp.roll)
|
|
self.lane_centering_applied = self.desired_curvature - baseline_curvature
|
|
lat_delay = self.sm["lateralDelay"].lateralDelay + LAT_SMOOTH_SECONDS
|
|
|
|
actuators.curvature = self.desired_curvature
|
|
automatic = getattr(self, 'global_learning', None)
|
|
if automatic is not None:
|
|
try:
|
|
automatic.prepare(self.sm, now_ns=time.monotonic_ns(), active=bool(CC.latActive),
|
|
healthy=bool(CS.canValid and not CS.canTimeout and not CS.steerFaultTemporary and
|
|
not CS.steerFaultPermanent and not CS.accFaulted and not CS.steeringPressed and
|
|
not CS.brakePressed and not CS.regenBraking and
|
|
not any(str(button.type) == 'cancel' for button in CS.buttonEvents) and
|
|
not any(event.softDisable or event.immediateDisable for event in self.sm['onroadEvents']) and
|
|
not self.sm['driverMonitoringState'].noResponseForceDecel), controller=self.LaC,
|
|
native_required=bool(self.aol_replay or getattr(self, 'ordinary_axis_ack_required', False)),
|
|
native_witness=getattr(self, 'flm_native_witness', None))
|
|
automatic.begin_frame(CS, self.VM, lp, active=bool(CC.latActive),
|
|
safety_limited=self.steer_limited_by_safety, desired_curvature=self.desired_curvature,
|
|
curvature_limited=curvature_limited, lat_delay=lat_delay,
|
|
raw_model_curvature=model_v2.action.desiredCurvature, model_source_ns=int(self.sm.logMonoTime['modelV2']))
|
|
except Exception:
|
|
self._discard_global_learning()
|
|
automatic = None
|
|
steer, lateral_output, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
|
|
self.steer_limited_by_safety, self.desired_curvature,
|
|
curvature_limited, lat_delay)
|
|
actuators.torque = float(steer)
|
|
if self.CP.steerControlType == car.CarParams.SteerControlType.curvature:
|
|
actuators.curvature = float(lateral_output)
|
|
else:
|
|
actuators.steeringAngleDeg = float(lateral_output)
|
|
# 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)
|
|
|
|
if automatic is not None:
|
|
try:
|
|
requested = (actuators.curvature if self.CP.steerControlType == car.CarParams.SteerControlType.curvature else
|
|
actuators.steeringAngleDeg if self.CP.steerControlType == car.CarParams.SteerControlType.angle else actuators.torque)
|
|
automatic.finish_frame(CS, self.VM, lp, lac_log, desired_curvature=self.desired_curvature,
|
|
curvature_limited=curvature_limited, lat_delay=lat_delay, requested=requested,
|
|
selected_command=(lateral_output if self.CP.steerControlType in
|
|
(car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvature) else steer))
|
|
except Exception:
|
|
self._discard_global_learning()
|
|
self.aol_previous_lateral_active = bool(CC.latActive)
|
|
return CC, lac_log
|
|
|
|
def publish(self, CC, lac_log):
|
|
CS = self.sm['carState']
|
|
|
|
# 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
|
|
CC.cruiseControl.cancel = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise)
|
|
CC.cruiseControl.resume = CC.enabled and CS.cruiseState.standstill and not self.sm['longitudinalPlan'].shouldStop
|
|
if self.LoC.extension is not None:
|
|
CC.cruiseControl.resume = self.LoC.extension.qualify_resume(
|
|
CC.cruiseControl.resume, preferences=self.vehicle_startup_preferences, inputs=self.longitudinal_inputs)
|
|
|
|
hudControl = CC.hudControl
|
|
hudControl.setSpeed = float(CS.vCruiseCluster * CV.KPH_TO_MS)
|
|
hudControl.speedVisible = CC.enabled
|
|
hudControl.lanesVisible = CC.enabled
|
|
hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
|
|
hudControl.leadVisible = self.longitudinal_inputs.lead_visible(hudControl.leadVisible)
|
|
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
|
|
|
|
feedback_input_limited = self.steer_limited_by_safety
|
|
if CC.latActive:
|
|
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
|
|
|
|
else:
|
|
self.steer_limited_by_safety = False
|
|
|
|
# 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))
|
|
|
|
# trigger the car's stock driver monitoring escalation
|
|
CC.driverMonitoringEscalation = cs.forceDecel
|
|
|
|
lat_tuning = self.CP.lateralTuning.which()
|
|
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
|
cs.lateralControlState.angleState = lac_log
|
|
elif self.CP.steerControlType == car.CarParams.SteerControlType.curvature:
|
|
cs.lateralControlState.curvatureState = 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)
|
|
feedback = feedback_message(
|
|
self.last_lane_centering_result, self.lane_centering_applied, CC.latActive,
|
|
model_mono_time=self.sm.logMonoTime['modelV2'], car_control_mono_time=cc_send.logMonoTime,
|
|
valid=bool(CS.canValid and self.sm.all_checks(['modelV2'])))
|
|
automatic = getattr(self, 'global_learning', None)
|
|
if automatic is not None:
|
|
try:
|
|
learning_feedback = feedback.as_reader().as_builder()
|
|
written = control_message(learning_feedback, self.LaC, CC, control_ns=cc_send.logMonoTime,
|
|
state_ns=self.sm.logMonoTime['carState'], limited=self.steer_limited_by_safety)
|
|
if written is False:
|
|
raise ValueError('optional control evidence write failed')
|
|
if automatic.outcome is not None:
|
|
write_application(learning_feedback.starpilotLateralState, automatic.outcome)
|
|
feedback = learning_feedback
|
|
except Exception:
|
|
self._discard_global_learning()
|
|
if (automatic is not None and automatic.eligible and automatic.source is not None and
|
|
automatic.source.enabled and not automatic.source.manual and
|
|
self.CP.steerControlType == car.CarParams.SteerControlType.torque):
|
|
try:
|
|
from openpilot.starpilot.flm.feedback_observation import capture_controls
|
|
from openpilot.starpilot.flm.feedback_wire import write_consumption
|
|
observation = capture_controls(self, CC, control_ns=int(cc_send.logMonoTime), input_limited=feedback_input_limited)
|
|
if observation is not None:
|
|
feedback_copy = feedback.as_reader().as_builder()
|
|
write_consumption(feedback_copy.starpilotLateralState, observation)
|
|
feedback = feedback_copy
|
|
except Exception:
|
|
self.learning_feedback_prior_publish_ns = 0
|
|
else:
|
|
self.learning_feedback_prior_publish_ns = 0
|
|
self.pm.send('starpilotLateralState', feedback)
|
|
|
|
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()
|
|
if HARDWARE.get_device_type() in ("tici", "tizi", "mici"):
|
|
set_core_affinity([6])
|
|
os.sched_setscheduler(0, os.SCHED_OTHER, os.sched_param(0))
|
|
else:
|
|
set_core_affinity([4, 5])
|
|
controls.run()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|