mirror of
https://github.com/infiniteCable2/openpilot.git
synced 2026-09-05 15:53:41 +08:00
Jason Wen
b51043770b
* car: initialize sunnypilot interface immediately * init * Revert "car: initialize sunnypilot interface immediately" This reverts commit 2e0fc4d87eb312a3a74a61bb8cbc780e33efbfc7. * show int * show int * work properly with cereal * unused * in its own module * bump * not yet * not yet * list comp and use structs directly * allow default key if needed * lint * send it with None * bump
64 lines
2.0 KiB
Python
64 lines
2.0 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 cereal.messaging as messaging
|
|
from cereal import custom
|
|
|
|
from opendbc.car import structs
|
|
from openpilot.common.params import Params
|
|
from openpilot.common.swaglog import cloudlog
|
|
from openpilot.sunnypilot.selfdrive.controls.lib.param_store import ParamStore
|
|
|
|
|
|
class ControlsExt:
|
|
def __init__(self, CP: structs.CarParams, params: Params):
|
|
self.CP = CP
|
|
self.params = params
|
|
self.param_store = ParamStore(self.CP)
|
|
self.get_params_sp()
|
|
|
|
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 = ['selfdriveStateSP']
|
|
self.pm_services_ext = ['carControlSP']
|
|
|
|
def get_params_sp(self) -> None:
|
|
self.param_store.update(self.params)
|
|
|
|
@staticmethod
|
|
def get_lat_active(sm: messaging.SubMaster) -> bool:
|
|
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)
|
|
|
|
def state_control_ext(self, sm: messaging.SubMaster) -> custom.CarControlSP:
|
|
CC_SP = custom.CarControlSP.new_message()
|
|
|
|
# MADS state
|
|
CC_SP.mads = sm['selfdriveStateSP'].mads
|
|
|
|
CC_SP.params = self.param_store.publish()
|
|
|
|
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)
|