diff --git a/cereal/custom.capnp b/cereal/custom.capnp index f324ef5da..b75983e9a 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -10,23 +10,23 @@ $Cxx.namespace("cereal"); # DO rename the structs # DON'T change the identifier (e.g. @0x81c2f05a394cf4af) +struct ModularAssistiveDrivingSystem { + state @0 :ModularAssistiveDrivingSystemState; + enabled @1 :Bool; + active @2 :Bool; + available @3 :Bool; + + enum ModularAssistiveDrivingSystemState { + disabled @0; + paused @1; + enabled @2; + softDisabling @3; + overriding @4; + } +} + struct SelfdriveStateSP @0x81c2f05a394cf4af { mads @0 :ModularAssistiveDrivingSystem; - - struct ModularAssistiveDrivingSystem { - state @0 :ModularAssistiveDrivingSystemState; - enabled @1 :Bool; - active @2 :Bool; - available @3 :Bool; - - enum ModularAssistiveDrivingSystemState { - disabled @0; - paused @1; - enabled @2; - softDisabling @3; - overriding @4; - } - } } struct ModelManagerSP @0xaedffd8f31e7b55d { @@ -137,7 +137,8 @@ struct CarParamsSP @0x80ae746ee2596b11 { flags @0 :UInt32; # flags for car specific quirks in sunnypilot } -struct CustomReserved5 @0xa5cd762cd951a455 { +struct CarControlSP @0xa5cd762cd951a455 { + mads @0 :ModularAssistiveDrivingSystem; } struct CustomReserved6 @0xf98d843bfd7004a3 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 95f741ee3..4b0e10c3f 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2570,7 +2570,7 @@ struct Event { longitudinalPlanSP @109 :Custom.LongitudinalPlanSP; onroadEventsSP @110 :List(Custom.OnroadEventSP); carParamsSP @111 :Custom.CarParamsSP; - customReserved5 @112 :Custom.CustomReserved5; + carControlSP @112 :Custom.CarControlSP; customReserved6 @113 :Custom.CustomReserved6; customReserved7 @114 :Custom.CustomReserved7; customReserved8 @115 :Custom.CustomReserved8; diff --git a/cereal/services.py b/cereal/services.py index e39284d55..f6f90d62a 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -80,6 +80,7 @@ _services: dict[str, tuple] = { "longitudinalPlanSP": (True, 20., 10), "onroadEventsSP": (True, 1., 1), "carParamsSP": (True, 0.02, 1), + "carControlSP": (True, 100., 10), # debug "uiDebug": (True, 0., 1), diff --git a/opendbc_repo b/opendbc_repo index 2bd2bcdf5..5e5885c75 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 2bd2bcdf5db6db09ac36851f2760804039a5a52a +Subproject commit 5e5885c75b0486c2ac4b75924a81412f65b09f57 diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 5c2baed71..62ecb1cba 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -21,7 +21,7 @@ from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp from openpilot.selfdrive.car.cruise import VCruiseHelper from openpilot.selfdrive.car.car_specific import MockCarState -from openpilot.selfdrive.car.helpers import convert_to_capnp +from openpilot.selfdrive.car.helpers import convert_carControlSP, convert_to_capnp from openpilot.sunnypilot.mads.mads import MadsParams from openpilot.sunnypilot.selfdrive.car import interfaces @@ -72,7 +72,7 @@ class Car: def __init__(self, CI=None, RI=None) -> None: self.can_sock = messaging.sub_sock('can', timeout=20) - self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents']) + self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP']) self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP']) self.can_rcv_cum_timeout_counter = 0 @@ -264,7 +264,7 @@ class Car: cp_sp_send.carParamsSP = self.CP_SP_capnp self.pm.send('carParamsSP', cp_sp_send) - def controls_update(self, CS: car.CarState, CC: car.CarControl): + def controls_update(self, CS: car.CarState, CC: car.CarControl, CC_SP: custom.CarControlSP): """control update loop, driven by carControl""" if not self.initialized_prev: @@ -278,7 +278,7 @@ class Car: if self.sm.all_alive(['carControl']): # send car controls over can now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) - self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos) + self.last_actuators_output, can_sends = self.CI.apply(CC, convert_carControlSP(CC_SP), now_nanos) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) self.CC_prev = CC @@ -291,7 +291,7 @@ class Car: initialized = (not any(e.name == EventName.selfdriveInitializing for e in self.sm['onroadEvents']) and self.sm.seen['onroadEvents']) if not self.CP.passive and initialized: - self.controls_update(CS, self.sm['carControl']) + self.controls_update(CS, self.sm['carControl'], self.sm['carControlSP']) self.initialized_prev = initialized self.CS_prev = CS diff --git a/selfdrive/car/helpers.py b/selfdrive/car/helpers.py index cb4a6bc67..3f7a322c0 100644 --- a/selfdrive/car/helpers.py +++ b/selfdrive/car/helpers.py @@ -44,3 +44,16 @@ def convert_to_capnp(struct: structs.CarParamsSP) -> capnp.lib.capnp._DynamicStr raise ValueError(f"Unsupported struct type: {type(struct)}") return struct_capnp + + +def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> structs.CarControlSP: + # TODO: recursively handle any car struct as needed + def remove_deprecated(s: dict) -> dict: + return {k: v for k, v in s.items() if not k.endswith('DEPRECATED')} + + struct_dict = struct.to_dict() + struct_dataclass = structs.CarControlSP(**remove_deprecated({k: v for k, v in struct_dict.items() if not isinstance(k, dict)})) + + struct_dataclass.mads = structs.ModularAssistiveDrivingSystem(**remove_deprecated(struct_dict.get('mads', {}))) + + return struct_dataclass diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index b64fa09d8..10b245d1b 100644 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -4,7 +4,7 @@ import hypothesis.strategies as st from hypothesis import Phase, given, settings from parameterized import parameterized -from cereal import car +from cereal import car, custom from opendbc.car import DT_CTRL from opendbc.car.car_helpers import interfaces from opendbc.car.structs import CarParams @@ -12,6 +12,7 @@ from opendbc.car.tests.test_car_interfaces import get_fuzzy_car_interface_args from opendbc.car.fingerprints import all_known_cars from opendbc.car.fw_versions import FW_VERSIONS, FW_QUERY_CONFIGS from opendbc.car.mock.values import CAR as MOCK +from openpilot.selfdrive.car.helpers import convert_carControlSP from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque @@ -72,13 +73,16 @@ class TestCarInterfaces: assert not math.isnan(tune.torque.friction) and tune.torque.friction > 0 cc_msg = FuzzyGenerator.get_random_msg(data.draw, car.CarControl, real_floats=True) + cc_sp_msg = FuzzyGenerator.get_random_msg(data.draw, custom.CarControlSP, real_floats=True) # Run car interface now_nanos = 0 CC = car.CarControl.new_message(**cc_msg) CC = CC.as_reader() + CC_SP = custom.CarControlSP.new_message(**cc_sp_msg) + CC_SP = convert_carControlSP(CC_SP.as_reader()) for _ in range(10): car_interface.update([]) - car_interface.apply(CC, now_nanos) + car_interface.apply(CC, CC_SP, now_nanos) now_nanos += DT_CTRL * 1e9 # 10 ms CC = car.CarControl.new_message(**cc_msg) @@ -86,7 +90,7 @@ class TestCarInterfaces: CC = CC.as_reader() for _ in range(10): car_interface.update([]) - car_interface.apply(CC, now_nanos) + car_interface.apply(CC, CC_SP, now_nanos) now_nanos += DT_CTRL * 1e9 # 10ms # Test controller initialization diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 88e4e34c1..9214066ec 100644 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -205,10 +205,11 @@ class TestCarModelBase(unittest.TestCase): can_invalid_cnt = 0 can_valid = False CC = structs.CarControl().as_reader() + CC_SP = structs.CarControlSP() for i, msg in enumerate(self.can_msgs): CS = self.CI.update(can_capnp_to_list((msg.as_builder().to_bytes(),))) - self.CI.apply(CC, msg.logMonoTime) + self.CI.apply(CC, CC_SP, msg.logMonoTime) if CS.canValid: can_valid = True @@ -277,13 +278,13 @@ class TestCarModelBase(unittest.TestCase): if self.CP.notCar: self.skipTest("Skipping test for notCar") - def test_car_controller(car_control): + def test_car_controller(car_control, car_control_sp): now_nanos = 0 msgs_sent = 0 CI = self.CarInterface(self.CP, self.CP_SP, self.CarController, self.CarState) for _ in range(round(10.0 / DT_CTRL)): # make sure we hit the slowest messages CI.update([]) - _, sendcan = CI.apply(car_control, now_nanos) + _, sendcan = CI.apply(car_control, car_control_sp, now_nanos) now_nanos += DT_CTRL * 1e9 msgs_sent += len(sendcan) @@ -296,17 +297,18 @@ class TestCarModelBase(unittest.TestCase): # Make sure we can send all messages while inactive CC = structs.CarControl() - test_car_controller(CC.as_reader()) + CC_SP = structs.CarControlSP() + test_car_controller(CC.as_reader(), CC_SP) # Test cancel + general messages (controls_allowed=False & cruise_engaged=True) self.safety.set_cruise_engaged_prev(True) CC = structs.CarControl(cruiseControl=structs.CarControl.CruiseControl(cancel=True)) - test_car_controller(CC.as_reader()) + test_car_controller(CC.as_reader(), CC_SP) # Test resume + general messages (controls_allowed=True & cruise_engaged=True) self.safety.set_controls_allowed(True) CC = structs.CarControl(cruiseControl=structs.CarControl.CruiseControl(resume=True)) - test_car_controller(CC.as_reader()) + test_car_controller(CC.as_reader(), CC_SP) # Skip stdout/stderr capture with pytest, causes elevated memory usage @pytest.mark.nocapture diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 94a4a3625..fb5bb8748 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -19,8 +19,6 @@ from openpilot.selfdrive.controls.lib.longcontrol import LongControl from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose -from opendbc.sunnypilot import SunnypilotParamFlags - State = log.SelfdriveState.OpenpilotState LaneChangeState = log.LaneChangeState LaneChangeDirection = log.LaneChangeDirection @@ -44,7 +42,7 @@ class Controls: 'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', 'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + ['selfdriveStateSP'], poll='selfdriveState') - self.pm = messaging.PubMaster(['carControl', 'controlsState']) + self.pm = messaging.PubMaster(['carControl', 'controlsState'] + ['carControlSP']) self.steer_limited = False self.desired_curvature = 0.0 @@ -96,9 +94,7 @@ class Controls: standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill ss_sp = self.sm['selfdriveStateSP'] - CC.madsEnabled = ss_sp.mads.enabled if ss_sp.mads.available: - CC.sunnypilotParams |= SunnypilotParamFlags.ENABLE_MADS.value _lat_active = ss_sp.mads.active else: _lat_active = self.sm['selfdriveState'].active @@ -141,9 +137,12 @@ class Controls: cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}") setattr(actuators, p, 0.0) - return CC, lac_log + CC_SP = custom.CarControlSP.new_message() + CC_SP.mads = ss_sp.mads - def publish(self, CC, lac_log): + return CC, CC_SP, lac_log + + def publish(self, CC, CC_SP, lac_log): CS = self.sm['carState'] # Orientation and angle rates can be useful for carcontroller @@ -219,12 +218,18 @@ class Controls: cc_send.carControl = CC self.pm.send('carControl', cc_send) + # carControlSP + cc_sp_send = messaging.new_message('carControlSP') + cc_sp_send.valid = CS.canValid + cc_sp_send.carControlSP = CC_SP + self.pm.send('carControlSP', cc_sp_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) + CC, CC_SP, lac_log = self.state_control() + self.publish(CC, CC_SP, lac_log) rk.monitor_time() def main(): diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 2bdac3936..3b565e0e1 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -75,8 +75,8 @@ void UIState::updateStatus() { auto state = ss.getState(); auto state_mads = mads.getState(); if (state == cereal::SelfdriveState::OpenpilotState::PRE_ENABLED || state == cereal::SelfdriveState::OpenpilotState::OVERRIDING || - state_mads == cereal::SelfdriveStateSP::ModularAssistiveDrivingSystem::ModularAssistiveDrivingSystemState::PAUSED || - state_mads == cereal::SelfdriveStateSP::ModularAssistiveDrivingSystem::ModularAssistiveDrivingSystemState::OVERRIDING) { + state_mads == cereal::ModularAssistiveDrivingSystem::ModularAssistiveDrivingSystemState::PAUSED || + state_mads == cereal::ModularAssistiveDrivingSystem::ModularAssistiveDrivingSystemState::OVERRIDING) { status = STATUS_OVERRIDE; } else { if (mads.getAvailable()) { diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index 486696a68..0e938b897 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -32,7 +32,7 @@ from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP from openpilot.sunnypilot.mads.helpers import MadsParams from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT -State = custom.SelfdriveStateSP.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState +State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState ButtonType = car.CarState.ButtonEvent.Type EventName = log.OnroadEvent.EventName EventNameSP = custom.OnroadEventSP.EventName diff --git a/sunnypilot/mads/state.py b/sunnypilot/mads/state.py index 092a31134..342b481dd 100644 --- a/sunnypilot/mads/state.py +++ b/sunnypilot/mads/state.py @@ -31,7 +31,7 @@ from openpilot.common.realtime import DT_CTRL from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP -State = custom.SelfdriveStateSP.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState +State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState EventName = log.OnroadEvent.EventName EventNameSP = custom.OnroadEventSP.EventName diff --git a/sunnypilot/mads/tests/test_mads_state_machine.py b/sunnypilot/mads/tests/test_mads_state_machine.py index 97412f7b7..694d62362 100644 --- a/sunnypilot/mads/tests/test_mads_state_machine.py +++ b/sunnypilot/mads/tests/test_mads_state_machine.py @@ -33,7 +33,7 @@ from openpilot.sunnypilot.mads.state import StateMachine, SOFT_DISABLE_TIME, GEA from openpilot.selfdrive.selfdrived.events import ET, NormalPermanentAlert from openpilot.sunnypilot.selfdrive.selfdrived.events import EVENTS_SP -State = custom.SelfdriveStateSP.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState +State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState EventNameSP = custom.OnroadEventSP.EventName # The event types that maintain the current state