github-actions[bot]
f9fcc7adab
date: 2026-06-28T09:48:35 master commit: da6313dbe95b3f24bb5d8018b0e5f950f5823ca7
120 lines
4.3 KiB
Python
120 lines
4.3 KiB
Python
"""
|
|
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
|
|
|
This file is part of sunnypilot and is licensed under the MIT License.
|
|
See the LICENSE.md file in the root directory for more details.
|
|
"""
|
|
import time
|
|
|
|
import cereal.messaging as messaging
|
|
from cereal import log, custom
|
|
|
|
from opendbc.car import structs
|
|
from openpilot.common.params import Params
|
|
from openpilot.common.swaglog import cloudlog
|
|
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
|
|
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
|
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
|
|
from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral
|
|
from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_v0 import LatControlTorque as LatControlTorqueV0
|
|
|
|
|
|
class ControlsExt(ModelStateBase):
|
|
def __init__(self, CP: structs.CarParams, params: Params):
|
|
ModelStateBase.__init__(self)
|
|
self.CP = CP
|
|
self.params = params
|
|
self._param_update_time: float = 0.0
|
|
self.blinker_pause_lateral = BlinkerPauseLateral()
|
|
|
|
cloudlog.info("controlsd_ext is waiting for CarParamsSP")
|
|
self.CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP)
|
|
cloudlog.info("controlsd_ext got CarParamsSP")
|
|
|
|
self.sm_services_ext = ['radarState', 'selfdriveStateSP']
|
|
self.pm_services_ext = ['carControlSP']
|
|
|
|
def initialize_lateral_control(self, lac, CI, dt):
|
|
enforce_torque_control = self.params.get_bool("EnforceTorqueControl")
|
|
torque_versions = self.params.get("TorqueControlTune")
|
|
if not enforce_torque_control:
|
|
if self.CP.lateralTuning.which() == 'torque':
|
|
return LatControlTorqueV0(self.CP, self.CP_SP, CI, dt) # FIXME-SP: revert when upstream fixes tuning issues with v1
|
|
return lac
|
|
|
|
if torque_versions == 0.0: # v0
|
|
return LatControlTorqueV0(self.CP, self.CP_SP, CI, dt)
|
|
else:
|
|
return lac
|
|
|
|
def get_params_sp(self, sm: messaging.SubMaster) -> None:
|
|
if time.monotonic() - self._param_update_time > PARAMS_UPDATE_PERIOD:
|
|
self.blinker_pause_lateral.get_params()
|
|
|
|
if self.CP.lateralTuning.which() == 'torque':
|
|
self.lat_delay = get_lat_delay(self.params, sm["liveDelay"].lateralDelay)
|
|
|
|
self._param_update_time = time.monotonic()
|
|
|
|
def get_lat_active(self, sm: messaging.SubMaster) -> bool:
|
|
if self.blinker_pause_lateral.update(sm['carState']):
|
|
return False
|
|
|
|
ss_sp = sm['selfdriveStateSP']
|
|
if ss_sp.mads.available:
|
|
return bool(ss_sp.mads.active)
|
|
|
|
# MADS not available, use stock state to engage
|
|
return bool(sm['selfdriveState'].active)
|
|
|
|
@staticmethod
|
|
def get_lead_data(_lead, src: log.RadarState.LeadData) -> None:
|
|
_lead.dRel = src.dRel
|
|
_lead.yRel = src.yRel
|
|
_lead.vRel = src.vRel
|
|
_lead.aRel = src.aRel
|
|
_lead.vLead = src.vLead
|
|
_lead.dPath = src.dPath
|
|
_lead.vLat = src.vLat
|
|
_lead.vLeadK = src.vLeadK
|
|
_lead.aLeadK = src.aLeadK
|
|
_lead.fcw = src.fcw
|
|
_lead.status = src.status
|
|
_lead.aLeadTau = src.aLeadTau
|
|
_lead.modelProb = src.modelProb
|
|
_lead.radar = src.radar
|
|
_lead.radarTrackId = src.radarTrackId
|
|
|
|
def state_control_ext(self, sm: messaging.SubMaster) -> custom.CarControlSP:
|
|
CC_SP = custom.CarControlSP.new_message()
|
|
|
|
self.get_lead_data(CC_SP.leadOne, sm['radarState'].leadOne)
|
|
self.get_lead_data(CC_SP.leadTwo, sm['radarState'].leadTwo)
|
|
|
|
# MADS state
|
|
mads_src = sm['selfdriveStateSP'].mads
|
|
CC_SP.mads.state = mads_src.state
|
|
CC_SP.mads.enabled = mads_src.enabled
|
|
CC_SP.mads.active = mads_src.active
|
|
CC_SP.mads.available = mads_src.available
|
|
|
|
# ICBM state
|
|
icbm_src = sm['selfdriveStateSP'].intelligentCruiseButtonManagement
|
|
CC_SP.intelligentCruiseButtonManagement.state = icbm_src.state
|
|
CC_SP.intelligentCruiseButtonManagement.sendButton = icbm_src.sendButton
|
|
CC_SP.intelligentCruiseButtonManagement.vTarget = icbm_src.vTarget
|
|
|
|
return CC_SP
|
|
|
|
@staticmethod
|
|
def publish_ext(CC_SP: custom.CarControlSP, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
|
|
cc_sp_send = messaging.new_message('carControlSP')
|
|
cc_sp_send.valid = sm['carState'].canValid
|
|
cc_sp_send.carControlSP = CC_SP
|
|
|
|
pm.send('carControlSP', cc_sp_send)
|
|
|
|
def run_ext(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
|
|
CC_SP = self.state_control_ext(sm)
|
|
self.publish_ext(CC_SP, sm, pm)
|