#!/usr/bin/env python3 import math from numbers import Number from openpilot.cereal import log, custom 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.vehicle_model import VehicleModel from openpilot.selfdrive.controls.lib.curvatured import CurvatureDController from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature 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.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_T_FOLLOW from openpilot.common.pt2 import PT2Filter 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() self.param_counter = 0 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) cloudlog.info("controlsd is waiting for CarParamsIC") self.CP_IC = messaging.log_from_bytes(self.params.get("CarParamsIC", block=True), custom.CarParamsIC) cloudlog.info("controlsd got CarParamsIC") self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP, self.CP_IC) ic_sm_services = ['carStateIC', 'lateralCurvatureParameters', 'longitudinalPlanIC'] ic_pm_services = ['carControlIC', 'controlsStateIC'] self.sm = messaging.SubMaster(['lateralDelay', 'vehicleParameters', 'lateralTorqueParameters', 'modelV2', 'selfdriveState', 'extrinsicsCalibration', 'deviceMotion', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput', 'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + ic_sm_services + self.sm_services_ext, poll='selfdriveState') self.pm = messaging.PubMaster(['carControl', 'controlsState'] + ic_pm_services + self.pm_services_ext) self.steer_limited_by_safety = False self.curvature = 0.0 self.roll_compensation = 0.0 self.model_desired_curvature = 0.0 self.desired_curvature = 0.0 self.enable_curvature_controller = self.params.get_bool("EnableCurvatureController") self.enable_curvatured = self.params.get_bool("EnableCurvatureD") self.enable_speed_limit_control = self.params.get_bool("EnableSpeedLimitControl") self.enable_speed_limit_predicative = self.params.get_bool("EnableSpeedLimitPredicative") self.enable_pred_react_to_speed_limits = self.params.get_bool("EnableSLPredReactToSL") self.enable_pred_react_to_curves = self.params.get_bool("EnableSLPredReactToCurves") self.enable_smooth_steer = self.params.get_bool("EnableSmoothSteer") self.enable_long_comfort_mode = self.params.get_bool("EnableLongComfortMode") self.smooth_steer = PT2Filter(46.0, 1.0, DT_CTRL) self.force_rhd_for_bsm = self.params.get_bool("ForceRHDForBSM") self.disable_car_steer_alerts = self.params.get_bool("DisableCarSteerAlerts") self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None self.LoC = LongControl(self.CP, self.CP_SP) self.VM = VehicleModel(self.CP) self.curvatured = CurvatureDController() if self.CP.steerControlType == car.CarParams.SteerControlType.curvature else None 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) self.param_counter += 1 if self.param_counter >= 100: self.param_counter = 0 self.enable_curvature_controller = self.params.get_bool("EnableCurvatureController") if self.CP.steerControlType == car.CarParams.SteerControlType.curvature: self.LaC.set_pid_enabled(self.enable_curvature_controller) self.enable_smooth_steer = self.params.get_bool("EnableSmoothSteer") self.enable_speed_limit_control = self.params.get_bool("EnableSpeedLimitControl") self.enable_speed_limit_predicative = self.params.get_bool("EnableSpeedLimitPredicative") self.enable_pred_react_to_speed_limits = self.params.get_bool("EnableSLPredReactToSL") self.enable_pred_react_to_curves = self.params.get_bool("EnableSLPredReactToCurves") self.force_rhd_for_bsm = self.params.get_bool("ForceRHDForBSM") self.enable_long_comfort_mode = self.params.get_bool("EnableLongComfortMode") self.disable_car_steer_alerts = self.params.get_bool("DisableCarSteerAlerts") def state_control(self): CS = self.sm['carState'] CS_IC = self.sm['carStateIC'] # 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) self.roll_compensation = -self.VM.roll_compensation(lp.roll, CS.vEgo) # 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) if self.CP.steerControlType == car.CarParams.SteerControlType.curvature: curvature_params = self.sm['lateralCurvatureParameters'] if self.sm.all_checks(['lateralCurvatureParameters']) and curvature_params.useParams: self.curvatured.update_live_params(curvature_params) else: self.curvatured.reset() 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() self.smooth_steer.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)) actuators.speed = float(self.sm['longitudinalPlanIC'].vTarget) # 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.model_desired_curvature = float(model_v2.action.desiredCurvature) if self.enable_smooth_steer: new_desired_curvature = self.smooth_steer.update(new_desired_curvature) if self.CP.steerControlType == car.CarParams.SteerControlType.curvature: self.LaC.set_steering_slightly_pressed(CS_IC.steeringSlightlyPressed) # CurvatureD correction routed as additive term on the controller output (not setpoint shift) if CC.latActive and self.enable_curvatured and self.sm.all_checks(['lateralCurvatureParameters']): correction = self.curvatured.get_correction(self.desired_curvature, CS.vEgo) else: correction = 0.0 self.LaC.set_curvature_correction(correction) 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) # 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'] CC_IC = custom.CarControlIC.new_message() CC_IC.curvatureControllerActive = self.enable_curvature_controller CC_IC.steerLimited = self.steer_limited_by_safety CC_IC.forceRHDForBSM = self.force_rhd_for_bsm CC_IC.longComfortMode = self.enable_long_comfort_mode CC_IC.disableCarSteerAlerts = self.disable_car_steer_alerts CC_IC.cruiseSpeedLimit = self.enable_speed_limit_control CC_IC.cruiseSpeedLimitPredicative = self.enable_speed_limit_predicative CC_IC.cruiseSpeedLimitPredReactToSL = self.enable_pred_react_to_speed_limits CC_IC.cruiseSpeedLimitPredReactToCurves = self.enable_pred_react_to_curves # Orientation and angle rates can be useful for carcontroller # Only calibrated (car) frame is relevant for the carcontroller CC.currentCurvature = self.curvature CC_IC.rollCompensation = self.roll_compensation 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 CC_IC.hudLeadDistance = self.sm['longitudinalPlanIC'].leadDistance hudControl.leadDistanceBars = self.sm['selfdriveState'].personality.raw + 1 CC_IC.hudLeadFollowTime = get_T_FOLLOW(hudControl.leadDistanceBars - 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) # carControlIC cc_ic_send = messaging.new_message('carControlIC') cc_ic_send.valid = CS.canValid cc_ic_send.carControlIC = CC_IC self.pm.send('carControlIC', cc_ic_send) # controlsStateIC cs_ic_send = messaging.new_message('controlsStateIC') cs_ic_send.valid = CS.canValid cs_ic_send.controlsStateIC.modelDesiredCurvature = self.model_desired_curvature self.pm.send('controlsStateIC', cs_ic_send) # 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()