mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-10 15:43:42 +08:00
Isaac Barham
5fbb583e59
Add a distance-integrated steering-curvature correction to the restored v1 heading request. Hold the correction at zero error, allow unwind at limits, and clear it for driver override or inactive PSCM control. Preserve C0, C2=C3=0, final output limits, the 100 Hz sender and the existing opt-in toggle. Validate build/hold/unwind, measurement cadence, anti-windup and the actual controlsd-to-CAN path. The combined suite passes 511 tests and 9,146 subtests; 178 inherited or unsupported safety variants skip. Stress and frozen b8 replay pass 578,569 Float32/CAN round trips. These checks do not establish physical tracking or stability. Record reproduction steps and source hashes.
308 lines
15 KiB
Python
Executable File
308 lines
15 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
import math
|
|
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.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.ford.values import FordFlags
|
|
from opendbc.car.vehicle_model import VehicleModel
|
|
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
|
|
from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, select_model_action_controller
|
|
from openpilot.selfdrive.controls.lib.ford_path import FordPath, FordPathController, FordPscmObserverPathController
|
|
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_curvature import LatControlCurvature
|
|
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
|
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.sunnypilot.selfdrive.controls.controlsd_ext import ControlsExt
|
|
|
|
State = log.SelfdriveState.OpenpilotState
|
|
LaneChangeState = log.LaneChangeState
|
|
LaneChangeDirection = log.LaneChangeDirection
|
|
|
|
ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
|
|
|
|
|
|
class Controls(ControlsExt):
|
|
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")
|
|
|
|
# Initialize sunnypilot controlsd extension and base model state
|
|
ControlsExt.__init__(self, self.CP, self.params)
|
|
|
|
self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP)
|
|
|
|
self.sm = messaging.SubMaster(['lateralDelay', 'vehicleParameters', 'lateralTorqueParameters', 'modelV2', 'selfdriveState',
|
|
'extrinsicsCalibration', 'deviceMotion', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carStateSP', 'carOutput',
|
|
'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + self.sm_services_ext,
|
|
poll='selfdriveState')
|
|
self.pm = messaging.PubMaster(['carControl', 'controlsState'] + self.pm_services_ext)
|
|
|
|
self.steer_limited_by_safety = False
|
|
self.curvature = 0.0
|
|
self.desired_curvature = 0.0
|
|
self.ford_pscm_observer = (self.CP.brand == "ford" and self.CP.flags & FordFlags.CANFD and
|
|
self.params.get_bool("FordPscmObserver"))
|
|
self.ford_path_controller = FordPscmObserverPathController() if self.ford_pscm_observer else FordPathController()
|
|
self.ford_path_controller = select_model_action_controller(self.CP, self.params.get_bool("FordModelActionController"),
|
|
self.ford_path_controller)
|
|
self.ford_model_action = isinstance(self.ford_path_controller, FordModelActionController)
|
|
if self.CP.brand == "ford":
|
|
cloudlog.event("Ford path controller selected", controller=type(self.ford_path_controller).__name__)
|
|
self.ford_path = FordPath()
|
|
|
|
self.pose_calibrator = PoseCalibrator()
|
|
self.calibrated_pose: Pose | None = None
|
|
|
|
self.LoC = LongControl(self.CP, self.CP_SP)
|
|
self.VM = VehicleModel(self.CP)
|
|
self.LaC: LatControl
|
|
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
|
self.LaC = LatControlAngle(self.CP, self.CP_SP, self.CI, DT_CTRL)
|
|
elif self.CP.steerControlType == car.CarParams.SteerControlType.curvature:
|
|
self.LaC = LatControlCurvature(self.CP, self.CP_SP, self.CI, DT_CTRL)
|
|
elif self.CP.lateralTuning.which() == 'pid':
|
|
self.LaC = LatControlPID(self.CP, self.CP_SP, self.CI, DT_CTRL)
|
|
elif self.CP.lateralTuning.which() == 'torque':
|
|
self.LaC = LatControlTorque(self.CP, self.CP_SP, self.CI, DT_CTRL)
|
|
|
|
self.LaC = ControlsExt.initialize_lateral_control(self, self.LaC, self.CI, DT_CTRL)
|
|
|
|
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 state_control(self):
|
|
CS = self.sm['carState']
|
|
|
|
# Update VehicleModel
|
|
lp = self.sm['vehicleParameters']
|
|
x = max(lp.stiffnessFactor, 0.1)
|
|
sr = max(lp.steerRatio, 0.1)
|
|
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['lateralTorqueParameters']
|
|
if self.sm.all_checks(['lateralTorqueParameters']) and torque_params.useParams:
|
|
self.LaC.update_torque_parameters(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
|
|
torque_params.frictionCoefficientFiltered)
|
|
|
|
self.LaC.extension.update_limits()
|
|
|
|
self.LaC.extension.update_model_v2(self.sm['modelV2'])
|
|
|
|
self.LaC.extension.update_lateral_lag(self.lat_delay)
|
|
|
|
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
|
|
|
|
# Get which state to use for active lateral control
|
|
_lat_active = self.get_lat_active(self.sm)
|
|
|
|
CC.latActive = _lat_active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
|
|
(not standstill or self.CP.steerAtStandstill)
|
|
CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and \
|
|
(self.CP.openpilotLongitudinalControl or not self.CP_SP.pcmCruiseSpeed)
|
|
|
|
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, self.CP_SP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
|
|
actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits))
|
|
|
|
# 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
|
|
self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll)
|
|
lat_delay = self.sm["lateralDelay"].lateralDelay + LAT_SMOOTH_SECONDS
|
|
|
|
actuators.curvature = self.desired_curvature
|
|
steer, lateral_output, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
|
|
self.steer_limited_by_safety, self.desired_curvature,
|
|
self.calibrated_pose, 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)
|
|
if self.CP.brand == "ford":
|
|
ford_model = model_v2 if self.sm.valid['modelV2'] else None
|
|
if self.ford_model_action:
|
|
reference_service = 'lateralManeuverPlan' if self.sm.valid['lateralManeuverPlan'] else 'modelV2'
|
|
self.ford_path = self.ford_path_controller.update(
|
|
ford_model, self.desired_curvature, current_curvature=self.curvature, yaw_rate=-CS.yawRate, speed=CS.vEgo, now=time.monotonic(),
|
|
measurement_time=self.sm.logMonoTime['carState'] * 1e-9,
|
|
model_time=self.sm.logMonoTime['modelV2'] * 1e-9,
|
|
reference_time=self.sm.logMonoTime[reference_service] * 1e-9,
|
|
active=CC.latActive, valid=CS.canValid and self.sm.all_checks(['carState', 'vehicleParameters', 'modelV2', reference_service]),
|
|
driver_pressed=CS.steeringPressed, driver_torque=CS.steeringTorque,
|
|
pscm_status=self.sm['carStateSP'].fordPscmStatus if self.sm.valid['carStateSP'] else None,
|
|
)
|
|
if not self.ford_path.valid:
|
|
CC.latActive = False
|
|
if self.sm.frame % 20 == 0:
|
|
cloudlog.event("Ford C2-free path tracking", model_mono_time=self.sm.logMonoTime['modelV2'],
|
|
measurement_mono_time=self.sm.logMonoTime['carState'],
|
|
reference_service=reference_service, reference_mono_time=self.sm.logMonoTime[reference_service],
|
|
measured_curvature=self.curvature,
|
|
**self.ford_path_controller.diagnostics)
|
|
elif self.ford_pscm_observer:
|
|
self.ford_path = self.ford_path_controller.update(ford_model, self.desired_curvature,
|
|
current_curvature=self.curvature, v_ego=CS.vEgo,
|
|
v_ego_raw=CS.vEgoRaw, active=CC.latActive)
|
|
else:
|
|
self.ford_path = self.ford_path_controller.update(ford_model, self.desired_curvature,
|
|
current_curvature=self.curvature, v_ego=CS.vEgo,
|
|
active=CC.latActive)
|
|
actuators.curvature = float(self.ford_path.curvature)
|
|
# 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']
|
|
|
|
# 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 or not self.CP_SP.pcmCruiseSpeed)
|
|
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
|
|
|
|
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.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.get_lat_active(self.sm):
|
|
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))
|
|
|
|
# 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)
|
|
|
|
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)
|
|
self.get_params_sp(self.sm)
|
|
self.run_ext(self.sm, self.pm)
|
|
rk.monitor_time()
|
|
|
|
|
|
def main():
|
|
config_realtime_process(4, Priority.CTRL_HIGH)
|
|
controls = Controls()
|
|
controls.run()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|