Car: Migrate sunnypilot CarControl to its own cereal (#606)

* carControlSP and move MADS to outside structs

* publish it

* apply to all car controller

* migrate sunnypilotParams

* migrate madsEnabled

* tldr

* convert to capnp

* unused

* wrong module

* fix name

* cancer is right (all tests should be passing now)

* bump opendbc

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 {

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;

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),

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

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

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

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

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():

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()) {

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

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
 

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