Merge branch 'model-switcher-v2' into cp-sp-model

Moved the initialization of 20Hz-specific variables to be conditional based on the `is_20hz` flag. This ensures that unnecessary memory allocations are avoided when the model is not running at 20Hz, improving efficiency and clarity.

cereal/custom.capnp

@@ -81,16 +81,61 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
     generation @5 :UInt32;
     environment @6 :Text;
     runner @7 :Runner;
+    is20hz @8 :Bool;
   }
 }
 
-struct CustomReserved2 @0xf35cc4560bbf6ec2 {
+struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
+  dec @0 :DynamicExperimentalControl;
+
+  struct DynamicExperimentalControl {
+    state @0 :DynamicExperimentalControlState;
+    enabled @1 :Bool;
+    active @2 :Bool;
+
+    enum DynamicExperimentalControlState {
+      acc @0;
+      blended @1;
+    }
+  }
 }
 
-struct CustomReserved3 @0xda96579883444c35 {
+struct OnroadEventSP @0xda96579883444c35 {
+  name @0 :EventName;
+
+  # event types
+  enable @1 :Bool;
+  noEntry @2 :Bool;
+  warning @3 :Bool;   # alerts presented only when  enabled or soft disabling
+  userDisable @4 :Bool;
+  softDisable @5 :Bool;
+  immediateDisable @6 :Bool;
+  preEnable @7 :Bool;
+  permanent @8 :Bool; # alerts presented regardless of openpilot state
+  overrideLateral @10 :Bool;
+  overrideLongitudinal @9 :Bool;
+
+  enum EventName {
+    lkasEnable @0;
+    lkasDisable @1;
+    manualSteeringRequired @2;
+    manualLongitudinalRequired @3;
+    silentLkasEnable @4;
+    silentLkasDisable @5;
+    silentBrakeHold @6;
+    silentWrongGear @7;
+    silentReverseGear @8;
+    silentDoorOpen @9;
+    silentSeatbeltNotLatched @10;
+    silentParkBrake @11;
+    controlsMismatchLateral @12;
+    hyundaiRadarTracksConfirmed @13;
+    experimentalModeSwitched @14;
+  }
 }
 
-struct CustomReserved4 @0x80ae746ee2596b11 {
+struct CarParamsSP @0x80ae746ee2596b11 {
+  flags @0 :UInt32;  # flags for car specific quirks in sunnypilot
 }
 
 struct CustomReserved5 @0xa5cd762cd951a455 {

cereal/log.capnp

@@ -125,80 +125,6 @@ struct OnroadEvent @0xc4fa6047f024e718 {
     espActive @90;
     personalityChanged @91;
     aeb @92;
-    eventReserved93 @93;
-    eventReserved94 @94;
-    eventReserved95 @95;
-    eventReserved96 @96;
-    eventReserved97 @97;
-    eventReserved98 @98;
-    eventReserved99 @99;
-    eventReserved100 @100;
-    eventReserved101 @101;
-    eventReserved102 @102;
-    eventReserved103 @103;
-    eventReserved104 @104;
-    eventReserved105 @105;
-    eventReserved106 @106;
-    eventReserved107 @107;
-    eventReserved108 @108;
-    eventReserved109 @109;
-    eventReserved110 @110;
-    eventReserved111 @111;
-    eventReserved112 @112;
-    eventReserved113 @113;
-    eventReserved114 @114;
-    eventReserved115 @115;
-    eventReserved116 @116;
-    eventReserved117 @117;
-    eventReserved118 @118;
-    eventReserved119 @119;
-    eventReserved120 @120;
-    eventReserved121 @121;
-    eventReserved122 @122;
-    eventReserved123 @123;
-    eventReserved124 @124;
-    eventReserved125 @125;
-    eventReserved126 @126;
-    eventReserved127 @127;
-    eventReserved128 @128;
-    eventReserved129 @129;
-    eventReserved130 @130;
-    eventReserved131 @131;
-    eventReserved132 @132;
-    eventReserved133 @133;
-    eventReserved134 @134;
-    eventReserved135 @135;
-    eventReserved136 @136;
-    eventReserved137 @137;
-    eventReserved138 @138;
-    eventReserved139 @139;
-    eventReserved140 @140;
-    eventReserved141 @141;
-    eventReserved142 @142;
-    eventReserved143 @143;
-    eventReserved144 @144;
-    eventReserved145 @145;
-    eventReserved146 @146;
-    eventReserved147 @147;
-    eventReserved148 @148;
-    eventReserved149 @149;
-    eventReserved150 @150;
-
-    # sunnypilot
-    lkasEnable @151;
-    lkasDisable @152;
-    manualSteeringRequired @153;
-    manualLongitudinalRequired @154;
-    silentLkasEnable @155;
-    silentLkasDisable @156;
-    silentBrakeHold @157;
-    silentWrongGear @158;
-    silentReverseGear @159;
-    silentDoorOpen @160;
-    silentSeatbeltNotLatched @161;
-    silentParkBrake @162;
-    controlsMismatchLateral @163;
-    hyundaiRadarTracksConfirmed @164;
 
     soundsUnavailableDEPRECATED @47;
   }
@@ -664,7 +590,6 @@ struct PandaState @0xa7649e2575e4591e {
 
   # safety stuff
   controlsAllowed @3 :Bool;
-  controlsAllowedLat @5 :Bool;
   safetyRxInvalid @19 :UInt32;
   safetyTxBlocked @24 :UInt32;
   safetyModel @14 :Car.CarParams.SafetyModel;
@@ -772,6 +697,7 @@ struct PandaState @0xa7649e2575e4591e {
   }
 
   gasInterceptorDetectedDEPRECATED @4 :Bool;
+  startedSignalDetectedDEPRECATED @5 :Bool;
   hasGpsDEPRECATED @6 :Bool;
   gmlanSendErrsDEPRECATED @9 :UInt32;
   fanSpeedRpmDEPRECATED @11 :UInt16;
@@ -2641,9 +2567,9 @@ struct Event {
     # DON'T change which struct it points to
     selfdriveStateSP @107 :Custom.SelfdriveStateSP;
     modelManagerSP @108 :Custom.ModelManagerSP;
-    customReserved2 @109 :Custom.CustomReserved2;
-    customReserved3 @110 :Custom.CustomReserved3;
-    customReserved4 @111 :Custom.CustomReserved4;
+    longitudinalPlanSP @109 :Custom.LongitudinalPlanSP;
+    onroadEventsSP @110 :List(Custom.OnroadEventSP);
+    carParamsSP @111 :Custom.CarParamsSP;
     customReserved5 @112 :Custom.CustomReserved5;
     customReserved6 @113 :Custom.CustomReserved6;
     customReserved7 @114 :Custom.CustomReserved7;

cereal/services.py

@@ -77,6 +77,9 @@ _services: dict[str, tuple] = {
   # sunnypilot
   "modelManagerSP": (False, 1., 1),
   "selfdriveStateSP": (True, 100., 10),
+  "longitudinalPlanSP": (True, 20., 10),
+  "onroadEventsSP": (True, 1., 1),
+  "carParamsSP": (True, 0.02, 1),
 
   # debug
   "uiDebug": (True, 0., 1),

common/params.cc

@@ -202,8 +202,13 @@ std::unordered_map<std::string, uint32_t> keys = {
 
     // --- sunnypilot params --- //
     {"ApiCache_DriveStats", PERSISTENT},
+    {"CarParamsSP", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
+    {"CarParamsSPCache", CLEAR_ON_MANAGER_START},
+    {"CarParamsSPPersistent", PERSISTENT},
     {"EnableGithubRunner", PERSISTENT | BACKUP},
     {"ModelRunnerTypeCache", CLEAR_ON_ONROAD_TRANSITION},
+    {"OffroadMode", CLEAR_ON_MANAGER_START},
+    {"OffroadMode_Status", CLEAR_ON_MANAGER_START},
 
     // MADS params
     {"Mads", PERSISTENT | BACKUP},
@@ -229,6 +234,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"HyundaiRadarTracksConfirmed", PERSISTENT},
     {"HyundaiRadarTracksPersistent", PERSISTENT},
     {"HyundaiRadarTracksToggle", PERSISTENT},
+
+    {"DynamicExperimentalControl", PERSISTENT},
 };
 
 } // namespace

selfdrive/car/card.py

@@ -5,7 +5,7 @@ import threading
 
 import cereal.messaging as messaging
 
-from cereal import car, log
+from cereal import car, log, custom
 
 from panda import ALTERNATIVE_EXPERIENCE
 
@@ -21,6 +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.sunnypilot.mads.mads import MadsParams
 from openpilot.sunnypilot.selfdrive.car import interfaces
@@ -66,12 +67,17 @@ class Car:
   CI: CarInterfaceBase
   RI: RadarInterfaceBase
   CP: car.CarParams
+  CP_SP: structs.CarParamsSP
+  CP_SP_capnp: custom.CarParamsSP
 
   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.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'])
 
+    sock_services = list(self.pm.sock.keys()) + ['carParamsSP']
+    self.pm = messaging.PubMaster(sock_services)
+
     self.can_rcv_cum_timeout_counter = 0
 
     self.CC_prev = car.CarControl.new_message()
@@ -102,14 +108,15 @@ class Car:
           cached_params = _cached_params
 
       self.CI = get_car(*self.can_callbacks, obd_callback(self.params), experimental_long_allowed, num_pandas, cached_params)
-      interfaces.setup_car_interface_sp(self.CI.CP, self.params)
-      self.RI = get_radar_interface(self.CI.CP)
+      interfaces.setup_car_interface_sp(self.CI.CP, self.CI.CP_SP, self.params)
+      self.RI = get_radar_interface(self.CI.CP, self.CI.CP_SP)
       self.CP = self.CI.CP
+      self.CP_SP = self.CI.CP_SP
 
       # continue onto next fingerprinting step in pandad
       self.params.put_bool("FirmwareQueryDone", True)
     else:
-      self.CI, self.CP = CI, CI.CP
+      self.CI, self.CP, self.CP_SP = CI, CI.CP, CI.CP_SP
       self.RI = RI
 
     # set alternative experiences from parameters
@@ -120,7 +127,10 @@ class Car:
 
     # mads
     MadsParams().set_alternative_experience(self.CP)
-    MadsParams().set_car_specific_params(self.CP)
+    MadsParams().set_car_specific_params(self.CP, self.CP_SP)
+
+    # Dynamic Experimental Control
+    self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl")
 
     openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle")
 
@@ -164,6 +174,14 @@ class Car:
     self.params.put_nonblocking("CarParamsCache", cp_bytes)
     self.params.put_nonblocking("CarParamsPersistent", cp_bytes)
 
+    # Write CarParamsSP for controls
+    # convert to pycapnp representation for caching and logging
+    self.CP_SP_capnp = convert_to_capnp(self.CP_SP)
+    cp_sp_bytes = self.CP_SP_capnp.to_bytes()
+    self.params.put("CarParamsSP", cp_sp_bytes)
+    self.params.put_nonblocking("CarParamsSPCache", cp_sp_bytes)
+    self.params.put_nonblocking("CarParamsSPPersistent", cp_sp_bytes)
+
     self.mock_carstate = MockCarState()
     self.v_cruise_helper = VCruiseHelper(self.CP)
 
@@ -204,7 +222,7 @@ class Car:
     self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric)
     if self.sm['carControl'].enabled and not self.CC_prev.enabled:
       # Use CarState w/ buttons from the step selfdrived enables on
-      self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode)
+      self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode, self.dynamic_experimental_control)
 
     # TODO: mirror the carState.cruiseState struct?
     CS.vCruise = float(self.v_cruise_helper.v_cruise_kph)
@@ -242,14 +260,21 @@ class Car:
       tracks_msg.liveTracks = RD
       self.pm.send('liveTracks', tracks_msg)
 
+    # carParamsSP - logged every 50 seconds (> 1 per segment)
+    if self.sm.frame % int(50. / DT_CTRL) == 0:
+      cp_sp_send = messaging.new_message('carParamsSP')
+      cp_sp_send.valid = True
+      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):
     """control update loop, driven by carControl"""
 
     if not self.initialized_prev:
       # Initialize CarInterface, once controls are ready
       # TODO: this can make us miss at least a few cycles when doing an ECU knockout
-      self.CI.init(self.CP, *self.can_callbacks)
-      interfaces.initialize_car_interface_sp(self.CP, self.params, *self.can_callbacks)
+      self.CI.init(self.CP, self.CP_SP, *self.can_callbacks)
+      interfaces.initialize_car_interface_sp(self.CP, self.CP_SP, self.params, *self.can_callbacks)
       # signal pandad to switch to car safety mode
       self.params.put_bool_nonblocking("ControlsReady", True)
 
@@ -278,6 +303,10 @@ class Car:
     while not evt.is_set():
       self.is_metric = self.params.get_bool("IsMetric")
       self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
+
+      # sunnypilot
+      self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl")
+
       time.sleep(0.1)
 
   def card_thread(self):

selfdrive/car/cruise.py

@@ -120,12 +120,13 @@ class VCruiseHelper:
         self.button_timers[b.type.raw] = 1 if b.pressed else 0
         self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled}
 
-  def initialize_v_cruise(self, CS, experimental_mode: bool) -> None:
+  def initialize_v_cruise(self, CS, experimental_mode: bool, dynamic_experimental_control: bool) -> None:
     # initializing is handled by the PCM
     if self.CP.pcmCruise:
       return
 
-    initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL
+    initial_experimental_mode = experimental_mode and not dynamic_experimental_control
+    initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if initial_experimental_mode else V_CRUISE_INITIAL
 
     if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_initialized:
       self.v_cruise_kph = self.v_cruise_kph_last

selfdrive/car/helpers.py

@@ -0,0 +1,46 @@
+import capnp
+from typing import Any
+
+from cereal import custom
+from opendbc.car import structs
+
+_FIELDS = '__dataclass_fields__'  # copy of dataclasses._FIELDS
+
+
+def is_dataclass(obj):
+  """Similar to dataclasses.is_dataclass without instance type check checking"""
+  return hasattr(obj, _FIELDS)
+
+
+def _asdictref_inner(obj) -> dict[str, Any] | Any:
+  if is_dataclass(obj):
+    ret = {}
+    for field in getattr(obj, _FIELDS):  # similar to dataclasses.fields()
+      ret[field] = _asdictref_inner(getattr(obj, field))
+    return ret
+  elif isinstance(obj, (tuple, list)):
+    return type(obj)(_asdictref_inner(v) for v in obj)
+  else:
+    return obj
+
+
+def asdictref(obj) -> dict[str, Any]:
+  """
+  Similar to dataclasses.asdict without recursive type checking and copy.deepcopy
+  Note that the resulting dict will contain references to the original struct as a result
+  """
+  if not is_dataclass(obj):
+    raise TypeError("asdictref() should be called on dataclass instances")
+
+  return _asdictref_inner(obj)
+
+
+def convert_to_capnp(struct: structs.CarParamsSP) -> capnp.lib.capnp._DynamicStructBuilder:
+  struct_dict = asdictref(struct)
+
+  if isinstance(struct, structs.CarParamsSP):
+    struct_capnp = custom.CarParamsSP.new_message(**struct_dict)
+  else:
+    raise ValueError(f"Unsupported struct type: {type(struct)}")
+
+  return struct_capnp

selfdrive/car/tests/test_car_interfaces.py

@@ -40,9 +40,12 @@ class TestCarInterfaces:
 
     car_params = CarInterface.get_params(car_name, args['fingerprints'], args['car_fw'],
                                          experimental_long=args['experimental_long'], docs=False)
+    car_params, car_params_sp = CarInterface.get_params_sp(car_params, car_name, args['fingerprints'], args['car_fw'],
+                                                           experimental_long=args['experimental_long'], docs=False)
     car_params = car_params.as_reader()
-    car_interface = CarInterface(car_params, CarController, CarState)
+    car_interface = CarInterface(car_params, car_params_sp, CarController, CarState)
     assert car_params
+    assert car_params_sp
     assert car_interface
 
     assert car_params.mass > 1

selfdrive/car/tests/test_cruise_speed.py

@@ -57,16 +57,16 @@ class TestVCruiseHelper:
     for _ in range(2):
       self.v_cruise_helper.update_v_cruise(car.CarState(cruiseState={"available": False}), enabled=False, is_metric=False)
 
-  def enable(self, v_ego, experimental_mode):
+  def enable(self, v_ego, experimental_mode, dynamic_experimental_control):
     # Simulates user pressing set with a current speed
-    self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode)
+    self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode, dynamic_experimental_control)
 
   def test_adjust_speed(self):
     """
     Asserts speed changes on falling edges of buttons.
     """
 
-    self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
+    self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False)
 
     for btn in (ButtonType.accelCruise, ButtonType.decelCruise):
       for pressed in (True, False):
@@ -90,7 +90,7 @@ class TestVCruiseHelper:
       CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=pressed)]
       self.v_cruise_helper.update_v_cruise(CS, enabled=enabled, is_metric=False)
       if pressed:
-        self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
+        self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False)
 
       # Expected diff on enabling. Speed should not change on falling edge of pressed
       assert not pressed == self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last
@@ -100,7 +100,7 @@ class TestVCruiseHelper:
     Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill.
     """
 
-    self.enable(0, False)
+    self.enable(0, False, False)
 
     for standstill in (True, False):
       for pressed in (True, False):
@@ -120,7 +120,7 @@ class TestVCruiseHelper:
 
     for v_ego in np.linspace(0, 100, 101):
       self.reset_cruise_speed_state()
-      self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
+      self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False)
 
       # first decrement speed, then perform gas pressed logic
       expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT
@@ -142,10 +142,11 @@ class TestVCruiseHelper:
     """
 
     for experimental_mode in (True, False):
-      for v_ego in np.linspace(0, 100, 101):
-        self.reset_cruise_speed_state()
-        assert not self.v_cruise_helper.v_cruise_initialized
+      for dynamic_experimental_control in (True, False):
+        for v_ego in np.linspace(0, 100, 101):
+          self.reset_cruise_speed_state()
+          assert not self.v_cruise_helper.v_cruise_initialized
 
-        self.enable(float(v_ego), experimental_mode)
-        assert V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX
-        assert self.v_cruise_helper.v_cruise_initialized
+          self.enable(float(v_ego), experimental_mode, dynamic_experimental_control)
+          assert V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX
+          assert self.v_cruise_helper.v_cruise_initialized

selfdrive/car/tests/test_models.py

@@ -1,4 +1,5 @@
 import capnp
+import copy
 import os
 import pytest
 import random
@@ -158,7 +159,9 @@ class TestCarModelBase(unittest.TestCase):
 
     cls.CarInterface, cls.CarController, cls.CarState, cls.RadarInterface = interfaces[cls.platform]
     cls.CP = cls.CarInterface.get_params(cls.platform,  cls.fingerprint, car_fw, experimental_long, docs=False)
+    cls.CP, cls.CP_SP = cls.CarInterface.get_params_sp(cls.CP, cls.platform,  cls.fingerprint, car_fw, experimental_long, docs=False)
     assert cls.CP
+    assert cls.CP_SP
     assert cls.CP.carFingerprint == cls.platform
 
     os.environ["COMMA_CACHE"] = DEFAULT_DOWNLOAD_CACHE_ROOT
@@ -168,7 +171,7 @@ class TestCarModelBase(unittest.TestCase):
     del cls.can_msgs
 
   def setUp(self):
-    self.CI = self.CarInterface(self.CP.copy(), self.CarController, self.CarState)
+    self.CI = self.CarInterface(self.CP.copy(), copy.deepcopy(self.CP_SP), self.CarController, self.CarState)
     assert self.CI
 
     Params().put_bool("OpenpilotEnabledToggle", self.openpilot_enabled)
@@ -217,7 +220,7 @@ class TestCarModelBase(unittest.TestCase):
     self.assertEqual(can_invalid_cnt, 0)
 
   def test_radar_interface(self):
-    RI = self.RadarInterface(self.CP)
+    RI = self.RadarInterface(self.CP, self.CP_SP)
     assert RI
 
     # Since OBD port is multiplexed to bus 1 (commonly radar bus) while fingerprinting,
@@ -277,7 +280,7 @@ class TestCarModelBase(unittest.TestCase):
     def test_car_controller(car_control):
       now_nanos = 0
       msgs_sent = 0
-      CI = self.CarInterface(self.CP, self.CarController, self.CarState)
+      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)
@@ -387,7 +390,7 @@ class TestCarModelBase(unittest.TestCase):
     controls_allowed_prev = False
     CS_prev = car.CarState.new_message()
     checks = defaultdict(int)
-    selfdrived = SelfdriveD(CP=self.CP)
+    selfdrived = SelfdriveD(CP=self.CP, CP_SP=self.CP_SP)
     selfdrived.initialized = True
     for idx, can in enumerate(self.can_msgs):
       CS = self.CI.update(can_capnp_to_list((can.as_builder().to_bytes(), ))).as_reader()

selfdrive/controls/controlsd.py

@@ -2,7 +2,7 @@
 import math
 from typing import SupportsFloat
 
-from cereal import car, log
+from cereal import car, log, custom
 import cereal.messaging as messaging
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.params import Params
@@ -34,7 +34,11 @@ class Controls:
     self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
     cloudlog.info("controlsd got CarParams")
 
-    self.CI = get_car_interface(self.CP)
+    cloudlog.info("controlsd is waiting for CarParamsSP")
+    self.CP_SP = messaging.log_from_bytes(self.params.get("CarParamsSP", block=True), custom.CarParamsSP)
+    cloudlog.info("controlsd got CarParamsSP")
+
+    self.CI = get_car_interface(self.CP, self.CP_SP)
 
     self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
                                    'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',

selfdrive/controls/lib/longitudinal_planner.py

@@ -15,6 +15,8 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
 from openpilot.common.swaglog import cloudlog
 
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP
+
 LON_MPC_STEP = 0.2  # first step is 0.2s
 A_CRUISE_MIN = -1.2
 A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
@@ -66,10 +68,11 @@ def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05):
   return a_target, should_stop
 
 
-class LongitudinalPlanner:
+class LongitudinalPlanner(LongitudinalPlannerSP):
   def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL):
     self.CP = CP
     self.mpc = LongitudinalMpc(dt=dt)
+    LongitudinalPlannerSP.__init__(self, self.CP, self.mpc)
     self.fcw = False
     self.dt = dt
     self.allow_throttle = True
@@ -104,7 +107,10 @@ class LongitudinalPlanner:
     return x, v, a, j, throttle_prob
 
   def update(self, sm):
+    LongitudinalPlannerSP.update(self, sm)
     self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
+    if dec_mpc_mode := self.get_mpc_mode():
+      self.mpc.mode = dec_mpc_mode
 
     if len(sm['carControl'].orientationNED) == 3:
       accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
@@ -205,3 +211,5 @@ class LongitudinalPlanner:
     longitudinalPlan.allowThrottle = bool(self.allow_throttle)
 
     pm.send('longitudinalPlan', plan_send)
+
+    self.publish_longitudinal_plan_sp(sm, pm)

selfdrive/controls/lib/tests/test_latcontrol.py

@@ -19,7 +19,8 @@ class TestLatControl:
   def test_saturation(self, car_name, controller):
     CarInterface, CarController, CarState, RadarInterface = interfaces[car_name]
     CP = CarInterface.get_non_essential_params(car_name)
-    CI = CarInterface(CP, CarController, CarState)
+    CP, CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
+    CI = CarInterface(CP, CP_SP, CarController, CarState)
     VM = VehicleModel(CP)
 
     controller = controller(CP.as_reader(), CI)

selfdrive/controls/plannerd.py

@@ -18,7 +18,7 @@ def main():
 
   ldw = LaneDepartureWarning()
   longitudinal_planner = LongitudinalPlanner(CP)
-  pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance'])
+  pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
   sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'],
                            poll='modelV2', ignore_avg_freq=['radarState'])
 

selfdrive/modeld/__init__.py

@@ -0,0 +1,5 @@
+from pathlib import Path
+
+MODEL_PATH = Path(__file__).parent / 'models/supercombo.onnx'
+MODEL_PKL_PATH = Path(__file__).parent / 'models/supercombo_tinygrad.pkl'
+METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl'

selfdrive/modeld/fill_model_msg.py

@@ -1,14 +1,35 @@
 import os
+
 import capnp
 import numpy as np
+from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED
+from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
+
 from cereal import log
-from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
 
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
 ConfidenceClass = log.ModelDataV2.ConfidenceClass
 
 
+def curv_from_psis(psi_target, psi_rate, vego, delay):
+  vego = np.clip(vego, MIN_SPEED, np.inf)
+  curv_from_psi = psi_target / (vego * delay)  # epsilon to prevent divide-by-zero
+  return 2 * curv_from_psi - psi_rate / vego
+
+
+def get_curvature_from_plan(plan, vego, delay):
+  psi_target = np.interp(delay, ModelConstants.T_IDXS, plan[:, Plan.T_FROM_CURRENT_EULER][:, 2])
+  psi_rate = plan[:, Plan.ORIENTATION_RATE][0, 2]
+  return curv_from_psis(psi_target, psi_rate, vego, delay)
+
+
+def get_curvature_from_output(output, vego, delay):
+  if desired_curv := output.get('desired_curvature'):  # If the model outputs the desired curvature, use that directly
+    return float(desired_curv[0, 0])
+
+  return float(get_curvature_from_plan(output['plan'][0], vego, delay))
+
 class PublishState:
   def __init__(self):
     self.disengage_buffer = np.zeros(ModelConstants.CONFIDENCE_BUFFER_LEN*ModelConstants.DISENGAGE_WIDTH, dtype=np.float32)
@@ -59,12 +80,14 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
                    net_output_data: dict[str, np.ndarray], v_ego: float, delay: float,
                    publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int,
                    frame_id: int, frame_drop: float, timestamp_eof: int, model_execution_time: float,
-                   valid: bool) -> None:
+                   valid: bool, model_meta) -> None:
   frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
   frame_drop_perc = frame_drop * 100
   extended_msg.valid = valid
   base_msg.valid = valid
 
+  desired_curv = float(get_curvature_from_output(net_output_data, v_ego, delay))
+
   driving_model_data = base_msg.drivingModelData
 
   driving_model_data.frameId = vipc_frame_id
@@ -73,7 +96,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   driving_model_data.modelExecutionTime = model_execution_time
 
   action = driving_model_data.action
-  action.desiredCurvature = float(net_output_data['desired_curvature'][0,0])
+  action.desiredCurvature = desired_curv
 
   modelV2 = extended_msg.modelV2
   modelV2.frameId = vipc_frame_id
@@ -108,7 +131,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
 
   # lateral planning
   action = modelV2.action
-  action.desiredCurvature = float(net_output_data['desired_curvature'][0,0])
+  action.desiredCurvature = desired_curv
 
   # times at X_IDXS according to model plan
   PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N
@@ -159,23 +182,25 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   meta = modelV2.meta
   meta.desireState = net_output_data['desire_state'][0].reshape(-1).tolist()
   meta.desirePrediction = net_output_data['desire_pred'][0].reshape(-1).tolist()
-  meta.engagedProb = net_output_data['meta'][0,Meta.ENGAGED].item()
+  meta.engagedProb = net_output_data['meta'][0,model_meta.ENGAGED].item()
   meta.init('disengagePredictions')
   disengage_predictions = meta.disengagePredictions
   disengage_predictions.t = ModelConstants.META_T_IDXS
-  disengage_predictions.brakeDisengageProbs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE].tolist()
-  disengage_predictions.gasDisengageProbs = net_output_data['meta'][0,Meta.GAS_DISENGAGE].tolist()
-  disengage_predictions.steerOverrideProbs = net_output_data['meta'][0,Meta.STEER_OVERRIDE].tolist()
-  disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_3].tolist()
-  disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_4].tolist()
-  disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_5].tolist()
-  #disengage_predictions.gasPressProbs = net_output_data['meta'][0,Meta.GAS_PRESS].tolist()
-  #disengage_predictions.brakePressProbs = net_output_data['meta'][0,Meta.BRAKE_PRESS].tolist()
+  disengage_predictions.brakeDisengageProbs = net_output_data['meta'][0,model_meta.BRAKE_DISENGAGE].tolist()
+  disengage_predictions.gasDisengageProbs = net_output_data['meta'][0,model_meta.GAS_DISENGAGE].tolist()
+  disengage_predictions.steerOverrideProbs = net_output_data['meta'][0,model_meta.STEER_OVERRIDE].tolist()
+  disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,model_meta.HARD_BRAKE_3].tolist()
+  disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,model_meta.HARD_BRAKE_4].tolist()
+  disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,model_meta.HARD_BRAKE_5].tolist()
+
+  if hasattr(model_meta, 'GAS_PRESS') and hasattr(model_meta, 'BRAKE_PRESS'):
+    disengage_predictions.gasPressProbs = net_output_data['meta'][0,model_meta.GAS_PRESS].tolist()
+    disengage_predictions.brakePressProbs = net_output_data['meta'][0,model_meta.BRAKE_PRESS].tolist()
 
   publish_state.prev_brake_5ms2_probs[:-1] = publish_state.prev_brake_5ms2_probs[1:]
-  publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_5][0]
+  publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,model_meta.HARD_BRAKE_5][0]
   publish_state.prev_brake_3ms2_probs[:-1] = publish_state.prev_brake_3ms2_probs[1:]
-  publish_state.prev_brake_3ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_3][0]
+  publish_state.prev_brake_3ms2_probs[-1] = net_output_data['meta'][0,model_meta.HARD_BRAKE_3][0]
   hard_brake_predicted = (publish_state.prev_brake_5ms2_probs > ModelConstants.FCW_THRESHOLDS_5MS2).all() and \
     (publish_state.prev_brake_3ms2_probs > ModelConstants.FCW_THRESHOLDS_3MS2).all()
   meta.hardBrakePredicted = hard_brake_predicted.item()
@@ -183,9 +208,9 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   # confidence
   if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:
     # any disengage prob
-    brake_disengage_probs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE]
-    gas_disengage_probs = net_output_data['meta'][0,Meta.GAS_DISENGAGE]
-    steer_override_probs = net_output_data['meta'][0,Meta.STEER_OVERRIDE]
+    brake_disengage_probs = net_output_data['meta'][0,model_meta.BRAKE_DISENGAGE]
+    gas_disengage_probs = net_output_data['meta'][0,model_meta.GAS_DISENGAGE]
+    steer_override_probs = net_output_data['meta'][0,model_meta.STEER_OVERRIDE]
     any_disengage_probs = 1-((1-brake_disengage_probs)*(1-gas_disengage_probs)*(1-steer_override_probs))
     # independent disengage prob for each 2s slice
     ind_disengage_probs = np.r_[any_disengage_probs[0], np.diff(any_disengage_probs) / (1 - any_disengage_probs[:-1])]

selfdrive/modeld/modeld.py

@@ -1,21 +1,13 @@
 #!/usr/bin/env python3
-import os
 from openpilot.system.hardware import TICI
 
+from openpilot.sunnypilot.modeld_v2.model_runner import ONNXRunner, TinygradRunner
+
 #
-if TICI:
-  from tinygrad.tensor import Tensor
-  from tinygrad.dtype import dtypes
-  from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
-  os.environ['QCOM'] = '1'
-else:
-  from openpilot.selfdrive.modeld.runners.ort_helpers import make_onnx_cpu_runner
 import time
-import pickle
 import numpy as np
 import cereal.messaging as messaging
 from cereal import car, log
-from pathlib import Path
 from setproctitle import setproctitle
 from cereal.messaging import PubMaster, SubMaster
 from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
@@ -33,13 +25,10 @@ from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext
 
+from sunnypilot.modeld_v2.meta_helper import load_meta_constants
 
 PROCESS_NAME = "selfdrive.modeld.modeld"
-SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
-MODEL_PATH = Path(__file__).parent / 'models/supercombo.onnx'
-MODEL_PKL_PATH = Path(__file__).parent / 'models/supercombo_tinygrad.pkl'
-METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl'
 
 class FrameMeta:
   frame_id: int = 0
@@ -57,81 +46,91 @@ class ModelState:
   prev_desire: np.ndarray  # for tracking the rising edge of the pulse
 
   def __init__(self, context: CLContext):
-    self.frames = {'input_imgs': DrivingModelFrame(context), 'big_input_imgs': DrivingModelFrame(context)}
+    try:
+      self.model_runner = TinygradRunner() if TICI else ONNXRunner()
+    except Exception as e:
+      cloudlog.exception(f"Failed to initialize model runner: {str(e)}")
+
+    buffer_length = 5 if self.model_runner.is_20hz else 2
+    self.frames = {'input_imgs': DrivingModelFrame(context, buffer_length), 'big_input_imgs': DrivingModelFrame(context, buffer_length)}
     self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
+    if self.model_runner.is_20hz:
+      self.full_features_20Hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32)
+      self.desire_20Hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN + 1, ModelConstants.DESIRE_LEN), dtype=np.float32)
+    # Initialize model runner
 
     # img buffers are managed in openCL transform code
-    self.numpy_inputs = {
-      'desire': np.zeros((1, (ModelConstants.FULL_HISTORY_BUFFER_LEN+1), ModelConstants.DESIRE_LEN), dtype=np.float32),
-      'traffic_convention': np.zeros((1, ModelConstants.TRAFFIC_CONVENTION_LEN), dtype=np.float32),
-      'lateral_control_params': np.zeros((1, ModelConstants.LATERAL_CONTROL_PARAMS_LEN), dtype=np.float32),
-      'prev_desired_curv': np.zeros((1, (ModelConstants.FULL_HISTORY_BUFFER_LEN+1), ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32),
-      'features_buffer': np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN,  ModelConstants.FEATURE_LEN), dtype=np.float32),
-    }
+    self.numpy_inputs = {}
 
-    with open(METADATA_PATH, 'rb') as f:
-      model_metadata = pickle.load(f)
-    self.input_shapes =  model_metadata['input_shapes']
+    for key, shape in self.model_runner.input_shapes.items():
+      if key not in self.frames: # Managed by opencl
+        self.numpy_inputs[key] = np.zeros(shape, dtype=np.float32)
 
-    self.output_slices = model_metadata['output_slices']
-    net_output_size = model_metadata['output_shapes']['outputs'][1]
-    self.output = np.zeros(net_output_size, dtype=np.float32)
     self.parser = Parser()
 
-    if TICI:
-      self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()}
-      with open(MODEL_PKL_PATH, "rb") as f:
-        self.model_run = pickle.load(f)
-    else:
-      self.onnx_cpu_runner = make_onnx_cpu_runner(MODEL_PATH)
+    if self.model_runner.is_20hz:
+      net_output_size = self.model_runner.model_metadata['output_shapes']['outputs'][1]
+      self.output = np.zeros(net_output_size, dtype=np.float32)
 
-  def slice_outputs(self, model_outputs: np.ndarray) -> dict[str, np.ndarray]:
-    parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in self.output_slices.items()}
-    if SEND_RAW_PRED:
-      parsed_model_outputs['raw_pred'] = model_outputs.copy()
-    return parsed_model_outputs
+      num_elements = self.numpy_inputs['features_buffer'].shape[1]
+      step_size = int(-100 / num_elements)
+      self.full_features_20Hz_idxs = np.arange(step_size, step_size * (num_elements + 1), step_size)[::-1]
+      self.desire_reshape_dims = (self.numpy_inputs['desire'].shape[0], self.numpy_inputs['desire'].shape[1], -1, self.numpy_inputs['desire'].shape[2])
 
   def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_wide: np.ndarray,
-                inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
+          inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
     # Model decides when action is completed, so desire input is just a pulse triggered on rising edge
     inputs['desire'][0] = 0
     new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
     self.prev_desire[:] = inputs['desire']
 
-    self.numpy_inputs['desire'][0,:-1] = self.numpy_inputs['desire'][0,1:]
-    self.numpy_inputs['desire'][0,-1] = new_desire
+    if self.model_runner.is_20hz:
+      self.desire_20Hz[:-1] = self.desire_20Hz[1:]
+      self.desire_20Hz[-1] = new_desire
+      self.numpy_inputs['desire'][:] = self.desire_20Hz.reshape(self.desire_reshape_dims).max(axis=2)
+    else:
+      length = inputs['desire'].shape[0]
+      self.numpy_inputs['desire'][0, :-1] = self.numpy_inputs['desire'][0, 1:]
+      self.numpy_inputs['desire'][0, -1, :length] = new_desire[:length]
+
+    for key in self.numpy_inputs:
+      if key in inputs and key not in ['desire']:
+        self.numpy_inputs[key][:] = inputs[key]
 
-    self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention']
-    self.numpy_inputs['lateral_control_params'][:] = inputs['lateral_control_params']
     imgs_cl = {'input_imgs': self.frames['input_imgs'].prepare(buf, transform.flatten()),
                'big_input_imgs': self.frames['big_input_imgs'].prepare(wbuf, transform_wide.flatten())}
 
-    if TICI:
-      # The imgs tensors are backed by opencl memory, only need init once
-      for key in imgs_cl:
-        if key not in self.tensor_inputs:
-          self.tensor_inputs[key] = qcom_tensor_from_opencl_address(imgs_cl[key].mem_address, self.input_shapes[key], dtype=dtypes.uint8)
-    else:
-      for key in imgs_cl:
-        self.numpy_inputs[key] = self.frames[key].buffer_from_cl(imgs_cl[key]).reshape(self.input_shapes[key]).astype(dtype=np.float32)
+    # Prepare inputs using the model runner
+    self.model_runner.prepare_inputs(imgs_cl, self.numpy_inputs, self.frames)
 
     if prepare_only:
       return None
 
-    if TICI:
-      self.output = self.model_run(**self.tensor_inputs).numpy().flatten()
+    # Run model inference
+    self.output = self.model_runner.run_model()
+    outputs = self.parser.parse_outputs(self.model_runner.slice_outputs(self.output))
+
+    if self.model_runner.is_20hz:
+      self.full_features_20Hz[:-1] = self.full_features_20Hz[1:]
+      self.full_features_20Hz[-1] = outputs['hidden_state'][0, :]
+      self.numpy_inputs['features_buffer'][:] = self.full_features_20Hz[self.full_features_20Hz_idxs]
     else:
-      self.output = self.onnx_cpu_runner.run(None, self.numpy_inputs)[0].flatten()
+      feature_len = outputs['hidden_state'].shape[1]
+      self.numpy_inputs['features_buffer'][0, :-1] = self.numpy_inputs['features_buffer'][0, 1:]
+      self.numpy_inputs['features_buffer'][0, -1, :feature_len] = outputs['hidden_state'][0, :feature_len]
 
-    outputs = self.parser.parse_outputs(self.slice_outputs(self.output))
+    if "desired_curvature" in outputs:
+      input_name_prev = None
 
-    self.numpy_inputs['features_buffer'][0,:-1] = self.numpy_inputs['features_buffer'][0,1:]
-    self.numpy_inputs['features_buffer'][0,-1] = outputs['hidden_state'][0, :]
+      if "prev_desired_curvs" in self.numpy_inputs.keys():
+        input_name_prev = 'prev_desired_curvs'
+      elif "prev_desired_curv" in self.numpy_inputs.keys():
+        input_name_prev = 'prev_desired_curv'
 
-
-    # TODO model only uses last value now
-    self.numpy_inputs['prev_desired_curv'][0,:-1] = self.numpy_inputs['prev_desired_curv'][0,1:]
-    self.numpy_inputs['prev_desired_curv'][0,-1,:] = outputs['desired_curvature'][0, :]
+      if input_name_prev is not None:
+        length = outputs['desired_curvature'][0].size
+        self.numpy_inputs[input_name_prev][0, :-length, 0] = self.numpy_inputs[input_name_prev][0, length:, 0]
+        self.numpy_inputs[input_name_prev][0, -length:, 0] = outputs['desired_curvature'][0]
     return outputs
 
 
@@ -242,7 +241,6 @@ def main(demo=False):
     is_rhd = sm["driverMonitoringState"].isRHD
     frame_id = sm["roadCameraState"].frameId
     v_ego = max(sm["carState"].vEgo, 0.)
-    lateral_control_params = np.array([v_ego, steer_delay], dtype=np.float32)
     if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
       dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
@@ -273,8 +271,10 @@ def main(demo=False):
     inputs:dict[str, np.ndarray] = {
       'desire': vec_desire,
       'traffic_convention': traffic_convention,
-      'lateral_control_params': lateral_control_params,
-      }
+    }
+
+    if "lateral_control_params" in model.numpy_inputs.keys():
+      inputs['lateral_control_params'] = np.array([v_ego, steer_delay], dtype=np.float32)
 
     mt1 = time.perf_counter()
     model_output = model.run(buf_main, buf_extra, model_transform_main, model_transform_extra, inputs, prepare_only)
@@ -287,7 +287,7 @@ def main(demo=False):
       posenet_send = messaging.new_message('cameraOdometry')
       fill_model_msg(drivingdata_send, modelv2_send, model_output, v_ego, steer_delay,
                      publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
-                     frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen)
+                     frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen, load_meta_constants())
 
       desire_state = modelv2_send.modelV2.meta.desireState
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]

selfdrive/modeld/models/commonmodel.cc

@@ -5,13 +5,14 @@
 
 #include "common/clutil.h"
 
-DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context) : ModelFrame(device_id, context) {
+DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context, uint8_t buffer_length) : ModelFrame(device_id, context), buffer_length(buffer_length) {
   input_frames = std::make_unique<uint8_t[]>(buf_size);
   input_frames_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buf_size, NULL, &err));
-  img_buffer_20hz_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, 2*frame_size_bytes, NULL, &err));
-  region.origin = 1 * frame_size_bytes;
+  img_buffer_20hz_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buffer_length*frame_size_bytes, NULL, &err));
+  region.origin = (buffer_length - 1)  * frame_size_bytes;
   region.size = frame_size_bytes;
   last_img_cl = CL_CHECK_ERR(clCreateSubBuffer(img_buffer_20hz_cl, CL_MEM_READ_WRITE, CL_BUFFER_CREATE_TYPE_REGION, &region, &err));
+  // printf("Buffer length: %d, region origin: %lu, region size: %lu\n", buffer_length, region.origin, region.size);
 
   loadyuv_init(&loadyuv, context, device_id, MODEL_WIDTH, MODEL_HEIGHT);
   init_transform(device_id, context, MODEL_WIDTH, MODEL_HEIGHT);
@@ -20,7 +21,7 @@ DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context)
 cl_mem* DrivingModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) {
   run_transform(yuv_cl, MODEL_WIDTH, MODEL_HEIGHT, frame_width, frame_height, frame_stride, frame_uv_offset, projection);
 
-  for (int i = 0; i < 1; i++) {
+  for (int i = 0; i < (buffer_length - 1); i++) {
     CL_CHECK(clEnqueueCopyBuffer(q, img_buffer_20hz_cl, img_buffer_20hz_cl, (i+1)*frame_size_bytes, i*frame_size_bytes, frame_size_bytes, 0, nullptr, nullptr));
   }
   loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, last_img_cl);

selfdrive/modeld/models/commonmodel.h

@@ -64,7 +64,7 @@ protected:
 
 class DrivingModelFrame : public ModelFrame {
 public:
-  DrivingModelFrame(cl_device_id device_id, cl_context context);
+  DrivingModelFrame(cl_device_id device_id, cl_context context, uint8_t buffer_length);
   ~DrivingModelFrame();
   cl_mem* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection);
 
@@ -73,6 +73,7 @@ public:
   const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2;
   const int buf_size = MODEL_FRAME_SIZE * 2;
   const size_t frame_size_bytes = MODEL_FRAME_SIZE * sizeof(uint8_t);
+  const uint8_t buffer_length;
 
 private:
   LoadYUVState loadyuv;

selfdrive/modeld/models/commonmodel.pxd

@@ -20,7 +20,7 @@ cdef extern from "selfdrive/modeld/models/commonmodel.h":
 
   cppclass DrivingModelFrame:
     int buf_size
-    DrivingModelFrame(cl_device_id, cl_context)
+    DrivingModelFrame(cl_device_id, cl_context, unsigned char)
 
   cppclass MonitoringModelFrame:
     int buf_size

selfdrive/modeld/models/commonmodel_pyx.pyx

@@ -4,7 +4,7 @@
 import numpy as np
 cimport numpy as cnp
 from libc.string cimport memcpy
-from libc.stdint cimport uintptr_t
+from libc.stdint cimport uintptr_t, uint8_t
 
 from msgq.visionipc.visionipc cimport cl_mem
 from msgq.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext
@@ -59,8 +59,8 @@ cdef class ModelFrame:
 cdef class DrivingModelFrame(ModelFrame):
   cdef cppDrivingModelFrame * _frame
 
-  def __cinit__(self, CLContext context):
-    self._frame = new cppDrivingModelFrame(context.device_id, context.context)
+  def __cinit__(self, CLContext context, int buffer_length=2):
+    self._frame = new cppDrivingModelFrame(context.device_id, context.context, buffer_length)
     self.frame = <cppModelFrame*>(self._frame)
     self.buf_size = self._frame.buf_size
 

selfdrive/pandad/panda_safety.cc

@@ -81,3 +81,8 @@ void PandaSafety::setSafetyMode(const std::string &params_string) {
     pandas_[i]->set_safety_model(safety_model, safety_param);
   }
 }
+
+bool PandaSafety::getOffroadMode() {
+  auto offroad_mode = params_.getBool("OffroadMode");
+  return offroad_mode;
+}

selfdrive/pandad/pandad.cc

@@ -158,7 +158,6 @@ void fill_panda_state(cereal::PandaState::Builder &ps, cereal::PandaState::Panda
   ps.setIgnitionLine(health.ignition_line_pkt);
   ps.setIgnitionCan(health.ignition_can_pkt);
   ps.setControlsAllowed(health.controls_allowed_pkt);
-  ps.setControlsAllowedLat(health.controls_allowed_lat_pkt);
   ps.setTxBufferOverflow(health.tx_buffer_overflow_pkt);
   ps.setRxBufferOverflow(health.rx_buffer_overflow_pkt);
   ps.setPandaType(hw_type);
@@ -206,7 +205,7 @@ void fill_panda_can_state(cereal::PandaState::PandaCanState::Builder &cs, const
   cs.setCanCoreResetCnt(can_health.can_core_reset_cnt);
 }
 
-std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool spoofing_started) {
+std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool spoofing_started, PandaSafety *panda_safety) {
   bool ignition_local = false;
   const uint32_t pandas_cnt = pandas.size();
 
@@ -254,7 +253,7 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
       health.ignition_line_pkt = 0;
     }
 
-    ignition_local |= ((health.ignition_line_pkt != 0) || (health.ignition_can_pkt != 0));
+    ignition_local |= ((health.ignition_line_pkt != 0) || (health.ignition_can_pkt != 0)) && !panda_safety->getOffroadMode();
 
     pandaStates.push_back(health);
   }
@@ -341,7 +340,7 @@ void send_peripheral_state(Panda *panda, PubMaster *pm) {
   pm->send("peripheralState", msg);
 }
 
-void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool spoofing_started) {
+void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool spoofing_started, PandaSafety *panda_safety) {
   static SubMaster sm({"selfdriveState", "selfdriveStateSP", "carParams"});
 
   std::vector<std::string> connected_serials;
@@ -350,7 +349,7 @@ void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool spoof
   }
 
   {
-    auto ignition_opt = send_panda_states(pm, pandas, spoofing_started);
+    auto ignition_opt = send_panda_states(pm, pandas, spoofing_started, panda_safety);
     if (!ignition_opt) {
       LOGE("Failed to get ignition_opt");
       return;
@@ -462,7 +461,7 @@ void pandad_run(std::vector<Panda *> &pandas) {
 
     // Process panda state at 10 Hz
     if (rk.frame() % 10 == 0) {
-      process_panda_state(pandas, &pm, spoofing_started);
+      process_panda_state(pandas, &pm, spoofing_started, &panda_safety);
       panda_safety.configureSafetyMode();
     }
 

selfdrive/pandad/pandad.h

@@ -12,6 +12,7 @@ class PandaSafety {
 public:
   PandaSafety(const std::vector<Panda *> &pandas) : pandas_(pandas) {}
   void configureSafetyMode();
+  bool getOffroadMode();
 
 private:
   void updateMultiplexingMode();

selfdrive/selfdrived/alertmanager.py

@@ -6,7 +6,8 @@ from dataclasses import dataclass
 
 from openpilot.common.basedir import BASEDIR
 from openpilot.common.params import Params
-from openpilot.selfdrive.selfdrived.events import Alert, EmptyAlert
+from openpilot.selfdrive.selfdrived.events import Alert
+from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EmptyAlert
 
 
 with open(os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json")) as f:

selfdrive/selfdrived/alerts_offroad.json

@@ -44,5 +44,9 @@
   "Offroad_Recalibration": {
     "text": "openpilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.",
     "severity": 0
+  },
+  "OffroadMode_Status": {
+    "text": "sunnypilot is now in Always Offroad mode. sunnypilot won't start until Always Offroad mode is disabled. Go to \"Settings\" -> \"Device\" to exit Always Offroad mode.",
+    "severity": 1
   }
 }

selfdrive/selfdrived/events.py

@@ -1,9 +1,6 @@
 #!/usr/bin/env python3
-import bisect
 import math
 import os
-from enum import IntEnum
-from collections.abc import Callable
 
 from cereal import log, car
 import cereal.messaging as messaging
@@ -12,6 +9,11 @@ from openpilot.common.git import get_short_branch
 from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER
 
+from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \
+  NoEntryAlert, SoftDisableAlert, UserSoftDisableAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, \
+  StartupAlert, AlertCallbackType
+
+
 AlertSize = log.SelfdriveState.AlertSize
 AlertStatus = log.SelfdriveState.AlertStatus
 VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -19,201 +21,23 @@ AudibleAlert = car.CarControl.HUDControl.AudibleAlert
 EventName = log.OnroadEvent.EventName
 
 
-# Alert priorities
-class Priority(IntEnum):
-  LOWEST = 0
-  LOWER = 1
-  LOW = 2
-  MID = 3
-  HIGH = 4
-  HIGHEST = 5
-
-
-# Event types
-class ET:
-  ENABLE = 'enable'
-  PRE_ENABLE = 'preEnable'
-  OVERRIDE_LATERAL = 'overrideLateral'
-  OVERRIDE_LONGITUDINAL = 'overrideLongitudinal'
-  NO_ENTRY = 'noEntry'
-  WARNING = 'warning'
-  USER_DISABLE = 'userDisable'
-  SOFT_DISABLE = 'softDisable'
-  IMMEDIATE_DISABLE = 'immediateDisable'
-  PERMANENT = 'permanent'
-
-
 # get event name from enum
 EVENT_NAME = {v: k for k, v in EventName.schema.enumerants.items()}
 
 
-class Events:
+class Events(EventsBase):
   def __init__(self):
-    self.events: list[int] = []
-    self.static_events: list[int] = []
+    super().__init__()
     self.event_counters = dict.fromkeys(EVENTS.keys(), 0)
 
-  @property
-  def names(self) -> list[int]:
-    return self.events
+  def get_events_mapping(self) -> dict[int, dict[str, Alert | AlertCallbackType]]:
+    return EVENTS
 
-  def __len__(self) -> int:
-    return len(self.events)
+  def get_event_name(self, event: int):
+    return EVENT_NAME[event]
 
-  def add(self, event_name: int, static: bool=False) -> None:
-    if static:
-      bisect.insort(self.static_events, event_name)
-    bisect.insort(self.events, event_name)
-
-  def clear(self) -> None:
-    self.event_counters = {k: (v + 1 if k in self.events else 0) for k, v in self.event_counters.items()}
-    self.events = self.static_events.copy()
-
-  def contains(self, event_type: str) -> bool:
-    return any(event_type in EVENTS.get(e, {}) for e in self.events)
-
-  def create_alerts(self, event_types: list[str], callback_args=None):
-    if callback_args is None:
-      callback_args = []
-
-    ret = []
-    for e in self.events:
-      types = EVENTS[e].keys()
-      for et in event_types:
-        if et in types:
-          alert = EVENTS[e][et]
-          if not isinstance(alert, Alert):
-            alert = alert(*callback_args)
-
-          if DT_CTRL * (self.event_counters[e] + 1) >= alert.creation_delay:
-            alert.alert_type = f"{EVENT_NAME[e]}/{et}"
-            alert.event_type = et
-            ret.append(alert)
-    return ret
-
-  def add_from_msg(self, events):
-    for e in events:
-      bisect.insort(self.events, e.name.raw)
-
-  def to_msg(self):
-    ret = []
-    for event_name in self.events:
-      event = log.OnroadEvent.new_message()
-      event.name = event_name
-      for event_type in EVENTS.get(event_name, {}):
-        setattr(event, event_type, True)
-      ret.append(event)
-    return ret
-
-  def has(self, event_name: int) -> bool:
-    return event_name in self.events
-
-  def contains_in_list(self, events_list: list[int]) -> bool:
-    return any(event_name in self.events for event_name in events_list)
-
-  def remove(self, event_name: int, static: bool = False) -> None:
-    if static and event_name in self.static_events:
-      self.static_events.remove(event_name)
-
-    if event_name in self.events:
-      self.event_counters[event_name] = self.event_counters[event_name] + 1
-      self.events.remove(event_name)
-
-  def replace(self, prev_event_name: int, cur_event_name: int, static: bool = False) -> None:
-    self.remove(prev_event_name, static)
-    self.add(cur_event_name, static)
-
-
-class Alert:
-  def __init__(self,
-               alert_text_1: str,
-               alert_text_2: str,
-               alert_status: log.SelfdriveState.AlertStatus,
-               alert_size: log.SelfdriveState.AlertSize,
-               priority: Priority,
-               visual_alert: car.CarControl.HUDControl.VisualAlert,
-               audible_alert: car.CarControl.HUDControl.AudibleAlert,
-               duration: float,
-               creation_delay: float = 0.):
-
-    self.alert_text_1 = alert_text_1
-    self.alert_text_2 = alert_text_2
-    self.alert_status = alert_status
-    self.alert_size = alert_size
-    self.priority = priority
-    self.visual_alert = visual_alert
-    self.audible_alert = audible_alert
-
-    self.duration = int(duration / DT_CTRL)
-
-    self.creation_delay = creation_delay
-
-    self.alert_type = ""
-    self.event_type: str | None = None
-
-  def __str__(self) -> str:
-    return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}"
-
-  def __gt__(self, alert2) -> bool:
-    if not isinstance(alert2, Alert):
-      return False
-    return self.priority > alert2.priority
-
-EmptyAlert = Alert("" , "", AlertStatus.normal, AlertSize.none, Priority.LOWEST,
-                   VisualAlert.none, AudibleAlert.none, 0)
-
-class NoEntryAlert(Alert):
-  def __init__(self, alert_text_2: str,
-               alert_text_1: str = "openpilot Unavailable",
-               visual_alert: car.CarControl.HUDControl.VisualAlert=VisualAlert.none):
-    super().__init__(alert_text_1, alert_text_2, AlertStatus.normal,
-                     AlertSize.mid, Priority.LOW, visual_alert,
-                     AudibleAlert.refuse, 3.)
-
-
-class SoftDisableAlert(Alert):
-  def __init__(self, alert_text_2: str):
-    super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2,
-                     AlertStatus.userPrompt, AlertSize.full,
-                     Priority.MID, VisualAlert.steerRequired,
-                     AudibleAlert.warningSoft, 2.),
-
-
-# less harsh version of SoftDisable, where the condition is user-triggered
-class UserSoftDisableAlert(SoftDisableAlert):
-  def __init__(self, alert_text_2: str):
-    super().__init__(alert_text_2),
-    self.alert_text_1 = "openpilot will disengage"
-
-
-class ImmediateDisableAlert(Alert):
-  def __init__(self, alert_text_2: str):
-    super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2,
-                     AlertStatus.critical, AlertSize.full,
-                     Priority.HIGHEST, VisualAlert.steerRequired,
-                     AudibleAlert.warningImmediate, 4.),
-
-
-class EngagementAlert(Alert):
-  def __init__(self, audible_alert: car.CarControl.HUDControl.AudibleAlert):
-    super().__init__("", "",
-                     AlertStatus.normal, AlertSize.none,
-                     Priority.MID, VisualAlert.none,
-                     audible_alert, .2),
-
-
-class NormalPermanentAlert(Alert):
-  def __init__(self, alert_text_1: str, alert_text_2: str = "", duration: float = 0.2, priority: Priority = Priority.LOWER, creation_delay: float = 0.):
-    super().__init__(alert_text_1, alert_text_2,
-                     AlertStatus.normal, AlertSize.mid if len(alert_text_2) else AlertSize.small,
-                     priority, VisualAlert.none, AudibleAlert.none, duration, creation_delay=creation_delay),
-
-
-class StartupAlert(Alert):
-  def __init__(self, alert_text_1: str, alert_text_2: str = "Always keep hands on wheel and eyes on road", alert_status=AlertStatus.normal):
-    super().__init__(alert_text_1, alert_text_2,
-                     alert_status, AlertSize.mid,
-                     Priority.LOWER, VisualAlert.none, AudibleAlert.none, 5.),
+  def get_event_msg_type(self):
+    return log.OnroadEvent
 
 
 # ********** helper functions **********
@@ -225,8 +49,6 @@ def get_display_speed(speed_ms: float, metric: bool) -> str:
 
 # ********** alert callback functions **********
 
-AlertCallbackType = Callable[[car.CarParams, car.CarState, messaging.SubMaster, bool, int, log.ControlsState], Alert]
-
 
 def soft_disable_alert(alert_text_2: str) -> AlertCallbackType:
   def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
@@ -972,102 +794,6 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.WARNING: personality_changed_alert,
   },
 
-  # sunnypilot
-  EventName.lkasEnable: {
-    ET.ENABLE: EngagementAlert(AudibleAlert.engage),
-  },
-
-  EventName.lkasDisable: {
-    ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
-  },
-
-  EventName.manualSteeringRequired: {
-    ET.USER_DISABLE: Alert(
-      "Automatic Lane Centering is OFF",
-      "Manual Steering Required",
-      AlertStatus.normal, AlertSize.mid,
-      Priority.LOW, VisualAlert.none, AudibleAlert.disengage, 1.),
-  },
-
-  EventName.manualLongitudinalRequired: {
-    ET.WARNING: Alert(
-      "Smart/Adaptive Cruise Control: OFF",
-      "Manual Speed Control Required",
-      AlertStatus.normal, AlertSize.mid,
-      Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
-  },
-
-  EventName.silentLkasEnable: {
-    ET.ENABLE: EngagementAlert(AudibleAlert.none),
-  },
-
-  EventName.silentLkasDisable: {
-    ET.USER_DISABLE: EngagementAlert(AudibleAlert.none),
-  },
-
-  EventName.silentBrakeHold: {
-    ET.USER_DISABLE: EngagementAlert(AudibleAlert.none),
-    ET.NO_ENTRY: NoEntryAlert("Brake Hold Active"),
-  },
-
-  EventName.silentWrongGear: {
-    ET.WARNING: Alert(
-      "",
-      "",
-      AlertStatus.normal, AlertSize.none,
-      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
-    ET.NO_ENTRY: Alert(
-      "Gear not D",
-      "openpilot Unavailable",
-      AlertStatus.normal, AlertSize.mid,
-      Priority.LOW, VisualAlert.none, AudibleAlert.none, 0.),
-  },
-
-  EventName.silentReverseGear: {
-    ET.PERMANENT: Alert(
-      "Reverse\nGear",
-      "",
-      AlertStatus.normal, AlertSize.full,
-      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5),
-    ET.NO_ENTRY: NoEntryAlert("Reverse Gear"),
-  },
-
-  EventName.silentDoorOpen: {
-    ET.WARNING: Alert(
-      "",
-      "",
-      AlertStatus.normal, AlertSize.none,
-      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
-    ET.NO_ENTRY: NoEntryAlert("Door Open"),
-  },
-
-  EventName.silentSeatbeltNotLatched: {
-    ET.WARNING: Alert(
-      "",
-      "",
-      AlertStatus.normal, AlertSize.none,
-      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
-    ET.NO_ENTRY: NoEntryAlert("Seatbelt Unlatched"),
-  },
-
-  EventName.silentParkBrake: {
-    ET.WARNING: Alert(
-      "",
-      "",
-      AlertStatus.normal, AlertSize.none,
-      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
-    ET.NO_ENTRY: NoEntryAlert("Parking Brake Engaged"),
-  },
-
-  EventName.controlsMismatchLateral: {
-    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch: Lateral"),
-    ET.NO_ENTRY: NoEntryAlert("Controls Mismatch: Lateral"),
-  },
-
-  EventName.hyundaiRadarTracksConfirmed: {
-    ET.PERMANENT: NormalPermanentAlert("Radar tracks available. Restart the car to initialize")
-  }
-
 }
 
 

selfdrive/selfdrived/selfdrived.py

@@ -5,7 +5,7 @@ import threading
 
 import cereal.messaging as messaging
 
-from cereal import car, log
+from cereal import car, log, custom
 from msgq.visionipc import VisionIpcClient, VisionStreamType
 from panda import ALTERNATIVE_EXPERIENCE
 
@@ -25,6 +25,8 @@ from openpilot.system.version import get_build_metadata
 
 from openpilot.sunnypilot.mads.mads import ModularAssistiveDrivingSystem
 from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP
+from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 
 REPLAY = "REPLAY" in os.environ
 SIMULATION = "SIMULATION" in os.environ
@@ -44,8 +46,8 @@ SafetyModel = car.CarParams.SafetyModel
 IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
 
 
-class SelfdriveD:
-  def __init__(self, CP=None):
+class SelfdriveD(CruiseHelper):
+  def __init__(self, CP=None, CP_SP=None):
     self.params = Params()
 
     # Ensure the current branch is cached, otherwise the first cycle lags
@@ -58,6 +60,13 @@ class SelfdriveD:
     else:
       self.CP = CP
 
+    if CP_SP is None:
+      cloudlog.info("selfdrived is waiting for CarParamsSP")
+      self.CP_SP = messaging.log_from_bytes(self.params.get("CarParamsSP", block=True), custom.CarParamsSP)
+      cloudlog.info("selfdrived got CarParamsSP")
+    else:
+      self.CP_SP = CP_SP
+
     self.car_events = CarSpecificEvents(self.CP)
     self.disengage_on_accelerator = not (self.CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS)
 
@@ -134,16 +143,22 @@ class SelfdriveD:
     elif self.CP.passive:
       self.events.add(EventName.dashcamMode, static=True)
 
+    self.events_sp = EventsSP()
+    self.events_sp_prev = []
+
     self.mads = ModularAssistiveDrivingSystem(self)
-    sock_services = list(self.pm.sock.keys()) + ['selfdriveStateSP']
+    sock_services = list(self.pm.sock.keys()) + ['selfdriveStateSP', 'onroadEventsSP']
     self.pm = messaging.PubMaster(sock_services)
 
     self.car_events_sp = CarSpecificEventsSP(self.CP, self.params)
 
+    CruiseHelper.__init__(self, self.CP)
+
   def update_events(self, CS):
     """Compute onroadEvents from carState"""
 
     self.events.clear()
+    self.events_sp.clear()
 
     if self.sm['controlsState'].lateralControlState.which() == 'debugState':
       self.events.add(EventName.joystickDebug)
@@ -181,7 +196,7 @@ class SelfdriveD:
       self.events.add_from_msg(car_events)
 
       car_events_sp = self.car_events_sp.update().to_msg()
-      self.events.add_from_msg(car_events_sp)
+      self.events_sp.add_from_msg(car_events_sp)
 
       if self.CP.notCar:
         # wait for everything to init first
@@ -367,12 +382,16 @@ class SelfdriveD:
       if self.sm['modelV2'].frameDropPerc > 20:
         self.events.add(EventName.modeldLagging)
 
+    CruiseHelper.update(self, CS, self.events_sp, self.experimental_mode)
+
     # decrement personality on distance button press
     if self.CP.openpilotLongitudinalControl:
       if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents):
-        self.personality = (self.personality - 1) % 3
-        self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
-        self.events.add(EventName.personalityChanged)
+        if not self.experimental_mode_switched:
+          self.personality = (self.personality - 1) % 3
+          self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
+          self.events.add(EventName.personalityChanged)
+        self.experimental_mode_switched = False
 
   def data_sample(self):
     car_state = messaging.recv_one(self.car_state_sock)
@@ -429,9 +448,13 @@ class SelfdriveD:
       clear_event_types.add(ET.NO_ENTRY)
 
     pers = LONGITUDINAL_PERSONALITY_MAP[self.personality]
-    alerts = self.events.create_alerts(self.state_machine.current_alert_types, [self.CP, CS, self.sm, self.is_metric,
-                                                                                self.state_machine.soft_disable_timer, pers])
-    self.AM.add_many(self.sm.frame, alerts)
+    callback_args = [self.CP, CS, self.sm, self.is_metric,
+                     self.state_machine.soft_disable_timer, pers]
+
+    alerts = self.events.create_alerts(self.state_machine.current_alert_types, callback_args)
+    alerts_sp = self.events_sp.create_alerts(self.state_machine.current_alert_types, callback_args)
+
+    self.AM.add_many(self.sm.frame, alerts + alerts_sp)
     self.AM.process_alerts(self.sm.frame, clear_event_types)
 
   def publish_selfdriveState(self, CS):
@@ -476,13 +499,21 @@ class SelfdriveD:
 
     self.pm.send('selfdriveStateSP', ss_sp_msg)
 
+    # onroadEventsSP - logged every second or on change
+    if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events_sp.names != self.events_sp_prev):
+      ce_send_sp = messaging.new_message('onroadEventsSP', len(self.events_sp))
+      ce_send_sp.valid = True
+      ce_send_sp.onroadEventsSP = self.events_sp.to_msg()
+      self.pm.send('onroadEventsSP', ce_send_sp)
+    self.events_sp_prev = self.events_sp.names.copy()
+
   def step(self):
     CS = self.data_sample()
     self.update_events(CS)
     if not self.CP.passive and self.initialized:
       self.enabled, self.active = self.state_machine.update(self.events)
     if not self.CP.notCar:
-      self.mads.update(CS, self.sm)
+      self.mads.update(CS)
     self.update_alerts(CS)
 
     self.publish_selfdriveState(CS)

selfdrive/selfdrived/tests/test_alertmanager.py

@@ -1,8 +1,10 @@
 import random
 
-from openpilot.selfdrive.selfdrived.events import Alert, EmptyAlert, EVENTS
+from openpilot.selfdrive.selfdrived.events import Alert, EVENTS
 from openpilot.selfdrive.selfdrived.alertmanager import AlertManager
 
+from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EmptyAlert
+
 
 class TestAlertManager:
 

selfdrive/test/process_replay/process_replay.py

@@ -23,7 +23,7 @@ from openpilot.common.prefix import OpenpilotPrefix
 from openpilot.common.timeout import Timeout
 from openpilot.common.realtime import DT_CTRL
 from panda.python import ALTERNATIVE_EXPERIENCE
-from openpilot.selfdrive.car.card import can_comm_callbacks
+from openpilot.selfdrive.car.card import can_comm_callbacks, convert_to_capnp
 from openpilot.system.manager.process_config import managed_processes
 from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state, available_streams
 from openpilot.selfdrive.test.process_replay.migration import migrate_all
@@ -344,6 +344,7 @@ def get_car_params_callback(rc, pm, msgs, fingerprint):
   if fingerprint:
     CarInterface, _, _, _ = interfaces[fingerprint]
     CP = CarInterface.get_non_essential_params(fingerprint)
+    CP, CP_SP = CarInterface.get_non_essential_params_sp(CP, fingerprint)
   else:
     can = DummySocket()
     sendcan = DummySocket()
@@ -364,12 +365,14 @@ def get_car_params_callback(rc, pm, msgs, fingerprint):
       with car.CarParams.from_bytes(cached_params_raw) as _cached_params:
         cached_params = _cached_params
 
-    CP = get_car(*can_callbacks, lambda obd: None, Params().get_bool("ExperimentalLongitudinalEnabled"), cached_params=cached_params).CP
+    _CI = get_car(*can_callbacks, lambda obd: None, Params().get_bool("ExperimentalLongitudinalEnabled"), cached_params=cached_params)
+    CP, CP_SP = _CI.CP, _CI.CP_SP
 
     if not params.get_bool("DisengageOnAccelerator"):
       CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
 
   params.put("CarParams", CP.to_bytes())
+  params.put("CarParamsSP", convert_to_capnp(CP_SP).to_bytes())
 
 
 def selfdrived_rcv_callback(msg, cfg, frame):

selfdrive/ui/qt/offroad/settings.cc

@@ -40,6 +40,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
       "",
       "../assets/img_experimental_white.svg",
     },
+    {
+      "DynamicExperimentalControl",
+      tr("Enable Dynamic Experimental Control"),
+      tr("Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal."),
+      "../assets/offroad/icon_blank.png",
+    },
     {
       "DisengageOnAccelerator",
       tr("Disengage on Accelerator Pedal"),

selfdrive/ui/qt/onroad/annotated_camera.h

@@ -2,14 +2,16 @@
 
 #include <QVBoxLayout>
 #include <memory>
-#include "selfdrive/ui/qt/onroad/buttons.h"
 #include "selfdrive/ui/qt/onroad/driver_monitoring.h"
 #include "selfdrive/ui/qt/onroad/model.h"
 #include "selfdrive/ui/qt/widgets/cameraview.h"
 
 #ifdef SUNNYPILOT
+#include "selfdrive/ui/sunnypilot/qt/onroad/buttons.h"
 #include "selfdrive/ui/sunnypilot/qt/onroad/hud.h"
+#define ExperimentalButton ExperimentalButtonSP
 #else
+#include "selfdrive/ui/qt/onroad/buttons.h"
 #include "selfdrive/ui/qt/onroad/hud.h"
 #endif
 

selfdrive/ui/qt/onroad/buttons.cc

@@ -44,6 +44,10 @@ void ExperimentalButton::updateState(const UIState &s) {
 
 void ExperimentalButton::paintEvent(QPaintEvent *event) {
   QPainter p(this);
+  drawButton(p);
+}
+
+void ExperimentalButton::drawButton(QPainter &p) {
   QPixmap img = experimental_mode ? experimental_img : engage_img;
   drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || !engageable) ? 0.6 : 1.0);
 }

selfdrive/ui/qt/onroad/buttons.h

@@ -16,13 +16,17 @@ class ExperimentalButton : public QPushButton {
 
 public:
   explicit ExperimentalButton(QWidget *parent = 0);
-  void updateState(const UIState &s);
+  virtual void updateState(const UIState &s);
 
 private:
   void paintEvent(QPaintEvent *event) override;
   void changeMode();
 
   Params params;
+
+protected:
+  virtual void drawButton(QPainter &p);
+
   QPixmap engage_img;
   QPixmap experimental_img;
   bool experimental_mode;

selfdrive/ui/sunnypilot/SConscript

@@ -23,6 +23,7 @@ qt_src = [
   "sunnypilot/qt/offroad/settings/sunnypilot_panel.cc",
   "sunnypilot/qt/offroad/settings/trips_panel.cc",
   "sunnypilot/qt/onroad/annotated_camera.cc",
+  "sunnypilot/qt/onroad/buttons.cc",
   "sunnypilot/qt/onroad/hud.cc",
   "sunnypilot/qt/onroad/model.cc",
   "sunnypilot/qt/onroad/onroad_home.cc",

selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.cc

@@ -74,23 +74,17 @@ DevicePanelSP::DevicePanelSP(SettingsWindowSP *parent) : DevicePanel(parent) {
 
   addItem(device_grid_layout);
 
-  QObject::connect(uiState(), &UIState::offroadTransition, [=](bool offroad) {
-    for (auto btn : findChildren<PushButtonSP*>()) {
-      btn->setEnabled(offroad);
-    }
-  });
-
   // offroad mode and power buttons
 
   QHBoxLayout *power_layout = new QHBoxLayout();
   power_layout->setSpacing(5);
 
-  QPushButton *rebootBtn = new PushButtonSP(tr("Reboot"), 720, this);
+  PushButtonSP *rebootBtn = new PushButtonSP(tr("Reboot"), 720, this);
   rebootBtn->setStyleSheet(rebootButtonStyle);
   power_layout->addWidget(rebootBtn);
   QObject::connect(rebootBtn, &PushButtonSP::clicked, this, &DevicePanelSP::reboot);
 
-  QPushButton *poweroffBtn = new PushButtonSP(tr("Power Off"), 720, this);
+  PushButtonSP *poweroffBtn = new PushButtonSP(tr("Power Off"), 720, this);
   poweroffBtn->setStyleSheet(powerOffButtonStyle);
   power_layout->addWidget(poweroffBtn);
   QObject::connect(poweroffBtn, &PushButtonSP::clicked, this, &DevicePanelSP::poweroff);
@@ -99,5 +93,60 @@ DevicePanelSP::DevicePanelSP(SettingsWindowSP *parent) : DevicePanel(parent) {
     connect(uiState(), &UIState::offroadTransition, poweroffBtn, &PushButtonSP::setVisible);
   }
 
-  addItem(power_layout);
+  offroadBtn = new PushButtonSP(tr("Offroad Mode"));
+  offroadBtn->setFixedWidth(power_layout->sizeHint().width());
+  QObject::connect(offroadBtn, &PushButtonSP::clicked, this, &DevicePanelSP::setOffroadMode);
+
+  QVBoxLayout *power_group_layout = new QVBoxLayout();
+  power_group_layout->setSpacing(30);
+  power_group_layout->addWidget(offroadBtn, 0, Qt::AlignHCenter);
+  power_group_layout->addLayout(power_layout);
+
+  addItem(power_group_layout);
+
+  QObject::connect(uiState(), &UIState::offroadTransition, [=](bool offroad) {
+    for (auto btn : findChildren<PushButtonSP*>()) {
+      if (btn != rebootBtn && btn != poweroffBtn && btn != offroadBtn) {
+        btn->setEnabled(offroad);
+      }
+    }
+  });
+}
+
+void DevicePanelSP::setOffroadMode() {
+  if (!uiState()->engaged()) {
+    if (params.getBool("OffroadMode")) {
+      if (ConfirmationDialog::confirm(tr("Are you sure you want to exit Always Offroad mode?"), tr("Confirm"), this)) {
+        // Check engaged again in case it changed while the dialog was open
+        if (!uiState()->engaged()) {
+          params.remove("OffroadMode");
+        }
+      }
+    } else {
+      if (ConfirmationDialog::confirm(tr("Are you sure you want to enter Always Offroad mode?"), tr("Confirm"), this)) {
+        // Check engaged again in case it changed while the dialog was open
+        if (!uiState()->engaged()) {
+          params.putBool("OffroadMode", true);
+        }
+      }
+    }
+  } else {
+    ConfirmationDialog::alert(tr("Disengage to Enter Always Offroad Mode"), this);
+  }
+
+  updateState();
+}
+
+void DevicePanelSP::showEvent(QShowEvent *event) {
+  updateState();
+}
+
+void DevicePanelSP::updateState() {
+  if (!isVisible()) {
+    return;
+  }
+
+  bool offroad_mode_param = params.getBool("OffroadMode");
+  offroadBtn->setText(offroad_mode_param ? tr("Exit Always Offroad") : tr("Always Offroad"));
+  offroadBtn->setStyleSheet(offroad_mode_param ? alwaysOffroadStyle : autoOffroadStyle);
 }

selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h

@@ -15,9 +15,43 @@ class DevicePanelSP : public DevicePanel {
 
 public:
   explicit DevicePanelSP(SettingsWindowSP *parent = 0);
+  void showEvent(QShowEvent *event) override;
+  void setOffroadMode();
+  void updateState();
 
 private:
   std::map<QString, PushButtonSP*> buttons;
+  PushButtonSP *offroadBtn;
+
+  const QString alwaysOffroadStyle = R"(
+    PushButtonSP {
+      border-radius: 20px;
+      font-size: 50px;
+      font-weight: 450;
+      height: 150px;
+      padding: 0 25px 0 25px;
+      color: #FFFFFF;
+      background-color: #393939;
+    }
+    PushButtonSP:pressed {
+      background-color: #4A4A4A;
+    }
+  )";
+
+  const QString autoOffroadStyle = R"(
+    PushButtonSP {
+      border-radius: 20px;
+      font-size: 50px;
+      font-weight: 450;
+      height: 150px;
+      padding: 0 25px 0 25px;
+      color: #FFFFFF;
+      background-color: #E22C2C;
+    }
+    PushButtonSP:pressed {
+      background-color: #FF2424;
+    }
+  )";
 
   const QString rebootButtonStyle = R"(
     PushButtonSP {

selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc

@@ -189,11 +189,16 @@ void SoftwarePanelSP::updateLabels() {
  * @brief Shows dialog prompting user to reset calibration after model download
  */
 void SoftwarePanelSP::showResetParamsDialog() {
-  const auto confirmMsg = tr("Model download has started in the background.") + "\n" +
-                          tr("We STRONGLY suggest you to reset calibration. Would you like to do that now?");
+  const auto confirmMsg = QString("%1<br><br><b>%2</b><br><br><b>%3</b>")
+                          .arg(tr("Model download has started in the background."))
+                          .arg(tr("We STRONGLY suggest you to reset calibration."))
+                          .arg(tr("Would you like to do that now?"));
   const auto button_text = tr("Reset Calibration");
 
-  if (showConfirmationDialog(confirmMsg, button_text, false)) {
+  QString content("<body><h2 style=\"text-align: center;\">" + tr("Driving Model Selector") + "</h2><br>"
+                  "<p style=\"text-align: center; margin: 0 128px; font-size: 50px;\">" + confirmMsg + "</p></body>");
+
+  if (showConfirmationDialog(content, button_text, false)) {
     params.remove("CalibrationParams");
     params.remove("LiveTorqueParameters");
   }

selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h

@@ -53,7 +53,7 @@ private:
     const QString final_message = QString("%1%2").arg(!message.isEmpty() ? message + "\n" : QString(), warning_message);
     const QString final_buttonText = !confirmButtonText.isEmpty() ? confirmButtonText : QString(tr("Continue") + " %1").arg(show_metered_warning ? tr("on Metered") : "");
 
-    return ConfirmationDialog::confirm(final_message, final_buttonText, parent);
+    return ConfirmationDialog(final_message, final_buttonText, tr("Cancel"), true, parent).exec();
   }
 
   bool is_metered{};

selfdrive/ui/sunnypilot/qt/onroad/buttons.cc

@@ -0,0 +1,51 @@
+/**
+ * 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.
+ */
+
+#include "selfdrive/ui/sunnypilot/qt/onroad/buttons.h"
+
+#include <QPainter>
+
+ExperimentalButtonSP::ExperimentalButtonSP(QWidget *parent) : ExperimentalButton(parent) {
+  QObject::disconnect(uiState(), &UIState::uiUpdate, this, &ExperimentalButton::updateState);
+  QObject::connect(uiState(), &UIState::uiUpdate, this, &ExperimentalButtonSP::updateState);
+}
+
+void ExperimentalButtonSP::updateState(const UIState &s) {
+  ExperimentalButton::updateState(s);
+  const auto long_plan_sp = (*s.sm)["longitudinalPlanSP"].getLongitudinalPlanSP();
+
+  int mode = int(long_plan_sp.getDec().getState());
+  if ((long_plan_sp.getDec().getActive() != dynamic_experimental_control) || (mode != dec_mpc_mode)) {
+    dynamic_experimental_control = long_plan_sp.getDec().getActive();
+    dec_mpc_mode = mode;
+    update();
+  }
+}
+
+void ExperimentalButtonSP::drawButton(QPainter &p) {
+  if (dynamic_experimental_control) {
+    QPixmap left_half = engage_img.copy(0, 0, engage_img.width() / 2, engage_img.height());
+    QPixmap right_half = experimental_img.copy(experimental_img.width() / 2, 0, experimental_img.width() / 2, experimental_img.height());
+
+    QPixmap combined_img(engage_img.width(), engage_img.height());
+    combined_img.fill(Qt::transparent);
+
+    QPainter combined_painter(&combined_img);
+
+    combined_painter.setOpacity(dec_mpc_mode == 1 ? 0.1 : 1.0);
+    combined_painter.drawPixmap(0, 0, left_half);
+
+    combined_painter.setOpacity(dec_mpc_mode == 1 ? 1.0 : 0.1);
+    combined_painter.drawPixmap(engage_img.width() / 2, 0, right_half);
+
+    combined_painter.end();
+
+    drawIcon(p, QPoint(btn_size / 2, btn_size / 2), combined_img, QColor(0, 0, 0, 166), (isDown() || !engageable) ? 0.6 : 1.0);
+  } else {
+    ExperimentalButton::drawButton(p);
+  }
+}

selfdrive/ui/sunnypilot/qt/onroad/buttons.h

@@ -0,0 +1,24 @@
+/**
+ * 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.
+ */
+
+#pragma once
+
+#include "selfdrive/ui/qt/onroad/buttons.h"
+
+class ExperimentalButtonSP : public ExperimentalButton {
+  Q_OBJECT
+
+public:
+  explicit ExperimentalButtonSP(QWidget *parent = nullptr);
+  void updateState(const UIState &s) override;
+
+private:
+  void drawButton(QPainter &p) override;
+
+  bool dynamic_experimental_control;
+  int dec_mpc_mode;
+};

selfdrive/ui/sunnypilot/ui.cc

@@ -18,7 +18,7 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) {
     "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
     "pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2",
     "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
-    "modelManagerSP", "selfdriveStateSP",
+    "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP",
   });
 
   // update timer

selfdrive/ui/translations/main_ar.ts

@@ -321,6 +321,34 @@
         <source>Power Off</source>
         <translation type="unfinished">إيقاف التشغيل</translation>
     </message>
+    <message>
+        <source>Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Are you sure you want to exit Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Confirm</source>
+        <translation type="unfinished">تأكيد</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to enter Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Disengage to Enter Always Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Exit Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DriveStats</name>
@@ -527,6 +555,10 @@ Pause Steering: ALC will be paused after the brake pedal is manually pressed.</s
         <source>openpilot detected a change in the device&apos;s mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.</source>
         <translation>لقد اكتشف openpilot تغييراً في موقع تركيب الجهاز. تأكد من تثبيت الجهاز بشكل كامل في موقعه وتثبيته بإحكام على الزجاج الأمامي.</translation>
     </message>
+    <message>
+        <source>sunnypilot is now in Always Offroad mode. sunnypilot won&apos;t start until Always Offroad mode is disabled. Go to &quot;Settings&quot; -&gt; &quot;Device&quot; to exit Always Offroad mode.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>OffroadHome</name>
@@ -1105,10 +1137,6 @@ This may take up to a minute.</source>
         <source>SELECT</source>
         <translation type="unfinished">اختيار</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">إعادة ضبط المعايرة</translation>
@@ -1193,6 +1221,22 @@ This may take up to a minute.</source>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">إلغاء</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1423,6 +1467,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>تمكين مراقبة السائق حتى عندما لا يكون نظام OpenPilot مُفعّلاً.</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_de.ts

@@ -321,6 +321,34 @@
         <source>Power Off</source>
         <translation type="unfinished">Ausschalten</translation>
     </message>
+    <message>
+        <source>Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Are you sure you want to exit Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Confirm</source>
+        <translation type="unfinished">Bestätigen</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to enter Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Disengage to Enter Always Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Exit Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DriveStats</name>
@@ -522,6 +550,10 @@ Pause Steering: ALC will be paused after the brake pedal is manually pressed.</s
         <source>Device temperature too high. System cooling down before starting. Current internal component temperature: %1</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>sunnypilot is now in Always Offroad mode. sunnypilot won&apos;t start until Always Offroad mode is disabled. Go to &quot;Settings&quot; -&gt; &quot;Device&quot; to exit Always Offroad mode.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>OffroadHome</name>
@@ -1089,10 +1121,6 @@ This may take up to a minute.</source>
         <source>SELECT</source>
         <translation type="unfinished">AUSWÄHLEN</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">Neu kalibrieren</translation>
@@ -1177,6 +1205,22 @@ This may take up to a minute.</source>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">Abbrechen</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1407,6 +1451,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_es.ts

@@ -321,6 +321,34 @@
         <source>Power Off</source>
         <translation type="unfinished">Apagar</translation>
     </message>
+    <message>
+        <source>Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Are you sure you want to exit Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Confirm</source>
+        <translation type="unfinished">Confirmar</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to enter Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Disengage to Enter Always Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Exit Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DriveStats</name>
@@ -523,6 +551,10 @@ Pause Steering: ALC will be paused after the brake pedal is manually pressed.</s
         <source>openpilot detected a change in the device&apos;s mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.</source>
         <translation>openpilot detectó un cambio en la posición de montaje del dispositivo. Asegúrese de que el dispositivo esté completamente asentado en el soporte y que el soporte esté firmemente asegurado al parabrisas.</translation>
     </message>
+    <message>
+        <source>sunnypilot is now in Always Offroad mode. sunnypilot won&apos;t start until Always Offroad mode is disabled. Go to &quot;Settings&quot; -&gt; &quot;Device&quot; to exit Always Offroad mode.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>OffroadHome</name>
@@ -1089,10 +1121,6 @@ Esto puede tardar un minuto.</translation>
         <source>SELECT</source>
         <translation type="unfinished">SELECCIONAR</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">Formatear Calibración</translation>
@@ -1177,6 +1205,22 @@ Esto puede tardar un minuto.</translation>
         <source>Default</source>
         <translation>Por Defecto</translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">Cancelar</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1407,6 +1451,14 @@ Esto puede tardar un minuto.</translation>
         <source>Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode.</source>
         <translation>Activar el control longitudinal (fase experimental) para permitir el modo Experimental.</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_fr.ts

@@ -321,6 +321,34 @@
         <source>Power Off</source>
         <translation type="unfinished">Éteindre</translation>
     </message>
+    <message>
+        <source>Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Are you sure you want to exit Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Confirm</source>
+        <translation type="unfinished">Confirmer</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to enter Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Disengage to Enter Always Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Exit Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DriveStats</name>
@@ -523,6 +551,10 @@ Pause Steering: ALC will be paused after the brake pedal is manually pressed.</s
         <source>openpilot detected a change in the device&apos;s mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.</source>
         <translation>openpilot a détecté un changement dans la position de montage de l&apos;appareil. Assurez-vous que l&apos;appareil est totalement inséré dans le support et que le support est fermement fixé au pare-brise.</translation>
     </message>
+    <message>
+        <source>sunnypilot is now in Always Offroad mode. sunnypilot won&apos;t start until Always Offroad mode is disabled. Go to &quot;Settings&quot; -&gt; &quot;Device&quot; to exit Always Offroad mode.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>OffroadHome</name>
@@ -1089,10 +1121,6 @@ Cela peut prendre jusqu&apos;à une minute.</translation>
         <source>SELECT</source>
         <translation type="unfinished">SÉLECTIONNER</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">Réinitialiser la calibration</translation>
@@ -1177,6 +1205,22 @@ Cela peut prendre jusqu&apos;à une minute.</translation>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">Annuler</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1407,6 +1451,14 @@ Cela peut prendre jusqu&apos;à une minute.</translation>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_ja.ts

@@ -321,6 +321,34 @@
         <source>Power Off</source>
         <translation type="unfinished">電源を切る</translation>
     </message>
+    <message>
+        <source>Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Are you sure you want to exit Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Confirm</source>
+        <translation type="unfinished">確認</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to enter Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Disengage to Enter Always Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Exit Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DriveStats</name>
@@ -521,6 +549,10 @@ Pause Steering: ALC will be paused after the brake pedal is manually pressed.</s
         <source>Device temperature too high. System cooling down before starting. Current internal component temperature: %1</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>sunnypilot is now in Always Offroad mode. sunnypilot won&apos;t start until Always Offroad mode is disabled. Go to &quot;Settings&quot; -&gt; &quot;Device&quot; to exit Always Offroad mode.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>OffroadHome</name>
@@ -1083,10 +1115,6 @@ This may take up to a minute.</source>
         <source>SELECT</source>
         <translation type="unfinished">選択</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">ｷｬﾘﾌﾞﾚｰｼｮﾝをﾘｾｯﾄ</translation>
@@ -1171,6 +1199,22 @@ This may take up to a minute.</source>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">キャンセル</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1401,6 +1445,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_ko.ts

@@ -321,6 +321,34 @@
         <source>Power Off</source>
         <translation type="unfinished">전원 끄기</translation>
     </message>
+    <message>
+        <source>Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Are you sure you want to exit Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Confirm</source>
+        <translation type="unfinished">확인</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to enter Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Disengage to Enter Always Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Exit Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DriveStats</name>
@@ -522,6 +550,10 @@ Pause Steering: ALC will be paused after the brake pedal is manually pressed.</s
         <source>Device temperature too high. System cooling down before starting. Current internal component temperature: %1</source>
         <translation>장치 온도가 너무 높습니다. 시작하기 전에 온도를 낮춰주세요. 현재 내부 부품 온도: %1</translation>
     </message>
+    <message>
+        <source>sunnypilot is now in Always Offroad mode. sunnypilot won&apos;t start until Always Offroad mode is disabled. Go to &quot;Settings&quot; -&gt; &quot;Device&quot; to exit Always Offroad mode.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>OffroadHome</name>
@@ -1085,10 +1117,6 @@ This may take up to a minute.</source>
         <source>SELECT</source>
         <translation type="unfinished">선택</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">캘리브레이션 초기화</translation>
@@ -1173,6 +1201,22 @@ This may take up to a minute.</source>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">취소</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1403,6 +1447,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>Openpilot이 활성화되지 않은 경우에도 드라이버 모니터링을 활성화합니다.</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_pt-BR.ts

@@ -321,6 +321,34 @@
         <source>Power Off</source>
         <translation type="unfinished">Desligar</translation>
     </message>
+    <message>
+        <source>Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Are you sure you want to exit Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Confirm</source>
+        <translation type="unfinished">Confirmar</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to enter Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Disengage to Enter Always Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Exit Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DriveStats</name>
@@ -523,6 +551,10 @@ Pause Steering: ALC will be paused after the brake pedal is manually pressed.</s
         <source>Device temperature too high. System cooling down before starting. Current internal component temperature: %1</source>
         <translation>Temperatura do dispositivo muito alta. O sistema está sendo resfriado antes de iniciar. A temperatura atual do componente interno é: %1</translation>
     </message>
+    <message>
+        <source>sunnypilot is now in Always Offroad mode. sunnypilot won&apos;t start until Always Offroad mode is disabled. Go to &quot;Settings&quot; -&gt; &quot;Device&quot; to exit Always Offroad mode.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>OffroadHome</name>
@@ -1089,10 +1121,6 @@ Isso pode levar até um minuto.</translation>
         <source>SELECT</source>
         <translation type="unfinished">SELECIONE</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">Reinicializar Calibragem</translation>
@@ -1177,6 +1205,22 @@ Isso pode levar até um minuto.</translation>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">Cancelar</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1407,6 +1451,14 @@ Isso pode levar até um minuto.</translation>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>Habilite o monitoramento do motorista mesmo quando o openpilot não estiver acionado.</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_th.ts

@@ -321,6 +321,34 @@
         <source>Power Off</source>
         <translation type="unfinished">ปิดเครื่อง</translation>
     </message>
+    <message>
+        <source>Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Are you sure you want to exit Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Confirm</source>
+        <translation type="unfinished">ยืนยัน</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to enter Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Disengage to Enter Always Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Exit Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DriveStats</name>
@@ -522,6 +550,10 @@ Pause Steering: ALC will be paused after the brake pedal is manually pressed.</s
         <source>openpilot detected a change in the device&apos;s mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.</source>
         <translation>openpilot ตรวจพบการเปลี่ยนแปลงของตำแหน่งที่ติดตั้ง กรุณาตรวจสอบว่าได้เลื่อนอุปกรณ์เข้ากับจุดติดตั้งจนสุดแล้ว และจุดติดตั้งได้ยึดติดกับกระจกหน้าอย่างแน่นหนา</translation>
     </message>
+    <message>
+        <source>sunnypilot is now in Always Offroad mode. sunnypilot won&apos;t start until Always Offroad mode is disabled. Go to &quot;Settings&quot; -&gt; &quot;Device&quot; to exit Always Offroad mode.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>OffroadHome</name>
@@ -1085,10 +1117,6 @@ This may take up to a minute.</source>
         <source>SELECT</source>
         <translation type="unfinished">เลือก</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">รีเซ็ตการคาลิเบรท</translation>
@@ -1173,6 +1201,22 @@ This may take up to a minute.</source>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">ยกเลิก</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1403,6 +1447,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_tr.ts

@@ -321,6 +321,34 @@
         <source>Power Off</source>
         <translation type="unfinished">Sistemi kapat</translation>
     </message>
+    <message>
+        <source>Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Are you sure you want to exit Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Confirm</source>
+        <translation type="unfinished">Onayla</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to enter Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Disengage to Enter Always Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Exit Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DriveStats</name>
@@ -521,6 +549,10 @@ Pause Steering: ALC will be paused after the brake pedal is manually pressed.</s
         <source>openpilot detected a change in the device&apos;s mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>sunnypilot is now in Always Offroad mode. sunnypilot won&apos;t start until Always Offroad mode is disabled. Go to &quot;Settings&quot; -&gt; &quot;Device&quot; to exit Always Offroad mode.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>OffroadHome</name>
@@ -1083,10 +1115,6 @@ This may take up to a minute.</source>
         <source>SELECT</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">Kalibrasyonu sıfırla</translation>
@@ -1171,6 +1199,22 @@ This may take up to a minute.</source>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1401,6 +1445,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_zh-CHS.ts

@@ -321,6 +321,34 @@
         <source>Power Off</source>
         <translation type="unfinished">关机</translation>
     </message>
+    <message>
+        <source>Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Are you sure you want to exit Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Confirm</source>
+        <translation type="unfinished">确认</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to enter Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Disengage to Enter Always Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Exit Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DriveStats</name>
@@ -522,6 +550,10 @@ Pause Steering: ALC will be paused after the brake pedal is manually pressed.</s
         <source>Device temperature too high. System cooling down before starting. Current internal component temperature: %1</source>
         <translation>设备温度过高。系统正在冷却中，等冷却完毕后才会启动。目前内部组件温度：%1</translation>
     </message>
+    <message>
+        <source>sunnypilot is now in Always Offroad mode. sunnypilot won&apos;t start until Always Offroad mode is disabled. Go to &quot;Settings&quot; -&gt; &quot;Device&quot; to exit Always Offroad mode.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>OffroadHome</name>
@@ -1085,10 +1117,6 @@ This may take up to a minute.</source>
         <source>SELECT</source>
         <translation type="unfinished">选择</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">重置设备校准</translation>
@@ -1173,6 +1201,22 @@ This may take up to a minute.</source>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">取消</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1403,6 +1447,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>即使在openpilot未激活时也启用驾驶员监控。</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_zh-CHT.ts

@@ -321,6 +321,34 @@
         <source>Power Off</source>
         <translation type="unfinished">關機</translation>
     </message>
+    <message>
+        <source>Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Are you sure you want to exit Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Confirm</source>
+        <translation type="unfinished">確認</translation>
+    </message>
+    <message>
+        <source>Are you sure you want to enter Always Offroad mode?</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Disengage to Enter Always Offroad Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Exit Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Always Offroad</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DriveStats</name>
@@ -522,6 +550,10 @@ Pause Steering: ALC will be paused after the brake pedal is manually pressed.</s
         <source>Device temperature too high. System cooling down before starting. Current internal component temperature: %1</source>
         <translation>裝置溫度過高。系統正在冷卻中，等冷卻完畢後才會啟動。目前內部組件溫度：%1</translation>
     </message>
+    <message>
+        <source>sunnypilot is now in Always Offroad mode. sunnypilot won&apos;t start until Always Offroad mode is disabled. Go to &quot;Settings&quot; -&gt; &quot;Device&quot; to exit Always Offroad mode.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>OffroadHome</name>
@@ -1085,10 +1117,6 @@ This may take up to a minute.</source>
         <source>SELECT</source>
         <translation type="unfinished">選取</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">重設校準</translation>
@@ -1173,6 +1201,22 @@ This may take up to a minute.</source>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">取消</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1403,6 +1447,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>即使在openpilot未激活時也啟用駕駛監控。</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

sunnypilot/mads/helpers.py

@@ -48,13 +48,13 @@ class MadsParams:
       if pause_lateral_on_brake:
         CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISENGAGE_LATERAL_ON_BRAKE
 
-  def set_car_specific_params(self, CP):
+  def set_car_specific_params(self, CP, CP_SP):
     if CP.carName == "hyundai":
       # TODO-SP: This should be separated from MADS module for future implementations
       #          Use "HyundaiLongitudinalMainCruiseToggleable" param
       hyundai_cruise_main_toggleable = True
       if hyundai_cruise_main_toggleable:
-        CP.sunnypilotFlags |= HyundaiFlagsSP.LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE.value
+        CP_SP.flags |= HyundaiFlagsSP.LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE.value
         CP.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG_MAIN_CRUISE_TOGGLEABLE
 
     # MADS is currently not supported in Tesla due to lack of consistent states to engage controls

sunnypilot/mads/mads.py

@@ -24,7 +24,7 @@ THE SOFTWARE.
 Last updated: July 29, 2024
 """
 
-from cereal import messaging, car, log, custom
+from cereal import car, log, custom
 
 from opendbc.car.hyundai.values import HyundaiFlags
 from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
@@ -35,6 +35,7 @@ from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SIL
 State = custom.SelfdriveStateSP.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
 ButtonType = car.CarState.ButtonEvent.Type
 EventName = log.OnroadEvent.EventName
+EventNameSP = custom.OnroadEventSP.EventName
 SafetyModel = car.CarParams.SafetyModel
 
 SET_SPEED_BUTTONS = (ButtonType.accelCruise, ButtonType.resumeCruise, ButtonType.decelCruise, ButtonType.setCruise)
@@ -48,15 +49,15 @@ class ModularAssistiveDrivingSystem:
     self.enabled = False
     self.active = False
     self.available = False
-    self.mismatch_counter = 0
     self.allow_always = False
     self.selfdrive = selfdrive
     self.selfdrive.enabled_prev = False
     self.state_machine = StateMachine(self)
     self.events = self.selfdrive.events
+    self.events_sp = self.selfdrive.events_sp
 
     if self.selfdrive.CP.carName == "hyundai":
-      if (self.selfdrive.CP.sunnypilotFlags & HyundaiFlagsSP.HAS_LFA_BUTTON) or \
+      if (self.selfdrive.CP_SP.flags & HyundaiFlagsSP.HAS_LFA_BUTTON) or \
             (self.selfdrive.CP.flags & HyundaiFlags.CANFD):
         self.allow_always = True
 
@@ -70,20 +71,7 @@ class ModularAssistiveDrivingSystem:
     self.main_enabled_toggle = self.mads_params.read_param("MadsMainCruiseAllowed")
     self.unified_engagement_mode = self.mads_params.read_param("MadsUnifiedEngagementMode")
 
-  def update_controls_mismatch(self, sm: messaging.SubMaster):
-    heartbeat_engaged = self.active if self.pause_lateral_on_brake_toggle else self.enabled
-
-    if not heartbeat_engaged:
-      self.mismatch_counter = 0
-
-    if heartbeat_engaged and any(not ps.controlsAllowedLat for ps in sm['pandaStates']
-                                 if ps.safetyModel not in IGNORED_SAFETY_MODES):
-      self.mismatch_counter += 1
-
-    if self.mismatch_counter >= 200:
-      self.events.add(EventName.controlsMismatchLateral)
-
-  def update_events(self, CS: car.CarState, sm: messaging.SubMaster):
+  def update_events(self, CS: car.CarState):
     def update_unified_engagement_mode():
       uem_blocked = self.enabled or (self.selfdrive.enabled and self.selfdrive.enabled_prev)
       if (self.unified_engagement_mode and uem_blocked) or not self.unified_engagement_mode:
@@ -92,26 +80,30 @@ class ModularAssistiveDrivingSystem:
 
     def transition_paused_state():
       if self.state_machine.state != State.paused:
-        self.events.add(EventName.silentLkasDisable)
+        self.events_sp.add(EventNameSP.silentLkasDisable)
+
+    def replace_event(old_event: int, new_event: int):
+      self.events.remove(old_event)
+      self.events_sp.add(new_event)
 
     if not self.selfdrive.enabled and self.enabled:
       if self.events.has(EventName.doorOpen):
-        self.events.replace(EventName.doorOpen, EventName.silentDoorOpen)
+        replace_event(EventName.doorOpen, EventNameSP.silentDoorOpen)
         transition_paused_state()
       if self.events.has(EventName.seatbeltNotLatched):
-        self.events.replace(EventName.seatbeltNotLatched, EventName.silentSeatbeltNotLatched)
+        replace_event(EventName.seatbeltNotLatched, EventNameSP.silentSeatbeltNotLatched)
         transition_paused_state()
       if self.events.has(EventName.wrongGear):
-        self.events.replace(EventName.wrongGear, EventName.silentWrongGear)
+        replace_event(EventName.wrongGear, EventNameSP.silentWrongGear)
         transition_paused_state()
       if self.events.has(EventName.reverseGear):
-        self.events.replace(EventName.reverseGear, EventName.silentReverseGear)
+        replace_event(EventName.reverseGear, EventNameSP.silentReverseGear)
         transition_paused_state()
       if self.events.has(EventName.brakeHold):
-        self.events.replace(EventName.brakeHold, EventName.silentBrakeHold)
+        replace_event(EventName.brakeHold, EventNameSP.silentBrakeHold)
         transition_paused_state()
       if self.events.has(EventName.parkBrake):
-        self.events.replace(EventName.parkBrake, EventName.silentParkBrake)
+        replace_event(EventName.parkBrake, EventNameSP.silentParkBrake)
         transition_paused_state()
 
       if self.pause_lateral_on_brake_toggle:
@@ -121,7 +113,7 @@ class ModularAssistiveDrivingSystem:
       if not (self.pause_lateral_on_brake_toggle and CS.brakePressed) and \
          not self.events.contains_in_list(GEARS_ALLOW_PAUSED_SILENT):
         if self.state_machine.state == State.paused:
-          self.events.add(EventName.silentLkasEnable)
+          self.events_sp.add(EventNameSP.silentLkasEnable)
 
       self.events.remove(EventName.preEnableStandstill)
       self.events.remove(EventName.belowEngageSpeed)
@@ -134,25 +126,25 @@ class ModularAssistiveDrivingSystem:
     else:
       if self.main_enabled_toggle:
         if CS.cruiseState.available and not self.selfdrive.CS_prev.cruiseState.available:
-          self.events.add(EventName.lkasEnable)
+          self.events_sp.add(EventNameSP.lkasEnable)
 
     for be in CS.buttonEvents:
       if be.type == ButtonType.cancel:
         if not self.selfdrive.enabled and self.selfdrive.enabled_prev:
-          self.events.add(EventName.manualLongitudinalRequired)
+          self.events_sp.add(EventNameSP.manualLongitudinalRequired)
       if be.type == ButtonType.lkas and be.pressed and (CS.cruiseState.available or self.allow_always):
         if self.enabled:
           if self.selfdrive.enabled:
-            self.events.add(EventName.manualSteeringRequired)
+            self.events_sp.add(EventNameSP.manualSteeringRequired)
           else:
-            self.events.add(EventName.lkasDisable)
+            self.events_sp.add(EventNameSP.lkasDisable)
         else:
-          self.events.add(EventName.lkasEnable)
+          self.events_sp.add(EventNameSP.lkasEnable)
 
     if not CS.cruiseState.available:
       self.events.remove(EventName.buttonEnable)
       if self.selfdrive.CS_prev.cruiseState.available:
-        self.events.add(EventName.lkasDisable)
+        self.events_sp.add(EventNameSP.lkasDisable)
 
     self.events.remove(EventName.pcmDisable)
     self.events.remove(EventName.buttonCancel)
@@ -161,16 +153,14 @@ class ModularAssistiveDrivingSystem:
     if not any(be.type in SET_SPEED_BUTTONS for be in CS.buttonEvents):
       self.events.remove(EventName.wrongCarMode)
 
-    self.update_controls_mismatch(sm)
-
-  def update(self, CS: car.CarState, sm: messaging.SubMaster):
+  def update(self, CS: car.CarState):
     if not self.enabled_toggle:
       return
 
-    self.update_events(CS, sm)
+    self.update_events(CS)
 
     if not self.selfdrive.CP.passive and self.selfdrive.initialized:
-      self.enabled, self.active = self.state_machine.update(self.events)
+      self.enabled, self.active = self.state_machine.update(self.events, self.events_sp)
 
     # Copy of previous SelfdriveD states for MADS events handling
     self.selfdrive.enabled_prev = self.selfdrive.enabled

sunnypilot/mads/state.py

@@ -29,14 +29,17 @@ from openpilot.selfdrive.selfdrived.events import ET, Events
 from openpilot.selfdrive.selfdrived.state import SOFT_DISABLE_TIME
 from openpilot.common.realtime import DT_CTRL
 
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+
 State = custom.SelfdriveStateSP.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
 EventName = log.OnroadEvent.EventName
+EventNameSP = custom.OnroadEventSP.EventName
 
 ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding)
 ENABLED_STATES = (State.paused, *ACTIVE_STATES)
 
-GEARS_ALLOW_PAUSED_SILENT = [EventName.silentWrongGear, EventName.silentReverseGear, EventName.silentBrakeHold,
-                             EventName.silentDoorOpen, EventName.silentSeatbeltNotLatched, EventName.silentParkBrake]
+GEARS_ALLOW_PAUSED_SILENT = [EventNameSP.silentWrongGear, EventNameSP.silentReverseGear, EventNameSP.silentBrakeHold,
+                             EventNameSP.silentDoorOpen, EventNameSP.silentSeatbeltNotLatched, EventNameSP.silentParkBrake]
 GEARS_ALLOW_PAUSED = [EventName.wrongGear, EventName.reverseGear, EventName.brakeHold,
                       EventName.doorOpen, EventName.seatbeltNotLatched, EventName.parkBrake,
                       *GEARS_ALLOW_PAUSED_SILENT]
@@ -49,45 +52,57 @@ class StateMachine:
 
     self.state = State.disabled
 
+    self._events = Events()
+    self._events_sp = EventsSP()
+
   def add_current_alert_types(self, alert_type):
     if not self.selfdrive.enabled:
       self.ss_state_machine.current_alert_types.append(alert_type)
 
-  def update(self, events: Events):
+  def check_contains(self, event_type: str) -> bool:
+    return bool(self._events.contains(event_type) or self._events_sp.contains(event_type))
+
+  def check_contains_in_list(self, events_list: list[int]) -> bool:
+    return bool(self._events.contains_in_list(events_list) or self._events_sp.contains_in_list(events_list))
+
+  def update(self, events: Events, events_sp: EventsSP):
     # soft disable timer and current alert types are from the state machine of openpilot
     # decrement the soft disable timer at every step, as it's reset on
     # entrance in SOFT_DISABLING state
 
+    self._events = events
+    self._events_sp = events_sp
+
     # ENABLED, SOFT DISABLING, PAUSED, OVERRIDING
     if self.state != State.disabled:
       # user and immediate disable always have priority in a non-disabled state
-      if events.contains(ET.USER_DISABLE):
-        if events.has(EventName.silentLkasDisable) or events.has(EventName.silentBrakeHold):
+      if self.check_contains(ET.USER_DISABLE):
+        if events_sp.has(EventNameSP.silentLkasDisable) or events_sp.has(EventNameSP.silentBrakeHold):
           self.state = State.paused
         else:
           self.state = State.disabled
         self.ss_state_machine.current_alert_types.append(ET.USER_DISABLE)
 
-      elif events.contains(ET.IMMEDIATE_DISABLE):
+      elif self.check_contains(ET.IMMEDIATE_DISABLE):
         self.state = State.disabled
         self.add_current_alert_types(ET.IMMEDIATE_DISABLE)
 
       else:
         # ENABLED
         if self.state == State.enabled:
-          if events.contains(ET.SOFT_DISABLE):
+          if self.check_contains(ET.SOFT_DISABLE):
             self.state = State.softDisabling
             if not self.selfdrive.enabled:
               self.ss_state_machine.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
               self.ss_state_machine.current_alert_types.append(ET.SOFT_DISABLE)
 
-          elif events.contains(ET.OVERRIDE_LATERAL):
+          elif self.check_contains(ET.OVERRIDE_LATERAL):
             self.state = State.overriding
             self.add_current_alert_types(ET.OVERRIDE_LATERAL)
 
         # SOFT DISABLING
         elif self.state == State.softDisabling:
-          if not events.contains(ET.SOFT_DISABLE):
+          if not self.check_contains(ET.SOFT_DISABLE):
             # no more soft disabling condition, so go back to ENABLED
             self.state = State.enabled
 
@@ -99,12 +114,12 @@ class StateMachine:
 
         # PAUSED
         elif self.state == State.paused:
-          if events.contains(ET.ENABLE):
-            if events.contains(ET.NO_ENTRY):
+          if self.check_contains(ET.ENABLE):
+            if self.check_contains(ET.NO_ENTRY):
               self.add_current_alert_types(ET.NO_ENTRY)
 
             else:
-              if events.contains(ET.OVERRIDE_LATERAL):
+              if self.check_contains(ET.OVERRIDE_LATERAL):
                 self.state = State.overriding
               else:
                 self.state = State.enabled
@@ -112,26 +127,26 @@ class StateMachine:
 
         # OVERRIDING
         elif self.state == State.overriding:
-          if events.contains(ET.SOFT_DISABLE):
+          if self.check_contains(ET.SOFT_DISABLE):
             self.state = State.softDisabling
             if not self.selfdrive.enabled:
               self.ss_state_machine.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
               self.ss_state_machine.current_alert_types.append(ET.SOFT_DISABLE)
-          elif not events.contains(ET.OVERRIDE_LATERAL):
+          elif not self.check_contains(ET.OVERRIDE_LATERAL):
             self.state = State.enabled
           else:
             self.ss_state_machine.current_alert_types += [ET.OVERRIDE_LATERAL]
 
     # DISABLED
     elif self.state == State.disabled:
-      if events.contains(ET.ENABLE):
-        if events.contains(ET.NO_ENTRY):
-          if events.contains_in_list(GEARS_ALLOW_PAUSED):
+      if self.check_contains(ET.ENABLE):
+        if self.check_contains(ET.NO_ENTRY):
+          if self.check_contains_in_list(GEARS_ALLOW_PAUSED):
             self.state = State.paused
           self.add_current_alert_types(ET.NO_ENTRY)
 
         else:
-          if events.contains(ET.OVERRIDE_LATERAL):
+          if self.check_contains(ET.OVERRIDE_LATERAL):
             self.state = State.overriding
           else:
             self.state = State.enabled

sunnypilot/mads/tests/test_mads_state_machine.py

@@ -27,13 +27,14 @@ Last updated: July 29, 2024
 import pytest
 from pytest_mock import MockerFixture
 
-from cereal import log, custom
+from cereal import custom
 from openpilot.common.realtime import DT_CTRL
 from openpilot.sunnypilot.mads.state import StateMachine, SOFT_DISABLE_TIME, GEARS_ALLOW_PAUSED
-from openpilot.selfdrive.selfdrived.events import Events, ET, EVENTS, NormalPermanentAlert
+from openpilot.selfdrive.selfdrived.events import ET, NormalPermanentAlert
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EVENTS_SP
 
 State = custom.SelfdriveStateSP.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
-EventName = log.OnroadEvent.EventName
+EventNameSP = custom.OnroadEventSP.EventName
 
 # The event types that maintain the current state
 MAINTAIN_STATES = {State.enabled: (None,), State.disabled: (None,), State.softDisabling: (ET.SOFT_DISABLE,),
@@ -47,7 +48,7 @@ def make_event(event_types):
   event = {}
   for ev in event_types:
     event[ev] = NormalPermanentAlert("alert")
-  EVENTS[0] = event
+  EVENTS_SP[0] = event
   return 0
 
 
@@ -62,87 +63,93 @@ class TestMADSStateMachine:
   @pytest.fixture(autouse=True)
   def setup_method(self, mocker: MockerFixture):
     self.mads = MockMADS(mocker)
-    self.events = Events()
     self.state_machine = StateMachine(self.mads)
+    self.events = self.state_machine._events
+    self.events_sp = self.state_machine._events_sp
     self.mads.selfdrive.state_machine.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
 
+  def reset(self):
+    self.events.clear()
+    self.events_sp.clear()
+    self.state_machine.state = State.disabled
+
   def test_immediate_disable(self):
     for state in ALL_STATES:
       for et in MAINTAIN_STATES[state]:
-        self.events.add(make_event([et, ET.IMMEDIATE_DISABLE]))
+        self.events_sp.add(make_event([et, ET.IMMEDIATE_DISABLE]))
         self.state_machine.state = state
-        self.state_machine.update(self.events)
+        self.state_machine.update(self.events, self.events_sp)
         assert State.disabled == self.state_machine.state
-        self.events.clear()
+        self.reset()
 
   def test_user_disable(self):
     for state in ALL_STATES:
       for et in MAINTAIN_STATES[state]:
-        self.events.add(make_event([et, ET.USER_DISABLE]))
+        self.events_sp.add(make_event([et, ET.USER_DISABLE]))
         self.state_machine.state = state
-        self.state_machine.update(self.events)
+        self.state_machine.update(self.events, self.events_sp)
         assert State.disabled == self.state_machine.state
-        self.events.clear()
+        self.reset()
 
   def test_user_disable_to_paused(self):
-    paused_events = (EventName.silentLkasDisable, EventName.silentBrakeHold)
+    paused_events = (EventNameSP.silentLkasDisable, EventNameSP.silentBrakeHold)
     for state in ALL_STATES:
       for et in MAINTAIN_STATES[state]:
-        self.events.add(make_event([et, ET.USER_DISABLE]))
+        self.events_sp.add(make_event([et, ET.USER_DISABLE]))
         for en in paused_events:
-          self.events.add(en)
+          self.events_sp.add(en)
           self.state_machine.state = state
-          self.state_machine.update(self.events)
-          final_state = State.paused if self.events.has(en) and state != State.disabled else State.disabled
+          self.state_machine.update(self.events, self.events_sp)
+          final_state = State.paused if self.events_sp.has(en) and state != State.disabled else State.disabled
           assert self.state_machine.state == final_state
-          self.events.clear()
+          self.reset()
 
   def test_soft_disable(self):
     for state in ALL_STATES:
       if state == State.paused:  # paused considers USER_DISABLE instead
         continue
       for et in MAINTAIN_STATES[state]:
-        self.events.add(make_event([et, ET.SOFT_DISABLE]))
+        self.events_sp.add(make_event([et, ET.SOFT_DISABLE]))
         self.state_machine.state = state
-        self.state_machine.update(self.events)
+        self.state_machine.update(self.events, self.events_sp)
         assert self.state_machine.state == State.disabled if state == State.disabled else State.softDisabling
-        self.events.clear()
+        self.reset()
 
   def test_soft_disable_timer(self):
     self.state_machine.state = State.enabled
-    self.events.add(make_event([ET.SOFT_DISABLE]))
-    self.state_machine.update(self.events)
+    self.events_sp.add(make_event([ET.SOFT_DISABLE]))
+    self.state_machine.update(self.events, self.events_sp)
     for _ in range(int(SOFT_DISABLE_TIME / DT_CTRL)):
       assert self.state_machine.state == State.softDisabling
       self.mads.selfdrive.state_machine.soft_disable_timer -= 1
-      self.state_machine.update(self.events)
+      self.state_machine.update(self.events, self.events_sp)
 
     assert self.state_machine.state == State.disabled
 
   def test_no_entry(self):
     for et in ENABLE_EVENT_TYPES:
-      self.events.add(make_event([ET.NO_ENTRY, et]))
-      if not self.events.contains_in_list(GEARS_ALLOW_PAUSED):
-        self.state_machine.update(self.events)
+      self.events_sp.add(make_event([ET.NO_ENTRY, et]))
+      if not self.state_machine.check_contains_in_list(GEARS_ALLOW_PAUSED):
+        self.state_machine.update(self.events, self.events_sp)
         assert self.state_machine.state == State.disabled
-        self.events.clear()
+        self.reset()
 
   def test_no_entry_paused(self):
     self.state_machine.state = State.paused
-    self.events.add(make_event([ET.NO_ENTRY]))
-    self.state_machine.update(self.events)
+    self.events_sp.add(make_event([ET.NO_ENTRY]))
+    self.state_machine.update(self.events, self.events_sp)
     assert self.state_machine.state == State.paused
 
   def test_override_lateral(self):
     self.state_machine.state = State.enabled
-    self.events.add(make_event([ET.OVERRIDE_LATERAL]))
-    self.state_machine.update(self.events)
+    self.events_sp.add(make_event([ET.OVERRIDE_LATERAL]))
+    self.state_machine.update(self.events, self.events_sp)
     assert self.state_machine.state == State.overriding
 
   def test_paused_to_enabled(self):
     self.state_machine.state = State.paused
-    self.events.add(make_event([ET.ENABLE]))
-    self.state_machine.update(self.events)
+    self.events_sp.add(make_event([ET.ENABLE]))
+    self.state_machine.update(self.events, self.events_sp)
     assert self.state_machine.state == State.enabled
 
   def test_maintain_states(self):
@@ -150,7 +157,7 @@ class TestMADSStateMachine:
       for et in MAINTAIN_STATES[state]:
         self.state_machine.state = state
         if et is not None:
-          self.events.add(make_event([et]))
-        self.state_machine.update(self.events)
+          self.events_sp.add(make_event([et]))
+        self.state_machine.update(self.events, self.events_sp)
         assert self.state_machine.state == state
-        self.events.clear()
+        self.reset()

sunnypilot/modeld_v2/meta_20hz.py

@@ -0,0 +1,17 @@
+from openpilot.selfdrive.modeld.constants import Meta
+
+
+class Meta20hz(Meta):
+  ENGAGED = slice(0, 1)
+  # next 2, 4, 6, 8, 10 seconds
+  GAS_DISENGAGE = slice(1, 31, 6)
+  BRAKE_DISENGAGE = slice(2, 31, 6)
+  STEER_OVERRIDE = slice(3, 31, 6)
+  HARD_BRAKE_3 = slice(4, 31, 6)
+  HARD_BRAKE_4 = slice(5, 31, 6)
+  HARD_BRAKE_5 = slice(6, 31, 6)
+  # next 0, 2, 4, 6, 8, 10 seconds
+  GAS_PRESS = slice(31, 55, 4)
+  BRAKE_PRESS = slice(32, 55, 4)
+  LEFT_BLINKER = slice(33, 55, 4)
+  RIGHT_BLINKER = slice(34, 55, 4)

sunnypilot/modeld_v2/meta_helper.py

@@ -0,0 +1,26 @@
+from openpilot.selfdrive.modeld.constants import Meta
+from cereal import custom
+from openpilot.sunnypilot.modeld_v2.meta_20hz import Meta20hz
+from openpilot.sunnypilot.models.helpers import get_active_bundle
+
+ModelBundle = custom.ModelManagerSP.ModelBundle
+
+
+def load_meta_constants():
+  """
+  Determines and loads the appropriate meta model class based on the metadata provided. The function checks
+  specific keys and conditions within the provided metadata dictionary to identify the corresponding meta
+  model class to return.
+
+  :param model_metadata: Dictionary containing metadata about the model. It includes
+      details such as input shapes, output slices, and other configurations for identifying
+      metadata-dependent meta model classes.
+  :type model_metadata: dict
+  :return: The appropriate meta model class (Meta, MetaSimPose, or MetaTombRaider)
+      based on the conditions and metadata provided.
+  :rtype: type
+  """
+  if (bundle := get_active_bundle()) and bundle.is20hz:
+    return Meta20hz
+
+  return Meta  # Default

sunnypilot/modeld_v2/model_runner.py

@@ -0,0 +1,135 @@
+import os
+import pickle
+from abc import ABC, abstractmethod
+
+import numpy as np
+from openpilot.selfdrive.modeld import MODEL_PATH, MODEL_PKL_PATH, METADATA_PATH
+from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLMem
+from openpilot.selfdrive.modeld.runners.ort_helpers import make_onnx_cpu_runner, ORT_TYPES_TO_NP_TYPES
+from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
+from openpilot.system.hardware import TICI
+from openpilot.system.hardware.hw import Paths
+
+from cereal import custom
+from openpilot.sunnypilot.models.helpers import get_active_bundle
+#
+from tinygrad.tensor import Tensor
+
+if TICI:
+  os.environ['QCOM'] = '1'
+
+SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
+CUSTOM_MODEL_PATH = Paths.model_root()
+ModelManager = custom.ModelManagerSP
+
+
+class ModelRunner(ABC):
+  """Abstract base class for model runners that defines the interface for running ML models."""
+
+  def __init__(self):
+    """Initialize the model runner with paths to model and metadata files."""
+    metadata_path = METADATA_PATH
+    self.is_20hz = None
+    self._drive_model = None
+    self._metadata_model = None
+
+    if bundle := get_active_bundle():
+      bundle_models = {model.type.raw: model for model in bundle.models}
+      self._drive_model = bundle_models.get(ModelManager.Type.drive)
+      self._metadata_model = bundle_models.get(ModelManager.Type.metadata)
+      self.is_20hz = bundle.is20hz
+
+    # Override the metadata path if a metadata model is found in the active bundle
+    if self._metadata_model:
+      metadata_path = f"{CUSTOM_MODEL_PATH}/{self._metadata_model.fileName}"
+
+    with open(metadata_path, 'rb') as f:
+      self.model_metadata = pickle.load(f)
+
+    self.input_shapes = self.model_metadata['input_shapes']
+    self.output_slices = self.model_metadata['output_slices']
+    self.inputs: dict = {}
+
+  @abstractmethod
+  def prepare_inputs(self, imgs_cl: dict[str, CLMem], numpy_inputs: dict[str, np.ndarray], frames: dict[str, DrivingModelFrame]) -> dict:
+    """Prepare inputs for model inference."""
+    raise NotImplementedError
+
+  @abstractmethod
+  def run_model(self):
+    """Run model inference with prepared inputs."""
+
+  def slice_outputs(self, model_outputs: np.ndarray) -> dict:
+    """Slice model outputs according to metadata configuration."""
+    parsed_outputs = {k: model_outputs[np.newaxis, v] for k, v in self.output_slices.items()}
+    if SEND_RAW_PRED:
+      parsed_outputs['raw_pred'] = model_outputs.copy()
+    return parsed_outputs
+
+
+class TinygradRunner(ModelRunner):
+  """Tinygrad implementation of model runner for TICI hardware."""
+
+  def __init__(self):
+    super().__init__()
+
+    model_pkl_path = MODEL_PKL_PATH
+    if self._drive_model:
+      model_pkl_path = f"{CUSTOM_MODEL_PATH}/{self._drive_model.fileName}"
+      assert model_pkl_path.endswith('_tinygrad.pkl'), f"Invalid model file: {model_pkl_path} for TinygradRunner"
+
+    # Load Tinygrad model
+    with open(model_pkl_path, "rb") as f:
+      try:
+        self.model_run = pickle.load(f)
+      except FileNotFoundError as e:
+        assert "/dev/kgsl-3d0" not in str(e), "Model was built on C3 or C3X, but is being loaded on PC"
+        raise
+
+    self.input_to_dtype = {}
+    self.input_to_device = {}
+
+    for idx, name in enumerate(self.model_run.captured.expected_names):
+      self.input_to_dtype[name] = self.model_run.captured.expected_st_vars_dtype_device[idx][2]  # 2 is the dtype
+      self.input_to_device[name] = self.model_run.captured.expected_st_vars_dtype_device[idx][3]  # 3 is the device
+
+  def prepare_inputs(self, imgs_cl: dict[str, CLMem], numpy_inputs: dict[str, np.ndarray], frames: dict[str, DrivingModelFrame]) -> dict:
+    # Initialize image tensors if not already done
+    for key in imgs_cl:
+      if TICI and key not in self.inputs:
+        self.inputs[key] = qcom_tensor_from_opencl_address(imgs_cl[key].mem_address, self.input_shapes[key], dtype=self.input_to_dtype[key])
+      elif not TICI:
+        shape = frames[key].buffer_from_cl(imgs_cl[key]).reshape(self.input_shapes[key])
+        self.inputs[key] = Tensor(shape, device=self.input_to_device[key], dtype=self.input_to_dtype[key]).realize()
+
+    # Update numpy inputs
+    for key, value in numpy_inputs.items():
+      if key not in imgs_cl:
+        self.inputs[key] = Tensor(value, device=self.input_to_device[key], dtype=self.input_to_dtype[key]).realize()
+
+    return self.inputs
+
+  def run_model(self):
+    return self.model_run(**self.inputs).numpy().flatten()
+
+
+class ONNXRunner(ModelRunner):
+  """ONNX implementation of model runner for non-TICI hardware."""
+
+  def __init__(self):
+    super().__init__()
+    self.runner = make_onnx_cpu_runner(MODEL_PATH)
+
+    self.input_to_nptype = {
+      model_input.name: ORT_TYPES_TO_NP_TYPES[model_input.type]
+      for model_input in self.runner.get_inputs()
+    }
+
+  def prepare_inputs(self, imgs_cl: dict[str, CLMem], numpy_inputs: dict[str, np.ndarray], frames: dict[str, DrivingModelFrame]) -> dict:
+    self.inputs = numpy_inputs
+    for key in imgs_cl:
+      self.inputs[key] = frames[key].buffer_from_cl(imgs_cl[key]).reshape(self.input_shapes[key]).astype(dtype=self.input_to_nptype[key])
+    return self.inputs
+
+  def run_model(self):
+    return self.runner.run(None, self.inputs)[0].flatten()

sunnypilot/models/fetcher.py

@@ -72,13 +72,12 @@ class ModelParser:
     model_bundle.generation = int(value["generation"])
     model_bundle.environment = value["environment"]
     model_bundle.runner = value.get("runner", custom.ModelManagerSP.Runner.snpe)
+    model_bundle.is20hz = value.get("is_20hz", False)
 
     return model_bundle
 
   @staticmethod
   def parse_models(json_data: dict) -> list[custom.ModelManagerSP.ModelBundle]:
-    # TODO-SP: Remove the following filter once we add support for tinygrad model switcher
-    json_data = {k: v for k, v in json_data.items() if v.get("runner", -1) == custom.ModelManagerSP.Runner.snpe}
     return [ModelParser._parse_bundle(key, value) for key, value in json_data.items()]
 
 

sunnypilot/selfdrive/car/car_specific.py

@@ -5,12 +5,12 @@ 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.
 """
 
-from cereal import log
+from cereal import custom
 from opendbc.car import structs
 
-from openpilot.selfdrive.selfdrived.events import Events
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 
-EventName = log.OnroadEvent.EventName
+EventNameSP = custom.OnroadEventSP.EventName
 
 
 class CarSpecificEventsSP:
@@ -26,9 +26,9 @@ class CarSpecificEventsSP:
     self.hyundai_radar_tracks_confirmed = self.params.get_bool("HyundaiRadarTracksConfirmed")
 
   def update(self):
-    events = Events()
+    events = EventsSP()
     if self.CP.carName == 'hyundai':
       if self.hyundai_radar_tracks and not self.hyundai_radar_tracks_confirmed:
-        events.add(EventName.hyundaiRadarTracksConfirmed)
+        events.add(EventNameSP.hyundaiRadarTracksConfirmed)
 
     return events

sunnypilot/selfdrive/car/cruise_helpers.py

@@ -0,0 +1,50 @@
+"""
+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.
+"""
+
+from cereal import car, custom
+from opendbc.car import structs
+from openpilot.common.params import Params
+
+ButtonType = car.CarState.ButtonEvent.Type
+EventNameSP = custom.OnroadEventSP.EventName
+
+DISTANCE_LONG_PRESS = 50
+
+
+class CruiseHelper:
+  def __init__(self, CP: structs.CarParams):
+    self.CP = CP
+    self.params = Params()
+
+    self.button_frame_counts = {ButtonType.gapAdjustCruise: 0}
+    self._experimental_mode = False
+    self.experimental_mode_switched = False
+
+  def update(self, CS, events, experimental_mode) -> None:
+    if self.CP.openpilotLongitudinalControl:
+      if CS.cruiseState.available:
+        self.update_button_frame_counts(CS)
+
+        # toggle experimental mode once on distance button hold
+        self.update_experimental_mode(events, experimental_mode)
+
+  def update_button_frame_counts(self, CS) -> None:
+    for button in self.button_frame_counts:
+      if self.button_frame_counts[button] > 0:
+        self.button_frame_counts[button] += 1
+
+    for button_event in CS.buttonEvents:
+      button = button_event.type.raw
+      if button in self.button_frame_counts:
+        self.button_frame_counts[button] = int(button_event.pressed)
+
+  def update_experimental_mode(self, events, experimental_mode) -> None:
+    if self.button_frame_counts[ButtonType.gapAdjustCruise] >= DISTANCE_LONG_PRESS and not self.experimental_mode_switched:
+      self._experimental_mode = not experimental_mode
+      self.params.put_bool_nonblocking("ExperimentalMode", self._experimental_mode)
+      events.add(EventNameSP.experimentalModeSwitched)
+      self.experimental_mode_switched = True

sunnypilot/selfdrive/car/interfaces.py

@@ -22,20 +22,21 @@ def log_fingerprint(CP: structs.CarParams) -> None:
     sentry.capture_fingerprint(CP.carFingerprint, CP.carName)
 
 
-def setup_car_interface_sp(CP: structs.CarParams, params):
+def setup_car_interface_sp(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params):
   if CP.carName == 'hyundai':
     if CP.flags & HyundaiFlags.MANDO_RADAR and CP.radarUnavailable:
       # Having this automatic without a toggle causes a weird process replay diff because
       # somehow it sees fewer logs than intended
       if params.get_bool("HyundaiRadarTracksToggle"):
-        CP.sunnypilotFlags |= HyundaiFlagsSP.ENABLE_RADAR_TRACKS.value
+        CP_SP.flags |= HyundaiFlagsSP.ENABLE_RADAR_TRACKS.value
         if params.get_bool("HyundaiRadarTracks"):
           CP.radarUnavailable = False
 
 
-def initialize_car_interface_sp(CP: structs.CarParams, params, can_recv: CanRecvCallable, can_send: CanSendCallable):
+def initialize_car_interface_sp(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params, can_recv: CanRecvCallable,
+                                can_send: CanSendCallable):
   if CP.carName == 'hyundai':
-    if CP.sunnypilotFlags & HyundaiFlagsSP.ENABLE_RADAR_TRACKS:
+    if CP_SP.flags & HyundaiFlagsSP.ENABLE_RADAR_TRACKS:
       can_recv()
       _, fingerprint = can_fingerprint(can_recv)
       radar_unavailable = RADAR_START_ADDR not in fingerprint[1] or Bus.radar not in HYUNDAI_DBC[CP.carFingerprint]

sunnypilot/selfdrive/car/tests/test_cruise_mode.py

@@ -0,0 +1,66 @@
+from parameterized import parameterized_class
+from cereal import car
+from openpilot.selfdrive.selfdrived.events import Events
+from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper, DISTANCE_LONG_PRESS
+
+ButtonEvent = car.CarState.ButtonEvent
+ButtonType = car.CarState.ButtonEvent.Type
+
+
+@parameterized_class(('openpilot_longitudinal',), [(True,)])
+class TestCruiseHelper:
+  def setup_method(self):
+    self.CP = car.CarParams(openpilotLongitudinalControl=self.openpilot_longitudinal)
+    self.cruise_helper = CruiseHelper(self.CP)
+    self.cruise_helper.experimental_mode_switched = False
+    self.events = Events()
+
+  def reset(self):
+    for _ in range(2):
+      CS = car.CarState(cruiseState={"available": False})
+      CS.buttonEvents = [ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=False)]
+      self.cruise_helper._experimental_mode = False
+      self.cruise_helper.experimental_mode_switched = False
+      self.cruise_helper.update(CS, self.events, False)
+
+
+  def test_gap_adjust_cruise_long_press_toggle_mode(self) -> None:
+    for pressed in (True, False):
+      for experimental_mode in (True, False):
+        self.reset()
+        self.cruise_helper._experimental_mode = experimental_mode
+        toggled_mode = not experimental_mode if pressed else experimental_mode
+
+        for i in range(DISTANCE_LONG_PRESS):
+          CS = car.CarState(cruiseState={"available": True})
+          CS.buttonEvents = [ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=pressed)] if i == 0 else []
+          self.cruise_helper.update(CS, self.events, experimental_mode)
+
+        # mode should be toggled
+        assert self.cruise_helper._experimental_mode == toggled_mode
+        assert self.cruise_helper.experimental_mode_switched is pressed
+
+        # keep holding button after switching mode
+        for _ in range(DISTANCE_LONG_PRESS):
+          CS = car.CarState(cruiseState={"available": True})
+          CS.buttonEvents = [ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=pressed)]
+          self.cruise_helper.update(CS, self.events, toggled_mode)
+
+        # mode should not be toggled
+        assert self.cruise_helper._experimental_mode == toggled_mode
+        assert self.cruise_helper.experimental_mode_switched is pressed
+
+  def test_gap_adjust_cruise_short_press_toggle_mode(self) -> None:
+    for pressed in (True, False):
+      for experimental_mode in (True, False):
+        self.reset()
+        self.cruise_helper._experimental_mode = experimental_mode
+
+        for i in range(DISTANCE_LONG_PRESS - 1):
+          CS = car.CarState(cruiseState={"available": True})
+          CS.buttonEvents = [ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=pressed)] if i == 0 else []
+          self.cruise_helper.update(CS, self.events, experimental_mode)
+
+        # mode should not be toggled
+        assert self.cruise_helper._experimental_mode == experimental_mode
+        assert self.cruise_helper.experimental_mode_switched is False

sunnypilot/selfdrive/controls/lib/dec/constants.py

@@ -0,0 +1,26 @@
+class WMACConstants:
+  LEAD_WINDOW_SIZE = 5
+  LEAD_PROB = 0.5
+
+  SLOW_DOWN_WINDOW_SIZE = 4
+  SLOW_DOWN_PROB = 0.6
+
+  SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
+  #SLOW_DOWN_DIST = [25., 38., 55., 75., 95., 115., 130., 150.]
+  SLOW_DOWN_DIST = [30., 45., 60., 80., 100., 120., 135., 150.]
+
+  SLOWNESS_WINDOW_SIZE = 12
+  SLOWNESS_PROB = 0.5
+  SLOWNESS_CRUISE_OFFSET = 1.05
+
+  DANGEROUS_TTC_WINDOW_SIZE = 3
+  DANGEROUS_TTC = 2.3
+
+  MPC_FCW_WINDOW_SIZE = 10
+  MPC_FCW_PROB = 0.5
+
+
+class SNG_State:
+  off = 0
+  stopped = 1
+  going = 2

sunnypilot/selfdrive/controls/lib/dec/dec.py

@@ -0,0 +1,390 @@
+# The MIT License
+#
+# Copyright (c) 2019-, Rick Lan, dragonpilot community, and a number of other of contributors.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+#
+# Version = 2025-1-18
+
+import numpy as np
+
+from cereal import messaging
+from opendbc.car import structs
+from openpilot.common.numpy_fast import interp
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants, SNG_State
+
+# d-e2e, from modeldata.h
+TRAJECTORY_SIZE = 33
+
+HIGHWAY_CRUISE_KPH = 70
+
+STOP_AND_GO_FRAME = 60
+
+SET_MODE_TIMEOUT = 10
+
+V_ACC_MIN = 9.72
+
+
+class GenericMovingAverageCalculator:
+  def __init__(self, window_size):
+    self.window_size = window_size
+    self.data = []
+    self.total = 0
+
+  def add_data(self, value: float) -> None:
+    if len(self.data) == self.window_size:
+      self.total -= self.data.pop(0)
+    self.data.append(value)
+    self.total += value
+
+  def get_moving_average(self) -> float | None:
+    return None if len(self.data) == 0 else self.total / len(self.data)
+
+  def reset_data(self) -> None:
+    self.data = []
+    self.total = 0
+
+
+class WeightedMovingAverageCalculator:
+  def __init__(self, window_size):
+    self.window_size = window_size
+    self.data = []
+    self.weights = np.linspace(1, 3, window_size)  # Linear weights, adjust as needed
+
+  def add_data(self, value: float) -> None:
+    if len(self.data) == self.window_size:
+      self.data.pop(0)
+    self.data.append(value)
+
+  def get_weighted_average(self) -> float | None:
+    if len(self.data) == 0:
+      return None
+    weighted_sum: float = float(np.dot(self.data, self.weights[-len(self.data):]))
+    weight_total: float = float(np.sum(self.weights[-len(self.data):]))
+    return weighted_sum / weight_total
+
+  def reset_data(self) -> None:
+    self.data = []
+
+
+class DynamicExperimentalController:
+  def __init__(self, CP: structs.CarParams, mpc, params=None):
+    self._CP = CP
+    self._mpc = mpc
+    self._params = params or Params()
+    self._enabled: bool = self._params.get_bool("DynamicExperimentalControl")
+    self._active: bool = False
+    self._mode: str = 'acc'
+    self._frame: int = 0
+
+    # Use weighted moving average for filtering leads
+    self._lead_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.LEAD_WINDOW_SIZE)
+    self._has_lead_filtered = False
+    self._has_lead_filtered_prev = False
+
+    self._slow_down_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.SLOW_DOWN_WINDOW_SIZE)
+    self._has_slow_down: bool = False
+    self._slow_down_confidence: float = 0.0
+
+    self._has_blinkers = False
+
+    self._slowness_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.SLOWNESS_WINDOW_SIZE)
+    self._has_slowness: bool = False
+
+    self._has_nav_instruction = False
+
+    self._dangerous_ttc_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.DANGEROUS_TTC_WINDOW_SIZE)
+    self._has_dangerous_ttc: bool = False
+
+    self._v_ego_kph = 0.
+    self._v_cruise_kph = 0.
+
+    self._has_lead = False
+
+    self._has_standstill = False
+    self._has_standstill_prev = False
+
+    self._sng_transit_frame = 0
+    self._sng_state = SNG_State.off
+
+    self._mpc_fcw_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.MPC_FCW_WINDOW_SIZE)
+    self._has_mpc_fcw: bool = False
+    self._mpc_fcw_crash_cnt = 0
+
+    self._set_mode_timeout = 0
+
+  def _read_params(self) -> None:
+    if self._frame % int(1. / DT_MDL) == 0:
+      self._enabled = self._params.get_bool("DynamicExperimentalControl")
+
+  def mode(self) -> str:
+    return str(self._mode)
+
+  def enabled(self) -> bool:
+    return self._enabled
+
+  def active(self) -> bool:
+    return self._active
+
+  @staticmethod
+  def _anomaly_detection(recent_data: list[float], threshold: float = 2.0, context_check: bool = True) -> bool:
+    """
+    Basic anomaly detection using standard deviation.
+    """
+    if len(recent_data) < 5:
+      return False
+    mean: float = float(np.mean(recent_data))
+    std_dev: float = float(np.std(recent_data))
+    anomaly: bool = bool(recent_data[-1] > mean + threshold * std_dev)
+
+    # Context check to ensure repeated anomaly
+    if context_check:
+      return np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1
+    return anomaly
+
+  def _adaptive_slowdown_threshold(self) -> float:
+    """
+    Adapts the slow-down threshold based on vehicle speed and recent behavior.
+    """
+    slowdown_scaling_factor: float = (1.0 + 0.05 * np.log(1 + len(self._slow_down_gmac.data)))
+    adaptive_threshold: float = float(
+      interp(self._v_ego_kph, WMACConstants.SLOW_DOWN_BP, WMACConstants.SLOW_DOWN_DIST) * slowdown_scaling_factor
+    )
+    return adaptive_threshold
+
+  def _smoothed_lead_detection(self, lead_prob: float, smoothing_factor: float = 0.2):
+    """
+    Smoothing the lead detection to avoid erratic behavior.
+    """
+    lead_filtering: float = (1 - smoothing_factor) * self._has_lead_filtered + smoothing_factor * lead_prob
+    return lead_filtering > WMACConstants.LEAD_PROB
+
+  def _adaptive_lead_prob_threshold(self) -> float:
+    """
+    Adapts lead probability threshold based on driving conditions.
+    """
+    if self._v_ego_kph > HIGHWAY_CRUISE_KPH:
+      return float(WMACConstants.LEAD_PROB + 0.1)  # Increase the threshold on highways
+    return float(WMACConstants.LEAD_PROB)
+
+  def _update_calculations(self, sm: messaging.SubMaster) -> None:
+    car_state = sm['carState']
+    lead_one = sm['radarState'].leadOne
+    md = sm['modelV2']
+
+    self._v_ego_kph = car_state.vEgo * 3.6
+    self._v_cruise_kph = car_state.vCruise
+    self._has_lead = lead_one.status
+    self._has_standstill = car_state.standstill
+
+    # fcw detection
+    self._mpc_fcw_gmac.add_data(self._mpc_fcw_crash_cnt > 0)
+    if _mpc_fcw_weighted_average := self._mpc_fcw_gmac.get_weighted_average():
+      self._has_mpc_fcw = _mpc_fcw_weighted_average > WMACConstants.MPC_FCW_PROB
+    else:
+      self._has_mpc_fcw = False
+
+    # nav enable detection
+    # self._has_nav_instruction = md.navEnabledDEPRECATED and maneuver_distance / max(car_state.vEgo, 1) < 13
+
+    # lead detection with smoothing
+    self._lead_gmac.add_data(lead_one.status)
+    self._has_lead_filtered = (self._lead_gmac.get_weighted_average() or -1.) > WMACConstants.LEAD_PROB
+    #lead_prob = self._lead_gmac.get_weighted_average() or 0
+    #self._has_lead_filtered = self._smoothed_lead_detection(lead_prob)
+
+    # adaptive slow down detection
+    adaptive_threshold = self._adaptive_slowdown_threshold()
+    slow_down_trigger = len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE and md.position.x[TRAJECTORY_SIZE - 1] < adaptive_threshold
+    self._slow_down_gmac.add_data(slow_down_trigger)
+    if _has_slow_down_weighted_average := self._slow_down_gmac.get_weighted_average():
+      self._has_slow_down = _has_slow_down_weighted_average > WMACConstants.SLOW_DOWN_PROB
+      self._slow_down_confidence = _has_slow_down_weighted_average  # Store confidence level
+    else:
+      self._has_slow_down = False
+      self._slow_down_confidence = 0.0  # No confidence if no slowdown
+
+    # anomaly detection for slow down events
+    if self._anomaly_detection(self._slow_down_gmac.data):
+      self._slow_down_confidence *= 0.85  # Reduce confidence
+      self._has_slow_down = self._slow_down_confidence > WMACConstants.SLOW_DOWN_PROB
+
+    # blinker detection
+    self._has_blinkers = car_state.leftBlinker or car_state.rightBlinker
+
+    # sng detection
+    if self._has_standstill:
+      self._sng_state = SNG_State.stopped
+      self._sng_transit_frame = 0
+    else:
+      if self._sng_transit_frame == 0:
+        if self._sng_state == SNG_State.stopped:
+          self._sng_state = SNG_State.going
+          self._sng_transit_frame = STOP_AND_GO_FRAME
+        elif self._sng_state == SNG_State.going:
+          self._sng_state = SNG_State.off
+      elif self._sng_transit_frame > 0:
+        self._sng_transit_frame -= 1
+
+    # slowness detection
+    if not self._has_standstill:
+      self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET))
+      if _slowness_weighted_average := self._slowness_gmac.get_weighted_average():
+        self._has_slowness = _slowness_weighted_average > WMACConstants.SLOWNESS_PROB
+      else:
+        self._has_slowness = False
+
+    # dangerous TTC detection
+    if not self._has_lead_filtered and self._has_lead_filtered_prev:
+      self._dangerous_ttc_gmac.reset_data()
+      self._has_dangerous_ttc = False
+
+    if self._has_lead and car_state.vEgo >= 0.01:
+      self._dangerous_ttc_gmac.add_data(lead_one.dRel / car_state.vEgo)
+
+    if _dangerous_ttc_weighted_average := self._dangerous_ttc_gmac.get_weighted_average():
+      self._has_dangerous_ttc = _dangerous_ttc_weighted_average <= WMACConstants.DANGEROUS_TTC
+    else:
+      self._has_dangerous_ttc = False
+
+    # keep prev values
+    self._has_standstill_prev = self._has_standstill
+    self._has_lead_filtered_prev = self._has_lead_filtered
+
+  def _radarless_mode(self) -> None:
+    # when mpc fcw crash prob is high
+    # use blended to slow down quickly
+    if self._has_mpc_fcw:
+      self._set_mode('blended')
+      return
+
+    # Nav enabled and distance to upcoming turning is 300 or below
+    # if self._has_nav_instruction:
+    #  self._set_mode('blended')
+    #  return
+
+    # when blinker is on and speed is driving below V_ACC_MIN: blended
+    # we don't want it to switch mode at higher speed, blended may trigger hard brake
+    # if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
+    #  self._set_mode('blended')
+    #  return
+
+    # when at highway cruise and SNG: blended
+    # ensuring blended mode is used because acc is bad at catching SNG lead car
+    # especially those who accel very fast and then brake very hard.
+    # if self._sng_state == SNG_State.going and self._v_cruise_kph >= V_ACC_MIN:
+    #  self._set_mode('blended')
+    #  return
+
+    # when standstill: blended
+    # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
+    if self._has_standstill:
+      self._set_mode('blended')
+      return
+
+    # when detecting slow down scenario: blended
+    # e.g. traffic light, curve, stop sign etc.
+    if self._has_slow_down:
+      self._set_mode('blended')
+      return
+
+    # when detecting lead slow down: blended
+    # use blended for higher braking capability
+    if self._has_dangerous_ttc:
+      self._set_mode('blended')
+      return
+
+    # car driving at speed lower than set speed: acc
+    if self._has_slowness:
+      self._set_mode('acc')
+      return
+
+    self._set_mode('acc')
+
+  def _radar_mode(self) -> None:
+    # when mpc fcw crash prob is high
+    # use blended to slow down quickly
+    if self._has_mpc_fcw:
+      self._set_mode('blended')
+      return
+
+    # If there is a filtered lead, the vehicle is not in standstill, and the lead vehicle's yRel meets the condition,
+    if self._has_lead_filtered and not self._has_standstill:
+      self._set_mode('acc')
+      return
+
+    # when blinker is on and speed is driving below V_ACC_MIN: blended
+    # we don't want it to switch mode at higher speed, blended may trigger hard brake
+    # if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
+    #  self._set_mode('blended')
+    #  return
+
+    # when standstill: blended
+    # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
+    if self._has_standstill:
+      self._set_mode('blended')
+      return
+
+    # when detecting slow down scenario: blended
+    # e.g. traffic light, curve, stop sign etc.
+    if self._has_slow_down:
+      self._set_mode('blended')
+      return
+
+    # car driving at speed lower than set speed: acc
+    if self._has_slowness:
+      self._set_mode('acc')
+      return
+
+    # Nav enabled and distance to upcoming turning is 300 or below
+    # if self._has_nav_instruction:
+    #  self._set_mode('blended')
+    #  return
+
+    self._set_mode('acc')
+
+  def set_mpc_fcw_crash_cnt(self) -> None:
+    self._mpc_fcw_crash_cnt = self._mpc.crash_cnt
+
+  def _set_mode(self, mode: str) -> None:
+    if self._set_mode_timeout == 0:
+      self._mode = mode
+      if mode == 'blended':
+        self._set_mode_timeout = SET_MODE_TIMEOUT
+
+    if self._set_mode_timeout > 0:
+      self._set_mode_timeout -= 1
+
+  def update(self, sm: messaging.SubMaster) -> None:
+    self._read_params()
+
+    self.set_mpc_fcw_crash_cnt()
+
+    self._update_calculations(sm)
+
+    if self._CP.radarUnavailable:
+      self._radarless_mode()
+    else:
+      self._radar_mode()
+
+    self._active = sm['selfdriveState'].experimentalMode and self._enabled
+
+    self._frame += 1

sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py

@@ -0,0 +1,248 @@
+import pytest
+import numpy as np
+from openpilot.common.params import Params
+
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants, SNG_State
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, TRAJECTORY_SIZE, STOP_AND_GO_FRAME
+
+class MockInterp:
+  def __call__(self, x, xp, fp):
+    return np.interp(x, xp, fp)
+
+class MockCarState:
+  def __init__(self, v_ego=0., standstill=False, left_blinker=False, right_blinker=False):
+    self.vEgo = v_ego
+    self.standstill = standstill
+    self.leftBlinker = left_blinker
+    self.rightBlinker = right_blinker
+
+class MockLeadOne:
+  def __init__(self, status=False, d_rel=0):
+    self.status = status
+    self.dRel = d_rel
+
+class MockModelData:
+  def __init__(self, x_vals=None, positions=None):
+    self.orientation = type('Orientation', (), {'x': x_vals})()
+    self.position = type('Position', (), {'x': positions})()
+
+class MockControlState:
+  def __init__(self, v_cruise=0):
+    self.vCruise = v_cruise
+
+@pytest.fixture
+def interp(monkeypatch):
+  mock_interp = MockInterp()
+  monkeypatch.setattr('openpilot.common.numpy_fast.interp', mock_interp)
+  return mock_interp
+
+@pytest.fixture
+def controller(interp):
+  params = Params()
+  params.put_bool("DynamicExperimentalControl", True)
+  return DynamicExperimentalController()
+
+def test_initial_state(controller):
+  """Test initial state of the controller"""
+  assert controller._mode == 'acc'
+  assert not controller._has_lead
+  assert not controller._has_standstill
+  assert controller._sng_state == SNG_State.off
+  assert not controller._has_lead_filtered
+  assert not controller._has_slow_down
+  assert not controller._has_dangerous_ttc
+  assert not controller._has_mpc_fcw
+
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_standstill_detection(controller, has_radar):
+  """Test standstill detection and state transitions"""
+  car_state = MockCarState(standstill=True)
+  lead_one = MockLeadOne()
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
+  controls_state = MockControlState()
+
+  # Test transition to standstill
+  controller.update(not has_radar, car_state, lead_one, md, controls_state)
+  assert controller._sng_state == SNG_State.stopped
+  assert controller.get_mpc_mode() == 'blended'
+
+  # Test transition from standstill to moving
+  car_state.standstill = False
+  controller.update(not has_radar, car_state, lead_one, md, controls_state)
+  assert controller._sng_state == SNG_State.going
+
+  # Test complete transition to normal driving
+  for _ in range(STOP_AND_GO_FRAME + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+  assert controller._sng_state == SNG_State.off
+
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_lead_detection(controller, has_radar):
+  """Test lead vehicle detection and filtering"""
+  car_state = MockCarState(v_ego=20)  # 72 kph
+  lead_one = MockLeadOne(status=True, d_rel=50)  # Safe distance
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
+  controls_state = MockControlState(v_cruise=72)
+
+  # Let moving average stabilize
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_lead_filtered
+  expected_mode = 'acc' if has_radar else 'blended'
+  assert controller.get_mpc_mode() == expected_mode
+
+  # Test lead loss detection
+  lead_one.status = False
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert not controller._has_lead_filtered
+
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_slow_down_detection(controller, has_radar):
+  """Test slow down detection based on trajectory"""
+  car_state = MockCarState(v_ego=10/3.6)  # 10 kph
+  lead_one = MockLeadOne()
+  x_vals = [0] * TRAJECTORY_SIZE
+  positions = [20] * TRAJECTORY_SIZE  # Position within slow down threshold
+  md = MockModelData(x_vals=x_vals, positions=positions)
+  controls_state = MockControlState(v_cruise=30)
+
+  # Test slow down detection
+  for _ in range(WMACConstants.SLOW_DOWN_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_slow_down
+  assert controller.get_mpc_mode() == 'blended'
+
+  # Test slow down recovery
+  positions = [200] * TRAJECTORY_SIZE  # Position outside slow down threshold
+  md = MockModelData(x_vals=x_vals, positions=positions)
+  for _ in range(WMACConstants.SLOW_DOWN_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert not controller._has_slow_down
+
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_dangerous_ttc_detection(controller, has_radar):
+  """Test Time-To-Collision detection and handling"""
+  car_state = MockCarState(v_ego=10)  # 36 kph
+  lead_one = MockLeadOne(status=True)
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
+  controls_state = MockControlState(v_cruise=36)
+
+  # First establish normal conditions with lead
+  lead_one.dRel = 100  # Safe distance
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):  # First establish lead detection
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_lead_filtered  # Verify lead is detected
+
+  # Now test dangerous TTC detection
+  lead_one.dRel = 10  # 10m distance - should trigger dangerous TTC
+  # TTC = dRel/vEgo = 10/10 = 1s (which is less than DANGEROUS_TTC = 2.3s)
+
+  # Need to update multiple times to allow the weighted average to stabilize
+  for _ in range(WMACConstants.DANGEROUS_TTC_WINDOW_SIZE * 2):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_dangerous_ttc, "TTC of 1s should be considered dangerous"
+  expected_mode = 'acc' if has_radar else 'blended'
+  assert controller.get_mpc_mode() == expected_mode, f"Should be in [{expected_mode}] mode with dangerous TTC"
+
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_mode_transitions(controller, has_radar):
+  """Test comprehensive mode transitions under different conditions"""
+  # Initialize with normal driving conditions
+  car_state = MockCarState(v_ego=25)  # 90 kph
+  lead_one = MockLeadOne(status=False)
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[200] * TRAJECTORY_SIZE)
+  controls_state = MockControlState(v_cruise=100)
+
+  def stabilize_filters():
+    """Helper to let all moving averages stabilize"""
+    for _ in range(max(WMACConstants.LEAD_WINDOW_SIZE, WMACConstants.SLOW_DOWN_WINDOW_SIZE,
+                       WMACConstants.DANGEROUS_TTC_WINDOW_SIZE, WMACConstants.MPC_FCW_WINDOW_SIZE) + 1):
+      controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  # Test 1: Normal driving -> ACC mode
+  stabilize_filters()
+  assert controller.get_mpc_mode() == 'acc', "Should be in ACC mode under normal driving conditions"
+
+  # Test 2: Standstill -> Blended mode
+  car_state.standstill = True
+  controller.update(not has_radar, car_state, lead_one, md, controls_state)
+  assert controller.get_mpc_mode() == 'blended', "Should be in blended mode during standstill"
+
+  # Test 3: Lead car appears -> ACC mode
+  car_state = MockCarState(v_ego=20)  # Reset car state
+  lead_one.status = True
+  lead_one.dRel = 50  # Safe distance
+  stabilize_filters()
+  assert not controller._has_dangerous_ttc, "Should not have dangerous TTC"
+  assert controller.get_mpc_mode() == 'acc', "Should be in ACC mode with safe lead distance"
+
+  # Test 4: Dangerous TTC -> Blended mode
+  car_state = MockCarState(v_ego=20)  # 72 kph
+  lead_one.status = True
+  lead_one.dRel = 50  # First establish normal lead detection
+
+  # First establish lead detection
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_lead_filtered  # Verify lead is detected
+
+  # Now create dangerous TTC condition
+  lead_one.dRel = 20  # This creates a TTC of 1s, well below DANGEROUS_TTC
+
+  for _ in range(WMACConstants.DANGEROUS_TTC_WINDOW_SIZE * 2):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_dangerous_ttc, "Should detect dangerous TTC condition"
+  expected_mode = 'acc' if has_radar else 'blended'
+  assert controller.get_mpc_mode() == expected_mode, f"Should be in [{expected_mode}] mode with dangerous TTC"
+
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_mpc_fcw_handling(controller, has_radar):
+  """Test MPC FCW crash count handling and mode transitions"""
+  car_state = MockCarState(v_ego=20)
+  lead_one = MockLeadOne()
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
+  controls_state = MockControlState(v_cruise=72)
+
+  # Test FCW activation
+  controller.set_mpc_fcw_crash_cnt(5)
+  for _ in range(WMACConstants.MPC_FCW_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_mpc_fcw
+  assert controller.get_mpc_mode() == 'blended'
+
+  # Test FCW recovery
+  controller.set_mpc_fcw_crash_cnt(0)
+  for _ in range(WMACConstants.MPC_FCW_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert not controller._has_mpc_fcw
+
+def test_radar_unavailable_handling(controller):
+  """Test behavior transitions between radar available and unavailable states"""
+  car_state = MockCarState(v_ego=27.78)  # 100 kph
+  lead_one = MockLeadOne(status=True, d_rel=50)
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
+  controls_state = MockControlState(v_cruise=100)
+
+  # Test with radar available
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):
+    controller.update(False, car_state, lead_one, md, controls_state)
+  radar_mode = controller.get_mpc_mode()
+
+  # Test with radar unavailable
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):
+    controller.update(True, car_state, lead_one, md, controls_state)
+  radarless_mode = controller.get_mpc_mode()
+
+  assert radar_mode is not None
+  assert radarless_mode is not None

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -0,0 +1,41 @@
+"""
+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.
+"""
+
+from cereal import messaging, custom
+from opendbc.car import structs
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
+
+DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
+
+
+class LongitudinalPlannerSP:
+  def __init__(self, CP: structs.CarParams, mpc):
+    self.dec = DynamicExperimentalController(CP, mpc)
+
+  def get_mpc_mode(self) -> str | None:
+    if not self.dec.active():
+      return None
+
+    return self.dec.mode()
+
+  def update(self, sm: messaging.SubMaster) -> None:
+    self.dec.update(sm)
+
+  def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
+    plan_sp_send = messaging.new_message('longitudinalPlanSP')
+
+    plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
+
+    longitudinalPlanSP = plan_sp_send.longitudinalPlanSP
+
+    # Dynamic Experimental Control
+    dec = longitudinalPlanSP.dec
+    dec.state = DecState.blended if self.dec.mode() == 'blended' else DecState.acc
+    dec.enabled = self.dec.enabled()
+    dec.active = self.dec.active()
+
+    pm.send('longitudinalPlanSP', plan_sp_send)

sunnypilot/selfdrive/selfdrived/events.py

@@ -0,0 +1,133 @@
+from cereal import log, car, custom
+from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \
+  NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType
+
+
+AlertSize = log.SelfdriveState.AlertSize
+AlertStatus = log.SelfdriveState.AlertStatus
+VisualAlert = car.CarControl.HUDControl.VisualAlert
+AudibleAlert = car.CarControl.HUDControl.AudibleAlert
+EventNameSP = custom.OnroadEventSP.EventName
+
+
+# get event name from enum
+EVENT_NAME_SP = {v: k for k, v in EventNameSP.schema.enumerants.items()}
+
+
+class EventsSP(EventsBase):
+  def __init__(self):
+    super().__init__()
+    self.event_counters = dict.fromkeys(EVENTS_SP.keys(), 0)
+
+  def get_events_mapping(self) -> dict[int, dict[str, Alert | AlertCallbackType]]:
+    return EVENTS_SP
+
+  def get_event_name(self, event: int):
+    return EVENT_NAME_SP[event]
+
+  def get_event_msg_type(self):
+    return custom.OnroadEventSP
+
+
+EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
+  # sunnypilot
+  EventNameSP.lkasEnable: {
+    ET.ENABLE: EngagementAlert(AudibleAlert.engage),
+  },
+
+  EventNameSP.lkasDisable: {
+    ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
+  },
+
+  EventNameSP.manualSteeringRequired: {
+    ET.USER_DISABLE: Alert(
+      "Automatic Lane Centering is OFF",
+      "Manual Steering Required",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.disengage, 1.),
+  },
+
+  EventNameSP.manualLongitudinalRequired: {
+    ET.WARNING: Alert(
+      "Smart/Adaptive Cruise Control: OFF",
+      "Manual Speed Control Required",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
+  },
+
+  EventNameSP.silentLkasEnable: {
+    ET.ENABLE: EngagementAlert(AudibleAlert.none),
+  },
+
+  EventNameSP.silentLkasDisable: {
+    ET.USER_DISABLE: EngagementAlert(AudibleAlert.none),
+  },
+
+  EventNameSP.silentBrakeHold: {
+    ET.USER_DISABLE: EngagementAlert(AudibleAlert.none),
+    ET.NO_ENTRY: NoEntryAlert("Brake Hold Active"),
+  },
+
+  EventNameSP.silentWrongGear: {
+    ET.WARNING: Alert(
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
+    ET.NO_ENTRY: Alert(
+      "Gear not D",
+      "openpilot Unavailable",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 0.),
+  },
+
+  EventNameSP.silentReverseGear: {
+    ET.PERMANENT: Alert(
+      "Reverse\nGear",
+      "",
+      AlertStatus.normal, AlertSize.full,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5),
+    ET.NO_ENTRY: NoEntryAlert("Reverse Gear"),
+  },
+
+  EventNameSP.silentDoorOpen: {
+    ET.WARNING: Alert(
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
+    ET.NO_ENTRY: NoEntryAlert("Door Open"),
+  },
+
+  EventNameSP.silentSeatbeltNotLatched: {
+    ET.WARNING: Alert(
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
+    ET.NO_ENTRY: NoEntryAlert("Seatbelt Unlatched"),
+  },
+
+  EventNameSP.silentParkBrake: {
+    ET.WARNING: Alert(
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
+    ET.NO_ENTRY: NoEntryAlert("Parking Brake Engaged"),
+  },
+
+  EventNameSP.controlsMismatchLateral: {
+    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch: Lateral"),
+    ET.NO_ENTRY: NoEntryAlert("Controls Mismatch: Lateral"),
+  },
+
+  EventNameSP.hyundaiRadarTracksConfirmed: {
+    ET.PERMANENT: NormalPermanentAlert("Radar tracks available. Restart the car to initialize")
+  },
+
+  EventNameSP.experimentalModeSwitched: {
+    ET.WARNING: NormalPermanentAlert("Experimental Mode Switched", duration=1.5)
+  }
+
+}

sunnypilot/selfdrive/selfdrived/events_base.py

@@ -0,0 +1,225 @@
+import bisect
+from enum import IntEnum
+from abc import abstractmethod
+from collections.abc import Callable
+
+from cereal import log, car
+import cereal.messaging as messaging
+from openpilot.common.realtime import DT_CTRL
+
+AlertSize = log.SelfdriveState.AlertSize
+AlertStatus = log.SelfdriveState.AlertStatus
+VisualAlert = car.CarControl.HUDControl.VisualAlert
+AudibleAlert = car.CarControl.HUDControl.AudibleAlert
+
+
+# Alert priorities
+class Priority(IntEnum):
+  LOWEST = 0
+  LOWER = 1
+  LOW = 2
+  MID = 3
+  HIGH = 4
+  HIGHEST = 5
+
+
+# Event types
+class ET:
+  ENABLE = 'enable'
+  PRE_ENABLE = 'preEnable'
+  OVERRIDE_LATERAL = 'overrideLateral'
+  OVERRIDE_LONGITUDINAL = 'overrideLongitudinal'
+  NO_ENTRY = 'noEntry'
+  WARNING = 'warning'
+  USER_DISABLE = 'userDisable'
+  SOFT_DISABLE = 'softDisable'
+  IMMEDIATE_DISABLE = 'immediateDisable'
+  PERMANENT = 'permanent'
+
+
+class Alert:
+  def __init__(self,
+               alert_text_1: str,
+               alert_text_2: str,
+               alert_status: log.SelfdriveState.AlertStatus,
+               alert_size: log.SelfdriveState.AlertSize,
+               priority: Priority,
+               visual_alert: car.CarControl.HUDControl.VisualAlert,
+               audible_alert: car.CarControl.HUDControl.AudibleAlert,
+               duration: float,
+               creation_delay: float = 0.):
+
+    self.alert_text_1 = alert_text_1
+    self.alert_text_2 = alert_text_2
+    self.alert_status = alert_status
+    self.alert_size = alert_size
+    self.priority = priority
+    self.visual_alert = visual_alert
+    self.audible_alert = audible_alert
+
+    self.duration = int(duration / DT_CTRL)
+
+    self.creation_delay = creation_delay
+
+    self.alert_type = ""
+    self.event_type: str | None = None
+
+  def __str__(self) -> str:
+    return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}"
+
+  def __gt__(self, alert2) -> bool:
+    if not isinstance(alert2, Alert):
+      return False
+    return self.priority > alert2.priority
+
+class AlertBase(Alert):
+  def __init__(self, alert_text_1: str, alert_text_2: str, alert_status: log.SelfdriveState.AlertStatus,
+               alert_size: log.SelfdriveState.AlertSize, priority: Priority,
+               visual_alert: car.CarControl.HUDControl.VisualAlert,
+               audible_alert: car.CarControl.HUDControl.AudibleAlert, duration: float):
+    super().__init__(alert_text_1, alert_text_2, alert_status, alert_size, priority, visual_alert, audible_alert, duration)
+
+
+AlertCallbackType = Callable[[car.CarParams, car.CarState, messaging.SubMaster, bool, int, log.ControlsState], Alert]
+
+
+class EventsBase:
+  def __init__(self):
+    self.events: list[int] = []
+    self.static_events: list[int] = []
+    self.event_counters = {}
+
+  @property
+  def names(self) -> list[int]:
+    return self.events
+
+  def __len__(self) -> int:
+    return len(self.events)
+
+  def add(self, event_name: int, static: bool = False) -> None:
+    if static:
+      bisect.insort(self.static_events, event_name)
+    bisect.insort(self.events, event_name)
+
+  def clear(self) -> None:
+    self.event_counters = {k: (v + 1 if k in self.events else 0) for k, v in self.event_counters.items()}
+    self.events = self.static_events.copy()
+
+  def contains(self, event_type: str) -> bool:
+    return any(event_type in self.get_events_mapping().get(e, {}) for e in self.events)
+
+  def create_alerts(self, event_types: list[str], callback_args=None):
+    if callback_args is None:
+      callback_args = []
+
+    ret = []
+    for e in self.events:
+      types = self.get_events_mapping()[e].keys()
+      for et in event_types:
+        if et in types:
+          alert = self.get_events_mapping()[e][et]
+          if not isinstance(alert, Alert):
+            alert = alert(*callback_args)
+
+          if DT_CTRL * (self.event_counters[e] + 1) >= alert.creation_delay:
+            alert.alert_type = f"{self.get_event_name(e)}/{et}"
+            alert.event_type = et
+            ret.append(alert)
+    return ret
+
+  def add_from_msg(self, events):
+    for e in events:
+      bisect.insort(self.events, e.name.raw)
+
+  def to_msg(self):
+    ret = []
+    for event_name in self.events:
+      event = self.get_event_msg_type().new_message()
+      event.name = event_name
+      for event_type in self.get_events_mapping().get(event_name, {}):
+        setattr(event, event_type, True)
+      ret.append(event)
+    return ret
+
+  def has(self, event_name: int) -> bool:
+    return event_name in self.events
+
+  def contains_in_list(self, events_list: list[int]) -> bool:
+    return any(event_name in self.events for event_name in events_list)
+
+  def remove(self, event_name: int, static: bool = False) -> None:
+    if static and event_name in self.static_events:
+      self.static_events.remove(event_name)
+
+    if event_name in self.events:
+      self.event_counters[event_name] = self.event_counters[event_name] + 1
+      self.events.remove(event_name)
+
+  @abstractmethod
+  def get_events_mapping(self) -> dict[int, dict[str, Alert | AlertCallbackType]]:
+    raise NotImplementedError
+
+  @abstractmethod
+  def get_event_name(self, event: int) -> str:
+    raise NotImplementedError
+
+  @abstractmethod
+  def get_event_msg_type(self):
+    raise NotImplementedError
+
+
+EmptyAlert = Alert("" , "", AlertStatus.normal, AlertSize.none, Priority.LOWEST,
+                   VisualAlert.none, AudibleAlert.none, 0)
+
+class NoEntryAlert(Alert):
+  def __init__(self, alert_text_2: str,
+               alert_text_1: str = "openpilot Unavailable",
+               visual_alert: car.CarControl.HUDControl.VisualAlert=VisualAlert.none):
+    super().__init__(alert_text_1, alert_text_2, AlertStatus.normal,
+                     AlertSize.mid, Priority.LOW, visual_alert,
+                     AudibleAlert.refuse, 3.)
+
+
+class SoftDisableAlert(Alert):
+  def __init__(self, alert_text_2: str):
+    super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2,
+                     AlertStatus.userPrompt, AlertSize.full,
+                     Priority.MID, VisualAlert.steerRequired,
+                     AudibleAlert.warningSoft, 2.),
+
+
+# less harsh version of SoftDisable, where the condition is user-triggered
+class UserSoftDisableAlert(SoftDisableAlert):
+  def __init__(self, alert_text_2: str):
+    super().__init__(alert_text_2),
+    self.alert_text_1 = "openpilot will disengage"
+
+
+class ImmediateDisableAlert(Alert):
+  def __init__(self, alert_text_2: str):
+    super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2,
+                     AlertStatus.critical, AlertSize.full,
+                     Priority.HIGHEST, VisualAlert.steerRequired,
+                     AudibleAlert.warningImmediate, 4.),
+
+
+class EngagementAlert(Alert):
+  def __init__(self, audible_alert: car.CarControl.HUDControl.AudibleAlert):
+    super().__init__("", "",
+                     AlertStatus.normal, AlertSize.none,
+                     Priority.MID, VisualAlert.none,
+                     audible_alert, .2),
+
+
+class NormalPermanentAlert(Alert):
+  def __init__(self, alert_text_1: str, alert_text_2: str = "", duration: float = 0.2, priority: Priority = Priority.LOWER, creation_delay: float = 0.):
+    super().__init__(alert_text_1, alert_text_2,
+                     AlertStatus.normal, AlertSize.mid if len(alert_text_2) else AlertSize.small,
+                     priority, VisualAlert.none, AudibleAlert.none, duration, creation_delay=creation_delay),
+
+
+class StartupAlert(Alert):
+  def __init__(self, alert_text_1: str, alert_text_2: str = "Always keep hands on wheel and eyes on road", alert_status=AlertStatus.normal):
+    super().__init__(alert_text_1, alert_text_2,
+                     alert_status, AlertSize.mid,
+                     Priority.LOWER, VisualAlert.none, AudibleAlert.none, 5.),

system/hardware/hardwared.py

@@ -310,6 +310,12 @@ def hardware_thread(end_event, hw_queue) -> None:
     # ensure device is fully booted
     startup_conditions["device_booted"] = startup_conditions.get("device_booted", False) or HARDWARE.booted()
 
+    # user-forced status
+    offroad_mode = params.get_bool("OffroadMode")
+    startup_conditions["not_always_offroad"] = not offroad_mode
+    onroad_conditions["not_always_offroad"] = not offroad_mode
+    set_offroad_alert("OffroadMode_Status", offroad_mode)
+
     # if the temperature enters the danger zone, go offroad to cool down
     onroad_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger
     extra_text = f"{offroad_comp_temp:.1f}C"
@@ -392,7 +398,7 @@ def hardware_thread(end_event, hw_queue) -> None:
       cloudlog.warning(f"shutting device down, offroad since {off_ts}")
       params.put_bool("DoShutdown", True)
 
-    msg.deviceState.started = started_ts is not None
+    msg.deviceState.started = started_ts is not None and not offroad_mode
     msg.deviceState.startedMonoTime = int(1e9*(started_ts or 0))
 
     last_ping = params.get("LastAthenaPingTime")

system/manager/manager.py

@@ -43,6 +43,7 @@ def manager_init() -> None:
   ]
 
   sunnypilot_default_params: list[tuple[str, str | bytes]] = [
+    ("DynamicExperimentalControl", "0"),
     ("Mads", "1"),
     ("MadsMainCruiseAllowed", "1"),
     ("MadsPauseLateralOnBrake", "0"),