Merge branch 'feature/slc' into ui-sla-control-only

# Conflicts:
#	selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h
#	sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py
#	sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py
#	sunnypilot/selfdrive/controls/lib/speed_limit_controller/state.py

.github/workflows/selfdrive_tests.yaml

@@ -115,9 +115,7 @@ jobs:
     - run: echo "CACHE_COMMIT_DATE=$(git log -1 --pretty='format:%cd' --date=format:'%Y-%m-%d-%H:%M')" >> $GITHUB_ENV
     - name: Homebrew cache
       uses: ./.github/workflows/auto-cache
-      if: false # disabling the cache for now because it is breaking macos builds...
       with:
-        save: false # No need save here if we manually save it later conditionally
         path: ~/Library/Caches/Homebrew
         key: brew-macos-${{ env.CACHE_COMMIT_DATE }}-${{ github.sha }}
         restore-keys: |
@@ -138,7 +136,6 @@ jobs:
     - name: Getting scons cache
       uses: ./.github/workflows/auto-cache
       with:
-        save: false # No need save here if we manually save it later conditionally
         path: /tmp/scons_cache
         key: scons-${{ runner.arch }}-macos-${{ env.CACHE_COMMIT_DATE }}-${{ github.sha }}
         restore-keys: |

cereal/custom.capnp

@@ -145,11 +145,8 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   dec @0 :DynamicExperimentalControl;
   longitudinalPlanSource @1 :LongitudinalPlanSource;
   smartCruiseControl @2 :SmartCruiseControl;
-  speedLimit @3 :SpeedLimit;
-  vTarget @4 :Float32;
-  aTarget @5 :Float32;
-  events @6 :List(OnroadEventSP.Event);
-  e2eAlerts @7 :E2eAlerts;
+  speedLimitAssist @3 :SpeedLimitAssist;
+  events @4 :List(OnroadEventSP.Event);
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -164,7 +161,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
 
   struct SmartCruiseControl {
     vision @0 :Vision;
-    map @1 :Map;
 
     struct Vision {
       state @0 :VisionState;
@@ -176,14 +172,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
       active @6 :Bool;
     }
 
-    struct Map {
-      state @0 :MapState;
-      vTarget @1 :Float32;
-      aTarget @2 :Float32;
-      enabled @3 :Bool;
-      active @4 :Bool;
-    }
-
     enum VisionState {
       disabled @0; # System disabled or inactive.
       enabled @1; # No predicted substantial turn on vision range.
@@ -192,65 +180,38 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
       leaving @4; # Road ahead straightens. Start to allow positive acceleration.
       overriding @5; # System overriding with manual control.
     }
-
-    enum MapState {
-      disabled @0; # System disabled or inactive.
-      enabled @1; # No predicted substantial turn on map range.
-      turning @2; # Actively turning. Managing acceleration to provide a roll on turn feeling.
-      overriding @3; # System overriding with manual control.
-    }
   }
 
-  struct SpeedLimit {
-    resolver @0 :Resolver;
-    assist @1 :Assist;
-
-    struct Resolver {
-      speedLimit @0 :Float32;
-      distToSpeedLimit @1 :Float32;
-      source @2 :Source;
-      speedLimitOffset @3 :Float32;
-      speedLimitLast @4 :Float32;
-      speedLimitFinal @5 :Float32;
-      speedLimitFinalLast @6 :Float32;
-      speedLimitValid @7 :Bool;
-      speedLimitLastValid @8 :Bool;
-    }
-
-    struct Assist {
-      state @0 :AssistState;
-      enabled @1 :Bool;
-      active @2 :Bool;
-      vTarget @3 :Float32;
-      aTarget @4 :Float32;
-    }
-
-    enum Source {
-      none @0;
-      car @1;
-      map @2;
-    }
-
-    enum AssistState {
-      disabled @0;
-      inactive @1; # No speed limit set or not enabled by parameter.
-      preActive @2;
-      pending @3; # Awaiting new speed limit.
-      adapting @4; # Reducing speed to match new speed limit.
-      active @5; # Cruising at speed limit.
-    }
+  struct SpeedLimitAssist {
+    state @0 :SpeedLimitAssistState;
+    enabled @1 :Bool;
+    active @2 :Bool;
+    speedLimit @3 :Float32;
+    speedLimitOffset @4 :Float32;
+    distToSpeedLimit @5 :Float32;
+    source @6 :SpeedLimitSource;
   }
 
   enum LongitudinalPlanSource {
     cruise @0;
     sccVision @1;
-    sccMap @2;
-    speedLimitAssist @3;
+    speedLimitAssist @2;
   }
 
-  struct E2eAlerts {
-    greenLightAlert @0 :Bool;
-    leadDepartAlert @1 :Bool;
+  enum SpeedLimitAssistState {
+    disabled @0;
+    inactive @1; # No speed limit set or not enabled by parameter.
+    preActive @2;
+    pending @3; # Awaiting new speed limit.
+    adapting @4; # Reducing speed to match new speed limit.
+    active @5; # Cruising at speed limit.
+    overriding @6; # System overriding with manual control.
+  }
+
+  enum SpeedLimitSource {
+    none @0;
+    car @1;
+    map @2;
   }
 }
 
@@ -295,18 +256,16 @@ struct OnroadEventSP @0xda96579883444c35 {
     laneTurnRight @18;
     speedLimitPreActive @19;
     speedLimitActive @20;
-    speedLimitChanged @21;
-    speedLimitPending @22;
-    e2eChime @23;
+    speedLimitConfirmed @21;
+    speedLimitChanged @22;
   }
 }
 
 struct CarParamsSP @0x80ae746ee2596b11 {
   flags @0 :UInt32;        # flags for car specific quirks in sunnypilot
   safetyParam @1 : Int16;  # flags for sunnypilot's custom safety flags
-  pcmCruiseSpeed @3 :Bool;
+  pcmCruiseSpeed @3 :Bool = true;
   intelligentCruiseButtonManagementAvailable @4 :Bool;
-  enableGasInterceptor @5 :Bool;
 
   neuralNetworkLateralControl @2 :NeuralNetworkLateralControl;
 
@@ -335,7 +294,7 @@ struct CarControlSP @0xa5cd762cd951a455 {
 
     valueDEPRECATED @1 :Text; # The data type change may cause issues with backwards compatibility.
   }
-
+  
   enum ParamType {
     string @0;
     bool @1;
@@ -390,7 +349,7 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
 }
 
 struct CarStateSP @0xb86e6369214c01c8 {
-  speedLimit @0 :Float32;
+  speedLimit @0 :Float32;  # m/s
 }
 
 struct LiveMapDataSP @0xf416ec09499d9d19 {

cereal/log.capnp

@@ -2684,7 +2684,7 @@ struct Event {
     lateralPlanDEPRECATED @64 :LateralPlan;
     navModelDEPRECATED @104 :NavModelData;
     uiPlanDEPRECATED @106 :UiPlan;
-    liveLocationKalman @72 :LiveLocationKalman;
+    liveLocationKalmanDEPRECATED @72 :LiveLocationKalman;
     liveTracksDEPRECATED @16 :List(LiveTracksDEPRECATED);
     onroadEventsDEPRECATED @68: List(Car.OnroadEventDEPRECATED);
   }

cereal/services.py

@@ -89,7 +89,6 @@ _services: dict[str, tuple] = {
   "carStateSP": (True, 100., 10),
   "liveMapDataSP": (True, 1., 1),
   "modelDataV2SP": (True, 20.),
-  "liveLocationKalman": (True, 20.),
 
   # debug
   "uiDebug": (True, 0., 1),

common/params_keys.h

@@ -148,24 +148,19 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}},
     {"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
     {"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
-    {"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}},
     {"IntelligentCruiseButtonManagement", {PERSISTENT | BACKUP , BOOL}},
     {"InteractivityTimeout", {PERSISTENT | BACKUP, INT, "0"}},
     {"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}},
-    {"LastGPSPositionLLK", {PERSISTENT, STRING}},
-    {"LeadDepartAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}},
     {"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}},
     {"OffroadMode", {CLEAR_ON_MANAGER_START, BOOL}},
     {"Offroad_TiciSupport", {CLEAR_ON_MANAGER_START, JSON}},
-    {"OnroadScreenOffBrightness", {PERSISTENT | BACKUP, INT, "100"}},
-    {"OnroadScreenOffControl", {PERSISTENT | BACKUP, BOOL}},
-    {"OnroadScreenOffTimer", {PERSISTENT | BACKUP, INT, "0"}},
     {"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"SmartCruiseControlVision", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     // MADS params
@@ -192,7 +187,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"SunnylinkCache_Users", {PERSISTENT, STRING}},
     {"SunnylinkDongleId", {PERSISTENT, STRING}},
     {"SunnylinkdPid", {PERSISTENT, INT}},
-    {"SunnylinkEnabled", {PERSISTENT, BOOL, "1"}},
+    {"SunnylinkEnabled", {PERSISTENT, BOOL}},
 
     // Backup Manager params
     {"BackupManager_CreateBackup", {PERSISTENT, BOOL}},
@@ -230,25 +225,14 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"OsmStateTitle", {PERSISTENT, STRING}},
     {"OsmWayTest", {PERSISTENT, STRING}},
     {"RoadName", {CLEAR_ON_ONROAD_TRANSITION, STRING}},
-    {"RoadNameToggle", {PERSISTENT, STRING}},
 
-    // Speed Limit
-    {"SpeedLimitMode", {PERSISTENT | BACKUP, INT, "1"}},
-    {"SpeedLimitOffsetType", {PERSISTENT | BACKUP, INT, "0"}},
+    // Speed Limit Control
+    {"SpeedLimitAssist", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"SpeedLimitPolicy", {PERSISTENT | BACKUP, INT, "3"}},
+    {"SpeedLimitEngageType", {PERSISTENT | BACKUP, INT, "0"}},
+    {"SpeedLimitOffsetType", {PERSISTENT | BACKUP, INT, "0"}},
     {"SpeedLimitValueOffset", {PERSISTENT | BACKUP, INT, "0"}},
-
-    // Smart Cruise Control
-    {"MapTargetVelocities", {CLEAR_ON_ONROAD_TRANSITION, STRING}},
-    {"SmartCruiseControlMap", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"SmartCruiseControlVision", {PERSISTENT | BACKUP, BOOL, "0"}},
-
-    // Torque lateral control custom params
-    {"CustomTorqueParams", {PERSISTENT | BACKUP , BOOL}},
-    {"EnforceTorqueControl", {PERSISTENT | BACKUP, BOOL}},
-    {"LiveTorqueParamsToggle", {PERSISTENT | BACKUP , BOOL}},
-    {"LiveTorqueParamsRelaxedToggle", {PERSISTENT | BACKUP , BOOL}},
-    {"TorqueParamsOverrideEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"TorqueParamsOverrideFriction", {PERSISTENT | BACKUP, FLOAT, "0.1"}},
-    {"TorqueParamsOverrideLatAccelFactor", {PERSISTENT | BACKUP, FLOAT, "2.5"}},
+    {"SpeedLimitWarningType", {PERSISTENT | BACKUP, INT, "0"}},
+    {"SpeedLimitWarningOffsetType", {PERSISTENT | BACKUP, INT, "0"}},
+    {"SpeedLimitWarningValueOffset", {PERSISTENT | BACKUP, INT, "0"}},
 };

common/util.h

@@ -36,7 +36,6 @@ const double MS_TO_KPH = 3.6;
 const double MS_TO_MPH = MS_TO_KPH * KM_TO_MILE;
 const double METER_TO_MILE = KM_TO_MILE / 1000.0;
 const double METER_TO_FOOT = 3.28084;
-const double METER_TO_KM = 1. / 1000.0;
 
 #define ALIGNED_SIZE(x, align) (((x) + (align)-1) & ~((align)-1))
 

selfdrive/car/card.py

@@ -71,7 +71,7 @@ class Car:
 
   def __init__(self, CI=None, RI=None) -> None:
     self.can_sock = messaging.sub_sock('can', timeout=20)
-    self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP', 'longitudinalPlanSP'])
+    self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP'])
     self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP', 'carStateSP'])
 
     self.can_rcv_cum_timeout_counter = 0
@@ -124,7 +124,7 @@ class Car:
 
     self.CP.alternativeExperience = 0
     # mads
-    set_alternative_experience(self.CP, self.CP_SP, self.params)
+    set_alternative_experience(self.CP, self.params)
     set_car_specific_params(self.CP, self.CP_SP, self.params)
 
     # Dynamic Experimental Control
@@ -216,7 +216,6 @@ class Car:
     if can_rcv_valid and REPLAY:
       self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
 
-    self.v_cruise_helper.update_speed_limit_assist(self.is_metric, self.sm['longitudinalPlanSP'])
     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

selfdrive/car/cruise.py

@@ -53,7 +53,6 @@ class VCruiseHelper(VCruiseHelperSP):
       if not self.CP.pcmCruise or (not self.CP_SP.pcmCruiseSpeed and _enabled):
         # if stock cruise is completely disabled, then we can use our own set speed logic
         self._update_v_cruise_non_pcm(CS, _enabled, is_metric)
-        self.update_speed_limit_assist_v_cruise_non_pcm()
         self.v_cruise_cluster_kph = self.v_cruise_kph
         self.update_button_timers(CS, enabled)
       else:
@@ -105,12 +104,6 @@ class VCruiseHelper(VCruiseHelperSP):
     if not self.button_change_states[button_type]["enabled"]:
       return
 
-    # Speed Limit Assist for Non PCM long cars.
-    # True: Disallow set speed changes when user confirmed the target set speed during preActive state
-    # False: Allow set speed changes as SLA is not requesting user confirmation
-    if self.update_speed_limit_assist_pre_active_confirmed(button_type):
-      return
-
     long_press, v_cruise_delta = VCruiseHelperSP.update_v_cruise_delta(self, long_press, v_cruise_delta)
     if long_press and self.v_cruise_kph % v_cruise_delta != 0:  # partial interval
       self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta

selfdrive/car/tests/test_car_interfaces.py

@@ -60,7 +60,7 @@ class TestCarInterfaces:
     # Test controller initialization
     # TODO: wait until card refactor is merged to run controller a few times,
     #  hypothesis also slows down significantly with just one more message draw
-    LongControl(car_params, car_params_sp)
+    LongControl(car_params)
     if car_params.steerControlType == CarParams.SteerControlType.angle:
       LatControlAngle(car_params, car_params_sp, car_interface)
     elif car_params.lateralTuning.which() == 'pid':

selfdrive/car/tests/test_cruise_speed.py

@@ -44,12 +44,12 @@ class TestCruiseSpeed:
     assert simulation_steady_state == pytest.approx(cruise_speed, abs=.01), f'Did not reach {self.speed} m/s'
 
 
-# TODO: test pcmCruise and pcmCruiseSpeed
-@parameterized_class(('pcm_cruise', 'pcm_cruise_speed'), [(False, True)])
+# TODO: test pcmCruise
+@parameterized_class(('pcm_cruise',), [(False,)])
 class TestVCruiseHelper:
   def setup_method(self):
     self.CP = car.CarParams(pcmCruise=self.pcm_cruise)
-    self.CP_SP = custom.CarParamsSP(pcmCruiseSpeed=self.pcm_cruise_speed)
+    self.CP_SP = custom.CarParamsSP()
     self.v_cruise_helper = VCruiseHelper(self.CP, self.CP_SP)
     self.reset_cruise_speed_state()
 

selfdrive/controls/controlsd.py

@@ -58,7 +58,7 @@ class Controls(ControlsExt, ModelStateBase):
     self.pose_calibrator = PoseCalibrator()
     self.calibrated_pose: Pose | None = None
 
-    self.LoC = LongControl(self.CP, self.CP_SP)
+    self.LoC = LongControl(self.CP)
     self.VM = VehicleModel(self.CP)
     self.LaC: LatControl
     if self.CP.steerControlType == car.CarParams.SteerControlType.angle:

selfdrive/controls/lib/latcontrol_torque.py

@@ -48,10 +48,6 @@ class LatControlTorque(LatControl):
                         self.lateral_accel_from_torque(-self.steer_max, self.torque_params))
 
   def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited):
-    # Override torque params from extension
-    if self.extension.update_override_torque_params(self.torque_params):
-      self.update_limits()
-
     pid_log = log.ControlsState.LateralTorqueState.new_message()
     if not active:
       output_torque = 0.0

selfdrive/controls/lib/longcontrol.py

@@ -10,11 +10,8 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
 
-def long_control_state_trans(CP, CP_SP, active, long_control_state, v_ego,
+def long_control_state_trans(CP, active, long_control_state, v_ego,
                              should_stop, brake_pressed, cruise_standstill):
-  # Gas Interceptor
-  cruise_standstill = cruise_standstill and not CP_SP.enableGasInterceptor
-
   stopping_condition = should_stop
   starting_condition = (not should_stop and
                         not cruise_standstill and
@@ -48,9 +45,8 @@ def long_control_state_trans(CP, CP_SP, active, long_control_state, v_ego,
   return long_control_state
 
 class LongControl:
-  def __init__(self, CP, CP_SP):
+  def __init__(self, CP):
     self.CP = CP
-    self.CP_SP = CP_SP
     self.long_control_state = LongCtrlState.off
     self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
                              (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
@@ -65,7 +61,7 @@ class LongControl:
     self.pid.neg_limit = accel_limits[0]
     self.pid.pos_limit = accel_limits[1]
 
-    self.long_control_state = long_control_state_trans(self.CP, self.CP_SP, active, self.long_control_state, CS.vEgo,
+    self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo,
                                                        should_stop, CS.brakePressed,
                                                        CS.cruiseState.standstill)
     if self.long_control_state == LongCtrlState.off:

selfdrive/controls/plannerd.py

@@ -24,11 +24,10 @@ def main():
   pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
   sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
                             'liveMapDataSP', 'carStateSP', gps_location_service],
-                           poll='carState')
+                           poll='modelV2')
 
   while True:
     sm.update()
-    longitudinal_planner.sla.update_car_state(sm['carState'])
     if sm.updated['modelV2']:
       longitudinal_planner.update(sm)
       longitudinal_planner.publish(sm, pm)

selfdrive/controls/tests/test_longcontrol.py

@@ -1,4 +1,4 @@
-from cereal import car, custom
+from cereal import car
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans
 
 
@@ -8,52 +8,49 @@ class TestLongControlStateTransition:
 
   def test_stay_stopped(self):
     CP = car.CarParams.new_message()
-    CP_SP = custom.CarParamsSP.new_message()
     active = True
     current_state = LongCtrlState.stopping
-    next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
+    next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
                              should_stop=True, brake_pressed=False, cruise_standstill=False)
     assert next_state == LongCtrlState.stopping
-    next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
+    next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
                              should_stop=False, brake_pressed=True, cruise_standstill=False)
     assert next_state == LongCtrlState.stopping
-    next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
+    next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
                              should_stop=False, brake_pressed=False, cruise_standstill=True)
     assert next_state == LongCtrlState.stopping
-    next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0,
+    next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0,
                              should_stop=False, brake_pressed=False, cruise_standstill=False)
     assert next_state == LongCtrlState.pid
     active = False
-    next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0,
+    next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0,
                              should_stop=False, brake_pressed=False, cruise_standstill=False)
     assert next_state == LongCtrlState.off
 
 def test_engage():
   CP = car.CarParams.new_message()
-  CP_SP = custom.CarParamsSP.new_message()
   active = True
   current_state = LongCtrlState.off
-  next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
+  next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
                              should_stop=True, brake_pressed=False, cruise_standstill=False)
   assert next_state == LongCtrlState.stopping
-  next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
+  next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
                              should_stop=False, brake_pressed=True, cruise_standstill=False)
   assert next_state == LongCtrlState.stopping
-  next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
+  next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
                              should_stop=False, brake_pressed=False, cruise_standstill=True)
   assert next_state == LongCtrlState.stopping
-  next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
+  next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
                              should_stop=False, brake_pressed=False, cruise_standstill=False)
   assert next_state == LongCtrlState.pid
 
 def test_starting():
   CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
-  CP_SP = custom.CarParamsSP.new_message()
   active = True
   current_state = LongCtrlState.starting
-  next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
+  next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
                              should_stop=False, brake_pressed=False, cruise_standstill=False)
   assert next_state == LongCtrlState.starting
-  next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0,
+  next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0,
                              should_stop=False, brake_pressed=False, cruise_standstill=False)
   assert next_state == LongCtrlState.pid

selfdrive/locationd/torqued.py

@@ -11,7 +11,6 @@ from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
-from openpilot.sunnypilot.selfdrive.locationd.torqued_ext import TorqueEstimatorExt
 
 HISTORY = 5  # secs
 POINTS_PER_BUCKET = 1500
@@ -51,10 +50,9 @@ class TorqueBuckets(PointBuckets):
         break
 
 
-class TorqueEstimator(ParameterEstimator, TorqueEstimatorExt):
+class TorqueEstimator(ParameterEstimator):
   def __init__(self, CP, decimated=False, track_all_points=False):
-    ParameterEstimator.__init__(self)
-    TorqueEstimatorExt.__init__(self, CP)
+    super().__init__()
     self.CP = CP
     self.hist_len = int(HISTORY / DT_MDL)
     self.lag = 0.0
@@ -84,8 +82,6 @@ class TorqueEstimator(ParameterEstimator, TorqueEstimatorExt):
 
     self.calibrator = PoseCalibrator()
 
-    TorqueEstimatorExt.initialize_custom_params(self, decimated)
-
     self.reset()
 
     initial_params = {
@@ -264,8 +260,6 @@ def main(demo=False):
           t = sm.logMonoTime[which] * 1e-9
           estimator.handle_log(t, which, sm[which])
 
-    TorqueEstimatorExt.update_use_params(estimator)
-
     # 4Hz driven by livePose
     if sm.frame % 5 == 0:
       pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks()))

selfdrive/selfdrived/selfdrived.py

@@ -95,9 +95,8 @@ class SelfdriveD(CruiseHelper):
     self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
                                    'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
-                                   'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback',
-                                   'modelDataV2SP', 'longitudinalPlanSP'] + \
-                                   self.camera_packets + self.sensor_packets + self.gps_packets,
+                                   'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback'] + \
+                                   self.camera_packets + self.sensor_packets + self.gps_packets + ['modelDataV2SP', 'longitudinalPlanSP'],
                                   ignore_alive=ignore, ignore_avg_freq=ignore,
                                   ignore_valid=ignore, frequency=int(1/DT_CTRL))
 
@@ -159,7 +158,7 @@ class SelfdriveD(CruiseHelper):
     self.mads = ModularAssistiveDrivingSystem(self)
     self.icbm = IntelligentCruiseButtonManagement(self.CP, self.CP_SP)
 
-    self.car_events_sp = CarSpecificEventsSP(self.CP, self.CP_SP)
+    self.car_events_sp = CarSpecificEventsSP(self.CP, self.params)
 
     CruiseHelper.__init__(self, self.CP)
 
@@ -445,7 +444,7 @@ class SelfdriveD(CruiseHelper):
           self.events.add(EventName.personalityChanged)
         self.experimental_mode_switched = False
 
-    self.icbm.run(CS, self.sm['carControl'], self.sm['longitudinalPlanSP'], self.is_metric)
+    self.icbm.run(CS, self.sm['carControl'], self.is_metric)
 
   def data_sample(self):
     _car_state = messaging.recv_one(self.car_state_sock)

selfdrive/test/process_replay/migration.py

@@ -28,8 +28,7 @@ MigrationFunc = Callable[[list[MessageWithIndex]], MigrationOps]
 # 3. product is the message type created by the migration function, and the function will be skipped if product type already exists in lr
 # 4. it must return a list of operations to be applied to the logreader (replace, add, delete)
 # 5. all migration functions must be independent of each other
-def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: bool = False, camera_states: bool = False,
-                live_location_kalman: bool = True):
+def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: bool = False, camera_states: bool = False):
   migrations = [
     migrate_sensorEvents,
     migrate_carParams,
@@ -38,6 +37,7 @@ def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: boo
     migrate_carOutput,
     migrate_controlsState,
     migrate_carState,
+    migrate_liveLocationKalman,
     migrate_liveTracks,
     migrate_driverAssistance,
     migrate_drivingModelData,
@@ -51,8 +51,6 @@ def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: boo
     migrations.extend([migrate_pandaStates, migrate_peripheralState])
   if camera_states:
     migrations.append(migrate_cameraStates)
-  if live_location_kalman:
-    migrations.append(migrate_liveLocationKalman)
 
   return migrate(lr, migrations)
 

selfdrive/test/process_replay/process_replay.py

@@ -496,7 +496,7 @@ CONFIGS = [
     pubs=[
       "cameraOdometry", "accelerometer", "gyroscope", "liveCalibration", "carState"
     ],
-    subs=["liveLocationKalman", "livePose"],
+    subs=["livePose"],
     ignore=["logMonoTime"],
     should_recv_callback=MessageBasedRcvCallback("cameraOdometry"),
     tolerance=NUMPY_TOLERANCE,

selfdrive/ui/qt/onroad/hud.cc

@@ -47,11 +47,10 @@ void HudRenderer::draw(QPainter &p, const QRect &surface_rect) {
   bg.setColorAt(1, QColor::fromRgbF(0, 0, 0, 0));
   p.fillRect(0, 0, surface_rect.width(), UI_HEADER_HEIGHT, bg);
 
-#ifndef SUNNYPILOT
+
   if (is_cruise_available) {
     drawSetSpeed(p, surface_rect);
   }
-#endif
   drawCurrentSpeed(p, surface_rect);
 
   p.restore();

selfdrive/ui/sunnypilot/SConscript

@@ -24,7 +24,6 @@ qt_src = [
   "sunnypilot/qt/offroad/offroad_home.cc",
   "sunnypilot/qt/offroad/settings/developer_panel.cc",
   "sunnypilot/qt/offroad/settings/device_panel.cc",
-  "sunnypilot/qt/offroad/settings/display_panel.cc",
   "sunnypilot/qt/offroad/settings/lateral_panel.cc",
   "sunnypilot/qt/offroad/settings/longitudinal_panel.cc",
   "sunnypilot/qt/offroad/settings/max_time_offroad.cc",
@@ -51,8 +50,6 @@ lateral_panel_qt_src = [
   "sunnypilot/qt/offroad/settings/lateral/lane_change_settings.cc",
   "sunnypilot/qt/offroad/settings/lateral/mads_settings.cc",
   "sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc",
-  "sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.cc",
-  "sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.cc",
 ]
 
 longitudinal_panel_qt_src = [
@@ -93,13 +90,9 @@ brand_settings_qt_src = [
   "sunnypilot/qt/offroad/settings/vehicle/volkswagen_settings.cc",
 ]
 
-display_panel_qt_src = [
-  "sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.cc",
-]
 
 sp_widgets_src = widgets_src + network_src
-sp_qt_src = qt_src + lateral_panel_qt_src + vehicle_panel_qt_src + brand_settings_qt_src + \
-            longitudinal_panel_qt_src + osm_panel_qt_src + display_panel_qt_src
+sp_qt_src = qt_src + lateral_panel_qt_src + vehicle_panel_qt_src + brand_settings_qt_src + longitudinal_panel_qt_src + osm_panel_qt_src
 sp_qt_util = qt_util
 
 Export('sp_widgets_src', 'sp_qt_src', "sp_qt_util")

selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.cc

@@ -1,63 +0,0 @@
-/**
- * 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/offroad/settings/display/onroad_screen_brightness.h"
-
-OnroadScreenBrightnessControl::OnroadScreenBrightnessControl(const QString &param, const QString &title,
-                                                             const QString &description, const QString &icon,
-                                                             QWidget *parent)
-  : ExpandableToggleRow(param, title, description, icon, parent) {
-  auto *mainFrame = new QFrame(this);
-  auto *mainFrameLayout = new QGridLayout();
-  mainFrame->setLayout(mainFrameLayout);
-  mainFrameLayout->setSpacing(0);
-
-  onroadScreenOffTimer = new OptionControlSP(
-    "OnroadScreenOffTimer",
-    "",
-    "",
-    "",
-    {0, 11}, 1, true, &onroadScreenOffTimerOptions);
-
-  onroadScreenBrightness = new OptionControlSP(
-    "OnroadScreenOffBrightness",
-    "",
-    "",
-    "",
-    {0, 100}, 10, true, nullptr, false);
-
-  connect(onroadScreenOffTimer, &OptionControlSP::updateLabels, this, &OnroadScreenBrightnessControl::refresh);
-  connect(onroadScreenBrightness, &OptionControlSP::updateLabels, this, &OnroadScreenBrightnessControl::refresh);
-  onroadScreenOffTimer->setFixedWidth(280);
-  onroadScreenBrightness->setFixedWidth(280);
-  mainFrameLayout->addWidget(onroadScreenOffTimer, 0, 0, Qt::AlignLeft);
-  mainFrameLayout->addWidget(onroadScreenBrightness, 0, 1, Qt::AlignRight);
-
-  addItem(mainFrame);
-
-  refresh();
-}
-
-void OnroadScreenBrightnessControl::refresh() {
-  // Driving Screen Off Timer
-  int valTimer = std::atoi(params.get("OnroadScreenOffTimer").c_str());
-  std::string labelTimer = "<span style='font-size: 45px; font-weight: 450; color: #FFFFFF;'>";
-  labelTimer += "Delay";
-  labelTimer += " <br><span style='font-size: 40px; font-weight: 450; color:rgb(174, 255, 195);'>";
-  labelTimer += (valTimer < 60 ? std::to_string(valTimer) + "s" : std::to_string(valTimer / 60) + "m");
-  labelTimer += "</span></span>";
-  onroadScreenOffTimer->setLabel(QString::fromStdString(labelTimer));
-
-  // Driving Screen Off Brightness
-  std::string valBrightness = params.get("OnroadScreenOffBrightness");
-  std::string labelBrightness = "<span style='font-size: 45px; font-weight: 450; color: #FFFFFF;'>";
-  labelBrightness += "Brightness";
-  labelBrightness += " <br><span style='font-size: 40px; font-weight: 450; color:rgb(174, 255, 195);'>";
-  labelBrightness += (valBrightness == "0" ? " Screen Off" : valBrightness + "%");
-  labelBrightness += "</span></span>";
-  onroadScreenBrightness->setLabel(QString::fromStdString(labelBrightness));
-}

selfdrive/ui/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.h

@@ -1,42 +0,0 @@
-/**
- * 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/sunnypilot/ui.h"
-#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
-#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
-#include "selfdrive/ui/sunnypilot/qt/widgets/expandable_row.h"
-
-static const QMap<QString, QString> onroadScreenOffTimerOptions = {
-  {"0", "15"},
-  {"1", "30"},
-  {"2", "60"},
-  {"3", "120"},
-  {"4", "180"},
-  {"5", "240"},
-  {"6", "300"},
-  {"7", "360"},
-  {"8", "420"},
-  {"9", "480"},
-  {"10", "540"},
-  {"11", "600"}
-};
-
-class OnroadScreenBrightnessControl : public ExpandableToggleRow {
-  Q_OBJECT
-
-public:
-  OnroadScreenBrightnessControl(const QString &param, const QString &title, const QString &desc, const QString &icon,
-                                QWidget *parent = nullptr);
-  void refresh();
-
-private:
-  Params params;
-  OptionControlSP *onroadScreenOffTimer;
-  OptionControlSP *onroadScreenBrightness;
-};

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

@@ -1,40 +0,0 @@
-/**
- * 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/offroad/settings/display_panel.h"
-
-DisplayPanel::DisplayPanel(QWidget *parent) : QWidget(parent) {
-  main_layout = new QStackedLayout(this);
-  ListWidgetSP *list = new ListWidgetSP(this, false);
-
-  sunnypilotScreen = new QWidget(this);
-  QVBoxLayout* vlayout = new QVBoxLayout(sunnypilotScreen);
-  vlayout->setContentsMargins(50, 20, 50, 20);
-
-  // Onroad Screen Off/Brightness
-  onroadScreenBrightnessControl = new OnroadScreenBrightnessControl(
-    "OnroadScreenOffControl",
-    tr("Driving Screen Off: Non-Critical Events"),
-    tr("Turn off device screen or reduce brightness after driving starts. "
-       "It automatically brightens again when screen is touched or a critical event occurs."),
-    "",
-    this);
-  list->addItem(onroadScreenBrightnessControl);
-
-  sunnypilotScroller = new ScrollViewSP(list, this);
-  vlayout->addWidget(sunnypilotScroller);
-  main_layout->addWidget(sunnypilotScreen);
-}
-
-void DisplayPanel::showEvent(QShowEvent *event) {
-  QWidget::showEvent(event);
-  refresh();
-}
-
-void DisplayPanel::refresh() {
-  onroadScreenBrightnessControl->refresh();
-}

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

@@ -1,28 +0,0 @@
-/**
- * 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/sunnypilot/qt/offroad/settings/display/onroad_screen_brightness.h"
-#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
-#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
-
-class DisplayPanel : public QWidget {
-  Q_OBJECT
-
-public:
-  explicit DisplayPanel(QWidget *parent = nullptr);
-  void showEvent(QShowEvent *event) override;
-  void refresh();
-
-private:
-  QStackedLayout* main_layout = nullptr;
-  QWidget* sunnypilotScreen = nullptr;
-  ScrollViewSP *sunnypilotScroller = nullptr;
-  Params params;
-  OnroadScreenBrightnessControl *onroadScreenBrightnessControl = nullptr;
-};

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc

@@ -52,16 +52,12 @@ void MadsSettings::updateToggles(bool _offroad) {
   );
 
   auto cp_bytes = params.get("CarParamsPersistent");
-  auto cp_sp_bytes = params.get("CarParamsSPPersistent");
-  if (!cp_bytes.empty() && !cp_sp_bytes.empty()) {
+  if (!cp_bytes.empty()) {
     AlignedBuffer aligned_buf;
-    AlignedBuffer aligned_buf_sp;
     capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size()));
-    capnp::FlatArrayMessageReader cmsg_sp(aligned_buf_sp.align(cp_sp_bytes.data(), cp_sp_bytes.size()));
     cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();
-    cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot<cereal::CarParamsSP>();
 
-    if (madsLimitedSettings(CP, CP_SP)) {
+    if (isBrandInList(CP.getBrand(), mads_limited_settings_brands)) {
       params.remove("MadsMainCruiseAllowed");
       params.putBool("MadsUnifiedEngagementMode", true);
       params.put("MadsSteeringMode", std::to_string(static_cast<int>(MadsSteeringMode::DISENGAGE)));

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h

@@ -12,16 +12,7 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
 
-inline bool madsLimitedSettings(const cereal::CarParams::Reader &CP, const cereal::CarParamsSP::Reader &CP_SP) {
-  if (CP.getBrand() == "rivian") {
-    return true;
-  }
-  if (CP.getBrand() == "tesla") {
-    return !(CP_SP.getFlags() & 1);  // 1 == TeslaFlagsSP.HAS_VEHICLE_BUS
-  }
-
-  return false;
-}
+const std::vector<std::string> mads_limited_settings_brands = {"rivian", "tesla"};
 
 enum class MadsSteeringMode {
   REMAIN_ACTIVE = 0,

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc

@@ -10,15 +10,14 @@
 NeuralNetworkLateralControl::NeuralNetworkLateralControl() :
   ParamControl("NeuralNetworkLateralControl", tr("Neural Network Lateral Control (NNLC)"),  "", "") {
   setConfirmation(true, false);
-  updateToggle(offroad);
+  updateToggle();
 }
 
-void NeuralNetworkLateralControl::updateToggle(bool _offroad) {
+void NeuralNetworkLateralControl::updateToggle() {
   QString statusInitText = "<font color='yellow'>" + STATUS_CHECK_COMPATIBILITY + "</font>";
   QString notLoadedText = "<font color='yellow'>" + STATUS_NOT_LOADED + "</font>";
   QString loadedText = "<font color=#00ff00>" + STATUS_LOADED + "</font>";
 
-  bool allowed = true;
   auto cp_bytes = params.get("CarParamsPersistent");
   auto cp_sp_bytes = params.get("CarParamsSPPersistent");
   if (!cp_bytes.empty() && !cp_sp_bytes.empty()) {
@@ -32,7 +31,7 @@ void NeuralNetworkLateralControl::updateToggle(bool _offroad) {
     if (CP.getSteerControlType() == cereal::CarParams::SteerControlType::ANGLE) {
       params.remove("NeuralNetworkLateralControl");
       setDescription(nnffDescriptionBuilder(STATUS_NOT_AVAILABLE));
-      allowed = false;
+      setEnabled(false);
     } else {
       QString nn_model_name = QString::fromStdString(CP_SP.getNeuralNetworkLateralControl().getModel().getName());
       QString nn_fuzzy = CP_SP.getNeuralNetworkLateralControl().getFuzzyFingerprint() ?
@@ -57,11 +56,4 @@ void NeuralNetworkLateralControl::updateToggle(bool _offroad) {
   if (getDescription() != getBaseDescription()) {
     showDescription();
   }
-
-  bool enforce_torque_toggle = params.getBool("EnforceTorqueControl");
-  setEnabled(_offroad && allowed && !enforce_torque_toggle);
-
-  refresh();
-
-  offroad = _offroad;
 }

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h

@@ -20,11 +20,10 @@ public:
   NeuralNetworkLateralControl();
 
 public slots:
-  void updateToggle(bool _offroad);
+  void updateToggle();
 
 private:
   Params params;
-  bool offroad;
 
   // Status messages
   const QString STATUS_NOT_AVAILABLE = tr("NNLC is currently not available on this platform.");

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.cc

@@ -1,58 +0,0 @@
-/**
- * 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/offroad/settings/lateral/torque_lateral_control_custom_params.h"
-
-TorqueLateralControlCustomParams::TorqueLateralControlCustomParams(const QString &param, const QString &title, const QString &description, const QString &icon, QWidget *parent)
-    : ExpandableToggleRow(param, title, description, icon, parent) {
-
-  QFrame *frame = new QFrame(this);
-  QGridLayout *frame_layout = new QGridLayout();
-  frame->setLayout(frame_layout);
-  frame_layout->setSpacing(0);
-
-  torqueLateralControlParamsOverride = new ParamControl(
-    "TorqueParamsOverrideEnabled",
-    tr("Manual Real-Time Tuning"),
-    tr("Enforces the torque lateral controller to use the fixed values instead of the learned values from Self-Tune. Enabling this toggle overrides Self-Tune values."),
-    "../assets/offroad/icon_blank.png",
-    this
-  );
-  connect(torqueLateralControlParamsOverride, &ParamControl::toggleFlipped, this, &TorqueLateralControlCustomParams::refresh);
-
-  torqueParamsOverrideLatAccelFactor = new OptionControlSP("TorqueParamsOverrideLatAccelFactor", tr("Lateral Acceleration Factor"), "", "", {1, 500}, 1, false, nullptr, true, false);
-  connect(torqueParamsOverrideLatAccelFactor, &OptionControlSP::updateLabels, this, &TorqueLateralControlCustomParams::refresh);
-  torqueParamsOverrideLatAccelFactor->setFixedWidth(280);
-
-  torqueParamsOverrideFriction = new OptionControlSP("TorqueParamsOverrideFriction", tr("Friction"), "", "", {1, 100}, 1, false, nullptr, true, false);
-  connect(torqueParamsOverrideFriction, &OptionControlSP::updateLabels, this, &TorqueLateralControlCustomParams::refresh);
-  torqueParamsOverrideFriction->setFixedWidth(280);
-
-  frame_layout->addWidget(torqueLateralControlParamsOverride, 0, 0, 1, 2);
-  QSpacerItem *spacer = new QSpacerItem(20, 40);
-  frame_layout->addItem(spacer, 1, 0, 1, 2);
-  frame_layout->addWidget(torqueParamsOverrideLatAccelFactor, 2, 0, Qt::AlignCenter);
-  frame_layout->addWidget(torqueParamsOverrideFriction, 2, 1, Qt::AlignCenter);
-
-  addItem(frame);
-
-  refresh();
-}
-
-void TorqueLateralControlCustomParams::refresh() {
-  bool torque_override_param = params.getBool("TorqueParamsOverrideEnabled");
-  float laf_param = QString::fromStdString(params.get("TorqueParamsOverrideLatAccelFactor")).toFloat();
-  const QString laf_unit = "m/s²";
-
-  float friction_param = QString::fromStdString(params.get("TorqueParamsOverrideFriction")).toFloat();
-
-  torqueParamsOverrideLatAccelFactor->setTitle(tr("Lateral Acceleration Factor") + "\n(" + (torque_override_param ? tr("Real-time and Offline") : tr("Offline Only")) + ")");
-  torqueParamsOverrideFriction->setTitle(tr("Friction") + "\n(" + (torque_override_param ? tr("Real-time and Offline") : tr("Offline Only")) + ")");
-
-  torqueParamsOverrideLatAccelFactor->setLabel(QString::number(laf_param, 'f', 2) + " " + laf_unit);
-  torqueParamsOverrideFriction->setLabel(QString::number(friction_param, 'f', 2));
-}

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.h

@@ -1,27 +0,0 @@
-/**
- * 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/sunnypilot/ui.h"
-#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
-#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
-#include "selfdrive/ui/sunnypilot/qt/widgets/expandable_row.h"
-
-class TorqueLateralControlCustomParams : public ExpandableToggleRow {
-  Q_OBJECT
-
-public:
-  TorqueLateralControlCustomParams(const QString &param, const QString &title, const QString &description, const QString &icon, QWidget *parent = nullptr);
-  void refresh();
-
-private:
-  Params params;
-  ParamControl *torqueLateralControlParamsOverride;
-  OptionControlSP *torqueParamsOverrideFriction;
-  OptionControlSP *torqueParamsOverrideLatAccelFactor;
-};

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.cc

@@ -1,81 +0,0 @@
-/**
- * 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/offroad/settings/lateral/torque_lateral_control_settings.h"
-
-#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
-
-TorqueLateralControlSettings::TorqueLateralControlSettings(QWidget *parent) : QWidget(parent) {
-  QVBoxLayout *main_layout = new QVBoxLayout(this);
-  main_layout->setContentsMargins(50, 20, 50, 20);
-  main_layout->setSpacing(20);
-
-  // Back button
-  PanelBackButton *back = new PanelBackButton();
-  connect(back, &QPushButton::clicked, [=]() { emit backPress(); });
-  main_layout->addWidget(back, 0, Qt::AlignLeft);
-
-  ListWidget *list = new ListWidget(this, false);
-  // param, title, desc, icon
-  std::vector<std::tuple<QString, QString, QString, QString>> toggle_defs{
-    {
-      "LiveTorqueParamsToggle",
-      tr("Self-Tune"),
-      tr("Enables self-tune for Torque lateral control for platforms that do not use Torque lateral control by default."),
-      "../assets/offroad/icon_blank.png",
-    },
-    {
-      "LiveTorqueParamsRelaxedToggle",
-      tr("Less Restrict Settings for Self-Tune (Beta)"),
-      tr("Less strict settings when using Self-Tune. This allows torqued to be more forgiving when learning values."),
-      "../assets/offroad/icon_blank.png",
-    }
-  };
-
-  for (auto &[param, title, desc, icon] : toggle_defs) {
-    auto toggle = new ParamControlSP(param, title, desc, icon, this);
-    list->addItem(toggle);
-    toggles[param.toStdString()] = toggle;
-  }
-
-  torqueLateralControlCustomParams = new TorqueLateralControlCustomParams(
-    "CustomTorqueParams",
-    tr("Enable Custom Tuning"),
-    tr("Enables custom tuning for Torque lateral control. Modifying Lateral Acceleration Factor and Friction below will override the offline values indicated in the YAML files within \"opendbc/car/torque_data\". "
-       "The values will also be used live when \"Manual Real-Time Tuning\" toggle is enabled."),
-    "../assets/offroad/icon_blank.png",
-    this);
-  list->addItem(torqueLateralControlCustomParams);
-
-  QObject::connect(uiState(), &UIState::offroadTransition, this, &TorqueLateralControlSettings::updateToggles);
-  QObject::connect(toggles["LiveTorqueParamsToggle"], &ParamControlSP::toggleFlipped, [=](bool state) {
-    if (!state) {
-      params.remove("LiveTorqueParamsRelaxedToggle");
-      toggles["LiveTorqueParamsRelaxedToggle"]->refresh();
-    }
-
-    updateToggles(offroad);
-  });
-
-  main_layout->addWidget(new ScrollViewSP(list, this));
-}
-
-void TorqueLateralControlSettings::showEvent(QShowEvent *event) {
-  updateToggles(offroad);
-}
-
-void TorqueLateralControlSettings::updateToggles(bool _offroad) {
-  bool live_toggle = toggles["LiveTorqueParamsToggle"]->isToggled();
-
-  toggles["LiveTorqueParamsToggle"]->setEnabled(_offroad);
-  toggles["LiveTorqueParamsRelaxedToggle"]->setEnabled(_offroad && live_toggle);
-
-  torqueLateralControlCustomParams->setEnabled(_offroad);
-  torqueLateralControlCustomParams->refresh();
-
-  offroad = _offroad;
-}

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.h

@@ -1,36 +0,0 @@
-/**
- * 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/util.h"
-#include "selfdrive/ui/sunnypilot/ui.h"
-#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_custom_params.h"
-#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
-#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
-
-class TorqueLateralControlSettings : public QWidget {
-  Q_OBJECT
-
-public:
-  explicit TorqueLateralControlSettings(QWidget *parent = nullptr);
-
-  void showEvent(QShowEvent *event) override;
-
-  signals:
-    void backPress();
-
-public slots:
-  void updateToggles(bool _offroad);
-
-private:
-  Params params;
-  bool offroad;
-  std::map<std::string, ParamControlSP*> toggles;
-
-  TorqueLateralControlCustomParams *torqueLateralControlCustomParams;
-};

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

@@ -75,36 +75,6 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
 
   list->addItem(horizontal_line());
 
-  // Customized Torque Lateral Control
-  torqueLateralControlToggle = new ParamControl(
-    "EnforceTorqueControl",
-    tr("Enforce Torque Lateral Control"),
-    tr("Enable this to enforce sunnypilot to steer with Torque lateral control."),
-    "");
-  list->addItem(torqueLateralControlToggle);
-
-  torqueLateralControlSettingsButton = new PushButtonSP(tr("Customize Params"));
-  torqueLateralControlSettingsButton->setObjectName("torque_btn");
-  connect(torqueLateralControlSettingsButton, &QPushButton::clicked, [=]() {
-    sunnypilotScroller->setLastScrollPosition();
-    main_layout->setCurrentWidget(torqueLateralControlWidget);
-  });
-  QObject::connect(torqueLateralControlToggle, &ToggleControl::toggleFlipped, [=](bool state) {
-    torqueLateralControlSettingsButton->setEnabled(state);
-    nnlcToggle->updateToggle(offroad);
-    updateToggles(offroad);
-  });
-
-  torqueLateralControlWidget = new TorqueLateralControlSettings(this);
-  connect(torqueLateralControlWidget, &TorqueLateralControlSettings::backPress, [=]() {
-    sunnypilotScroller->restoreScrollPosition();
-    main_layout->setCurrentWidget(sunnypilotScreen);
-  });
-  list->addItem(torqueLateralControlSettingsButton);
-
-  list->addItem(vertical_space(0));
-  list->addItem(horizontal_line());
-
   // Neural Network Lateral Control
   nnlcToggle = new NeuralNetworkLateralControl();
   list->addItem(nnlcToggle);
@@ -116,10 +86,12 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
       nnlcToggle->hideDescription();
     }
 
-    nnlcToggle->updateToggle(offroad);
-    updateToggles(offroad);
+    nnlcToggle->updateToggle();
   });
 
+  toggleOffroadOnly = {
+    madsToggle, nnlcToggle,
+  };
   QObject::connect(uiState(), &UIState::offroadTransition, this, &LateralPanel::updateToggles);
 
   sunnypilotScroller = new ScrollViewSP(list, this);
@@ -128,7 +100,6 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
   main_layout->addWidget(sunnypilotScreen);
   main_layout->addWidget(madsWidget);
   main_layout->addWidget(laneChangeWidget);
-  main_layout->addWidget(torqueLateralControlWidget);
 
   setStyleSheet(R"(
     #back_btn {
@@ -149,7 +120,7 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
 }
 
 void LateralPanel::showEvent(QShowEvent *event) {
-  nnlcToggle->updateToggle(offroad);
+  nnlcToggle->updateToggle();
   updateToggles(offroad);
 }
 
@@ -158,40 +129,27 @@ void LateralPanel::hideEvent(QHideEvent *event) {
 }
 
 void LateralPanel::updateToggles(bool _offroad) {
-  bool torque_allowed = true;
-  auto cp_bytes = params.get("CarParamsPersistent");
-  auto cp_sp_bytes = params.get("CarParamsSPPersistent");
-  if (!cp_bytes.empty() && !cp_sp_bytes.empty()) {
-    AlignedBuffer aligned_buf;
-    AlignedBuffer aligned_buf_sp;
-    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size()));
-    capnp::FlatArrayMessageReader cmsg_sp(aligned_buf_sp.align(cp_sp_bytes.data(), cp_sp_bytes.size()));
-    cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();
-    cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot<cereal::CarParamsSP>();
+  for (auto *toggle : toggleOffroadOnly) {
+    toggle->setEnabled(_offroad);
+  }
 
-    if (madsLimitedSettings(CP, CP_SP)) {
+  auto cp_bytes = params.get("CarParamsPersistent");
+  if (!cp_bytes.empty()) {
+    AlignedBuffer aligned_buf;
+    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size()));
+    cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();
+
+    if (isBrandInList(CP.getBrand(), mads_limited_settings_brands)) {
       madsToggle->setDescription(descriptionBuilder(STATUS_MADS_SETTINGS_LIMITED_COMPATIBILITY, MADS_BASE_DESC));
     } else {
       madsToggle->setDescription(descriptionBuilder(STATUS_MADS_SETTINGS_FULL_COMPATIBILITY, MADS_BASE_DESC));
     }
-
-    if (CP.getSteerControlType() == cereal::CarParams::SteerControlType::ANGLE) {
-      params.remove("EnforceTorqueControl");
-      torque_allowed = false;
-    }
   } else {
     madsToggle->setDescription(descriptionBuilder(STATUS_MADS_CHECK_COMPATIBILITY, MADS_BASE_DESC));
-
-    params.remove("EnforceTorqueControl");
-    torque_allowed = false;
   }
 
-  madsToggle->setEnabled(_offroad);
   madsSettingsButton->setEnabled(madsToggle->isToggled());
 
-  torqueLateralControlToggle->setEnabled(_offroad && torque_allowed && !nnlcToggle->isToggled());
-  torqueLateralControlSettingsButton->setEnabled(torqueLateralControlToggle->isToggled());
-
   blinkerPauseLateralSettings->refresh();
 
   offroad = _offroad;

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

@@ -15,7 +15,6 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.h"
-#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/torque_lateral_control_settings.h"
 #include "selfdrive/ui/qt/util.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
@@ -36,6 +35,7 @@ private:
   QStackedLayout* main_layout = nullptr;
   QWidget* sunnypilotScreen = nullptr;
   ScrollViewSP *sunnypilotScroller = nullptr;
+  std::vector<ParamControl *> toggleOffroadOnly;
   bool offroad;
 
   ParamControl *madsToggle;
@@ -45,9 +45,6 @@ private:
   LaneChangeSettings *laneChangeWidget = nullptr;
   NeuralNetworkLateralControl *nnlcToggle = nullptr;
   BlinkerPauseLateralSettings *blinkerPauseLateralSettings = nullptr;
-  ParamControl *torqueLateralControlToggle;
-  PushButtonSP *torqueLateralControlSettingsButton;
-  TorqueLateralControlSettings *torqueLateralControlWidget = nullptr;
 
   const QString MADS_BASE_DESC = tr("Enables independent engagements of Automatic Lane Centering (ALC) and Adaptive Cruise Control (ACC).");
 

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h

@@ -6,34 +6,6 @@
  */
 #pragma once
 
-enum class SpeedLimitOffsetType {
-  NONE,
-  FIXED,
-  PERCENT,
-};
-
-inline const QString SpeedLimitOffsetTypeTexts[]{
-  QObject::tr("None"),
-  QObject::tr("Fixed"),
-  QObject::tr("Percent"),
-};
-
-enum class SpeedLimitSourcePolicy {
-  CAR_ONLY,
-  MAP_ONLY,
-  CAR_FIRST,
-  MAP_FIRST,
-  COMBINED,
-};
-
-inline const QString SpeedLimitSourcePolicyTexts[]{
-  QObject::tr("Car\nOnly"),
-  QObject::tr("Map\nOnly"),
-  QObject::tr("Car\nFirst"),
-  QObject::tr("Map\nFirst"),
-  QObject::tr("Combined\nData")
-};
-
 enum class SpeedLimitMode {
   OFF,
   INFORMATION,
@@ -41,9 +13,37 @@ enum class SpeedLimitMode {
   ASSIST,
 };
 
-inline const QString SpeedLimitModeTexts[]{
-  QObject::tr("Off"),
-  QObject::tr("Information"),
-  QObject::tr("Warning"),
-  QObject::tr("Assist"),
+inline const char *SpeedLimitModeTexts[]{
+  QT_TR_NOOP("Off"),
+  QT_TR_NOOP("Information"),
+  QT_TR_NOOP("Warning"),
+  QT_TR_NOOP("Assist"),
+};
+
+enum class SpeedLimitOffsetType {
+  NONE,
+  FIXED,
+  PERCENT,
+};
+
+inline const char *SpeedLimitOffsetTypeTexts[]{
+  QT_TR_NOOP("None"),
+  QT_TR_NOOP("Fixed"),
+  QT_TR_NOOP("Percent"),
+};
+
+enum class SpeedLimitSourcePolicy {
+  CAR_ONLY,
+  MAP_ONLY,
+  CAR_FIRST,
+  MAP_FIRST,
+  COMBINED
+};
+
+inline const char *SpeedLimitSourcePolicyTexts[]{
+  QT_TR_NOOP("Car\nOnly"),
+  QT_TR_NOOP("Map\nOnly"),
+  QT_TR_NOOP("Car\nFirst"),
+  QT_TR_NOOP("Map\nFirst"),
+  QT_TR_NOOP("Combined\nData")
 };

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc

@@ -4,7 +4,6 @@
  * 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 <QScrollBar>
 
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h"
 
@@ -20,7 +19,7 @@ SpeedLimitPolicy::SpeedLimitPolicy(QWidget *parent) : QWidget(parent) {
 
   main_layout->addSpacing(10);
 
-  ListWidgetSP *list = new ListWidgetSP(this);
+  ListWidgetSP *list = new ListWidgetSP(this, true);
 
   std::vector<QString> speed_limit_policy_texts{
     SpeedLimitSourcePolicyTexts[static_cast<int>(SpeedLimitSourcePolicy::CAR_ONLY)],
@@ -39,9 +38,8 @@ SpeedLimitPolicy::SpeedLimitPolicy(QWidget *parent) : QWidget(parent) {
   list->addItem(speed_limit_policy);
   connect(speed_limit_policy, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitPolicy::refresh);
 
-  speedLimitPolicyScroller = new ScrollViewSP(list, this);
-  main_layout->addWidget(speedLimitPolicyScroller);
   refresh();
+  main_layout->addWidget(list);
 };
 
 void SpeedLimitPolicy::refresh() {
@@ -50,7 +48,6 @@ void SpeedLimitPolicy::refresh() {
 }
 
 void SpeedLimitPolicy::showEvent(QShowEvent *event) {
-  speedLimitPolicyScroller->verticalScrollBar()->setValue(0);
   refresh();
   speed_limit_policy->showDescription();
 }

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h

@@ -8,9 +8,9 @@
 #pragma once
 
 #include "selfdrive/ui/sunnypilot/ui.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
-#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
 
 class SpeedLimitPolicy : public QWidget {
   Q_OBJECT
@@ -20,13 +20,13 @@ public:
   void refresh();
   void showEvent(QShowEvent *event) override;
 
+signals:
 signals:
   void backPress();
 
 private:
   Params params;
   ButtonParamControlSP *speed_limit_policy;
-  ScrollViewSP *speedLimitPolicyScroller;
 
   static QString sourceDescription(SpeedLimitSourcePolicy type = SpeedLimitSourcePolicy::CAR_ONLY) {
     QString car_only = tr("⦿ Car Only: Use Speed Limit data only from Car");

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc

@@ -20,45 +20,57 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent)
 
   subPanelLayout->addSpacing(20);
 
-  ListWidgetSP *list = new ListWidgetSP(this, false);
+  ListWidgetSP *list = new ListWidgetSP(this, true);
+
+  // Speed Limit Assist
+  // TODO-SP: OptionControlSP with Speed Limit Information/Warning as a single option
+  speedLimitAssistToggle = new ParamControlSP(
+    "SpeedLimitAssist",
+    tr("Speed Limit Assist (SLA)"),
+    tr("When you engage ACC, you will be prompted to set the cruising speed to the speed limit of the road adjusted by the Offset and Source Policy specified, or the current driving speed. "
+      "The maximum cruising speed will always be the MAX set speed."),
+    "",
+    this);
+  list->addItem(speedLimitAssistToggle);
+
+  // TODO-SP: Combine Assist/Warning/Information
+  // std::vector<QString> speed_limit_texts{
+  //   SpeedLimitModeTexts[static_cast<int>(SpeedLimitMode::OFF)],
+  //   SpeedLimitModeTexts[static_cast<int>(SpeedLimitMode::INFORMATION)],
+  //   SpeedLimitModeTexts[static_cast<int>(SpeedLimitMode::WARNING)],
+  //   SpeedLimitModeTexts[static_cast<int>(SpeedLimitMode::ASSIST)],
+  // };
+  // speed_limit_mode_settings = new ButtonParamControlSP(
+  //   "SpeedLimitMode",
+  //   tr("Speed Limit Mode"),
+  //   tr("When you engage ACC, you will be prompted to set the cruising speed to the speed limit of the road adjusted by the Offset and Source Policy specified, or the current driving speed. "
+  //     "The maximum cruising speed will always be the MAX set speed."),
+  //   "",
+  //   speed_limit_texts,
+  //   250);
+  // list->addItem(speed_limit_mode_settings);
+
+  auto *speedLimitBtnFrame = new QFrame(this);
+  auto *speedLimitBtnFrameLayout = new QGridLayout();
+  speedLimitBtnFrame->setLayout(speedLimitBtnFrameLayout);
+  speedLimitBtnFrameLayout->setContentsMargins(0, 40, 0, 40);
+  speedLimitBtnFrameLayout->setSpacing(0);
 
   speedLimitPolicyScreen = new SpeedLimitPolicy(this);
 
-  std::vector<QString> speed_limit_mode_texts{
-    SpeedLimitModeTexts[static_cast<int>(SpeedLimitMode::OFF)],
-    SpeedLimitModeTexts[static_cast<int>(SpeedLimitMode::INFORMATION)],
-    SpeedLimitModeTexts[static_cast<int>(SpeedLimitMode::WARNING)],
-    SpeedLimitModeTexts[static_cast<int>(SpeedLimitMode::ASSIST)],
-  };
-  speed_limit_mode_settings = new ButtonParamControlSP(
-    "SpeedLimitMode",
-    tr("Speed Limit"),
-    "",
-    "",
-    speed_limit_mode_texts,
-    380);
-  list->addItem(speed_limit_mode_settings);
-
-  list->addItem(horizontal_line());
-  list->addItem(vertical_space());
-
   speedLimitSource = new PushButtonSP(tr("Customize Source"));
   connect(speedLimitSource, &QPushButton::clicked, [&]() {
-    speedLimitScroller->setLastScrollPosition();
     setCurrentWidget(speedLimitPolicyScreen);
     speedLimitPolicyScreen->refresh();
   });
   connect(speedLimitPolicyScreen, &SpeedLimitPolicy::backPress, [&]() {
-    speedLimitScroller->restoreScrollPosition();
     setCurrentWidget(subPanelFrame);
     showEvent(new QShowEvent());
   });
 
   speedLimitSource->setFixedWidth(720);
-  list->addItem(speedLimitSource);
-
-  list->addItem(vertical_space(0));
-  list->addItem(horizontal_line());
+  speedLimitBtnFrameLayout->addWidget(speedLimitSource, 0, 0, Qt::AlignLeft);
+  list->addItem(speedLimitBtnFrame);
 
   QFrame *offsetFrame = new QFrame(this);
   QVBoxLayout *offsetLayout = new QVBoxLayout(offsetFrame);
@@ -89,51 +101,26 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent)
 
   list->addItem(offsetFrame);
 
-  connect(speed_limit_mode_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitSettings::refresh);
+  // connect(speed_limit_mode_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitSettings::refresh);
   connect(speed_limit_offset, &OptionControlSP::updateLabels, this, &SpeedLimitSettings::refresh);
   connect(speed_limit_offset_settings, &ButtonParamControlSP::showDescriptionEvent, speed_limit_offset, &OptionControlSP::showDescription);
   connect(speed_limit_offset_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitSettings::refresh);
 
   refresh();
-  speedLimitScroller = new ScrollViewSP(list, this);
-  subPanelLayout->addWidget(speedLimitScroller);
+  subPanelLayout->addWidget(list);
   addWidget(subPanelFrame);
   addWidget(speedLimitPolicyScreen);
   setCurrentWidget(subPanelFrame);
-}
+};
 
 void SpeedLimitSettings::refresh() {
-  bool is_metric_param = params.getBool("IsMetric");
-  SpeedLimitMode speed_limit_mode_param = static_cast<SpeedLimitMode>(std::atoi(params.get("SpeedLimitMode").c_str()));
   SpeedLimitOffsetType offset_type_param = static_cast<SpeedLimitOffsetType>(std::atoi(params.get("SpeedLimitOffsetType").c_str()));
   QString offsetLabel = QString::fromStdString(params.get("SpeedLimitValueOffset"));
 
-  bool has_longitudinal_control;
-  bool intelligent_cruise_button_management_available;
-  auto cp_bytes = params.get("CarParamsPersistent");
-  auto cp_sp_bytes = params.get("CarParamsSPPersistent");
-  if (!cp_bytes.empty() && !cp_sp_bytes.empty()) {
-    AlignedBuffer aligned_buf;
-    AlignedBuffer aligned_buf_sp;
-    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size()));
-    capnp::FlatArrayMessageReader cmsg_sp(aligned_buf_sp.align(cp_sp_bytes.data(), cp_sp_bytes.size()));
-    cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();
-    cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot<cereal::CarParamsSP>();
-
-    has_longitudinal_control = hasLongitudinalControl(CP);
-    intelligent_cruise_button_management_available = CP_SP.getIntelligentCruiseButtonManagementAvailable();
-  } else {
-    has_longitudinal_control = false;
-    intelligent_cruise_button_management_available = false;
-  }
-
-  speed_limit_mode_settings->setDescription(modeDescription(speed_limit_mode_param));
   speed_limit_offset->setDescription(offsetDescription(offset_type_param));
 
   if (offset_type_param == SpeedLimitOffsetType::PERCENT) {
     offsetLabel += "%";
-  } else if (offset_type_param == SpeedLimitOffsetType::FIXED) {
-    offsetLabel += QString(" %1").arg(is_metric_param ? "km/h" : "mph");
   }
 
   if (offset_type_param == SpeedLimitOffsetType::NONE) {
@@ -143,18 +130,9 @@ void SpeedLimitSettings::refresh() {
     speed_limit_offset->setLabel(offsetLabel);
     speed_limit_offset->showDescription();
   }
-
-  if (has_longitudinal_control || intelligent_cruise_button_management_available) {
-    speed_limit_mode_settings->setEnableSelectedButtons(true, convertSpeedLimitModeValues(getSpeedLimitModeValues()));
-  } else {
-    speed_limit_mode_settings->setEnableSelectedButtons(true, convertSpeedLimitModeValues(
-      {SpeedLimitMode::OFF,SpeedLimitMode::INFORMATION, SpeedLimitMode::WARNING}));
-  }
-
-  speed_limit_mode_settings->showDescription();
-  speed_limit_offset->showDescription();
 }
 
 void SpeedLimitSettings::showEvent(QShowEvent *event) {
   refresh();
+  speed_limit_offset->showDescription();
 }

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h

@@ -12,7 +12,6 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
-#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
 
 class SpeedLimitSettings : public QStackedWidget {
   Q_OBJECT
@@ -27,13 +26,13 @@ signals:
 
 private:
   Params params;
-  ScrollViewSP *speedLimitScroller;
   QFrame *subPanelFrame;
-  ButtonParamControlSP *speed_limit_mode_settings;
+  ParamControlSP *speedLimitAssistToggle;
+  // ButtonParamControlSP *speed_limit_mode_settings;
   PushButtonSP *speedLimitSource;
-  SpeedLimitPolicy *speedLimitPolicyScreen;
   ButtonParamControlSP *speed_limit_offset_settings;
   OptionControlSP *speed_limit_offset;
+  SpeedLimitPolicy *speedLimitPolicyScreen;
 
   static QString offsetDescription(SpeedLimitOffsetType type = SpeedLimitOffsetType::NONE) {
     QString none_str = tr("⦿ None: No Offset");
@@ -53,44 +52,4 @@ private:
         .arg(fixed_str)
         .arg(percent_str);
   }
-
-  static QString modeDescription(SpeedLimitMode mode = SpeedLimitMode::OFF) {
-    QString off_str = tr("⦿ Off: Disables the Speed Limit functions.");
-    QString info_str = tr("⦿ Information: Displays the current road's speed limit.");
-    QString warning_str = tr("⦿ Warning: Provides a warning when exceeding the current road's speed limit.");
-    QString assist_str = tr("⦿ Assist: Adjusts the vehicle's cruise speed based on the current road's speed limit when operating the +/- buttons.");
-
-    if (mode == SpeedLimitMode::ASSIST) {
-      assist_str = "<font color='white'><b>" + assist_str + "</b></font>";
-    } else if (mode == SpeedLimitMode::WARNING) {
-      warning_str = "<font color='white'><b>" + warning_str + "</b></font>";
-    } else if (mode == SpeedLimitMode::INFORMATION) {
-      info_str = "<font color='white'><b>" + info_str + "</b></font>";
-    } else {
-      off_str = "<font color='white'><b>" + off_str + "</b></font>";
-    }
-
-    return QString("%1<br>%2<br>%3<br>%4")
-        .arg(off_str)
-        .arg(info_str)
-        .arg(warning_str)
-        .arg(assist_str);
-  }
-
-  static std::vector<SpeedLimitMode> getSpeedLimitModeValues() {
-    std::vector<SpeedLimitMode> values;
-    for (int i = static_cast<int>(SpeedLimitMode::OFF);
-         i <= static_cast<int>(SpeedLimitMode::ASSIST); ++i) {
-      values.push_back(static_cast<SpeedLimitMode>(i));
-    }
-    return values;
-  }
-
-  static std::vector<int> convertSpeedLimitModeValues(const std::vector<SpeedLimitMode> &modes) {
-    std::vector<int> values;
-    for (const auto& mode : modes) {
-      values.push_back(static_cast<int>(mode));
-    }
-    return values;
-  }
 };

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

@@ -50,13 +50,6 @@ LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) {
     "");
   list->addItem(SmartCruiseControlVision);
 
-  SmartCruiseControlMap = new ParamControl(
-    "SmartCruiseControlMap",
-    tr("Smart Cruise Control - Map"),
-    tr("Use map data to estimate the appropriate speed to drive through turns ahead."),
-    "");
-  list->addItem(SmartCruiseControlMap);
-
   customAccIncrement = new CustomAccIncrement("CustomAccIncrementsEnabled", tr("Custom ACC Speed Increments"), "", "", this);
   list->addItem(customAccIncrement);
 
@@ -141,7 +134,6 @@ void LongitudinalPanel::refresh(bool _offroad) {
   customAccIncrement->refresh();
 
   SmartCruiseControlVision->setEnabled(has_longitudinal_control || icbm_allowed);
-  SmartCruiseControlMap->setEnabled(has_longitudinal_control || icbm_allowed);
 
   offroad = _offroad;
 }

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

@@ -11,6 +11,7 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
+#include "selfdrive/ui/sunnypilot/ui.h"
 
 class LongitudinalPanel : public QWidget {
   Q_OBJECT
@@ -32,7 +33,6 @@ private:
   QWidget *cruisePanelScreen = nullptr;
   CustomAccIncrement *customAccIncrement = nullptr;
   ParamControl *SmartCruiseControlVision;
-  ParamControl *SmartCruiseControlMap;
   ParamControl *intelligentCruiseButtonManagement = nullptr;
   SpeedLimitSettings *speedLimitScreen;
   PushButtonSP *speedLimitSettings;

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

@@ -13,7 +13,6 @@
 
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/developer_panel.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h"
-#include "selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink_panel.h"
@@ -88,7 +87,6 @@ SettingsWindowSP::SettingsWindowSP(QWidget *parent) : SettingsWindow(parent) {
     PanelInfo("   " + tr("Steering"), new LateralPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_lateral.png"),
     PanelInfo("   " + tr("Cruise"), new LongitudinalPanel(this), "../assets/icons/speed_limit.png"),
     PanelInfo("   " + tr("Visuals"), new VisualsPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_visuals.png"),
-    PanelInfo("   " + tr("Display"), new DisplayPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_display.png"),
     PanelInfo("   " + tr("OSM"), new OsmPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_map.png"),
     PanelInfo("   " + tr("Trips"), new TripsPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_trips.png"),
     PanelInfo("   " + tr("Vehicle"), new VehiclePanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_vehicle.png"),

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

@@ -25,48 +25,21 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
       "BlindSpot",
       tr("Show Blind Spot Warnings"),
       tr("Enabling this will display warnings when a vehicle is detected in your blind spot as long as your car has BSM supported."),
-      "",
+      "../assets/offroad/icon_monitoring.png",
       false,
     },
     {
       "RainbowMode",
       tr("Enable Tesla Rainbow Mode"),
       RainbowizeWords(tr("A beautiful rainbow effect on the path the model wants to take.")) + "<br/><i>" + tr("It")+ " <b>" + tr("does not") + "</b> " + tr("affect driving in any way.") + "</i>",
-      "",
+      "../assets/offroad/icon_monitoring.png",
       false,
     },
     {
       "StandstillTimer",
       tr("Enable Standstill Timer"),
       tr("Show a timer on the HUD when the car is at a standstill."),
-      "",
-      false,
-    },
-    {
-      "RoadNameToggle",
-      tr("Display Road Name"),
-      tr("Displays the name of the road the car is traveling on. The OpenStreetMap database of the location must be downloaded from the OSM panel to fetch the road name."),
-      "",
-      false,
-    },
-    {
-      "GreenLightAlert",
-      tr("Green Traffic Light Alert (Beta)"),
-      QString("%1<br>"
-        "<h4>%2</h4><br>")
-        .arg(tr("A chime and on-screen alert will play when the traffic light you are waiting for turns green and you have no vehicle in front of you."))
-        .arg(tr("Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly.")),
-      "",
-      false,
-    },
-    {
-      "LeadDepartAlert",
-      tr("Lead Departure Alert (Beta)"),
-      QString("%1<br>"
-        "<h4>%2</h4><br>")
-        .arg(tr("A chime and on-screen alert will play when you are stopped, and the vehicle in front of you start moving."))
-        .arg(tr("Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly.")),
-      "",
+      "../assets/offroad/icon_monitoring.png",
       false,
     },
   };

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

@@ -11,22 +11,13 @@
 #include "selfdrive/ui/qt/util.h"
 
 
-HudRendererSP::HudRendererSP() {
-  plus_arrow_up_img = loadPixmap("../../sunnypilot/selfdrive/assets/img_plus_arrow_up", {105, 105});
-  minus_arrow_down_img = loadPixmap("../../sunnypilot/selfdrive/assets/img_minus_arrow_down", {105, 105});
-
-  int small_max = e2e_alert_small * 2 - 40;
-  int large_max = e2e_alert_large * 2 - 40;
-  green_light_alert_small_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/green_light.png", {small_max, small_max});
-  green_light_alert_large_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/green_light.png", {large_max, large_max});
-  lead_depart_alert_small_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/lead_depart.png", {small_max, small_max});
-  lead_depart_alert_large_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/lead_depart.png", {large_max, large_max});
-}
+HudRendererSP::HudRendererSP() {}
 
 void HudRendererSP::updateState(const UIState &s) {
   HudRenderer::updateState(s);
 
   const SubMaster &sm = *(s.sm);
+  const bool cs_alive = sm.alive("controlsState");
   const auto cs = sm["controlsState"].getControlsState();
   const auto car_state = sm["carState"].getCarState();
   const auto car_control = sm["carControl"].getCarControl();
@@ -36,31 +27,6 @@ void HudRendererSP::updateState(const UIState &s) {
   const auto ltp = sm["liveTorqueParameters"].getLiveTorqueParameters();
   const auto car_params = sm["carParams"].getCarParams();
   const auto lp_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP();
-  const auto lmd = sm["liveMapDataSP"].getLiveMapDataSP();
-
-  float speedConv = is_metric ? MS_TO_KPH : MS_TO_MPH;
-  speedLimit = lp_sp.getSpeedLimit().getResolver().getSpeedLimit() * speedConv;
-  speedLimitLast = lp_sp.getSpeedLimit().getResolver().getSpeedLimitLast() * speedConv;
-  speedLimitOffset = lp_sp.getSpeedLimit().getResolver().getSpeedLimitOffset() * speedConv;
-  speedLimitValid = lp_sp.getSpeedLimit().getResolver().getSpeedLimitValid();
-  speedLimitLastValid = lp_sp.getSpeedLimit().getResolver().getSpeedLimitLastValid();
-  speedLimitFinalLast = lp_sp.getSpeedLimit().getResolver().getSpeedLimitFinalLast() * speedConv;
-  speedLimitMode = static_cast<SpeedLimitMode>(s.scene.speed_limit_mode);
-  speedLimitAssistState = lp_sp.getSpeedLimit().getAssist().getState();
-  speedLimitAssistActive = lp_sp.getSpeedLimit().getAssist().getActive();
-  roadName = s.scene.road_name;
-  if (sm.updated("liveMapDataSP")) {
-    roadNameStr = QString::fromStdString(lmd.getRoadName());
-    speedLimitAheadValid = lmd.getSpeedLimitAheadValid();
-    speedLimitAhead = lmd.getSpeedLimitAhead() * speedConv;
-    speedLimitAheadDistance = lmd.getSpeedLimitAheadDistance();
-    if (speedLimitAheadDistance < speedLimitAheadDistancePrev && speedLimitAheadValidFrame < SPEED_LIMIT_AHEAD_VALID_FRAME_THRESHOLD) {
-      speedLimitAheadValidFrame++;
-    } else if (speedLimitAheadDistance > speedLimitAheadDistancePrev && speedLimitAheadValidFrame > 0) {
-      speedLimitAheadValidFrame--;
-    }
-  }
-  speedLimitAheadDistancePrev = speedLimitAheadDistance;
 
   static int reverse_delay = 0;
   bool reverse_allowed = false;
@@ -75,6 +41,13 @@ void HudRendererSP::updateState(const UIState &s) {
   }
 
   reversing = reverse_allowed;
+  is_metric = s.scene.is_metric;
+
+  // Handle older routes where vEgoCluster is not set
+  v_ego_cluster_seen = v_ego_cluster_seen || car_state.getVEgoCluster() != 0.0;
+  float v_ego = v_ego_cluster_seen ? car_state.getVEgoCluster() : car_state.getVEgo();
+  speed = cs_alive ? std::max<float>(0.0, v_ego) : 0.0;
+  speed *= is_metric ? MS_TO_KPH : MS_TO_MPH;
 
   latActive = car_control.getLatActive();
   steerOverride = car_state.getSteeringPressed();
@@ -110,50 +83,26 @@ void HudRendererSP::updateState(const UIState &s) {
   longOverride = car_control.getCruiseControl().getOverride();
   smartCruiseControlVisionEnabled = lp_sp.getSmartCruiseControl().getVision().getEnabled();
   smartCruiseControlVisionActive = lp_sp.getSmartCruiseControl().getVision().getActive();
-  smartCruiseControlMapEnabled = lp_sp.getSmartCruiseControl().getMap().getEnabled();
-  smartCruiseControlMapActive = lp_sp.getSmartCruiseControl().getMap().getActive();
-
-  greenLightAlert = lp_sp.getE2eAlerts().getGreenLightAlert();
-  leadDepartAlert = lp_sp.getE2eAlerts().getLeadDepartAlert();
 }
 
 void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
   HudRenderer::draw(p, surface_rect);
-
-  e2eAlertDisplayTimer = std::max(0, e2eAlertDisplayTimer - 1);
-
-  p.save();
-
-  if (is_cruise_available) {
-    drawSetSpeedSP(p, surface_rect);
-  }
-
   if (!reversing) {
     // Smart Cruise Control
     int x_offset = -260;
     int y1_offset = -80;
-    int y2_offset = -140;
+    // int y2_offset = -140;  // reserved for 2 icons
 
-    int y_scc_v = 0, y_scc_m = 0;
-    const int orders[2] = {y1_offset, y2_offset};
-    int i = 0;
-    // SCC-V takes first order
-    if (smartCruiseControlVisionEnabled) y_scc_v = orders[i++];
-    if (smartCruiseControlMapEnabled) y_scc_m = orders[i++];
-
-    // Smart Cruise Control - Vision
     bool scc_vision_active_pulse = pulseElement(smartCruiseControlVisionFrame);
     if ((smartCruiseControlVisionEnabled && !smartCruiseControlVisionActive) || (smartCruiseControlVisionActive && scc_vision_active_pulse)) {
-      drawSmartCruiseControlOnroadIcon(p, surface_rect, x_offset, y_scc_v, "SCC-V");
+      drawSmartCruiseControlOnroadIcon(p, surface_rect, x_offset, y1_offset, "SCC-V");
     }
-    smartCruiseControlVisionFrame = smartCruiseControlVisionActive ? (smartCruiseControlVisionFrame + 1) : 0;
 
-    // Smart Cruise Control - Map
-    bool scc_map_active_pulse = pulseElement(smartCruiseControlMapFrame);
-    if ((smartCruiseControlMapEnabled && !smartCruiseControlMapActive) || (smartCruiseControlMapActive && scc_map_active_pulse)) {
-      drawSmartCruiseControlOnroadIcon(p, surface_rect, x_offset, y_scc_m, "SCC-M");
+    if (smartCruiseControlVisionActive) {
+      smartCruiseControlVisionFrame++;
+    } else {
+      smartCruiseControlVisionFrame = 0;
     }
-    smartCruiseControlMapFrame = smartCruiseControlMapActive ? (smartCruiseControlMapFrame + 1) : 0;
 
     // Bottom Dev UI
     if (devUiInfo == 2) {
@@ -174,63 +123,7 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
     if (standstillTimer) {
       drawStandstillTimer(p, surface_rect.right() / 12 * 10, surface_rect.bottom() / 12 * 1.53);
     }
-
-    // Speed Limit
-    bool showSpeedLimit;
-    bool speed_limit_assist_pre_active_pulse = pulseElement(speedLimitAssistFrame);
-
-    // Position speed limit sign next to set speed box
-    const int sign_width = is_metric ? 200 : 172;
-    const int sign_x = is_metric ? 280 : 272;
-    const int sign_y = 45;
-    const int sign_height = 204;
-    QRect sign_rect(sign_x, sign_y, sign_width, sign_height);
-
-    if (speedLimitAssistState == cereal::LongitudinalPlanSP::SpeedLimit::AssistState::PRE_ACTIVE) {
-      speedLimitAssistFrame++;
-      showSpeedLimit = speed_limit_assist_pre_active_pulse;
-      drawSpeedLimitPreActiveArrow(p, sign_rect);
-    } else {
-      speedLimitAssistFrame = 0;
-      showSpeedLimit = speedLimitMode != SpeedLimitMode::OFF;
-    }
-
-    if (showSpeedLimit) {
-      drawSpeedLimitSigns(p, sign_rect);
-
-      // do not show during SLA's preActive state
-      if (speedLimitAssistState != cereal::LongitudinalPlanSP::SpeedLimit::AssistState::PRE_ACTIVE) {
-        drawUpcomingSpeedLimit(p);
-      }
-    }
-
-    // Road Name
-    drawRoadName(p, surface_rect);
-
-    // Green Light & Lead Depart Alerts
-    if (greenLightAlert or leadDepartAlert) {
-      e2eAlertDisplayTimer = 3 * UI_FREQ;
-      // reset onroad sleep timer for e2e alerts
-      uiStateSP()->reset_onroad_sleep_timer();
-    }
-
-    if (e2eAlertDisplayTimer > 0) {
-      e2eAlertFrame++;
-      if (greenLightAlert) {
-        alert_text = tr("GREEN\nLIGHT");
-        alert_img = devUiInfo > 0 ? green_light_alert_small_img : green_light_alert_large_img;
-      }
-      else if (leadDepartAlert) {
-        alert_text = tr("LEAD VEHICLE\nDEPARTING");
-        alert_img = devUiInfo > 0 ? lead_depart_alert_small_img : lead_depart_alert_large_img;
-      }
-      drawE2eAlert(p, surface_rect);
-    } else {
-      e2eAlertFrame = 0;
-    }
   }
-
-  p.restore();
 }
 
 void HudRendererSP::drawText(QPainter &p, int x, int y, const QString &text, QColor color) {
@@ -418,316 +311,3 @@ void HudRendererSP::drawStandstillTimer(QPainter &p, int x, int y) {
     standstillElapsedTime = 0.0;
   }
 }
-
-void HudRendererSP::drawSpeedLimitSigns(QPainter &p, QRect &sign_rect) {
-  bool speedLimitWarningEnabled = speedLimitMode >= SpeedLimitMode::WARNING;  // TODO-SP: update to include SpeedLimitMode::ASSIST
-  bool hasSpeedLimit = speedLimitValid || speedLimitLastValid;
-  bool overspeed = hasSpeedLimit && std::nearbyint(speedLimitFinalLast) < std::nearbyint(speed);
-  QString speedLimitStr = hasSpeedLimit ? QString::number(std::nearbyint(speedLimitLast)) : "---";
-
-  // Offset display text
-  QString speedLimitSubText = "";
-  if (speedLimitOffset != 0) {
-    speedLimitSubText = (speedLimitOffset > 0 ? "" : "-") + QString::number(std::nearbyint(speedLimitOffset));
-  }
-
-  float speedLimitSubTextFactor = is_metric ? 0.5 : 0.6;
-  if (speedLimitSubText.size() >= 3) {
-    speedLimitSubTextFactor = 0.475;
-  }
-
-  int alpha = 255;
-  QColor red_color = QColor(255, 0, 0, alpha);
-  QColor speed_color = (speedLimitWarningEnabled && overspeed) ? red_color :
-                       (!speedLimitValid && speedLimitLastValid ? QColor(0x91, 0x9b, 0x95, 0xf1) : QColor(0, 0, 0, alpha));
-
-  if (is_metric) {
-    // EU Vienna Convention style circular sign
-    QRect vienna_rect = sign_rect;
-    int circle_size = std::min(vienna_rect.width(), vienna_rect.height());
-    QRect circle_rect(vienna_rect.x(), vienna_rect.y(), circle_size, circle_size);
-
-    if (vienna_rect.width() > vienna_rect.height()) {
-      circle_rect.moveLeft(vienna_rect.x() + (vienna_rect.width() - circle_size) / 2);
-    } else if (vienna_rect.height() > vienna_rect.width()) {
-      circle_rect.moveTop(vienna_rect.y() + (vienna_rect.height() - circle_size) / 2);
-    }
-
-    // White background circle
-    p.setPen(Qt::NoPen);
-    p.setBrush(QColor(255, 255, 255, alpha));
-    p.drawEllipse(circle_rect);
-
-    // Red border ring with color coding
-    QRect red_ring = circle_rect;
-
-    p.setBrush(red_color);
-    p.drawEllipse(red_ring);
-
-    // Center white circle for text
-    int ring_size = circle_size * 0.12;
-    QRect center_circle = red_ring.adjusted(ring_size, ring_size, -ring_size, -ring_size);
-    p.setBrush(QColor(255, 255, 255, alpha));
-    p.drawEllipse(center_circle);
-
-    // Speed value, smaller font for 3+ digits
-    int font_size = (speedLimitStr.size() >= 3) ? 70 : 85;
-    p.setFont(InterFont(font_size, QFont::Bold));
-
-    p.setPen(speed_color);
-    p.drawText(center_circle, Qt::AlignCenter, speedLimitStr);
-
-    // Offset value in small circular box
-    if (!speedLimitSubText.isEmpty() && hasSpeedLimit) {
-      int offset_circle_size = circle_size * 0.4;
-      int overlap = offset_circle_size * 0.25;
-      QRect offset_circle_rect(
-        circle_rect.right() - offset_circle_size/1.25 + overlap,
-        circle_rect.top() - offset_circle_size/1.75 + overlap,
-        offset_circle_size,
-        offset_circle_size
-      );
-
-      p.setPen(QPen(QColor(77, 77, 77, 255), 6));
-      p.setBrush(QColor(0, 0, 0, alpha));
-      p.drawEllipse(offset_circle_rect);
-
-      p.setFont(InterFont(offset_circle_size * speedLimitSubTextFactor, QFont::Bold));
-      p.setPen(QColor(255, 255, 255, alpha));
-      p.drawText(offset_circle_rect, Qt::AlignCenter, speedLimitSubText);
-    }
-  } else {
-    // US/Canada MUTCD style sign
-    p.setPen(Qt::NoPen);
-    p.setBrush(QColor(255, 255, 255, alpha));
-    p.drawRoundedRect(sign_rect, 32, 32);
-
-    // Inner border with violation color coding
-    QRect inner_rect = sign_rect.adjusted(10, 10, -10, -10);
-    QColor border_color = QColor(0, 0, 0, alpha);
-
-    p.setPen(QPen(border_color, 4));
-    p.setBrush(QColor(255, 255, 255, alpha));
-    p.drawRoundedRect(inner_rect, 22, 22);
-
-    // "SPEED LIMIT" text
-    p.setFont(InterFont(40, QFont::DemiBold));
-    p.setPen(QColor(0, 0, 0, alpha));
-    p.drawText(inner_rect.adjusted(0, 10, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
-    p.drawText(inner_rect.adjusted(0, 50, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
-
-    // Speed value with color coding
-    p.setFont(InterFont(90, QFont::Bold));
-
-    p.setPen(speed_color);
-    p.drawText(inner_rect.adjusted(0, 80, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
-
-    // Offset value in small box
-    if (!speedLimitSubText.isEmpty() && hasSpeedLimit) {
-      int offset_box_size = sign_rect.width() * 0.4;
-      int overlap = offset_box_size * 0.25;
-      QRect offset_box_rect(
-        sign_rect.right() - offset_box_size/1.5 + overlap,
-        sign_rect.top() - offset_box_size/1.25 + overlap,
-        offset_box_size,
-        offset_box_size
-      );
-
-      int corner_radius = offset_box_size * 0.2;
-      p.setPen(QPen(QColor(77, 77, 77, 255), 6));
-      p.setBrush(QColor(0, 0, 0, alpha));
-      p.drawRoundedRect(offset_box_rect, corner_radius, corner_radius);
-
-      p.setFont(InterFont(offset_box_size * speedLimitSubTextFactor, QFont::Bold));
-      p.setPen(QColor(255, 255, 255, alpha));
-      p.drawText(offset_box_rect, Qt::AlignCenter, speedLimitSubText);
-    }
-  }
-}
-
-void HudRendererSP::drawUpcomingSpeedLimit(QPainter &p) {
-  bool speed_limit_ahead = speedLimitAheadValid && speedLimitAhead > 0 && speedLimitAhead != speedLimit && speedLimitAheadValidFrame > 0;
-  if (!speed_limit_ahead) {
-    return;
-  }
-
-  auto roundToInterval = [&](float distance, int interval, int threshold) {
-    int base = static_cast<int>(distance / interval) * interval;
-    return (distance - base >= threshold) ? base + interval : base;
-  };
-
-  auto outputDistance = [&] {
-    if (is_metric) {
-      if (speedLimitAheadDistance < 50) return tr("Near");
-      if (speedLimitAheadDistance >= 1000) return QString::number(speedLimitAheadDistance * METER_TO_KM, 'f', 1) + tr("km");
-
-      int rounded = (speedLimitAheadDistance < 200) ? std::max(10, roundToInterval(speedLimitAheadDistance, 10, 5)) : roundToInterval(speedLimitAheadDistance, 100, 50);
-      return QString::number(rounded) + tr("m");
-    } else {
-      float distance_ft = speedLimitAheadDistance * METER_TO_FOOT;
-      if (distance_ft < 100) return tr("Near");
-      if (distance_ft >= 900) return QString::number(speedLimitAheadDistance * METER_TO_MILE, 'f', 1) + tr("mi");
-
-      int rounded = (distance_ft < 500) ? std::max(50, roundToInterval(distance_ft, 50, 25)) : roundToInterval(distance_ft, 100, 50);
-      return QString::number(rounded) + tr("ft");
-    }
-  };
-
-  QString speedStr = QString::number(std::nearbyint(speedLimitAhead));
-  QString distanceStr = outputDistance();
-
-  // Position below current speed limit sign
-  const int sign_width = is_metric ? 200 : 172;
-  const int sign_x = is_metric ? 280 : 272;
-  const int sign_y = 45;
-  const int sign_height = 204;
-
-  const int ahead_width = 170;
-  const int ahead_height = 160;
-  const int ahead_x = sign_x + (sign_width - ahead_width) / 2;
-  const int ahead_y = sign_y + sign_height + 10;
-
-  QRect ahead_rect(ahead_x, ahead_y, ahead_width, ahead_height);
-  p.setPen(QPen(QColor(255, 255, 255, 100), 3));
-  p.setBrush(QColor(0, 0, 0, 180));
-  p.drawRoundedRect(ahead_rect, 16, 16);
-
-  // "AHEAD" label
-  p.setFont(InterFont(40, QFont::DemiBold));
-  p.setPen(QColor(200, 200, 200, 255));
-  p.drawText(ahead_rect.adjusted(0, 4, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("AHEAD"));
-
-  // Speed value
-  p.setFont(InterFont(70, QFont::Bold));
-  p.setPen(QColor(255, 255, 255, 255));
-  p.drawText(ahead_rect.adjusted(0, 38, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedStr);
-
-  // Distance
-  p.setFont(InterFont(40, QFont::Normal));
-  p.setPen(QColor(180, 180, 180, 255));
-  p.drawText(ahead_rect.adjusted(0, 110, 0, 0), Qt::AlignTop | Qt::AlignHCenter, distanceStr);
-}
-
-void HudRendererSP::drawRoadName(QPainter &p, const QRect &surface_rect) {
-  if (!roadName || roadNameStr.isEmpty()) return;
-
-  // Measure text to size container
-  p.setFont(InterFont(46, QFont::DemiBold));
-  QFontMetrics fm(p.font());
-
-  int text_width = fm.horizontalAdvance(roadNameStr);
-  int padding = 40;
-  int rect_width = text_width + padding;
-
-  // Constrain to reasonable bounds
-  int min_width = 200;
-  int max_width = surface_rect.width() - 40;
-  rect_width = std::max(min_width, std::min(rect_width, max_width));
-
-  // Center at top of screen
-  QRect road_rect(surface_rect.width() / 2 - rect_width / 2, -4, rect_width, 60);
-
-  p.setPen(Qt::NoPen);
-  p.setBrush(QColor(0, 0, 0, 120));
-  p.drawRoundedRect(road_rect, 12, 12);
-
-  p.setPen(QColor(255, 255, 255, 200));
-
-  // Truncate if still too long
-  QString truncated = fm.elidedText(roadNameStr, Qt::ElideRight, road_rect.width() - 20);
-  p.drawText(road_rect, Qt::AlignCenter, truncated);
-}
-
-void HudRendererSP::drawSpeedLimitPreActiveArrow(QPainter &p, QRect &sign_rect) {
-  const int sign_margin = 12;
-  const int arrow_spacing = sign_margin * 3;
-  int arrow_x = sign_rect.right() + arrow_spacing;
-
-  int _set_speed = std::nearbyint(set_speed);
-  int _speed_limit_final_last = std::nearbyint(speedLimitFinalLast);
-
-  // Calculate the vertical offset using a sinusoidal function for smooth bouncing
-  double bounce_frequency = 2.0 * M_PI / UI_FREQ;  // 20 frames for one full oscillation
-  int bounce_offset = 20 * sin(speedLimitAssistFrame * bounce_frequency);  // Adjust the amplitude (20 pixels) as needed
-
-  if (_set_speed < _speed_limit_final_last) {
-    QPoint iconPosition(arrow_x, sign_rect.center().y() - plus_arrow_up_img.height() / 2 + bounce_offset);
-    p.drawPixmap(iconPosition, plus_arrow_up_img);
-  } else if (_set_speed > _speed_limit_final_last) {
-    QPoint iconPosition(arrow_x, sign_rect.center().y() - minus_arrow_down_img.height() / 2 - bounce_offset);
-    p.drawPixmap(iconPosition, minus_arrow_down_img);
-  }
-}
-
-void HudRendererSP::drawSetSpeedSP(QPainter &p, const QRect &surface_rect) {
-  // Draw outer box + border to contain set speed
-  const QSize default_size = {172, 204};
-  QSize set_speed_size = is_metric ? QSize(200, 204) : default_size;
-  QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size);
-
-  // Draw set speed box
-  p.setPen(QPen(QColor(255, 255, 255, 75), 6));
-  p.setBrush(QColor(0, 0, 0, 166));
-  p.drawRoundedRect(set_speed_rect, 32, 32);
-
-  // Colors based on status
-  QColor max_color = QColor(0xa6, 0xa6, 0xa6, 0xff);
-  QColor set_speed_color = QColor(0x72, 0x72, 0x72, 0xff);
-  if (is_cruise_set) {
-    set_speed_color = QColor(255, 255, 255);
-    if (speedLimitAssistActive) {
-      set_speed_color = longOverride ? QColor(0x91, 0x9b, 0x95, 0xff) : QColor(0, 0xff, 0, 0xff);
-      max_color = longOverride ? QColor(0x91, 0x9b, 0x95, 0xff) : QColor(0x80, 0xd8, 0xa6, 0xff);
-    } else {
-      if (status == STATUS_DISENGAGED) {
-        max_color = QColor(255, 255, 255);
-      } else if (status == STATUS_OVERRIDE) {
-        max_color = QColor(0x91, 0x9b, 0x95, 0xff);
-      } else {
-        max_color = QColor(0x80, 0xd8, 0xa6, 0xff);
-      }
-    }
-  }
-
-  // Draw "MAX" text
-  p.setFont(InterFont(40, QFont::DemiBold));
-  p.setPen(max_color);
-  p.drawText(set_speed_rect.adjusted(0, 27, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("MAX"));
-
-  // Draw set speed
-  QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(set_speed)) : "–";
-  p.setFont(InterFont(90, QFont::Bold));
-  p.setPen(set_speed_color);
-  p.drawText(set_speed_rect.adjusted(0, 77, 0, 0), Qt::AlignTop | Qt::AlignHCenter, setSpeedStr);
-}
-
-void HudRendererSP::drawE2eAlert(QPainter &p, const QRect &surface_rect) {
-  int size = devUiInfo > 0 ? e2e_alert_small : e2e_alert_large;
-  int x = surface_rect.center().x() + surface_rect.width() / 4;
-  int y = surface_rect.center().y() + 40;
-  x += devUiInfo > 0 ? 0 : 50;
-  y += devUiInfo > 0 ? 0 : 80;
-  QRect alertRect(x - size, y - size, size * 2, size * 2);
-
-  // Alert Circle
-  QPoint center = alertRect.center();
-  QColor frameColor = pulseElement(e2eAlertFrame) ? QColor(255, 255, 255, 75) : QColor(0, 255, 0, 75);
-  p.setPen(QPen(frameColor, 15));
-  p.setBrush(QColor(0, 0, 0, 190));
-  p.drawEllipse(center, size, size);
-
-  // Alert Text
-  QColor txtColor = pulseElement(e2eAlertFrame) ? QColor(255, 255, 255, 255) : QColor(0, 255, 0, 255);
-  p.setFont(InterFont(48, QFont::Bold));
-  p.setPen(txtColor);
-  QFontMetrics fm(p.font());
-  QRect textRect = fm.boundingRect(alertRect, Qt::TextWordWrap, alert_text);
-  textRect.moveCenter({alertRect.center().x(), alertRect.center().y()});
-  textRect.moveBottom(alertRect.bottom() - alertRect.height() / 7);
-  p.drawText(textRect, Qt::AlignCenter, alert_text);
-
-  // Alert Image
-  QPointF pixmapCenterOffset = QPointF(alert_img.width() / 2.0, alert_img.height() / 2.0);
-  QPointF drawPoint = center - pixmapCenterOffset;
-  p.drawPixmap(drawPoint, alert_img);
-}

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

@@ -8,11 +8,8 @@
 #pragma once
 
 #include "selfdrive/ui/qt/onroad/hud.h"
-#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h"
 #include "selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.h"
 
-constexpr int SPEED_LIMIT_AHEAD_VALID_FRAME_THRESHOLD = 5;
-
 class HudRendererSP : public HudRenderer {
   Q_OBJECT
 
@@ -31,12 +28,6 @@ private:
   void drawStandstillTimer(QPainter &p, int x, int y);
   bool pulseElement(int frame);
   void drawSmartCruiseControlOnroadIcon(QPainter &p, const QRect &surface_rect, int x_offset, int y_offset, std::string name);
-  void drawSpeedLimitSigns(QPainter &p, QRect &sign_rect);
-  void drawUpcomingSpeedLimit(QPainter &p);
-  void drawRoadName(QPainter &p, const QRect &surface_rect);
-  void drawSpeedLimitPreActiveArrow(QPainter &p, QRect &sign_rect);
-  void drawSetSpeedSP(QPainter &p, const QRect &surface_rect);
-  void drawE2eAlert(QPainter &p, const QRect &surface_rect);
 
   bool lead_status;
   float lead_d_rel;
@@ -72,38 +63,4 @@ private:
   bool smartCruiseControlVisionEnabled;
   bool smartCruiseControlVisionActive;
   int smartCruiseControlVisionFrame;
-  bool smartCruiseControlMapEnabled;
-  bool smartCruiseControlMapActive;
-  int smartCruiseControlMapFrame;
-  float speedLimit;
-  float speedLimitLast;
-  float speedLimitOffset;
-  bool speedLimitValid;
-  bool speedLimitLastValid;
-  float speedLimitFinalLast;
-  bool speedLimitAheadValid;
-  float speedLimitAhead;
-  float speedLimitAheadDistance;
-  float speedLimitAheadDistancePrev;
-  int speedLimitAheadValidFrame;
-  SpeedLimitMode speedLimitMode = SpeedLimitMode::OFF;
-  bool roadName;
-  QString roadNameStr;
-  cereal::LongitudinalPlanSP::SpeedLimit::AssistState speedLimitAssistState;
-  bool speedLimitAssistActive;
-  int speedLimitAssistFrame;
-  QPixmap plus_arrow_up_img;
-  QPixmap minus_arrow_down_img;
-  int e2e_alert_small = 250;
-  int e2e_alert_large = 300;
-  QPixmap green_light_alert_small_img;
-  QPixmap green_light_alert_large_img;
-  bool greenLightAlert;
-  int e2eAlertFrame;
-  int e2eAlertDisplayTimer = 0;
-  bool leadDepartAlert;
-  QPixmap lead_depart_alert_small_img;
-  QPixmap lead_depart_alert_large_img;
-  QString alert_text;
-  QPixmap alert_img;
 };

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

@@ -25,10 +25,8 @@ void OnroadWindowSP::updateState(const UIStateSP &s) {
 
 void OnroadWindowSP::mousePressEvent(QMouseEvent *e) {
   OnroadWindow::mousePressEvent(e);
-  uiStateSP()->reset_onroad_sleep_timer();
 }
 
 void OnroadWindowSP::offroadTransition(bool offroad) {
   OnroadWindow::offroadTransition(offroad);
-  uiStateSP()->reset_onroad_sleep_timer();
 }

selfdrive/ui/sunnypilot/qt/widgets/controls.h

@@ -7,7 +7,6 @@
 
 #pragma once
 
-#include <cmath>
 #include <map>
 #include <optional>
 #include <string>
@@ -501,7 +500,7 @@ private:
 
   int getParamValueScaled() {
     const auto param_value = QString::fromStdString(params.get(key));
-    return std::nearbyint(param_value.toFloat() * 100.0f);
+    return static_cast<int>(param_value.toFloat() * 100);
   }
 
   void setParamValueScaled(const int new_value) {

selfdrive/ui/sunnypilot/qt/widgets/expandable_row.cc

@@ -18,6 +18,7 @@ ExpandableToggleRow::ExpandableToggleRow(const QString &param, const QString &ti
   collapsibleWidget->setVisible(false);
   QVBoxLayout *collapsible_layout = new QVBoxLayout();
   collapsibleWidget->setLayout(collapsible_layout);
+  collapsible_layout->setContentsMargins(0, 20, 0, 20);
 
   list = new ListWidgetSP(this, false);
 

selfdrive/ui/sunnypilot/ui.cc

@@ -11,15 +11,6 @@
 
 void UIStateSP::updateStatus() {
   UIState::updateStatus();
-
-  if (scene.started && scene.onroadScreenOffControl) {
-    auto selfdriveState = (*sm)["selfdriveState"].getSelfdriveState();
-    if (selfdriveState.getAlertSize() != cereal::SelfdriveState::AlertSize::NONE) {
-      reset_onroad_sleep_timer();
-    } else if (scene.onroadScreenOffTimer > 0) {
-      scene.onroadScreenOffTimer--;
-    }
-  }
 }
 
 UIStateSP::UIStateSP(QObject *parent) : UIState(parent) {
@@ -29,7 +20,7 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) {
     "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
     "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
     "carControl", "gpsLocationExternal", "gpsLocation", "liveTorqueParameters",
-    "carStateSP", "liveParameters", "liveMapDataSP"
+    "carStateSP", "liveParameters"
   });
 
   // update timer
@@ -62,22 +53,6 @@ void ui_update_params_sp(UIStateSP *s) {
   auto params = Params();
   s->scene.dev_ui_info = std::atoi(params.get("DevUIInfo").c_str());
   s->scene.standstill_timer = params.getBool("StandstillTimer");
-  s->scene.speed_limit_mode = std::atoi(params.get("SpeedLimitMode").c_str());
-  s->scene.road_name = params.getBool("RoadNameToggle");
-
-  // Onroad Screen Brightness
-  s->scene.onroadScreenOffBrightness = std::atoi(params.get("OnroadScreenOffBrightness").c_str());
-  s->scene.onroadScreenOffControl = params.getBool("OnroadScreenOffControl");
-  s->scene.onroadScreenOffTimerParam = std::atoi(params.get("OnroadScreenOffTimer").c_str());
-  s->reset_onroad_sleep_timer();
-}
-
-void UIStateSP::reset_onroad_sleep_timer() {
-  if (scene.onroadScreenOffTimerParam >= 0 and scene.onroadScreenOffControl) {
-    scene.onroadScreenOffTimer = scene.onroadScreenOffTimerParam * UI_FREQ;
-  } else {
-    scene.onroadScreenOffTimer = -1;
-  }
 }
 
 DeviceSP::DeviceSP(QObject *parent) : Device(parent) {

selfdrive/ui/sunnypilot/ui.h

@@ -61,7 +61,6 @@ public:
       return user.user_id.toLower() != "unregisteredsponsor" && user.user_id.toLower() != "temporarysponsor";
     });
   }
-  void reset_onroad_sleep_timer();
 
 signals:
   void sunnylinkRoleChanged(bool subscriber);

selfdrive/ui/sunnypilot/ui_scene.h

@@ -10,9 +10,4 @@
 typedef struct UISceneSP : UIScene {
   int dev_ui_info = 0;
   bool standstill_timer = false;
-  int speed_limit_mode = 0;
-  bool road_name = false;
-  int onroadScreenOffBrightness, onroadScreenOffTimer = 0;
-  bool onroadScreenOffControl;
-  int onroadScreenOffTimerParam;
 } UISceneSP;

selfdrive/ui/ui.cc

@@ -204,13 +204,6 @@ void Device::updateBrightness(const UIState &s) {
     brightness = 0;
   }
 
-  // Onroad Brightness Control
-#ifdef SUNNYPILOT
-  if (awake && s.scene.started && s.scene.onroadScreenOffControl && s.scene.onroadScreenOffTimer == 0) {
-    brightness = s.scene.onroadScreenOffBrightness * 0.01 * brightness;
-  }
-#endif
-
   if (brightness != last_brightness) {
     if (!brightness_future.isRunning()) {
       brightness_future = QtConcurrent::run(Hardware::set_brightness, brightness);

sunnypilot/SConscript

@@ -1,3 +1,2 @@
 SConscript(['modeld/SConscript'])
-SConscript(['modeld_v2/SConscript'])
-SConscript(['selfdrive/locationd/SConscript'])
+SConscript(['modeld_v2/SConscript'])

sunnypilot/mads/helpers.py

@@ -9,10 +9,6 @@ from openpilot.common.params import Params
 from opendbc.car import structs
 from opendbc.safety import ALTERNATIVE_EXPERIENCE
 from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP, HyundaiSafetyFlagsSP
-from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
-
-
-MADS_NO_ACC_MAIN_BUTTON = ("rivian", "tesla")
 
 
 class MadsSteeringModeOnBrake:
@@ -21,25 +17,20 @@ class MadsSteeringModeOnBrake:
   DISENGAGE = 2
 
 
-def get_mads_limited_brands(CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> bool:
-  if CP.brand == 'rivian':
-    return True
-  if CP.brand == 'tesla':
-    return not CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS
-
-  return False
+def get_mads_limited_brands(CP: structs.CarParams) -> bool:
+  return CP.brand in ("rivian", "tesla")
 
 
-def read_steering_mode_param(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params):
-  if get_mads_limited_brands(CP, CP_SP):
+def read_steering_mode_param(CP: structs.CarParams, params: Params):
+  if get_mads_limited_brands(CP):
     return MadsSteeringModeOnBrake.DISENGAGE
 
   return params.get("MadsSteeringMode", return_default=True)
 
 
-def set_alternative_experience(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params):
+def set_alternative_experience(CP: structs.CarParams, params: Params):
   enabled = params.get_bool("Mads")
-  steering_mode = read_steering_mode_param(CP, CP_SP, params)
+  steering_mode = read_steering_mode_param(CP, params)
 
   if enabled:
     CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS
@@ -62,12 +53,9 @@ def set_car_specific_params(CP: structs.CarParams, CP_SP: structs.CarParamsSP, p
   # MADS Partial Support
   # MADS is currently partially supported for these platforms due to lack of consistent states to engage controls
   # Only MadsSteeringModeOnBrake.DISENGAGE is supported for these platforms
-  # TODO-SP: To enable MADS full support for Rivian and most Tesla, identify consistent signals for MADS toggling
-  mads_partial_support = get_mads_limited_brands(CP, CP_SP)
+  # TODO-SP: To enable MADS full support for Rivian/Tesla, identify consistent signals for MADS toggling
+  mads_partial_support = get_mads_limited_brands(CP)
   if mads_partial_support:
     params.put("MadsSteeringMode", 2)
     params.put_bool("MadsUnifiedEngagementMode", True)
-
-  # no ACC MAIN button for these brands
-  if CP.brand in MADS_NO_ACC_MAIN_BUTTON:
     params.remove("MadsMainCruiseAllowed")

sunnypilot/mads/mads.py

@@ -10,7 +10,7 @@ from cereal import log, custom
 from opendbc.car import structs
 from opendbc.car.hyundai.values import HyundaiFlags
 from openpilot.common.params import Params
-from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param, MADS_NO_ACC_MAIN_BUTTON
+from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param, get_mads_limited_brands
 from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT
 
 State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
@@ -27,7 +27,6 @@ IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
 class ModularAssistiveDrivingSystem:
   def __init__(self, selfdrive):
     self.CP = selfdrive.CP
-    self.CP_SP = selfdrive.CP_SP
     self.params = selfdrive.params
 
     self.enabled = False
@@ -44,16 +43,14 @@ class ModularAssistiveDrivingSystem:
     if self.CP.brand == "hyundai":
       if self.CP.flags & (HyundaiFlags.HAS_LDA_BUTTON | HyundaiFlags.CANFD):
         self.allow_always = True
-    if self.CP.brand == "tesla":
-      self.allow_always = True
 
-    if self.CP.brand in MADS_NO_ACC_MAIN_BUTTON:
+    if get_mads_limited_brands(self.CP):
       self.no_main_cruise = True
 
     # read params on init
     self.enabled_toggle = self.params.get_bool("Mads")
     self.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed")
-    self.steering_mode_on_brake = read_steering_mode_param(self.CP, self.CP_SP, self.params)
+    self.steering_mode_on_brake = read_steering_mode_param(self.CP, self.params)
     self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode")
 
   def read_params(self):
@@ -61,7 +58,6 @@ class ModularAssistiveDrivingSystem:
     self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode")
 
   def pedal_pressed_non_gas_pressed(self, CS: structs.CarState) -> bool:
-    # ignore `pedalPressed` events caused by gas presses
     if self.events.has(EventName.pedalPressed) and not (CS.gasPressed and not self.selfdrive.CS_prev.gasPressed and self.disengage_on_accelerator):
       return True
 
@@ -106,13 +102,12 @@ class ModularAssistiveDrivingSystem:
 
   def update_events(self, CS: structs.CarState):
     if not self.selfdrive.enabled and self.enabled:
-      if CS.standstill:
-        if self.events.has(EventName.doorOpen):
-          self.replace_event(EventName.doorOpen, EventNameSP.silentDoorOpen)
-          self.transition_paused_state()
-        if self.events.has(EventName.seatbeltNotLatched):
-          self.replace_event(EventName.seatbeltNotLatched, EventNameSP.silentSeatbeltNotLatched)
-          self.transition_paused_state()
+      if self.events.has(EventName.doorOpen):
+        self.replace_event(EventName.doorOpen, EventNameSP.silentDoorOpen)
+        self.transition_paused_state()
+      if self.events.has(EventName.seatbeltNotLatched):
+        self.replace_event(EventName.seatbeltNotLatched, EventNameSP.silentSeatbeltNotLatched)
+        self.transition_paused_state()
       if self.events.has(EventName.wrongGear) and (CS.vEgo < 2.5 or CS.gearShifter == GearShifter.reverse):
         self.replace_event(EventName.wrongGear, EventNameSP.silentWrongGear)
         self.transition_paused_state()
@@ -131,6 +126,7 @@ class ModularAssistiveDrivingSystem:
           self.transition_paused_state()
 
       self.events.remove(EventName.preEnableStandstill)
+      self.events.remove(EventName.belowEngageSpeed)
       self.events.remove(EventName.speedTooLow)
       self.events.remove(EventName.cruiseDisabled)
       self.events.remove(EventName.manualRestart)

sunnypilot/mapd/live_map_data/base_map_data.py

@@ -4,23 +4,25 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
 This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
+import time
 from abc import abstractmethod, ABC
 
-import cereal.messaging as messaging
+from cereal import messaging
+from openpilot.common.gps import get_gps_location_service
 from openpilot.common.params import Params
-from openpilot.sunnypilot.navd.helpers import coordinate_from_param
+from openpilot.sunnypilot.navd.helpers import Coordinate, coordinate_from_param
 
 
 class BaseMapData(ABC):
   def __init__(self):
     self.params = Params()
 
-    self.sm = messaging.SubMaster(['liveLocationKalman'])
+    self.gps_location_service = get_gps_location_service(self.params)
+    self.sm = messaging.SubMaster(['livePose', 'carControl'] + [self.gps_location_service])
     self.pm = messaging.PubMaster(['liveMapDataSP'])
 
-    self.localizer_valid = False
-    self.last_bearing = None
-    self.last_position = coordinate_from_param("LastGPSPositionLLK", self.params)
+    self.last_position = coordinate_from_param("LastGPSPosition", self.params)
+    self.last_altitude = None
 
   @abstractmethod
   def update_location(self) -> None:
@@ -38,12 +40,24 @@ class BaseMapData(ABC):
   def get_current_road_name(self) -> str:
     pass
 
+  def get_current_location(self) -> None:
+    gps = self.sm[self.gps_location_service]
+
+    # ignore the message if the fix is invalid
+    gps_ok = self.sm.updated[self.gps_location_service] or (time.monotonic() - self.sm.logMonoTime[self.gps_location_service] / 1e9) > 2.0
+    if not gps_ok and self.sm['livePose'].inputsOK:
+      return None
+
+    # livePose has these data, but aren't on cereal
+    self.last_position = Coordinate(gps.latitude, gps.longitude)
+    self.last_altitude = gps.altitude
+
   def publish(self) -> None:
     speed_limit = self.get_current_speed_limit()
     next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance()
 
     mapd_sp_send = messaging.new_message('liveMapDataSP')
-    mapd_sp_send.valid = self.sm['liveLocationKalman'].gpsOK
+    mapd_sp_send.valid = self.sm.all_checks(service_list=[self.gps_location_service, 'livePose'])
     live_map_data = mapd_sp_send.liveMapDataSP
 
     live_map_data.speedLimitValid = bool(speed_limit > 0)
@@ -56,6 +70,7 @@ class BaseMapData(ABC):
     self.pm.send('liveMapDataSP', mapd_sp_send)
 
   def tick(self) -> None:
-    self.sm.update(0)
+    self.sm.update()
+    self.get_current_location()
     self.update_location()
     self.publish()

sunnypilot/mapd/live_map_data/osm_map_data.py

@@ -5,10 +5,8 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
 import json
-import math
 import platform
 
-from cereal import log
 from openpilot.common.params import Params
 from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData
 from openpilot.sunnypilot.navd.helpers import Coordinate
@@ -17,27 +15,19 @@ from openpilot.sunnypilot.navd.helpers import Coordinate
 class OsmMapData(BaseMapData):
   def __init__(self):
     super().__init__()
+    self.params = Params()
     self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
 
   def update_location(self) -> None:
-    location = self.sm['liveLocationKalman']
-    self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
-
-    if self.localizer_valid:
-      self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
-      self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
-
-    if self.last_position is None:
+    if self.last_position is None or self.last_altitude is None:
       return
 
     params = {
       "latitude": self.last_position.latitude,
       "longitude": self.last_position.longitude,
+      "altitude": self.last_altitude,
     }
 
-    if self.last_bearing is not None:
-      params['bearing'] = self.last_bearing
-
     self.mem_params.put("LastGPSPosition", json.dumps(params))
 
   def get_current_speed_limit(self) -> float:

sunnypilot/mapd/mapd_installer.py

@@ -22,7 +22,7 @@ from openpilot.system.version import is_prebuilt
 from openpilot.sunnypilot.mapd import MAPD_PATH, MAPD_BIN_DIR
 import openpilot.system.sentry as sentry
 
-VERSION = 'v1.11.0'
+VERSION = 'v1.10.0'
 URL = f"https://github.com/pfeiferj/openpilot-mapd/releases/download/{VERSION}/mapd"
 
 

sunnypilot/selfdrive/assets/images/green_light.png

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:3aa5ec9ac1daee6a549e62647d90bcaa66d2485f7df7f386ff902fcfb04c1716
-size 6583

sunnypilot/selfdrive/assets/images/lead_depart.png

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:087db35bd469e85aefe3b45636f11ab3e8b55ceb7bc94ea059cfd9a69c2f338f
-size 8914

sunnypilot/selfdrive/assets/img_minus_arrow_down.png

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:0f4a0bf26cfd2c64939759d43bee839aadfd017a6f74065095a27b03615e55a4
-size 26627

sunnypilot/selfdrive/assets/img_plus_arrow_up.png

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:3e451ff31e9b3f144822ab282b8e47cd5a603ca59ff94bd1849271181b86c6b1
-size 29220

sunnypilot/selfdrive/assets/offroad/icon_display.png

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:614767308d435d165a9ab78e3eac22ee15697d94066df8200ef32afb33ef8d60
-size 5698

sunnypilot/selfdrive/car/car_specific.py

@@ -18,9 +18,9 @@ GearShifter = structs.CarState.GearShifter
 
 
 class CarSpecificEventsSP:
-  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
+  def __init__(self, CP: structs.CarParams, params):
     self.CP = CP
-    self.CP_SP = CP_SP
+    self.params = params
 
     self.low_speed_alert = False
 
@@ -37,15 +37,9 @@ class CarSpecificEventsSP:
         # TODO-SP: add 1 m/s hysteresis
         if CS.vEgo >= self.CP.minEnableSpeed:
           self.low_speed_alert = False
-        if self.CP.minEnableSpeed >= 14.5 and CS.gearShifter != GearShifter.drive:
+        if CS.gearShifter != GearShifter.drive:
           self.low_speed_alert = True
       if self.low_speed_alert:
         events.add(EventName.belowSteerSpeed)
 
-    elif self.CP.brand == 'toyota':
-      if self.CP.openpilotLongitudinalControl:
-        if CS.cruiseState.standstill and not CS.brakePressed and self.CP_SP.enableGasInterceptor:
-          if events.has(EventName.resumeRequired):
-            events.remove(EventName.resumeRequired)
-
     return events_sp

sunnypilot/selfdrive/car/cruise_ext.py

@@ -6,24 +6,18 @@ See the LICENSE.md file in the root directory for more details.
 """
 import numpy as np
 
-from cereal import car, custom
+from cereal import car
 from opendbc.car import structs
-from openpilot.common.constants import CV
 from openpilot.common.params import Params
 from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import ACTIVE_STATES as SLA_ACTIVE_STATES
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target
 
 ButtonType = car.CarState.ButtonEvent.Type
-SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
 
 CRUISE_BUTTON_TIMER = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0,
                        ButtonType.setCruise: 0, ButtonType.resumeCruise: 0,
                        ButtonType.cancel: 0, ButtonType.mainCruise: 0}
 
 V_CRUISE_MIN = 8
-V_CRUISE_MAX = 145
-V_CRUISE_UNSET = 255
 
 
 def update_manual_button_timers(CS: car.CarState, button_timers: dict[car.CarState.ButtonEvent.Type, int]) -> None:
@@ -38,12 +32,11 @@ def update_manual_button_timers(CS: car.CarState, button_timers: dict[car.CarSta
       button_timers[b.type.raw] = 1 if b.pressed else 0
 
 
+
 class VCruiseHelperSP:
   def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> None:
     self.CP = CP
     self.CP_SP = CP_SP
-    self.v_cruise_kph = V_CRUISE_UNSET
-    self.v_cruise_cluster_kph = V_CRUISE_UNSET
     self.params = Params()
     self.v_cruise_min = 0
     self.enabled_prev = False
@@ -54,16 +47,6 @@ class VCruiseHelperSP:
 
     self.enable_button_timers = CRUISE_BUTTON_TIMER
 
-    # Speed Limit Assist
-    self.sla_state = SpeedLimitAssistState.disabled
-    self.prev_sla_state = SpeedLimitAssistState.disabled
-    self.has_speed_limit = False
-    self.speed_limit_final_last = 0.
-    self.speed_limit_final_last_kph = 0.
-    self.prev_speed_limit_final_last_kph = 0.
-    self.req_plus = False
-    self.req_minus = False
-
   def read_custom_set_speed_params(self) -> None:
     self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled")
     self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True)
@@ -106,33 +89,3 @@ class VCruiseHelperSP:
       return enabled and self.enabled_prev
 
     return enabled
-
-  def update_speed_limit_assist(self, is_metric, LP_SP: custom.LongitudinalPlanSP) -> None:
-    resolver = LP_SP.speedLimit.resolver
-    self.has_speed_limit = resolver.speedLimitValid or resolver.speedLimitLastValid
-    self.speed_limit_final_last = LP_SP.speedLimit.resolver.speedLimitFinalLast
-    self.speed_limit_final_last_kph = self.speed_limit_final_last * CV.MS_TO_KPH
-    self.sla_state = LP_SP.speedLimit.assist.state
-    self.req_plus, self.req_minus = compare_cluster_target(self.v_cruise_cluster_kph * CV.KPH_TO_MS,
-                                                           self.speed_limit_final_last, is_metric)
-
-  @property
-  def update_speed_limit_final_last_changed(self) -> bool:
-    return self.has_speed_limit and bool(self.speed_limit_final_last_kph != self.prev_speed_limit_final_last_kph)
-
-  def update_speed_limit_assist_pre_active_confirmed(self, button_type: car.CarState.ButtonEvent.Type) -> bool:
-    if self.sla_state == SpeedLimitAssistState.preActive or self.prev_sla_state == SpeedLimitAssistState.preActive:
-      if button_type == ButtonType.decelCruise and self.req_minus:
-        return True
-      if button_type == ButtonType.accelCruise and self.req_plus:
-        return True
-
-    return False
-
-  def update_speed_limit_assist_v_cruise_non_pcm(self) -> None:
-    if self.sla_state in SLA_ACTIVE_STATES and (self.prev_sla_state not in SLA_ACTIVE_STATES or
-                                                self.update_speed_limit_final_last_changed):
-      self.v_cruise_kph = np.clip(round(self.speed_limit_final_last_kph, 1), self.v_cruise_min, V_CRUISE_MAX)
-
-    self.prev_sla_state = self.sla_state
-    self.prev_speed_limit_final_last_kph = self.speed_limit_final_last_kph

sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py

@@ -16,7 +16,7 @@ State = custom.IntelligentCruiseButtonManagement.IntelligentCruiseButtonManageme
 SendButtonState = custom.IntelligentCruiseButtonManagement.SendButtonState
 
 ALLOWED_SPEED_THRESHOLD = 1.8  # m/s, ~4 MPH
-HYST_GAP = 0.0  # currently disabled; TODO-SP: might need to be brand-specific
+HYST_GAP = 0.75
 INACTIVE_TIMER = 0.4
 
 
@@ -49,11 +49,19 @@ class IntelligentCruiseButtonManagement:
   def v_cruise_equal(self) -> bool:
     return self.v_target == self.v_cruise_cluster
 
-  def update_calculations(self, CS: car.CarState, LP_SP: custom.LongitudinalPlanSP) -> None:
+  def update_calculations(self, CS: car.CarState) -> None:
     speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH
     ms_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
+    v_cruise_ms = CS.vCruise * CV.KPH_TO_MS
 
-    self.v_target_ms_last = apply_hysteresis(LP_SP.vTarget, self.v_target_ms_last, HYST_GAP * ms_conv)
+    # all targets in m/s
+    v_targets = {
+      LongitudinalPlanSource.cruise: v_cruise_ms
+    }
+    source = min(v_targets, key=lambda k: v_targets[k])
+    v_target_ms = v_targets[source]
+
+    self.v_target_ms_last = apply_hysteresis(v_target_ms, self.v_target_ms_last, HYST_GAP * ms_conv)
 
     self.v_target = round(self.v_target_ms_last * speed_conv)
     self.v_cruise_min = get_minimum_set_speed(self.is_metric)
@@ -108,18 +116,20 @@ class IntelligentCruiseButtonManagement:
   def update_readiness(self, CS: car.CarState, CC: car.CarControl) -> None:
     update_manual_button_timers(CS, self.cruise_button_timers)
 
-    ready = CC.enabled and not CC.cruiseControl.override and not CC.cruiseControl.cancel and not CC.cruiseControl.resume
+    allowed_speed = CS.vEgo > ALLOWED_SPEED_THRESHOLD
+    ready = CS.cruiseState.enabled and allowed_speed and not CC.cruiseControl.override and not CC.cruiseControl.cancel and \
+            not CC.cruiseControl.resume
     button_pressed = any(self.cruise_button_timers[k] > 0 for k in self.cruise_button_timers)
 
     self.is_ready = ready and not button_pressed
 
-  def run(self, CS: car.CarState, CC: car.CarControl, LP_SP: custom.LongitudinalPlanSP, is_metric: bool) -> None:
+  def run(self, CS: car.CarState, CC: car.CarControl, is_metric: bool) -> None:
     if self.CP_SP.pcmCruiseSpeed:
       return
 
     self.is_metric = is_metric
 
-    self.update_calculations(CS, LP_SP)
+    self.update_calculations(CS)
     self.update_readiness(CS, CC)
 
     self.cruise_button = self.update_state_machine()

sunnypilot/selfdrive/car/interfaces.py

@@ -22,18 +22,8 @@ def log_fingerprint(CP: structs.CarParams) -> None:
     sentry.capture_fingerprint(CP.carFingerprint, CP.brand)
 
 
-def _enforce_torque_lateral_control(CP: structs.CarParams, params: Params = None, enabled: bool = False) -> bool:
-  if params is None:
-    params = Params()
-
-  if CP.steerControlType != structs.CarParams.SteerControlType.angle:
-    enabled = params.get_bool("EnforceTorqueControl")
-
-  return enabled
-
-
-def _initialize_neural_network_lateral_control(CP: structs.CarParams, CP_SP: structs.CarParamsSP,
-                                               params: Params = None, enabled: bool = False) -> bool:
+def _initialize_neural_network_lateral_control(CI: CarInterfaceBase, CP: structs.CarParams, CP_SP: structs.CarParamsSP,
+                                               params: Params = None, enabled: bool = False) -> None:
   if params is None:
     params = Params()
 
@@ -45,12 +35,13 @@ def _initialize_neural_network_lateral_control(CP: structs.CarParams, CP_SP: str
   if nnlc_model_name != "MOCK" and CP.steerControlType != structs.CarParams.SteerControlType.angle:
     enabled = params.get_bool("NeuralNetworkLateralControl")
 
+  if enabled:
+    CI.configure_torque_tune(CP.carFingerprint, CP.lateralTuning)
+
   CP_SP.neuralNetworkLateralControl.model.path = nnlc_model_path
   CP_SP.neuralNetworkLateralControl.model.name = nnlc_model_name
   CP_SP.neuralNetworkLateralControl.fuzzyFingerprint = not exact_match
 
-  return enabled
-
 
 def _initialize_intelligent_cruise_button_management(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params = None) -> None:
   if params is None:
@@ -61,19 +52,12 @@ def _initialize_intelligent_cruise_button_management(CP: structs.CarParams, CP_S
     CP_SP.pcmCruiseSpeed = False
 
 
-def _initialize_torque_lateral_control(CI: CarInterfaceBase, CP: structs.CarParams, enforce_torque: bool, nnlc_enabled: bool) -> None:
-  if nnlc_enabled or enforce_torque:
-    CI.configure_torque_tune(CP.carFingerprint, CP.lateralTuning)
-
-
 def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None:
   CP = CI.CP
   CP_SP = CI.CP_SP
 
-  enforce_torque = _enforce_torque_lateral_control(CP, params)
-  nnlc_enabled = _initialize_neural_network_lateral_control(CP, CP_SP, params)
+  _initialize_neural_network_lateral_control(CI, CP, CP_SP, params)
   _initialize_intelligent_cruise_button_management(CP, CP_SP, params)
-  _initialize_torque_lateral_control(CI, CP, enforce_torque, nnlc_enabled)
 
 
 def initialize_params(params) -> list[dict[str, Any]]:

sunnypilot/selfdrive/car/tests/test_custom_cruise.py

@@ -11,8 +11,7 @@ ButtonEvent = car.CarState.ButtonEvent
 ButtonType = car.CarState.ButtonEvent.Type
 
 
-# TODO: test pcmCruise and pcmCruiseSpeed
-@parameterized_class(('pcm_cruise', 'pcm_cruise_speed'), [(False, True)])
+@parameterized_class(('pcm_cruise',), [(False,)])
 class TestCustomAccIncrements(TestVCruiseHelper):
   def setup_method(self):
     TestVCruiseHelper.setup_method(self)

sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py

@@ -1,58 +0,0 @@
-"""
-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 openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
-from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
-
-TRIGGER_THRESHOLD = 30
-
-
-class E2EAlertsHelper:
-  def __init__(self):
-    self._params = Params()
-    self._frame = -1
-
-    self.green_light_alert = False
-    self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert")
-    self.lead_depart_alert = False
-    self.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert")
-
-  def _read_params(self) -> None:
-    if self._frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
-      self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert")
-      self.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert")
-
-    self._frame += 1
-
-  def update(self, sm: messaging.SubMaster, events_sp: EventsSP) -> None:
-    self._read_params()
-
-    if not (self.green_light_alert_enabled or self.lead_depart_alert_enabled):
-      return
-
-    CS = sm['carState']
-    CC = sm['carControl']
-
-    model_x = sm['modelV2'].position.x
-    max_idx = len(model_x) - 1
-    has_lead = sm['radarState'].leadOne.status
-    lead_vRel: float = sm['radarState'].leadOne.vRel
-
-    # Green light alert
-    self.green_light_alert = (self.green_light_alert_enabled and model_x[max_idx] > TRIGGER_THRESHOLD
-                              and not has_lead and CS.standstill and not CS.gasPressed and not CC.enabled)
-
-    # Lead Departure Alert
-    self.lead_depart_alert = (self.lead_depart_alert_enabled and CS.standstill and model_x[max_idx] > 30
-                              and has_lead and lead_vRel > 1 and not CS.gasPressed)
-
-    if self.green_light_alert or self.lead_depart_alert:
-      events_sp.add(custom.OnroadEventSP.EventName.e2eChime)

sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py

@@ -6,13 +6,11 @@ See the LICENSE.md file in the root directory for more details.
 """
 
 from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.nnlc import NeuralNetworkLateralControl
-from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_override import LatControlTorqueExtOverride
 
 
-class LatControlTorqueExt(NeuralNetworkLateralControl, LatControlTorqueExtOverride):
+class LatControlTorqueExt(NeuralNetworkLateralControl):
   def __init__(self, lac_torque, CP, CP_SP, CI):
-    NeuralNetworkLateralControl.__init__(self, lac_torque, CP, CP_SP, CI)
-    LatControlTorqueExtOverride.__init__(self, CP)
+    super().__init__(lac_torque, CP, CP_SP, CI)
 
   def update(self, CS, VM, pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
              desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel,

sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_override.py

@@ -1,34 +0,0 @@
-"""
-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 openpilot.common.params import Params
-
-
-class LatControlTorqueExtOverride:
-  def __init__(self, CP):
-    self.CP = CP
-    self.params = Params()
-    self.enforce_torque_control_toggle = self.params.get_bool("EnforceTorqueControl")  # only during init
-    self.torque_override_enabled = self.params.get_bool("TorqueParamsOverrideEnabled")
-    self.frame = -1
-
-  def update_override_torque_params(self, torque_params) -> bool:
-    if not self.enforce_torque_control_toggle:
-      return False
-
-    self.frame += 1
-    if self.frame % 300 == 0:
-      self.torque_override_enabled = self.params.get_bool("TorqueParamsOverrideEnabled")
-
-      if not self.torque_override_enabled:
-        return False
-
-      torque_params.latAccelFactor = float(self.params.get("TorqueParamsOverrideLatAccelFactor", return_default=True))
-      torque_params.friction = float(self.params.get("TorqueParamsOverrideFriction", return_default=True))
-      return True
-
-    return False

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -7,34 +7,28 @@ See the LICENSE.md file in the root directory for more details.
 
 from cereal import messaging, custom
 from opendbc.car import structs
-from openpilot.common.constants import CV
-from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
-from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.speed_limit_assist import SpeedLimitAssist
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.speed_limit_resolver import SpeedLimitResolver
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 from openpilot.sunnypilot.models.helpers import get_active_bundle
 
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
-LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
+Source = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
 
 class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, mpc):
     self.events_sp = EventsSP()
+
     self.resolver = SpeedLimitResolver()
+
     self.dec = DynamicExperimentalController(CP, mpc)
     self.scc = SmartCruiseControl()
-    self.resolver = SpeedLimitResolver()
     self.sla = SpeedLimitAssist(CP)
     self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
-    self.source = LongitudinalPlanSource.cruise
-    self.e2e_alerts_helper = E2EAlertsHelper()
-
-    self.output_v_target = 0.
-    self.output_a_target = 0.
+    self.source = Source.cruise
 
   @property
   def mlsim(self) -> bool:
@@ -48,39 +42,31 @@ class LongitudinalPlannerSP:
     return self.dec.mode()
 
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
-    CS = sm['carState']
-    v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
-    v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS
-
     long_enabled = sm['carControl'].enabled
     long_override = sm['carControl'].cruiseControl.override
 
-    # Smart Cruise Control
+    self.events_sp.clear()
+
     self.scc.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)
 
-    # Speed Limit Resolver
-    self.resolver.update(v_ego, sm)
-
     # Speed Limit Assist
-    has_speed_limit = self.resolver.speed_limit_valid or self.resolver.speed_limit_last_valid
-    self.sla.update(long_enabled, long_override, v_ego, a_ego, v_cruise_cluster, self.resolver.speed_limit,
-                    self.resolver.speed_limit_final_last, has_speed_limit, self.resolver.distance, self.events_sp)
+    self.resolver.update(v_ego, sm)
+    v_cruise_sla = self.sla.update(long_enabled, long_override, v_ego, a_ego, sm['carState'].vCruiseCluster,
+                                   self.resolver.speed_limit, self.resolver.distance, self.resolver.source, self.events_sp)
 
     targets = {
-      LongitudinalPlanSource.cruise: (v_cruise, a_ego),
-      LongitudinalPlanSource.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target),
-      LongitudinalPlanSource.sccMap: (self.scc.map.output_v_target, self.scc.map.output_a_target),
-      LongitudinalPlanSource.speedLimitAssist: (self.sla.output_v_target, self.sla.output_a_target),
+      Source.cruise: (v_cruise, a_ego),
+      Source.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target),
+      Source.speedLimitAssist: (v_cruise_sla, a_ego),
     }
 
     self.source = min(targets, key=lambda k: targets[k][0])
-    self.output_v_target, self.output_a_target = targets[self.source]
-    return self.output_v_target, self.output_a_target
+    v_target, a_target = targets[self.source]
+
+    return v_target, a_target
 
   def update(self, sm: messaging.SubMaster) -> None:
-    self.events_sp.clear()
     self.dec.update(sm)
-    self.e2e_alerts_helper.update(sm, self.events_sp)
 
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
     plan_sp_send = messaging.new_message('longitudinalPlanSP')
@@ -89,8 +75,6 @@ class LongitudinalPlannerSP:
 
     longitudinalPlanSP = plan_sp_send.longitudinalPlanSP
     longitudinalPlanSP.longitudinalPlanSource = self.source
-    longitudinalPlanSP.vTarget = float(self.output_v_target)
-    longitudinalPlanSP.aTarget = float(self.output_a_target)
     longitudinalPlanSP.events = self.events_sp.to_msg()
 
     # Dynamic Experimental Control
@@ -101,7 +85,7 @@ class LongitudinalPlannerSP:
 
     # Smart Cruise Control
     smartCruiseControl = longitudinalPlanSP.smartCruiseControl
-    # Vision Control
+    # Vision Turn Speed Control
     sccVision = smartCruiseControl.vision
     sccVision.state = self.scc.vision.state
     sccVision.vTarget = float(self.scc.vision.output_v_target)
@@ -110,36 +94,15 @@ class LongitudinalPlannerSP:
     sccVision.maxPredictedLateralAccel = float(self.scc.vision.max_pred_lat_acc)
     sccVision.enabled = self.scc.vision.is_enabled
     sccVision.active = self.scc.vision.is_active
-    # Map Control
-    sccMap = smartCruiseControl.map
-    sccMap.state = self.scc.map.state
-    sccMap.vTarget = float(self.scc.map.output_v_target)
-    sccMap.aTarget = float(self.scc.map.output_a_target)
-    sccMap.enabled = self.scc.map.is_enabled
-    sccMap.active = self.scc.map.is_active
 
-    # Speed Limit
-    speedLimit = longitudinalPlanSP.speedLimit
-    resolver = speedLimit.resolver
-    resolver.speedLimit = float(self.resolver.speed_limit)
-    resolver.speedLimitLast = float(self.resolver.speed_limit_last)
-    resolver.speedLimitFinal = float(self.resolver.speed_limit_final)
-    resolver.speedLimitFinalLast = float(self.resolver.speed_limit_final_last)
-    resolver.speedLimitValid = self.resolver.speed_limit_valid
-    resolver.speedLimitLastValid = self.resolver.speed_limit_last_valid
-    resolver.speedLimitOffset = float(self.resolver.speed_limit_offset)
-    resolver.distToSpeedLimit = float(self.resolver.distance)
-    resolver.source = self.resolver.source
-    assist = speedLimit.assist
-    assist.state = self.sla.state
-    assist.enabled = self.sla.is_enabled
-    assist.active = self.sla.is_active
-    assist.vTarget = float(self.sla.output_v_target)
-    assist.aTarget = float(self.sla.output_a_target)
-
-    # E2E Alerts
-    e2eAlerts = longitudinalPlanSP.e2eAlerts
-    e2eAlerts.greenLightAlert = self.e2e_alerts_helper.green_light_alert
-    e2eAlerts.leadDepartAlert = self.e2e_alerts_helper.lead_depart_alert
+    # Speed Limit Assist
+    speedLimitAssist = longitudinalPlanSP.speedLimitAssist
+    speedLimitAssist.state = self.sla.state
+    speedLimitAssist.enabled = self.sla.is_enabled
+    speedLimitAssist.active = self.sla.is_active
+    speedLimitAssist.speedLimit = float(self.resolver.speed_limit)
+    speedLimitAssist.speedLimitOffset = float(self.sla.speed_limit_offset)
+    speedLimitAssist.distToSpeedLimit = float(self.resolver.distance)
+    speedLimitAssist.source = self.resolver.source
 
     pm.send('longitudinalPlanSP', plan_sp_send)

sunnypilot/selfdrive/controls/lib/param_store.py

@@ -22,7 +22,7 @@ class ParamStore:
     self.keys = universal_params + brand_params
     self._params = {}
 
-    self.frame = -1
+    self.frame = 0
 
   def update(self, params: Params) -> None:
     self.frame += 1

sunnypilot/selfdrive/controls/lib/smart_cruise_control/__init__.py

@@ -1,9 +0,0 @@
-"""
-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 openpilot.common.constants import CV
-
-MIN_V = 20 * CV.KPH_TO_MS  # Do not operate under 20 km/h

sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py

@@ -1,261 +0,0 @@
-import json
-import math
-import platform
-
-from cereal import custom
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
-from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
-from openpilot.sunnypilot.navd.helpers import coordinate_from_param, Coordinate
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
-
-MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
-
-ACTIVE_STATES = (MapState.turning, )
-ENABLED_STATES = (MapState.enabled, MapState.overriding, *ACTIVE_STATES)
-
-R = 6373000.0  # approximate radius of earth in meters
-TO_RADIANS = math.pi / 180
-TO_DEGREES = 180 / math.pi
-TARGET_JERK = -0.6  # m/s^3 There's some jounce limits that are not consistent so we're fudging this some
-TARGET_ACCEL = -1.2  # m/s^2 should match up with the long planner limit
-TARGET_OFFSET = 1.0  # seconds - This controls how soon before the curve you reach the target velocity. It also helps
-                     # reach the target velocity when inaccuracies in the distance modeling logic would cause overshoot.
-                     # The value is multiplied against the target velocity to determine the additional distance. This is
-                     # done to keep the distance calculations consistent but results in the offset actually being less
-                     # time than specified depending on how much of a speed differential there is between v_ego and the
-                     # target velocity.
-
-
-def velocities_from_param(param: str, params: Params):
-  if params is None:
-    params = Params()
-
-  json_str = params.get(param)
-  if json_str is None:
-    return None
-
-  velocities = json.loads(json_str)
-
-  return velocities
-
-
-def calculate_accel(t, target_jerk, a_ego):
-  return a_ego + target_jerk * t
-
-
-def calculate_velocity(t, target_jerk, a_ego, v_ego):
-  return v_ego + a_ego * t + target_jerk/2 * (t ** 2)
-
-
-def calculate_distance(t, target_jerk, a_ego, v_ego):
-  return t * v_ego + a_ego/2 * (t ** 2) + target_jerk/6 * (t ** 3)
-
-
-# points should be in radians
-# output is meters
-def distance_to_point(ax, ay, bx, by):
-  a = math.sin((bx-ax)/2)*math.sin((bx-ax)/2) + math.cos(ax) * math.cos(bx)*math.sin((by-ay)/2)*math.sin((by-ay)/2)
-  c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
-
-  return R * c  # in meters
-
-
-class SmartCruiseControlMap:
-  v_target: float = 0
-  a_target: float = 0.
-  v_ego: float = 0.
-  a_ego: float = 0.
-  output_v_target: float = V_CRUISE_UNSET
-  output_a_target: float = 0.
-
-  def __init__(self):
-    self.params = Params()
-    self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
-    self.enabled = self.params.get_bool("SmartCruiseControlMap")
-    self.long_enabled = False
-    self.long_override = False
-    self.is_enabled = False
-    self.is_active = False
-    self.state = MapState.disabled
-    self.v_cruise = 0
-    self.target_lat = 0.0
-    self.target_lon = 0.0
-    self.frame = -1
-
-    self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0)
-    self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or []
-
-  def get_v_target_from_control(self) -> float:
-    if self.is_active:
-      return max(self.v_target, MIN_V)
-
-    return V_CRUISE_UNSET
-
-  def get_a_target_from_control(self) -> float:
-    return self.a_ego
-
-  def update_params(self):
-    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
-      self.enabled = self.params.get_bool("SmartCruiseControlMap")
-
-  def update_calculations(self) -> None:
-    self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0)
-    lat = self.last_position.latitude
-    lon = self.last_position.longitude
-
-    self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or []
-
-    if self.last_position is None or self.target_velocities is None:
-      return
-
-    min_dist = 1000
-    min_idx = 0
-    distances = []
-
-    # find our location in the path
-    for i in range(len(self.target_velocities)):
-      target_velocity = self.target_velocities[i]
-      tlat = target_velocity["latitude"]
-      tlon = target_velocity["longitude"]
-      d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, tlat * TO_RADIANS, tlon * TO_RADIANS)
-      distances.append(d)
-      if d < min_dist:
-        min_dist = d
-        min_idx = i
-
-    # only look at values from our current position forward
-    forward_points = self.target_velocities[min_idx:]
-    forward_distances = distances[min_idx:]
-
-    # find velocities that we are within the distance we need to adjust for
-    valid_velocities = []
-    for i in range(len(forward_points)):
-      target_velocity = forward_points[i]
-      tlat = target_velocity["latitude"]
-      tlon = target_velocity["longitude"]
-      tv = target_velocity["velocity"]
-      if tv > self.v_ego:
-        continue
-
-      d = forward_distances[i]
-
-      a_diff = (self.a_ego - TARGET_ACCEL)
-      accel_t = abs(a_diff / TARGET_JERK)
-      min_accel_v = calculate_velocity(accel_t, TARGET_JERK, self.a_ego, self.v_ego)
-
-      max_d = 0
-      if tv > min_accel_v:
-        # calculate time needed based on target jerk
-        a = 0.5 * TARGET_JERK
-        b = self.a_ego
-        c = self.v_ego - tv
-        t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a
-        t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a
-        if not isinstance(t_a, complex) and t_a > 0:
-          t = t_a
-        else:
-          t = t_b
-        if isinstance(t, complex):
-          continue
-
-        max_d = max_d + calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego)
-      else:
-        t = accel_t
-        max_d = calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego)
-
-        # calculate additional time needed based on target accel
-        t = abs((min_accel_v - tv) / TARGET_ACCEL)
-        max_d += calculate_distance(t, 0, TARGET_ACCEL, min_accel_v)
-
-      if d < max_d + tv * TARGET_OFFSET:
-        valid_velocities.append((float(tv), tlat, tlon))
-
-    # Find the smallest velocity we need to adjust for
-    min_v = 100.0
-    target_lat = 0.0
-    target_lon = 0.0
-    for tv, lat, lon in valid_velocities:
-      if tv < min_v:
-        min_v = tv
-        target_lat = lat
-        target_lon = lon
-
-    if self.v_target < min_v and not (self.target_lat == 0 and self.target_lon == 0):
-      for i in range(len(forward_points)):
-        target_velocity = forward_points[i]
-        tlat = target_velocity["latitude"]
-        tlon = target_velocity["longitude"]
-        tv = target_velocity["velocity"]
-        if tv > self.v_ego:
-          continue
-
-        if tlat == self.target_lat and tlon == self.target_lon and tv == self.v_target:
-          return
-
-      # not found so let's reset
-      self.v_target = 0.0
-      self.target_lat = 0.0
-      self.target_lon = 0.0
-
-    self.v_target = min_v
-    self.target_lat = target_lat
-    self.target_lon = target_lon
-
-  def _update_state_machine(self) -> tuple[bool, bool]:
-    # ENABLED, TURNING
-    if self.state != MapState.disabled:
-      if not self.long_enabled or not self.enabled:
-        self.state = MapState.disabled
-      elif self.long_override:
-        self.state = MapState.overriding
-
-      else:
-        # ENABLED
-        if self.state == MapState.enabled:
-          if self.v_cruise > self.v_target != 0:
-            self.state = MapState.turning
-
-        # TURNING
-        elif self.state == MapState.turning:
-          if self.v_cruise <= self.v_target or self.v_target == 0:
-            self.state = MapState.enabled
-
-        # OVERRIDING
-        elif self.state == MapState.overriding:
-          if not self.long_override:
-            if self.v_cruise > self.v_target != 0:
-              self.state = MapState.turning
-            else:
-              self.state = MapState.enabled
-
-    # DISABLED
-    elif self.state == MapState.disabled:
-      if self.long_enabled and self.enabled:
-        if self.long_override:
-          self.state = MapState.overriding
-        else:
-          self.state = MapState.enabled
-
-    enabled = self.state in ENABLED_STATES
-    active = self.state in ACTIVE_STATES
-
-    return enabled, active
-
-  def update(self, long_enabled: bool, long_override: bool, v_ego, a_ego, v_cruise) -> None:
-    self.long_enabled = long_enabled
-    self.long_override = long_override
-    self.v_ego = v_ego
-    self.a_ego = a_ego
-    self.v_cruise = v_cruise
-
-    self.update_params()
-    self.update_calculations()
-
-    self.is_enabled, self.is_active = self._update_state_machine()
-
-    self.output_v_target = self.get_v_target_from_control()
-    self.output_a_target = self.get_a_target_from_control()
-
-    self.frame += 1

sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py

@@ -6,14 +6,11 @@ See the LICENSE.md file in the root directory for more details.
 """
 import cereal.messaging as messaging
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
 
 
 class SmartCruiseControl:
   def __init__(self):
     self.vision = SmartCruiseControlVision()
-    self.map = SmartCruiseControlMap()
 
   def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise: float) -> None:
-    self.map.update(long_enabled, long_override, v_ego, a_ego, v_cruise)
     self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)

sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_map_controller.py

@@ -1,58 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import platform
-
-from cereal import custom
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
-
-MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
-
-
-class TestSmartCruiseControlMap:
-
-  def setup_method(self):
-    self.params = Params()
-    self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
-    self.reset_params()
-    self.scc_m = SmartCruiseControlMap()
-
-  def reset_params(self):
-    self.params.put_bool("SmartCruiseControlMap", True)
-
-    # TODO-SP: mock data from gpsLocation
-    self.params.put("LastGPSPosition", "{}")
-    self.params.put("MapTargetVelocities", "{}")
-
-  def test_initial_state(self):
-    assert self.scc_m.state == VisionState.disabled
-    assert not self.scc_m.is_active
-    assert self.scc_m.output_v_target == V_CRUISE_UNSET
-    assert self.scc_m.output_a_target == 0.
-
-  def test_system_disabled(self):
-    self.params.put_bool("SmartCruiseControlMap", False)
-    self.scc_m.enabled = self.params.get_bool("SmartCruiseControlMap")
-
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_m.update(True, False, 0., 0., 0.)
-    assert self.scc_m.state == VisionState.disabled
-    assert not self.scc_m.is_active
-
-  def test_disabled(self):
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_m.update(False, False, 0., 0., 0.)
-    assert self.scc_m.state == VisionState.disabled
-
-  def test_transition_disabled_to_enabled(self):
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_m.update(True, False, 0., 0., 0.)
-    assert self.scc_m.state == VisionState.enabled
-
-  # TODO-SP: mock data from modelV2 to test other states

sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py

@@ -8,17 +8,19 @@ import numpy as np
 
 import cereal.messaging as messaging
 from cereal import custom
+from openpilot.common.constants import CV
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
 from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
 
 VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
 
 ACTIVE_STATES = (VisionState.entering, VisionState.turning, VisionState.leaving)
 ENABLED_STATES = (VisionState.enabled, VisionState.overriding, *ACTIVE_STATES)
 
+_MIN_V = 20 * CV.KPH_TO_MS  # Do not operate under 20 km/h
+
 _ENTERING_PRED_LAT_ACC_TH = 1.3  # Predicted Lat Acc threshold to trigger entering turn state.
 _ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1  # Predicted Lat Acc threshold to abort entering state if speed drops.
 
@@ -71,7 +73,7 @@ class SmartCruiseControlVision:
 
   def get_v_target_from_control(self) -> float:
     if self.is_active:
-      return max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON
+      return max(self.v_target, _MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON
 
     return V_CRUISE_UNSET
 
@@ -112,7 +114,7 @@ class SmartCruiseControlVision:
         # ENABLED
         if self.state == VisionState.enabled:
           # Do not enter a turn control cycle if the speed is low.
-          if self.v_ego <= MIN_V:
+          if self.v_ego <= _MIN_V:
             pass
           # If significant lateral acceleration is predicted ahead, then move to Entering turn state.
           elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:

sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py

@@ -1,19 +0,0 @@
-"""
-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.
-"""
-LIMIT_ADAPT_ACC = -1.  # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits.
-LIMIT_MAX_MAP_DATA_AGE = 10.  # s Maximum time to hold to map data, then consider it invalid inside limits controllers.
-
-# Speed Limit Assist constants
-PCM_LONG_REQUIRED_MAX_SET_SPEED = {
-  True: (33.3333, 36.1111),  # km/h, (120, 130)
-  False: (31.2928, 35.7632),  # mph, (70, 80)
-}
-
-CONFIRM_SPEED_THRESHOLD = {
-  True: 80,   # km/h
-  False: 50,  # mph
-}

sunnypilot/selfdrive/controls/lib/speed_limit/common.py

@@ -1,28 +0,0 @@
-"""
-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 enum import IntEnum
-
-
-class Policy(IntEnum):
-  car_state_only = 0
-  map_data_only = 1
-  car_state_priority = 2
-  map_data_priority = 3
-  combined = 4
-
-
-class OffsetType(IntEnum):
-  off = 0
-  fixed = 1
-  percentage = 2
-
-
-class Mode(IntEnum):
-  off = 0
-  information = 1
-  warning = 2
-  assist = 3

sunnypilot/selfdrive/controls/lib/speed_limit/helpers.py

@@ -1,19 +0,0 @@
-"""
-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 openpilot.common.constants import CV
-
-
-def compare_cluster_target(v_cruise_cluster: float, target_set_speed: float, is_metric: bool) -> tuple[bool, bool]:
-  speed_conv = CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH
-  v_cruise_cluster_conv = round(v_cruise_cluster * speed_conv)
-  target_set_speed_conv = round(target_set_speed * speed_conv)
-
-  req_plus = v_cruise_cluster_conv < target_set_speed_conv
-  req_minus = v_cruise_cluster_conv > target_set_speed_conv
-
-  return req_plus, req_minus

sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py

@@ -1,397 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import time
-
-from cereal import custom, car
-from openpilot.common.params import Params
-from openpilot.common.constants import CV
-from openpilot.common.realtime import DT_MDL
-from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
-from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
-from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target
-from openpilot.selfdrive.modeld.constants import ModelConstants
-
-ButtonType = car.CarState.ButtonEvent.Type
-EventNameSP = custom.OnroadEventSP.EventName
-SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
-SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
-
-ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting)
-ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, *ACTIVE_STATES)
-
-DISABLED_GUARD_PERIOD = 0.5  # secs.
-PRE_ACTIVE_GUARD_PERIOD = 15  # secs. Time to wait after activation before considering temp deactivation signal.
-SPEED_LIMIT_CHANGED_HOLD_PERIOD = 1  # secs. Time to wait after speed limit change before switching to preActive.
-
-LIMIT_MIN_ACC = -1.5  # m/s^2 Maximum deceleration allowed for limit controllers to provide.
-LIMIT_MAX_ACC = 1.0   # m/s^2 Maximum acceleration allowed for limit controllers to provide while active.
-LIMIT_MIN_SPEED = 8.33  # m/s, Minimum speed limit to provide as solution on limit controllers.
-LIMIT_SPEED_OFFSET_TH = -1.  # m/s Maximum offset between speed limit and current speed for adapting state.
-V_CRUISE_UNSET = 255
-
-CRUISE_BUTTONS_PLUS = (ButtonType.accelCruise, ButtonType.resumeCruise)
-CRUISE_BUTTONS_MINUS = (ButtonType.decelCruise, ButtonType.setCruise)
-CRUISE_BUTTON_CONFIRM_HOLD = 0.5  # secs.
-
-
-class SpeedLimitAssist:
-  _speed_limit_final_last: float
-  _distance: float
-  v_ego: float
-  a_ego: float
-  v_offset: float
-
-  def __init__(self, CP):
-    self.params = Params()
-    self.CP = CP
-    self.frame = -1
-    self.long_engaged_timer = 0
-    self.pre_active_timer = 0
-    self.is_metric = self.params.get_bool("IsMetric")
-    self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
-    self.long_enabled = False
-    self.long_enabled_prev = False
-    self.is_enabled = False
-    self.is_active = False
-    self.output_v_target = V_CRUISE_UNSET
-    self.output_a_target = 0.
-    self.v_ego = 0.
-    self.a_ego = 0.
-    self.v_offset = 0.
-    self.target_set_speed_conv = 0
-    self.prev_target_set_speed_conv = 0
-    self.v_cruise_cluster = 0.
-    self.v_cruise_cluster_prev = 0.
-    self.v_cruise_cluster_conv = 0
-    self.prev_v_cruise_cluster_conv = 0
-    self._has_speed_limit = False
-    self._speed_limit = 0.
-    self._speed_limit_final_last = 0.
-    self.speed_limit_prev = 0.
-    self.speed_limit_final_last_conv = 0
-    self.prev_speed_limit_final_last_conv = 0
-    self._distance = 0.
-    self.state = SpeedLimitAssistState.disabled
-    self._state_prev = SpeedLimitAssistState.disabled
-    self.pcm_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise
-
-    self._plus_hold = 0.
-    self._minus_hold = 0.
-    self._last_carstate_ts = 0.
-
-    # TODO-SP: SLA's own output_a_target for planner
-    # Solution functions mapped to respective states
-    self.acceleration_solutions = {
-      SpeedLimitAssistState.disabled: self.get_current_acceleration_as_target,
-      SpeedLimitAssistState.inactive: self.get_current_acceleration_as_target,
-      SpeedLimitAssistState.preActive: self.get_current_acceleration_as_target,
-      SpeedLimitAssistState.pending: self.get_current_acceleration_as_target,
-      SpeedLimitAssistState.adapting: self.get_adapting_state_target_acceleration,
-      SpeedLimitAssistState.active: self.get_active_state_target_acceleration,
-    }
-
-  @property
-  def speed_limit_changed(self) -> bool:
-    return self._has_speed_limit and bool(self._speed_limit != self.speed_limit_prev)
-
-  @property
-  def v_cruise_cluster_changed(self) -> bool:
-    return bool(self.v_cruise_cluster_conv != self.prev_v_cruise_cluster_conv)
-
-  @property
-  def target_set_speed_confirmed(self) -> bool:
-    return bool(self.v_cruise_cluster_conv == self.target_set_speed_conv)
-
-  def get_v_target_from_control(self) -> float:
-    if self._has_speed_limit:
-      if self.pcm_op_long and self.is_enabled:
-        return self._speed_limit_final_last
-      if not self.pcm_op_long and self.is_active:
-        return self._speed_limit_final_last
-
-    # Fallback
-    return V_CRUISE_UNSET
-
-  # TODO-SP: SLA's own output_a_target for planner
-  def get_a_target_from_control(self) -> float:
-    return self.a_ego
-
-  def update_params(self) -> None:
-    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
-      self.is_metric = self.params.get_bool("IsMetric")
-      self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
-
-  def update_car_state(self, CS: car.CarState) -> None:
-    now = time.monotonic()
-    self._last_carstate_ts = now
-
-    for b in CS.buttonEvents:
-      if not b.pressed:
-        if b.type in CRUISE_BUTTONS_PLUS:
-          self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
-        elif b.type in CRUISE_BUTTONS_MINUS:
-          self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
-
-  def _get_button_release(self, req_plus: bool, req_minus: bool) -> bool:
-    now = time.monotonic()
-    if req_plus and now <= self._plus_hold:
-      self._plus_hold = 0.
-      return True
-    elif req_minus and now <= self._minus_hold:
-      self._minus_hold = 0.
-      return True
-
-    # expired
-    if now > self._plus_hold:
-      self._plus_hold = 0.
-    if now > self._minus_hold:
-      self._minus_hold = 0.
-    return False
-
-  def update_calculations(self, v_cruise_cluster: float) -> None:
-    speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH
-    self.v_cruise_cluster = v_cruise_cluster
-
-    # Update current velocity offset (error)
-    self.v_offset = self._speed_limit_final_last - self.v_ego
-
-    self.speed_limit_final_last_conv = round(self._speed_limit_final_last * speed_conv)
-    self.v_cruise_cluster_conv = round(self.v_cruise_cluster * speed_conv)
-
-    cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.is_metric]
-    pcm_long_required_max = cst_low if self._has_speed_limit and self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric] else \
-                            cst_high
-    pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv)
-
-    self.target_set_speed_conv = pcm_long_required_max_set_speed_conv if self.pcm_op_long else self.speed_limit_final_last_conv
-
-  @property
-  def apply_confirm_speed_threshold(self) -> bool:
-    # below CST: always require user confirmation
-    if self.v_cruise_cluster_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric]:
-      return True
-
-    # at/above CST:
-    # - new speed limit >= CST: auto change
-    # - new speed limit < CST: user confirmation required
-    return bool(self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric])
-
-  def get_current_acceleration_as_target(self) -> float:
-    return self.a_ego
-
-  def get_adapting_state_target_acceleration(self) -> float:
-    if self._distance > 0:
-      return (self._speed_limit_final_last ** 2 - self.v_ego ** 2) / (2. * self._distance)
-
-    return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
-
-  def get_active_state_target_acceleration(self) -> float:
-    return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
-
-  def _update_confirmed_state(self):
-    if self._has_speed_limit:
-      if self.v_offset < LIMIT_SPEED_OFFSET_TH:
-        self.state = SpeedLimitAssistState.adapting
-      else:
-        self.state = SpeedLimitAssistState.active
-    else:
-      self.state = SpeedLimitAssistState.pending
-
-  def _update_non_pcm_long_confirmed_state(self) -> bool:
-    if self.target_set_speed_confirmed:
-      return True
-
-    if self.state != SpeedLimitAssistState.preActive:
-      return False
-
-    req_plus, req_minus = compare_cluster_target(self.v_cruise_cluster, self._speed_limit_final_last, self.is_metric)
-
-    return self._get_button_release(req_plus, req_minus)
-
-  def update_state_machine_pcm_op_long(self):
-    self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
-    self.pre_active_timer = max(0, self.pre_active_timer - 1)
-
-    # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE
-    if self.state != SpeedLimitAssistState.disabled:
-      if not self.long_enabled or not self.enabled:
-        self.state = SpeedLimitAssistState.disabled
-
-      else:
-        # ACTIVE
-        if self.state == SpeedLimitAssistState.active:
-          if self.v_cruise_cluster_changed:
-            self.state = SpeedLimitAssistState.inactive
-          elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
-          elif self._has_speed_limit and self.v_offset < LIMIT_SPEED_OFFSET_TH:
-            self.state = SpeedLimitAssistState.adapting
-
-        # ADAPTING
-        elif self.state == SpeedLimitAssistState.adapting:
-          if self.v_cruise_cluster_changed:
-            self.state = SpeedLimitAssistState.inactive
-          elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
-          elif self.v_offset >= LIMIT_SPEED_OFFSET_TH:
-            self.state = SpeedLimitAssistState.active
-
-        # PENDING
-        elif self.state == SpeedLimitAssistState.pending:
-          if self.target_set_speed_confirmed:
-            self._update_confirmed_state()
-          elif self.speed_limit_changed:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
-
-        # PRE_ACTIVE
-        elif self.state == SpeedLimitAssistState.preActive:
-          if self.target_set_speed_confirmed:
-            self._update_confirmed_state()
-          elif self.pre_active_timer <= 0:
-            # Timeout - session ended
-            self.state = SpeedLimitAssistState.inactive
-
-        # INACTIVE
-        elif self.state == SpeedLimitAssistState.inactive:
-          pass
-
-    # DISABLED
-    elif self.state == SpeedLimitAssistState.disabled:
-      if self.long_enabled and self.enabled:
-        # start or reset preActive timer if initially enabled or manual set speed change detected
-        if not self.long_enabled_prev or self.v_cruise_cluster_changed:
-          self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
-
-        elif self.long_engaged_timer <= 0:
-          if self.target_set_speed_confirmed:
-            self._update_confirmed_state()
-          elif self._has_speed_limit:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
-          else:
-            self.state = SpeedLimitAssistState.pending
-
-    enabled = self.state in ENABLED_STATES
-    active = self.state in ACTIVE_STATES
-
-    return enabled, active
-
-  def update_state_machine_non_pcm_long(self):
-    self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
-    self.pre_active_timer = max(0, self.pre_active_timer - 1)
-
-    # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE
-    if self.state != SpeedLimitAssistState.disabled:
-      if not self.long_enabled or not self.enabled:
-        self.state = SpeedLimitAssistState.disabled
-
-      else:
-        # ACTIVE
-        if self.state == SpeedLimitAssistState.active:
-          if self.v_cruise_cluster_changed:
-            self.state = SpeedLimitAssistState.inactive
-
-          elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
-
-        # PRE_ACTIVE
-        elif self.state == SpeedLimitAssistState.preActive:
-          if self._update_non_pcm_long_confirmed_state():
-            self.state = SpeedLimitAssistState.active
-          elif self.pre_active_timer <= 0:
-            # Timeout - session ended
-            self.state = SpeedLimitAssistState.inactive
-
-        # INACTIVE
-        elif self.state == SpeedLimitAssistState.inactive:
-          if self.speed_limit_changed:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
-          elif self._update_non_pcm_long_confirmed_state():
-            self.state = SpeedLimitAssistState.active
-
-    # DISABLED
-    elif self.state == SpeedLimitAssistState.disabled:
-      if self.long_enabled and self.enabled:
-        # start or reset preActive timer if initially enabled or manual set speed change detected
-        if not self.long_enabled_prev or self.v_cruise_cluster_changed:
-          self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
-
-        elif self.long_engaged_timer <= 0:
-          if self._update_non_pcm_long_confirmed_state():
-            self.state = SpeedLimitAssistState.active
-          elif self._has_speed_limit:
-            self.state = SpeedLimitAssistState.preActive
-            self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
-          else:
-            self.state = SpeedLimitAssistState.inactive
-
-    enabled = self.state in ENABLED_STATES
-    active = self.state in ACTIVE_STATES
-
-    return enabled, active
-
-  def update_events(self, events_sp: EventsSP) -> None:
-    if self.state == SpeedLimitAssistState.preActive:
-      events_sp.add(EventNameSP.speedLimitPreActive)
-
-    if self.state == SpeedLimitAssistState.pending and self._state_prev != SpeedLimitAssistState.pending:
-      events_sp.add(EventNameSP.speedLimitPending)
-
-    if self.is_active:
-      if self._state_prev not in ACTIVE_STATES:
-        events_sp.add(EventNameSP.speedLimitActive)
-
-      # only notify if we acquire a valid speed limit
-      # do not check has_speed_limit here
-      elif self._speed_limit != self.speed_limit_prev:
-        if self.speed_limit_prev <= 0:
-          events_sp.add(EventNameSP.speedLimitActive)
-        elif self.speed_limit_prev > 0 and self._speed_limit > 0:
-          events_sp.add(EventNameSP.speedLimitChanged)
-
-  def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float, speed_limit: float,
-             speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP) -> None:
-    self.long_enabled = long_enabled
-    self.v_ego = v_ego
-    self.a_ego = a_ego
-
-    self._has_speed_limit = has_speed_limit
-    self._speed_limit = speed_limit
-    self._speed_limit_final_last = speed_limit_final_last
-    self._distance = distance
-
-    self.update_params()
-    self.update_calculations(v_cruise_cluster)
-
-    self._state_prev = self.state
-    if self.pcm_op_long:
-      self.is_enabled, self.is_active = self.update_state_machine_pcm_op_long()
-    else:
-      self.is_enabled, self.is_active = self.update_state_machine_non_pcm_long()
-
-    self.update_events(events_sp)
-
-    # Update change tracking variables
-    self.speed_limit_prev = self._speed_limit
-    self.v_cruise_cluster_prev = self.v_cruise_cluster
-    self.long_enabled_prev = self.long_enabled
-    self.prev_target_set_speed_conv = self.target_set_speed_conv
-    self.prev_v_cruise_cluster_conv = self.v_cruise_cluster_conv
-    self.prev_speed_limit_final_last_conv = self.speed_limit_final_last_conv
-
-    self.output_v_target = self.get_v_target_from_control()
-    self.output_a_target = self.get_a_target_from_control()
-
-    self.frame += 1

sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py

@@ -1,178 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import time
-
-import cereal.messaging as messaging
-from cereal import custom
-from openpilot.common.constants import CV
-from openpilot.common.gps import get_gps_location_service
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy, OffsetType
-
-SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
-
-ALL_SOURCES = tuple(SpeedLimitSource.schema.enumerants.values())
-
-
-class SpeedLimitResolver:
-  limit_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float]
-  distance_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float]
-  v_ego: float
-  speed_limit: float
-  speed_limit_last: float
-  speed_limit_final: float
-  speed_limit_final_last: float
-  distance: float
-  source: custom.LongitudinalPlanSP.SpeedLimit.Source
-  speed_limit_offset: float
-
-  def __init__(self):
-    self.params = Params()
-    self.frame = -1
-
-    self._gps_location_service = get_gps_location_service(self.params)
-    self.limit_solutions = {}  # Store for speed limit solutions from different sources
-    self.distance_solutions = {}  # Store for distance to current speed limit start for different sources
-
-    self.policy = self.params.get("SpeedLimitPolicy", return_default=True)
-    self._policy_to_sources_map = {
-      Policy.car_state_only: [SpeedLimitSource.car],
-      Policy.map_data_only: [SpeedLimitSource.map],
-      Policy.car_state_priority: [SpeedLimitSource.car, SpeedLimitSource.map],
-      Policy.map_data_priority: [SpeedLimitSource.map, SpeedLimitSource.car],
-      Policy.combined: [SpeedLimitSource.car, SpeedLimitSource.map],
-    }
-    self.source = SpeedLimitSource.none
-    for source in ALL_SOURCES:
-      self._reset_limit_sources(source)
-
-    self.is_metric = self.params.get_bool("IsMetric")
-    self.offset_type = self.params.get("SpeedLimitOffsetType", return_default=True)
-    self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
-
-    self.speed_limit = 0.
-    self.speed_limit_last = 0.
-    self.speed_limit_final = 0.
-    self.speed_limit_final_last = 0.
-    self.speed_limit_offset = 0.
-
-  def update_speed_limit_states(self) -> None:
-    self.speed_limit_final = self.speed_limit + self.speed_limit_offset
-
-    if self.speed_limit > 0.:
-      self.speed_limit_last = self.speed_limit
-      self.speed_limit_final_last = self.speed_limit_final
-
-  @property
-  def speed_limit_valid(self) -> bool:
-    return self.speed_limit > 0.
-
-  @property
-  def speed_limit_last_valid(self) -> bool:
-    return self.speed_limit_last > 0.
-
-  def update_params(self):
-    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
-      self.policy = self.params.get("SpeedLimitPolicy", return_default=True)
-      self.is_metric = self.params.get_bool("IsMetric")
-      self.offset_type = self.params.get("SpeedLimitOffsetType", return_default=True)
-      self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
-
-  def _get_speed_limit_offset(self) -> float:
-    if self.offset_type == OffsetType.off:
-      return 0
-    elif self.offset_type == OffsetType.fixed:
-      return float(self.offset_value * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS))
-    elif self.offset_type == OffsetType.percentage:
-      return float(self.offset_value * 0.01 * self.speed_limit)
-    else:
-      raise NotImplementedError("Offset not supported")
-
-  def _reset_limit_sources(self, source: custom.LongitudinalPlanSP.SpeedLimit.Source) -> None:
-    self.limit_solutions[source] = 0.
-    self.distance_solutions[source] = 0.
-
-  def _get_from_car_state(self, sm: messaging.SubMaster) -> None:
-    self._reset_limit_sources(SpeedLimitSource.car)
-    self.limit_solutions[SpeedLimitSource.car] = sm['carStateSP'].speedLimit
-    self.distance_solutions[SpeedLimitSource.car] = 0.
-
-  def _get_from_map_data(self, sm: messaging.SubMaster) -> None:
-    self._reset_limit_sources(SpeedLimitSource.map)
-    self._process_map_data(sm)
-
-  def _process_map_data(self, sm: messaging.SubMaster) -> None:
-    gps_data = sm[self._gps_location_service]
-    map_data = sm['liveMapDataSP']
-
-    gps_fix_age = time.monotonic() - gps_data.unixTimestampMillis * 1e-3
-    if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE:
-      return
-
-    speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0.
-    next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0.
-
-    self._calculate_map_data_limits(sm, speed_limit, next_speed_limit)
-
-  def _calculate_map_data_limits(self, sm: messaging.SubMaster, speed_limit: float, next_speed_limit: float) -> None:
-    gps_data = sm[self._gps_location_service]
-    map_data = sm['liveMapDataSP']
-
-    distance_since_fix = self.v_ego * (time.monotonic() - gps_data.unixTimestampMillis * 1e-3)
-    distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix)
-
-    self.limit_solutions[SpeedLimitSource.map] = speed_limit
-    self.distance_solutions[SpeedLimitSource.map] = 0.
-
-    if 0. < next_speed_limit < self.v_ego:
-      adapt_time = (next_speed_limit - self.v_ego) / LIMIT_ADAPT_ACC
-      adapt_distance = self.v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time ** 2
-
-      if distance_to_speed_limit_ahead <= adapt_distance:
-        self.limit_solutions[SpeedLimitSource.map] = next_speed_limit
-        self.distance_solutions[SpeedLimitSource.map] = distance_to_speed_limit_ahead
-
-  def _get_source_solution_according_to_policy(self) -> custom.LongitudinalPlanSP.SpeedLimit.Source:
-    sources_for_policy = self._policy_to_sources_map[self.policy]
-
-    if self.policy != Policy.combined:
-      # They are ordered in the order of preference, so we pick the first that's non-zero
-      for source in sources_for_policy:
-        if self.limit_solutions[source] > 0.:
-          return source
-      return SpeedLimitSource.none
-
-    sources_with_limits = [(s, limit) for s, limit in [(s, self.limit_solutions[s]) for s in sources_for_policy] if limit > 0.]
-    if sources_with_limits:
-      return min(sources_with_limits, key=lambda x: x[1])[0]
-
-    return SpeedLimitSource.none
-
-  def _resolve_limit_sources(self, sm: messaging.SubMaster) -> tuple[float, float, custom.LongitudinalPlanSP.SpeedLimit.Source]:
-    """Get limit solutions from each data source"""
-    self._get_from_car_state(sm)
-    self._get_from_map_data(sm)
-
-    source = self._get_source_solution_according_to_policy()
-    speed_limit = self.limit_solutions[source] if source else 0.
-    distance = self.distance_solutions[source] if source else 0.
-
-    return speed_limit, distance, source
-
-  def update(self, v_ego: float, sm: messaging.SubMaster) -> None:
-    self.v_ego = v_ego
-    self.update_params()
-
-    self.speed_limit, self.distance, self.source = self._resolve_limit_sources(sm)
-    self.speed_limit_offset = self._get_speed_limit_offset()
-
-    self.update_speed_limit_states()
-
-    self.frame += 1

sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py

@@ -1,241 +0,0 @@
-"""
-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 custom
-
-from opendbc.car.car_helpers import interfaces
-from opendbc.car.toyota.values import CAR as TOYOTA
-from openpilot.common.constants import CV
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
-from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist, \
-  PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES
-from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
-
-SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
-
-ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values())
-
-SPEED_LIMITS = {
-  'residential': 25 * CV.MPH_TO_MS,  # 25 mph
-  'city': 35 * CV.MPH_TO_MS,         # 35 mph
-  'highway': 65 * CV.MPH_TO_MS,      # 65 mph
-  'freeway': 80 * CV.MPH_TO_MS,      # 80 mph
-}
-
-
-class TestSpeedLimitAssist:
-
-  def setup_method(self):
-    self.params = Params()
-    self.reset_custom_params()
-    self.events_sp = EventsSP()
-    CI = self._setup_platform(TOYOTA.TOYOTA_RAV4_TSS2)
-    self.sla = SpeedLimitAssist(CI.CP)
-    self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
-    self.pcm_long_max_set_speed = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric][1]  # use 80 MPH for now
-    self.speed_conv = CV.MS_TO_KPH if self.sla.is_metric else CV.MS_TO_MPH
-
-  def teardown_method(self, method):
-    self.reset_state()
-
-  def _setup_platform(self, car_name):
-    CarInterface = interfaces[car_name]
-    CP = CarInterface.get_non_essential_params(car_name)
-    CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
-    CI = CarInterface(CP, CP_SP)
-    sunnypilot_interfaces.setup_interfaces(CI, self.params)
-    return CI
-
-  def reset_custom_params(self):
-    self.params.put("SpeedLimitMode", int(Mode.assist))
-    self.params.put_bool("IsMetric", False)
-    self.params.put("SpeedLimitOffsetType", 0)
-    self.params.put("SpeedLimitValueOffset", 0)
-
-  def reset_state(self):
-    self.sla.state = SpeedLimitAssistState.disabled
-    self.sla.frame = -1
-    self.sla.last_op_engaged_frame = 0
-    self.sla.op_engaged = False
-    self.sla.op_engaged_prev = False
-    self.sla._speed_limit = 0.
-    self.sla.speed_limit_prev = 0.
-    self.sla.last_valid_speed_limit_offsetted = 0.
-    self.sla._distance = 0.
-    self.events_sp.clear()
-
-  def initialize_active_state(self, initialize_v_cruise):
-    self.sla.state = SpeedLimitAssistState.active
-    self.sla.v_cruise_cluster = initialize_v_cruise
-    self.sla.v_cruise_cluster_prev = initialize_v_cruise
-    self.sla.prev_v_cruise_cluster_conv = round(initialize_v_cruise * self.speed_conv)
-
-  def test_initial_state(self):
-    assert self.sla.state == SpeedLimitAssistState.disabled
-    assert not self.sla.is_enabled
-    assert not self.sla.is_active
-    assert V_CRUISE_UNSET == self.sla.get_v_target_from_control()
-
-  def test_disabled(self):
-    self.params.put("SpeedLimitMode", int(Mode.off))
-    for _ in range(int(10. / DT_MDL)):
-      self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.disabled
-
-  def test_transition_disabled_to_preactive(self):
-    for _ in range(int(3. / DT_MDL)):
-      self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.preActive
-    assert self.sla.is_enabled and not self.sla.is_active
-
-  def test_transition_disabled_to_pending_no_speed_limit_not_max_initial_set_speed(self):
-    for _ in range(int(3. / DT_MDL)):
-      self.sla.update(True, False, SPEED_LIMITS['highway'], 0, SPEED_LIMITS['city'], 0, 0, False, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.pending
-    assert self.sla.is_enabled and not self.sla.is_active
-
-  def test_preactive_to_active_with_max_speed_confirmation(self):
-    self.sla.state = SpeedLimitAssistState.preActive
-    self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
-                    SPEED_LIMITS['highway'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.active
-    assert self.sla.is_enabled and self.sla.is_active
-    assert self.sla.output_v_target == SPEED_LIMITS['highway']
-
-  def test_preactive_timeout_to_inactive(self):
-    self.sla.state = SpeedLimitAssistState.preActive
-    self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-
-    for _ in range(int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)):
-      self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.inactive
-
-  def test_preactive_to_pending_no_speed_limit(self):
-    self.sla.state = SpeedLimitAssistState.preActive
-    self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed, 0, 0, False, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.pending
-    assert self.sla.is_enabled and not self.sla.is_active
-
-  def test_pending_to_active_when_speed_limit_available(self):
-    self.sla.state = SpeedLimitAssistState.pending
-    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
-    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
-
-    self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed,
-                    SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.active
-
-  def test_pending_to_adapting_when_below_speed_limit(self):
-    self.sla.state = SpeedLimitAssistState.pending
-    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
-    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
-
-    self.sla.update(True, False, SPEED_LIMITS['highway'] + 5, 0, self.pcm_long_max_set_speed,
-                    SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.adapting
-    assert self.sla.is_enabled and self.sla.is_active
-
-  def test_active_to_adapting_transition(self):
-    self.initialize_active_state(self.pcm_long_max_set_speed)
-
-    self.sla.update(True, False, SPEED_LIMITS['highway'] + 2, 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
-                    SPEED_LIMITS['highway'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.adapting
-
-  def test_adapting_to_active_transition(self):
-    self.sla.state = SpeedLimitAssistState.adapting
-    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
-    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
-
-    self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
-                    SPEED_LIMITS['highway'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.active
-
-  def test_manual_cruise_change_detection(self):
-    self.sla.state = SpeedLimitAssistState.active
-    expected_cruise = SPEED_LIMITS['highway']
-    self.sla.v_cruise_cluster_prev = expected_cruise
-
-    different_cruise = SPEED_LIMITS['highway'] + 5
-    self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.inactive
-
-  # TODO-SP: test lower CST cases
-  def test_rapid_speed_limit_changes(self):
-    self.initialize_active_state(self.pcm_long_max_set_speed)
-    speed_limits = [SPEED_LIMITS['highway'], SPEED_LIMITS['freeway']]
-
-    for _, speed_limit in enumerate(speed_limits):
-      self.sla.update(True, False, speed_limit, 0, self.pcm_long_max_set_speed, speed_limit, speed_limit, True, 0, self.events_sp)
-    assert self.sla.state in ACTIVE_STATES
-
-  def test_invalid_speed_limits_handling(self):
-    self.initialize_active_state(self.pcm_long_max_set_speed)
-
-    invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS]
-
-    for invalid_limit in invalid_limits:
-      self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, invalid_limit, SPEED_LIMITS['city'], True, 0, self.events_sp)
-      assert isinstance(self.sla.output_v_target, (int, float))
-      assert self.sla.output_v_target == V_CRUISE_UNSET or self.sla.output_v_target > 0
-
-  def test_stale_data_handling(self):
-    self.initialize_active_state(self.pcm_long_max_set_speed)
-    old_speed_limit = SPEED_LIMITS['city']
-
-    self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, 0, old_speed_limit, True, 0, self.events_sp)
-    assert self.sla.state in ACTIVE_STATES
-    assert self.sla.output_v_target == old_speed_limit
-
-  def test_distance_based_adapting(self):
-    self.sla.state = SpeedLimitAssistState.adapting
-    self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
-    self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
-
-    distance = 100.0
-    current_speed = SPEED_LIMITS['freeway']
-    target_speed = SPEED_LIMITS['highway']
-
-    self.sla.update(True, False, current_speed, 0, self.pcm_long_max_set_speed, target_speed, target_speed, True, distance, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.adapting
-    assert self.sla.output_v_target == target_speed  # TODO-SP: assert expected accel, need to enable self.acceleration_solutions
-
-  def test_long_disengaged_to_disabled(self):
-    self.initialize_active_state(self.pcm_long_max_set_speed)
-
-    self.sla.update(False, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'],
-                    SPEED_LIMITS['city'], True, 0, self.events_sp)
-    assert self.sla.state == SpeedLimitAssistState.disabled
-    assert self.sla.output_v_target == V_CRUISE_UNSET
-
-  def test_maintain_states_with_no_changes(self):
-    """Test that states are maintained when no significant changes occur"""
-    test_states = [
-      SpeedLimitAssistState.preActive,
-      SpeedLimitAssistState.pending,
-      SpeedLimitAssistState.active,
-      SpeedLimitAssistState.adapting
-    ]
-
-    for state in test_states:
-      self.sla.state = state
-      self.sla.op_engaged = True
-
-      initial_state = state
-
-      self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
-
-      assert self.sla.state in ALL_STATES  # Sanity check
-
-      if initial_state == SpeedLimitAssistState.preActive:
-        assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active]
-      elif initial_state in ACTIVE_STATES:
-        assert self.sla.state in ACTIVE_STATES

sunnypilot/selfdrive/controls/lib/speed_limit_assist/__init__.py

@@ -0,0 +1,20 @@
+from cereal import custom
+
+SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimitAssistState
+
+PARAMS_UPDATE_PERIOD = 3.  # secs. Time between parameter updates.
+DISABLED_GUARD_PERIOD = 2  # secs.
+PRE_ACTIVE_GUARD_PERIOD = 5  # secs. Time to wait after activation before considering temp deactivation signal.
+
+# Constants for Limit controllers.
+LIMIT_ADAPT_ACC = -1.  # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits.
+LIMIT_MIN_ACC = -1.5  # m/s^2 Maximum deceleration allowed for limit controllers to provide.
+LIMIT_MAX_ACC = 1.0   # m/s^2 Maximum acceleration allowed for limit controllers to provide while active.
+LIMIT_MIN_SPEED = 8.33  # m/s, Minimum speed limit to provide as solution on limit controllers.
+LIMIT_SPEED_OFFSET_TH = -1.  # m/s Maximum offset between speed limit and current speed for adapting state.
+LIMIT_MAX_MAP_DATA_AGE = 10.  # s Maximum time to hold to map data, then consider it invalid inside limits controllers.
+
+# Speed Limit Assist Auto mode constants
+REQUIRED_INITIAL_MAX_SET_SPEED = 35.7632  # m/s 80 MPH  # TODO-SP: customizable with params
+CRUISE_SPEED_TOLERANCE = 0.44704  # m/s ±1 MPH tolerance  # TODO-SP: metric vs imperial
+FALLBACK_CRUISE_SPEED = 255.0  # m/s fallback when no speed limit available

sunnypilot/selfdrive/controls/lib/speed_limit_assist/common.py

@@ -0,0 +1,15 @@
+from enum import IntEnum
+
+
+class Policy(IntEnum):
+  map_data_only = 0
+  car_state_only = 1
+  map_data_priority = 2
+  car_state_priority = 3
+  combined = 4
+
+
+class OffsetType(IntEnum):
+  off = 0
+  fixed = 1
+  percentage = 2

sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_assist.py

@@ -0,0 +1,267 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import numpy as np
+
+from cereal import custom
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import PARAMS_UPDATE_PERIOD, LIMIT_SPEED_OFFSET_TH, \
+  SpeedLimitAssistState, PRE_ACTIVE_GUARD_PERIOD, REQUIRED_INITIAL_MAX_SET_SPEED, CRUISE_SPEED_TOLERANCE, DISABLED_GUARD_PERIOD
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import OffsetType
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+from openpilot.selfdrive.modeld.constants import ModelConstants
+
+EventNameSP = custom.OnroadEventSP.EventName
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource
+
+ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting)
+ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, SpeedLimitAssistState.overriding, *ACTIVE_STATES)
+
+
+class SpeedLimitAssist:
+  _speed_limit: float
+  _distance: float
+  _source: custom.LongitudinalPlanSP.SpeedLimitSource
+  v_ego: float
+  a_ego: float
+  v_offset: float
+  last_valid_speed_limit_final: float
+
+  def __init__(self, CP):
+    self.params = Params()
+    self.CP = CP
+    self.frame = -1
+    self.long_engaged_timer = 0
+    self.pre_active_timer = 0
+    self.is_metric = self.params.get_bool("IsMetric")
+    self.enabled = self.params.get_bool("SpeedLimitAssist")
+    self.long_enabled = False
+    self.long_enabled_prev = False
+    self.long_override = False
+    self.is_enabled = False
+    self.is_active = False
+    self.v_ego = 0.
+    self.a_ego = 0.
+    self.v_offset = 0.
+    self.v_cruise_setpoint = 0.
+    self.v_cruise_setpoint_prev = 0.
+    self.initial_max_set = False
+    self._speed_limit = 0.
+    self.speed_limit_prev = 0.
+    self.last_valid_speed_limit_final = 0.
+    self._distance = 0.
+    self._source = SpeedLimitSource.none
+    self.state = SpeedLimitAssistState.disabled
+    self._state_prev = SpeedLimitAssistState.disabled
+    self.pcm_cruise_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise
+
+    self.offset_type = OffsetType(self.params.get("SpeedLimitOffsetType", return_default=True))
+    self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
+
+    # Solution functions mapped to respective states
+    self.acceleration_solutions = {
+      SpeedLimitAssistState.disabled: self.get_current_acceleration_as_target,
+      SpeedLimitAssistState.inactive: self.get_current_acceleration_as_target,
+      SpeedLimitAssistState.preActive: self.get_current_acceleration_as_target,
+      SpeedLimitAssistState.pending: self.get_current_acceleration_as_target,
+      SpeedLimitAssistState.adapting: self.get_adapting_state_target_acceleration,
+      SpeedLimitAssistState.active: self.get_active_state_target_acceleration,
+    }
+
+  @property
+  def speed_limit_final(self) -> float:
+    return self._speed_limit + self.speed_limit_offset
+
+  @property
+  def speed_limit_changed(self) -> bool:
+    return bool(self._speed_limit != self.speed_limit_prev)
+
+  @property
+  def speed_limit_offset(self) -> float:
+    return self.get_offset(self.offset_type, self.offset_value)
+
+  @property
+  def v_cruise_setpoint_changed(self) -> bool:
+    return bool(self.v_cruise_setpoint != self.v_cruise_setpoint_prev)
+
+  def get_v_target_from_control(self) -> float:
+    if self.is_enabled:
+      # If we have a current valid speed limit, use it
+      if self._speed_limit > 0:
+        self.last_valid_speed_limit_final = self.speed_limit_final
+        return self.speed_limit_final
+
+      # If no current speed limit but we have a last valid one, use that
+      if self.last_valid_speed_limit_final > 0:
+        return self.last_valid_speed_limit_final
+
+    # Fallback
+    return V_CRUISE_UNSET
+
+  def get_offset(self, offset_type: OffsetType, offset_value: int) -> float:
+    if offset_type == OffsetType.off:
+      return 0
+    elif offset_type == OffsetType.fixed:
+      return offset_value * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS)
+    elif offset_type == OffsetType.percentage:
+      return offset_value * 0.01 * self._speed_limit
+    else:
+      raise NotImplementedError("Offset not supported")
+
+  def update_params(self) -> None:
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.enabled = self.params.get_bool("SpeedLimitAssist")
+      self.offset_type = OffsetType(self.params.get("SpeedLimitOffsetType", return_default=True))
+      self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
+      self.is_metric = self.params.get_bool("IsMetric")
+
+  def initial_max_set_confirmed(self) -> bool:
+    return bool(abs(self.v_cruise_setpoint - REQUIRED_INITIAL_MAX_SET_SPEED) <= CRUISE_SPEED_TOLERANCE)
+
+  def detect_manual_cruise_change(self) -> bool:
+    # If cruise speed changed and it's not what SLA would set
+    if self.v_cruise_setpoint_changed:
+      expected_cruise = self.speed_limit_final
+      return bool(abs(self.v_cruise_setpoint - expected_cruise) > CRUISE_SPEED_TOLERANCE)
+
+    return False
+
+  def update_calculations(self, v_cruise_setpoint: float) -> None:
+    self.v_cruise_setpoint = v_cruise_setpoint if not np.isnan(v_cruise_setpoint) else 0.0
+
+    # Update current velocity offset (error)
+    self.v_offset = self.speed_limit_final - self.v_ego
+
+  def get_current_acceleration_as_target(self) -> float:
+    return self.a_ego
+
+  def get_adapting_state_target_acceleration(self) -> float:
+    if self._distance > 0:
+      return (self.speed_limit_final ** 2 - self.v_ego ** 2) / (2. * self._distance)
+
+    return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
+
+  def get_active_state_target_acceleration(self) -> float:
+    return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
+
+  def update_state_machine(self):
+    self._state_prev = self.state
+
+    self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
+    self.pre_active_timer = max(0, self.pre_active_timer - 1)
+
+    # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE, OVERRIDING
+    if self.state != SpeedLimitAssistState.disabled:
+      if not self.long_enabled or not self.enabled:
+        self.state = SpeedLimitAssistState.disabled
+        self.initial_max_set = False
+      elif self.long_override:
+        self.state = SpeedLimitAssistState.overriding
+
+      else:
+        # ACTIVE
+        if self.state == SpeedLimitAssistState.active:
+          if self.detect_manual_cruise_change():
+            self.state = SpeedLimitAssistState.inactive
+          elif self._speed_limit > 0 and self.v_offset < LIMIT_SPEED_OFFSET_TH:
+            self.state = SpeedLimitAssistState.adapting
+
+        # ADAPTING
+        elif self.state == SpeedLimitAssistState.adapting:
+          if self.detect_manual_cruise_change():
+            self.state = SpeedLimitAssistState.inactive
+          elif self.v_offset >= LIMIT_SPEED_OFFSET_TH:
+            self.state = SpeedLimitAssistState.active
+
+        # PENDING
+        elif self.state == SpeedLimitAssistState.pending:
+          if self._speed_limit > 0:
+            if self.v_offset < LIMIT_SPEED_OFFSET_TH:
+              self.state = SpeedLimitAssistState.adapting
+            else:
+              self.state = SpeedLimitAssistState.active
+
+        # PRE_ACTIVE
+        elif self.state == SpeedLimitAssistState.preActive:
+          if self.initial_max_set_confirmed():
+            self.initial_max_set = True
+            if self._speed_limit > 0:
+              if self.v_offset < LIMIT_SPEED_OFFSET_TH:
+                self.state = SpeedLimitAssistState.adapting
+              else:
+                self.state = SpeedLimitAssistState.active
+            else:
+              self.state = SpeedLimitAssistState.pending
+          elif self.pre_active_timer <= PRE_ACTIVE_GUARD_PERIOD:
+            # Timeout - session ended
+            self.state = SpeedLimitAssistState.inactive
+
+        # OVERRIDING
+        elif self.state == SpeedLimitAssistState.overriding:
+          if not self.long_override:
+            self.state = SpeedLimitAssistState.preActive
+
+        # INACTIVE
+        elif self.state == SpeedLimitAssistState.inactive:
+          pass
+
+    # DISABLED
+    elif self.state == SpeedLimitAssistState.disabled:
+      if self.long_enabled and self.enabled:
+        if self.long_override:
+          self.state = SpeedLimitAssistState.overriding
+
+        elif not self.long_enabled_prev:
+          self.pre_active_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
+
+        elif self.pre_active_timer <= 0:
+          self.state = SpeedLimitAssistState.preActive
+          self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+          self.initial_max_set = False
+
+    enabled = self.state in ENABLED_STATES
+    active = self.state in ACTIVE_STATES
+
+    return enabled, active
+
+  def update_events(self, events_sp: EventsSP) -> None:
+    if self.state == SpeedLimitAssistState.preActive and self._state_prev != SpeedLimitAssistState.preActive:
+      events_sp.add(EventNameSP.speedLimitPreActive)
+    elif self.is_active:
+      if self._state_prev not in ACTIVE_STATES:
+        events_sp.add(EventNameSP.speedLimitActive)
+      elif self.speed_limit_changed:
+        events_sp.add(EventNameSP.speedLimitChanged)
+
+  def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_setpoint: float,
+             speed_limit: float, distance: float, source: custom.LongitudinalPlanSP.SpeedLimitSource, events_sp: EventsSP) -> float:
+    self.long_enabled = long_enabled
+    self.long_override = long_override
+    self.v_ego = v_ego
+    self.a_ego = a_ego
+
+    self._speed_limit = speed_limit
+    self._distance = distance
+    self._source = source
+
+    self.update_params()
+    self.update_calculations(v_cruise_setpoint)
+    self.is_enabled, self.is_active = self.update_state_machine()
+    self.update_events(events_sp)
+
+    # Update change tracking variables
+    self.speed_limit_prev = self._speed_limit
+    self.v_cruise_setpoint_prev = self.v_cruise_setpoint
+    self.long_enabled_prev = self.long_enabled
+    self.frame += 1
+
+    v_target = self.get_v_target_from_control()
+
+    return v_target

sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_resolver.py

@@ -0,0 +1,133 @@
+import time
+import numpy as np
+
+import cereal.messaging as messaging
+from cereal import custom
+from openpilot.common.gps import get_gps_location_service
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC, PARAMS_UPDATE_PERIOD
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import Policy
+
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource
+
+ALL_SOURCES = tuple(SpeedLimitSource.schema.enumerants.values())
+
+
+class SpeedLimitResolver:
+  _limit_solutions: dict[custom.LongitudinalPlanSP.SpeedLimitSource, float]
+  _distance_solutions: dict[custom.LongitudinalPlanSP.SpeedLimitSource, float]
+  _v_ego: float
+  speed_limit: float
+  distance: float
+  source: custom.LongitudinalPlanSP.SpeedLimitSource
+
+  def __init__(self):
+    self.params = Params()
+    self.frame = -1
+
+    self._gps_location_service = get_gps_location_service(self.params)
+    self._limit_solutions = {}  # Store for speed limit solutions from different sources
+    self._distance_solutions = {}  # Store for distance to current speed limit start for different sources
+
+    self.policy = self.params.get("SpeedLimitPolicy", return_default=True)
+    self._policy_to_sources_map = {
+      Policy.car_state_only: [SpeedLimitSource.car],
+      Policy.car_state_priority: [SpeedLimitSource.car, SpeedLimitSource.map],
+      Policy.map_data_priority: [SpeedLimitSource.map, SpeedLimitSource.car],
+      Policy.map_data_only: [SpeedLimitSource.map],
+      Policy.combined: [SpeedLimitSource.car, SpeedLimitSource.map],
+    }
+    self.source = SpeedLimitSource.none
+    for source in ALL_SOURCES:
+      self._reset_limit_sources(source)
+
+  def update_params(self):
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.policy = Policy(self.params.get("SpeedLimitPolicy", return_default=True))
+      self.change_policy(self.policy)
+
+  def change_policy(self, policy: Policy) -> None:
+    self.policy = policy
+
+  def _reset_limit_sources(self, source: custom.LongitudinalPlanSP.SpeedLimitSource) -> None:
+    self._limit_solutions[source] = 0.
+    self._distance_solutions[source] = 0.
+
+  def _resolve_limit_sources(self, sm: messaging.SubMaster) -> None:
+    """Get limit solutions from each data source"""
+    self._get_from_car_state(sm)
+    self._get_from_map_data(sm)
+
+  def _get_from_car_state(self, sm: messaging.SubMaster) -> None:
+    self._reset_limit_sources(SpeedLimitSource.car)
+    self._limit_solutions[SpeedLimitSource.car] = sm['carStateSP'].speedLimit
+    self._distance_solutions[SpeedLimitSource.car] = 0.
+
+  def _get_from_map_data(self, sm: messaging.SubMaster) -> None:
+    self._reset_limit_sources(SpeedLimitSource.map)
+    self._process_map_data(sm)
+
+  def _process_map_data(self, sm: messaging.SubMaster) -> None:
+    gps_data = sm[self._gps_location_service]
+    map_data = sm['liveMapDataSP']
+
+    gps_fix_age = time.monotonic() - gps_data.unixTimestampMillis * 1e-3
+    if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE:
+      return
+
+    speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0.
+    next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0.
+
+    self._calculate_map_data_limits(sm, speed_limit, next_speed_limit)
+
+  def _calculate_map_data_limits(self, sm: messaging.SubMaster, speed_limit: float, next_speed_limit: float) -> None:
+    gps_data = sm[self._gps_location_service]
+    map_data = sm['liveMapDataSP']
+
+    distance_since_fix = self._v_ego * (time.monotonic() - gps_data.unixTimestampMillis * 1e-3)
+    distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix)
+
+    self._limit_solutions[SpeedLimitSource.map] = speed_limit
+    self._distance_solutions[SpeedLimitSource.map] = 0.
+
+    if 0. < next_speed_limit < self._v_ego:
+      adapt_time = (next_speed_limit - self._v_ego) / LIMIT_ADAPT_ACC
+      adapt_distance = self._v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time ** 2
+
+      if distance_to_speed_limit_ahead <= adapt_distance:
+        self._limit_solutions[SpeedLimitSource.map] = next_speed_limit
+        self._distance_solutions[SpeedLimitSource.map] = distance_to_speed_limit_ahead
+
+  def _consolidate(self) -> tuple[float, float, custom.LongitudinalPlanSP.SpeedLimitSource]:
+    source = self._get_source_solution_according_to_policy()
+    speed_limit = self._limit_solutions[source] if source else 0.
+    distance = self._distance_solutions[source] if source else 0.
+
+    return speed_limit, distance, source
+
+  def _get_source_solution_according_to_policy(self) -> custom.LongitudinalPlanSP.SpeedLimitSource:
+    sources_for_policy = self._policy_to_sources_map[self.policy]
+
+    if self.policy != Policy.combined:
+      # They are ordered in the order of preference, so we pick the first that's non zero
+      for source in sources_for_policy:
+        return source if self._limit_solutions[source] > 0. else SpeedLimitSource.none
+
+    limits = np.array([self._limit_solutions[source] for source in sources_for_policy], dtype=float)
+    sources = np.array(sources_for_policy, dtype=int)
+
+    if len(limits) > 0:
+      min_idx = np.argmin(limits)
+      return SpeedLimitSource(int(sources[min_idx]))
+
+    return SpeedLimitSource.none
+
+  def update(self, v_ego: float, sm: messaging.SubMaster) -> None:
+    self._v_ego = v_ego
+    self.update_params()
+    self._resolve_limit_sources(sm)
+
+    self.speed_limit, self.distance, self.source = self._consolidate()
+
+    self.frame += 1

sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_assist.py

@@ -0,0 +1,247 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import pytest
+
+from cereal import custom
+
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.toyota.values import CAR as TOYOTA
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
+from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import OffsetType
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import SpeedLimitAssistState, REQUIRED_INITIAL_MAX_SET_SPEED, \
+  PRE_ACTIVE_GUARD_PERIOD
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.speed_limit_assist import SpeedLimitAssist, ACTIVE_STATES
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource
+
+ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values())
+
+SPEED_LIMITS = {
+  'residential': 25 * CV.MPH_TO_MS,  # 25 mph
+  'city': 35 * CV.MPH_TO_MS,         # 35 mph
+  'highway': 65 * CV.MPH_TO_MS,      # 65 mph
+  'freeway': 80 * CV.MPH_TO_MS,      # 80 mph
+}
+
+
+class TestSpeedLimitAssist:
+
+  def setup_method(self):
+    self.params = Params()
+    self.reset_custom_params()
+    self.events_sp = EventsSP()
+    CI = self._setup_platform(TOYOTA.TOYOTA_RAV4_TSS2_2022)
+    self.sla = SpeedLimitAssist(CI.CP)
+    self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+
+  def teardown_method(self, method):
+    self.reset_state()
+
+  def _setup_platform(self, car_name):
+    CarInterface = interfaces[car_name]
+    CP = CarInterface.get_non_essential_params(car_name)
+    CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
+    CI = CarInterface(CP, CP_SP)
+    sunnypilot_interfaces.setup_interfaces(CI, self.params)
+    return CI
+
+  def reset_custom_params(self):
+    self.params.put_bool("SpeedLimitAssist", True)
+    self.params.put_bool("IsMetric", False)
+    self.params.put("SpeedLimitOffsetType", 0)
+    self.params.put("SpeedLimitValueOffset", 0)
+
+  def reset_state(self):
+    self.sla.state = SpeedLimitAssistState.disabled
+    self.sla.frame = -1
+    self.sla.last_op_engaged_frame = 0
+    self.sla.op_engaged = False
+    self.sla.op_engaged_prev = False
+    self.sla.initial_max_set = False
+    self.sla._speed_limit = 0.
+    self.sla.speed_limit_prev = 0.
+    self.sla.last_valid_speed_limit_offsetted = 0.
+    self.sla._distance = 0.
+    self.sla._source = SpeedLimitSource.none
+    self.sla.v_cruise_setpoint = 0.
+    self.sla.v_cruise_setpoint_prev = 0.
+    self.events_sp.clear()
+
+  def initialize_active_state(self, v_cruise_setpoint):
+    self.sla.state = SpeedLimitAssistState.active
+    self.sla.v_cruise_setpoint = v_cruise_setpoint
+    self.sla.v_cruise_setpoint_prev = v_cruise_setpoint
+
+  def test_initial_state(self):
+    assert self.sla.state == SpeedLimitAssistState.disabled
+    assert not self.sla.is_enabled
+    assert not self.sla.is_active
+    assert V_CRUISE_UNSET == self.sla.get_v_target_from_control()
+
+  def test_disabled(self):
+    self.params.put_bool("SpeedLimitAssist", False)
+    for _ in range(int(10. / DT_MDL)):
+      _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.disabled
+
+  def test_transition_disabled_to_preactive(self):
+    for _ in range(int(3. / DT_MDL)):
+      _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.preActive
+    assert self.sla.is_enabled and not self.sla.is_active
+
+  def test_preactive_to_active_with_max_speed_confirmation(self):
+    self.sla.state = SpeedLimitAssistState.preActive
+    v_cruise_sla = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.active
+    assert self.sla.is_enabled and self.sla.is_active, f"enabled: {self.sla.is_enabled}, active: {self.sla.is_active}"
+    assert v_cruise_sla == SPEED_LIMITS['city']
+
+  def test_preactive_timeout_to_inactive(self):
+    self.sla.state = SpeedLimitAssistState.preActive
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+
+    for _ in range(int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)):
+      _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.inactive
+
+  def test_preactive_to_pending_no_speed_limit(self):
+    self.sla.state = SpeedLimitAssistState.preActive
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.none, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.pending
+    assert self.sla.is_enabled and not self.sla.is_active
+
+  def test_pending_to_active_when_speed_limit_available(self):
+    self.sla.state = SpeedLimitAssistState.pending
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.active
+
+  def test_pending_to_adapting_when_below_speed_limit(self):
+    self.sla.state = SpeedLimitAssistState.pending
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'] + 5, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.adapting
+    assert self.sla.is_enabled and self.sla.is_active
+
+  def test_active_to_adapting_transition(self):
+    self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'] + 2, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.adapting
+
+  def test_adapting_to_active_transition(self):
+    self.sla.state = SpeedLimitAssistState.adapting
+    self.sla.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED
+
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.active
+
+  def test_manual_cruise_change_detection(self):
+    self.sla.state = SpeedLimitAssistState.active
+    expected_cruise = SPEED_LIMITS['highway']
+    self.sla.v_cruise_setpoint_prev = expected_cruise
+
+    different_cruise = SPEED_LIMITS['highway'] + 5
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.inactive
+
+  @pytest.mark.parametrize("offset_type, offset_value, speed_limit, expected_offset", [
+    (OffsetType.fixed, 5, SPEED_LIMITS['city'], 5 * CV.MPH_TO_MS),  # 5 MPH fixed offset
+    (OffsetType.percentage, 10, SPEED_LIMITS['city'], 0.1 * SPEED_LIMITS['city']),  # 10% offset
+    (OffsetType.off, 0, SPEED_LIMITS['city'], 0),  # Off
+    (OffsetType.fixed, 10, SPEED_LIMITS['highway'], 10 * CV.MPH_TO_MS),  # Different speed, fixed offset
+    (OffsetType.percentage, 5, SPEED_LIMITS['highway'], 0.05 * SPEED_LIMITS['highway']),  # Different speed, percentage
+  ])
+  def test_offset_calculations(self, offset_type, offset_value, speed_limit, expected_offset):
+    self.sla._speed_limit = speed_limit
+    actual_offset = self.sla.get_offset(offset_type, offset_value)
+    assert actual_offset == pytest.approx(expected_offset, rel=0.01)
+
+  def test_rapid_speed_limit_changes(self):
+    self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+    speed_limits = [SPEED_LIMITS['city'], SPEED_LIMITS['highway'], SPEED_LIMITS['residential']]
+
+    for _, speed_limit in enumerate(speed_limits):
+      _ = self.sla.update(True, False, speed_limit, 0, REQUIRED_INITIAL_MAX_SET_SPEED, speed_limit, 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state in ACTIVE_STATES
+
+  def test_invalid_speed_limits_handling(self):
+    self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+    self.sla.last_valid_speed_limit_final = SPEED_LIMITS['city']
+
+    invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS]
+
+    for invalid_limit in invalid_limits:
+      v_cruise_sla = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, invalid_limit, 0, SpeedLimitSource.car, self.events_sp)
+      assert isinstance(v_cruise_sla, (int, float))
+      assert v_cruise_sla == V_CRUISE_UNSET or v_cruise_sla > 0
+
+  def test_stale_data_handling(self):
+    self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+    old_speed_limit = SPEED_LIMITS['city']
+    self.sla.last_valid_speed_limit_final = old_speed_limit
+
+    v_cruise_sla = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state in ACTIVE_STATES
+    assert v_cruise_sla == old_speed_limit
+
+  def test_different_speed_limit_sources(self):
+    self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+
+    for source in (SpeedLimitSource.car, SpeedLimitSource.map):
+      v_cruise_sla = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, source, self.events_sp)
+      assert v_cruise_sla != V_CRUISE_UNSET
+
+  def test_distance_based_adapting(self):
+    self.sla.state = SpeedLimitAssistState.adapting
+    self.sla.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED
+
+    distance = 100.0
+    current_speed = SPEED_LIMITS['highway']
+    target_speed = SPEED_LIMITS['city']
+
+    v_cruise_sla = self.sla.update(True, False, current_speed, 0, REQUIRED_INITIAL_MAX_SET_SPEED, target_speed, distance, SpeedLimitSource.map, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.adapting
+    assert v_cruise_sla == target_speed  # TODO-SP: assert expected accel, need to enable self.acceleration_solutions
+
+  def test_long_disengaged_to_disabled(self):
+    self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+
+    v_cruise_sla = self.sla.update(False, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'],
+                                   0, SpeedLimitSource.car, self.events_sp)
+    assert self.sla.state == SpeedLimitAssistState.disabled
+    assert v_cruise_sla == V_CRUISE_UNSET
+
+  def test_maintain_states_with_no_changes(self):
+    """Test that states are maintained when no significant changes occur"""
+    test_states = [
+      SpeedLimitAssistState.preActive,
+      SpeedLimitAssistState.pending,
+      SpeedLimitAssistState.active,
+      SpeedLimitAssistState.adapting
+    ]
+
+    for state in test_states:
+      self.sla.state = state
+      self.sla.op_engaged = True
+      if state in [SpeedLimitAssistState.pending, SpeedLimitAssistState.active, SpeedLimitAssistState.adapting]:
+        self.sla.initial_max_set = True
+
+      initial_state = state
+
+      _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED,SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+
+      assert self.sla.state in ALL_STATES  # Sanity check
+
+      if initial_state == SpeedLimitAssistState.preActive:
+        assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active]
+      elif initial_state in ACTIVE_STATES:
+        assert self.sla.state in ACTIVE_STATES

sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_resolver.py

@@ -1,9 +1,3 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
 import random
 import time
 
@@ -11,12 +5,12 @@ import pytest
 from pytest_mock import MockerFixture
 
 from cereal import custom
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import LIMIT_MAX_MAP_DATA_AGE
 
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import Policy
 
-SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource
 
 
 def create_mock(properties, mocker: MockerFixture):
@@ -77,9 +71,9 @@ class TestSpeedLimitResolverValidation:
     resolver = resolver_class()
     resolver.policy = policy
     for source in ALL_SOURCES:
-      if source in resolver.limit_solutions:
-        assert resolver.limit_solutions[source] == 0.
-        assert resolver.distance_solutions[source] == 0.
+      if source in resolver._limit_solutions:
+        assert resolver._limit_solutions[source] == 0.
+        assert resolver._distance_solutions[source] == 0.
 
   @parametrized_policies
   def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
@@ -116,8 +110,8 @@ class TestSpeedLimitResolverValidation:
 
     # Assert the parsing
     resolver.update(source_speed_limit, sm_mock)
-    assert resolver.limit_solutions[ALL_SOURCES[function_key]] == source_speed_limit
-    assert resolver.distance_solutions[ALL_SOURCES[function_key]] == 0.
+    assert resolver._limit_solutions[ALL_SOURCES[function_key]] == source_speed_limit
+    assert resolver._distance_solutions[ALL_SOURCES[function_key]] == 0.
 
   @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
   def test_resolve_interaction_in_update(self, resolver_class, policy, mocker: MockerFixture):
@@ -140,5 +134,5 @@ class TestSpeedLimitResolverValidation:
     sm_mock = mocker.MagicMock()
     sm_mock['gpsLocation'].unixTimestampMillis = (time.monotonic() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3
     resolver._get_from_map_data(sm_mock)
-    assert resolver.limit_solutions[SpeedLimitSource.map] == 0.
-    assert resolver.distance_solutions[SpeedLimitSource.map] == 0.
+    assert resolver._limit_solutions[SpeedLimitSource.map] == 0.
+    assert resolver._distance_solutions[SpeedLimitSource.map] == 0.

sunnypilot/selfdrive/locationd/.gitignore

@@ -1,3 +0,0 @@
-params_learner
-paramsd
-locationd

sunnypilot/selfdrive/locationd/SConscript

@@ -1,37 +0,0 @@
-Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations')
-
-loc_libs = [messaging, common, 'pthread', 'dl']
-
-# build ekf models
-rednose_gen_dir = 'models/generated'
-rednose_gen_deps = [
-  "models/constants.py",
-]
-live_ekf = env.RednoseCompileFilter(
-  target='live',
-  filter_gen_script='models/live_kf.py',
-  output_dir=rednose_gen_dir,
-  extra_gen_artifacts=['live_kf_constants.h'],
-  gen_script_deps=rednose_gen_deps,
-)
-car_ekf = env.RednoseCompileFilter(
-  target='car',
-  filter_gen_script='models/car_kf.py',
-  output_dir=rednose_gen_dir,
-  extra_gen_artifacts=[],
-  gen_script_deps=rednose_gen_deps,
-)
-
-# locationd build
-locationd_sources = ["locationd.cc", "models/live_kf.cc"]
-
-lenv = env.Clone()
-# ekf filter libraries need to be linked, even if no symbols are used
-if arch != "Darwin":
-  lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"]
-
-lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath)
-lenv["RPATH"].append(Dir(rednose_gen_dir).abspath)
-locationd = lenv.Program("locationd", locationd_sources, LIBS=["live", "ekf_sym"] + loc_libs + transformations)
-lenv.Depends(locationd, rednose)
-lenv.Depends(locationd, live_ekf)

sunnypilot/selfdrive/locationd/locationd.cc

@@ -1,750 +0,0 @@
-#include "sunnypilot/selfdrive/locationd/locationd.h"
-
-#include <sys/time.h>
-#include <sys/resource.h>
-
-#include <algorithm>
-#include <cmath>
-#include <vector>
-
-using namespace EKFS;
-using namespace Eigen;
-
-ExitHandler do_exit;
-const double ACCEL_SANITY_CHECK = 100.0;  // m/s^2
-const double ROTATION_SANITY_CHECK = 10.0;  // rad/s
-const double TRANS_SANITY_CHECK = 200.0;  // m/s
-const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg)
-const double ALTITUDE_SANITY_CHECK = 10000; // m
-const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
-const double VALID_TIME_SINCE_RESET = 1.0; // s
-const double VALID_POS_STD = 50.0; // m
-const double MAX_RESET_TRACKER = 5.0;
-const double SANE_GPS_UNCERTAINTY = 1500.0; // m
-const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker
-const double RESET_TRACKER_DECAY = 0.99995;
-const double DECAY = 0.9993; // ~10 secs to resume after a bad input
-const double MAX_FILTER_REWIND_TIME = 0.8; // s
-const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30;
-
-// TODO: GPS sensor time offsets are empirically calculated
-// They should be replaced with synced time from a real clock
-const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s
-const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s
-const float  GPS_POS_STD_THRESHOLD = 50.0;
-const float  GPS_VEL_STD_THRESHOLD = 5.0;
-const float  GPS_POS_ERROR_RESET_THRESHOLD = 300.0;
-const float  GPS_POS_STD_RESET_THRESHOLD = 2.0;
-const float  GPS_VEL_STD_RESET_THRESHOLD = 0.5;
-const float  GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0;
-const int    GPS_ORIENTATION_ERROR_RESET_CNT = 3;
-
-const bool   DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0";
-
-static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
-  VectorXd res(floatlist.size());
-  for (int i = 0; i < floatlist.size(); i++) {
-    res[i] = floatlist[i];
-  }
-  return res;
-}
-
-static Vector4d quat2vector(const Quaterniond& quat) {
-  return Vector4d(quat.w(), quat.x(), quat.y(), quat.z());
-}
-
-static Quaterniond vector2quat(const VectorXd& vec) {
-  return Quaterniond(vec(0), vec(1), vec(2), vec(3));
-}
-
-static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) {
-  meas.setValue(kj::arrayPtr(val.data(), val.size()));
-  meas.setStd(kj::arrayPtr(std.data(), std.size()));
-  meas.setValid(valid);
-}
-
-
-static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) {
-  // To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix
-  return ((rot_matrix *  cov_in) * rot_matrix.transpose());
-}
-
-static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) {
-  // Stds cannot be rotated like values, only covariances can be rotated
-  return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt();
-}
-
-Localizer::Localizer(LocalizerGnssSource gnss_source) {
-  this->kf = std::make_unique<LiveKalman>();
-  this->reset_kalman();
-
-  this->calib = Vector3d(0.0, 0.0, 0.0);
-  this->device_from_calib = MatrixXdr::Identity(3, 3);
-  this->calib_from_device = MatrixXdr::Identity(3, 3);
-
-  for (int i = 0; i < POSENET_STD_HIST_HALF * 2; i++) {
-    this->posenet_stds.push_back(10.0);
-  }
-
-  VectorXd ecef_pos = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
-  this->converter = std::make_unique<LocalCoord>((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
-  this->configure_gnss_source(gnss_source);
-}
-
-void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
-  VectorXd predicted_state = this->kf->get_x();
-  MatrixXdr predicted_cov = this->kf->get_P();
-  VectorXd predicted_std = predicted_cov.diagonal().array().sqrt();
-
-  VectorXd fix_ecef = predicted_state.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
-  ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
-  VectorXd fix_ecef_std = predicted_std.segment<STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START);
-  VectorXd vel_ecef = predicted_state.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
-  VectorXd vel_ecef_std = predicted_std.segment<STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START);
-  VectorXd fix_pos_geo_vec = this->get_position_geodetic();
-  VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
-  VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START);
-  MatrixXdr orientation_ecef_cov = predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
-  MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose();
-  VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose());
-
-  VectorXd acc_calib = this->calib_from_device * predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
-  MatrixXdr acc_calib_cov = predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START);
-  VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt();
-  VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
-
-  MatrixXdr vel_angular_cov = predicted_cov.block<STATE_ANGULAR_VELOCITY_ERR_LEN, STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START);
-  VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt();
-
-  VectorXd vel_device = device_from_ecef * vel_ecef;
-  VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))).transpose();
-  MatrixXdr condensed_cov(STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
-  condensed_cov.topLeftCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
-    predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
-  condensed_cov.topRightCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
-    predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
-  condensed_cov.bottomRightCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
-    predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
-  condensed_cov.bottomLeftCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
-    predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
-  VectorXd H_input(device_from_ecef_eul.size() + vel_ecef.size());
-  H_input << device_from_ecef_eul, vel_ecef;
-  MatrixXdr HH = this->kf->H(H_input);
-  MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose();
-  VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt();
-
-  VectorXd vel_calib = this->calib_from_device * vel_device;
-  VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt();
-
-  VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef);
-  VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt();
-  VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef);
-  VectorXd nextfix_ecef = fix_ecef + vel_ecef;
-  VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector();
-
-  VectorXd accDevice = predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
-  VectorXd accDeviceErr = predicted_std.segment<STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START);
-
-  VectorXd angVelocityDevice = predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
-  VectorXd angVelocityDeviceErr = predicted_std.segment<STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START);
-
-  Vector3d nans = Vector3d(NAN, NAN, NAN);
-
-  // TODO fill in NED and Calibrated stds
-  // write measurements to msg
-  init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode);
-  init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode);
-  init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode);
-  init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode);
-  init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
-  init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
-  init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode);
-  init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode);
-  init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode);
-  init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode);
-  init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
-  init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
-  init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
-  init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated);
-  if (DEBUG) {
-    init_measurement(fix.initFilterState(), predicted_state, predicted_std, true);
-  }
-
-  double old_mean = 0.0, new_mean = 0.0;
-  int i = 0;
-  for (double x : this->posenet_stds) {
-    if (i < POSENET_STD_HIST_HALF) {
-      old_mean += x;
-    } else {
-      new_mean += x;
-    }
-    i++;
-  }
-  old_mean /= POSENET_STD_HIST_HALF;
-  new_mean /= POSENET_STD_HIST_HALF;
-  // experimentally found these values, no false positives in 20k minutes of driving
-  bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0);
-
-  fix.setPosenetOK(!(std_spike && this->car_speed > 5.0));
-  fix.setDeviceStable(!this->device_fell);
-  fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER);
-  fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff);
-  this->device_fell = false;
-
-  //fix.setGpsWeek(this->time.week);
-  //fix.setGpsTimeOfWeek(this->time.tow);
-  fix.setUnixTimestampMillis(this->unix_timestamp_millis);
-
-  double time_since_reset = this->kf->get_filter_time() - this->last_reset_time;
-  fix.setTimeSinceReset(time_since_reset);
-  if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) {
-    fix.setStatus(cereal::LiveLocationKalman::Status::VALID);
-  } else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) {
-    fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED);
-  } else {
-    fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED);
-  }
-}
-
-VectorXd Localizer::get_position_geodetic() {
-  VectorXd fix_ecef = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
-  ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
-  Geodetic fix_pos_geo = ecef2geodetic(fix_ecef_ecef);
-  return Vector3d(fix_pos_geo.lat, fix_pos_geo.lon, fix_pos_geo.alt);
-}
-
-VectorXd Localizer::get_state() {
-  return this->kf->get_x();
-}
-
-VectorXd Localizer::get_stdev() {
-  return this->kf->get_P().diagonal().array().sqrt();
-}
-
-bool Localizer::are_inputs_ok() {
-  return this->critical_services_valid(this->observation_values_invalid) && !this->observation_timings_invalid;
-}
-
-void Localizer::observation_timings_invalid_reset(){
-  this->observation_timings_invalid = false;
-}
-
-void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) {
-  // TODO does not yet account for double sensor readings in the log
-
-  // Ignore empty readings (e.g. in case the magnetometer had no data ready)
-  if (log.getTimestamp() == 0) {
-    return;
-  }
-
-  double sensor_time = 1e-9 * log.getTimestamp();
-
-  // sensor time and log time should be close
-  if (std::abs(current_time - sensor_time) > 0.1) {
-    LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
-    this->observation_timings_invalid = true;
-    return;
-  } else if (!this->is_timestamp_valid(sensor_time)) {
-    this->observation_timings_invalid = true;
-    return;
-  }
-
-  // TODO: handle messages from two IMUs at the same time
-  if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
-    return;
-  }
-
-  // Gyro Uncalibrated
-  if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
-    auto v = log.getGyroUncalibrated().getV();
-    auto meas = Vector3d(-v[2], -v[1], -v[0]);
-
-    VectorXd gyro_bias = this->kf->get_x().segment<STATE_GYRO_BIAS_LEN>(STATE_GYRO_BIAS_START);
-    float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]);
-    float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1];
-    bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold;
-
-    if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) {
-      this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas });
-      this->observation_values_invalid["gyroscope"] *= DECAY;
-    } else {
-      this->observation_values_invalid["gyroscope"] += 1.0;
-    }
-  }
-
-  // Accelerometer
-  if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) {
-    auto v = log.getAcceleration().getV();
-
-    // TODO: reduce false positives and re-enable this check
-    // check if device fell, estimate 10 for g
-    // 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
-    // this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
-
-    auto meas = Vector3d(-v[2], -v[1], -v[0]);
-    if (meas.norm() < ACCEL_SANITY_CHECK) {
-      this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas });
-      this->observation_values_invalid["accelerometer"] *= DECAY;
-    } else {
-      this->observation_values_invalid["accelerometer"] += 1.0;
-    }
-  }
-}
-
-void Localizer::input_fake_gps_observations(double current_time) {
-  // This is done to make sure that the error estimate of the position does not blow up
-  // when the filter is in no-gps mode
-  // Steps : first predict -> observe current obs with reasonable STD
-  this->kf->predict(current_time);
-
-  VectorXd current_x = this->kf->get_x();
-  VectorXd ecef_pos = current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
-  VectorXd ecef_vel = current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
-  const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov();
-  const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov();
-
-  this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
-  this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
-}
-
-void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) {
-  bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
-  bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
-  bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
-  bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
-
-  if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
-    //this->gps_valid = false;
-    this->determine_gps_mode(current_time);
-    return;
-  }
-
-  double sensor_time = current_time - sensor_time_offset;
-
-  // Process message
-  //this->gps_valid = true;
-  this->gps_mode = true;
-  Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
-  this->converter = std::make_unique<LocalCoord>(geodetic);
-
-  VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
-  VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
-  float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getHorizontalAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2));
-  MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal();
-  MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal();
-
-  this->unix_timestamp_millis = log.getUnixTimestampMillis();
-  double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
-
-  VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
-  VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef);
-  VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg()));
-  VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
-  for (int i = 0; i < orientation_error.size(); i++) {
-    orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
-    if (orientation_error(i) < 0.0) {
-      orientation_error(i) += 2.0 * M_PI;
-    }
-    orientation_error(i) -= M_PI;
-  }
-  VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
-
-  if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) {
-    LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset");
-    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
-    this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
-  } else if (gps_est_error > 100.0) {
-    LOGE("Locationd vs ubloxLocation position difference too large, kalman reset");
-    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
-  }
-
-  this->last_gps_msg = sensor_time;
-  this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
-  this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
-}
-
-void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) {
-
-  if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) {
-    this->determine_gps_mode(current_time);
-    return;
-  }
-
-  double sensor_time = log.getMeasTime() * 1e-9;
-  sensor_time -= this->gps_time_offset;
-
-  auto ecef_pos_v = log.getPositionECEF().getValue();
-  VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]);
-
-  // indexed at 0 cause all std values are the same MAE
-  auto ecef_pos_std = log.getPositionECEF().getStd()[0];
-  MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal();
-
-  auto ecef_vel_v = log.getVelocityECEF().getValue();
-  VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]);
-
-  // indexed at 0 cause all std values are the same MAE
-  auto ecef_vel_std = log.getVelocityECEF().getStd()[0];
-  MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal();
-
-  double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
-
-  VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
-  VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos[0], ecef_pos[1], ecef_pos[2] }, orientation_ecef);
-
-  LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
-  ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]};
-  VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector();
-  double bearing_rad = atan2(ned_vel[1], ned_vel[0]);
-
-  VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad);
-  VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
-  for (int i = 0; i < orientation_error.size(); i++) {
-    orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
-    if (orientation_error(i) < 0.0) {
-      orientation_error(i) += 2.0 * M_PI;
-    }
-    orientation_error(i) -= M_PI;
-  }
-  VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
-
-  if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) {
-    this->determine_gps_mode(current_time);
-    return;
-  }
-
-  // prevent jumping gnss measurements (covered lots, standstill...)
-  bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD;
-  orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD;
-  orientation_reset &= !this->standstill;
-  if (orientation_reset) {
-    this->orientation_reset_count++;
-  } else {
-    this->orientation_reset_count = 0;
-  }
-
-  if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) {
-    // always reset on first gps message and if the location is off but the accuracy is high
-    LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset");
-    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
-  } else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) {
-    LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset");
-    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
-    this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
-    this->orientation_reset_count = 0;
-  }
-
-  this->gps_mode = true;
-  this->last_gps_msg = sensor_time;
-  this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
-  this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
-}
-
-void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) {
-  this->car_speed = std::abs(log.getVEgo());
-  this->standstill = log.getStandstill();
-  if (this->standstill) {
-    this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) });
-    this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) });
-  }
-}
-
-void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) {
-  VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
-  VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
-
-  if (!this->is_timestamp_valid(current_time)) {
-    this->observation_timings_invalid = true;
-    return;
-  }
-
-  if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) {
-    this->observation_values_invalid["cameraOdometry"] += 1.0;
-    return;
-  }
-
-  VectorXd rot_calib_std = floatlist2vector(log.getRotStd());
-  VectorXd trans_calib_std = floatlist2vector(log.getTransStd());
-
-  if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) {
-    this->observation_values_invalid["cameraOdometry"] += 1.0;
-    return;
-  }
-
-  if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) {
-    this->observation_values_invalid["cameraOdometry"] += 1.0;
-    return;
-  }
-
-  this->posenet_stds.pop_front();
-  this->posenet_stds.push_back(trans_calib_std[0]);
-
-  // Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise
-  trans_calib_std *= 10.0;
-  rot_calib_std *= 10.0;
-  MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal();
-  MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal();
-  this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION,
-    { rot_device }, { rot_device_cov });
-  this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
-    { trans_device }, { trans_device_cov });
-  this->observation_values_invalid["cameraOdometry"] *= DECAY;
-  this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]);
-}
-
-void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) {
-  if (!this->is_timestamp_valid(current_time)) {
-    this->observation_timings_invalid = true;
-    return;
-  }
-
-  if (log.getRpyCalib().size() > 0) {
-    auto live_calib = floatlist2vector(log.getRpyCalib());
-    if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) {
-      this->observation_values_invalid["liveCalibration"] += 1.0;
-      return;
-    }
-
-    this->calib = live_calib;
-    this->device_from_calib = euler2rot(this->calib);
-    this->calib_from_device = this->device_from_calib.transpose();
-    this->calibrated = log.getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED;
-    this->observation_values_invalid["liveCalibration"] *= DECAY;
-  }
-}
-
-void Localizer::reset_kalman(double current_time) {
-  const VectorXd &init_x = this->kf->get_initial_x();
-  const MatrixXdr &init_P = this->kf->get_initial_P();
-  this->reset_kalman(current_time, init_x, init_P);
-}
-
-void Localizer::finite_check(double current_time) {
-  bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all();
-  if (!all_finite) {
-    LOGE("Non-finite values detected, kalman reset");
-    this->reset_kalman(current_time);
-  }
-}
-
-void Localizer::time_check(double current_time) {
-  if (std::isnan(this->last_reset_time)) {
-    this->last_reset_time = current_time;
-  }
-  if (std::isnan(this->first_valid_log_time)) {
-    this->first_valid_log_time = current_time;
-  }
-  double filter_time = this->kf->get_filter_time();
-  bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10);
-  if (big_time_gap) {
-    LOGE("Time gap of over 10s detected, kalman reset");
-    this->reset_kalman(current_time);
-  }
-}
-
-void Localizer::update_reset_tracker() {
-  // reset tracker is tuned to trigger when over 1reset/10s over 2min period
-  if (this->is_gps_ok()) {
-    this->reset_tracker *= RESET_TRACKER_DECAY;
-  } else {
-    this->reset_tracker = 0.0;
-  }
-}
-
-void Localizer::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) {
-  // too nonlinear to init on completely wrong
-  VectorXd current_x = this->kf->get_x();
-  MatrixXdr current_P = this->kf->get_P();
-  MatrixXdr init_P = this->kf->get_initial_P();
-  const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P();
-  int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
-
-  current_x.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START) = init_orient;
-  current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START) = init_vel;
-  current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) = init_pos;
-
-  init_P.block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal();
-  init_P.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal();
-  init_P.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal();
-  init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START,
-    STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal();
-
-  this->reset_kalman(current_time, current_x, init_P);
-}
-
-void Localizer::reset_kalman(double current_time, const VectorXd &init_x, const MatrixXdr &init_P) {
-  this->kf->init_state(init_x, init_P, current_time);
-  this->last_reset_time = current_time;
-  this->reset_tracker += 1.0;
-}
-
-void Localizer::handle_msg_bytes(const char *data, const size_t size) {
-  AlignedBuffer aligned_buf;
-
-  capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size));
-  cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
-
-  this->handle_msg(event);
-}
-
-void Localizer::handle_msg(const cereal::Event::Reader& log) {
-  double t = log.getLogMonoTime() * 1e-9;
-  this->time_check(t);
-  if (log.isAccelerometer()) {
-    this->handle_sensor(t, log.getAccelerometer());
-  } else if (log.isGyroscope()) {
-    this->handle_sensor(t, log.getGyroscope());
-  } else if (log.isGpsLocation()) {
-    this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
-  } else if (log.isGpsLocationExternal()) {
-    this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
-  //} else if (log.isGnssMeasurements()) {
-  //  this->handle_gnss(t, log.getGnssMeasurements());
-  } else if (log.isCarState()) {
-    this->handle_car_state(t, log.getCarState());
-  } else if (log.isCameraOdometry()) {
-    this->handle_cam_odo(t, log.getCameraOdometry());
-  } else if (log.isLiveCalibration()) {
-    this->handle_live_calib(t, log.getLiveCalibration());
-  }
-  this->finite_check();
-  this->update_reset_tracker();
-}
-
-kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, bool inputsOK,
-                                                       bool sensorsOK, bool gpsOK, bool msgValid) {
-  cereal::Event::Builder evt = msg_builder.initEvent();
-  evt.setValid(msgValid);
-  cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
-  this->build_live_location(liveLoc);
-  liveLoc.setSensorsOK(sensorsOK);
-  liveLoc.setGpsOK(gpsOK);
-  liveLoc.setInputsOK(inputsOK);
-  return msg_builder.toBytes();
-}
-
-bool Localizer::is_gps_ok() {
-  return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0;
-}
-
-bool Localizer::critical_services_valid(const std::map<std::string, double> &critical_services) {
-  for (auto &kv : critical_services){
-    if (kv.second >= INPUT_INVALID_THRESHOLD){
-      return false;
-    }
-  }
-  return true;
-}
-
-bool Localizer::is_timestamp_valid(double current_time) {
-  double filter_time = this->kf->get_filter_time();
-  if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) {
-    LOGE("Observation timestamp is older than the max rewind threshold of the filter");
-    return false;
-  }
-  return true;
-}
-
-void Localizer::determine_gps_mode(double current_time) {
-  // 1. If the pos_std is greater than what's not acceptable and localizer is in gps-mode, reset to no-gps-mode
-  // 2. If the pos_std is greater than what's not acceptable and localizer is in no-gps-mode, fake obs
-  // 3. If the pos_std is smaller than what's not acceptable, let gps-mode be whatever it is
-  VectorXd current_pos_std = this->kf->get_P().block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt();
-  if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){
-    if (this->gps_mode){
-      this->gps_mode = false;
-      this->reset_kalman(current_time);
-    } else {
-      this->input_fake_gps_observations(current_time);
-    }
-  }
-}
-
-void Localizer::configure_gnss_source(const LocalizerGnssSource &source) {
-  this->gnss_source = source;
-  if (source == LocalizerGnssSource::UBLOX) {
-    this->gps_std_factor = 10.0;
-    this->gps_variance_factor = 1.0;
-    this->gps_vertical_variance_factor = 1.0;
-    this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET;
-  } else {
-    this->gps_std_factor = 2.0;
-    this->gps_variance_factor = 0.0;
-    this->gps_vertical_variance_factor = 3.0;
-    this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET;
-  }
-}
-
-int Localizer::locationd_thread() {
-  Params params;
-  LocalizerGnssSource source;
-  const char* gps_location_socket;
-  if (params.getBool("UbloxAvailable")) {
-    source = LocalizerGnssSource::UBLOX;
-    gps_location_socket = "gpsLocationExternal";
-  } else {
-    source = LocalizerGnssSource::QCOM;
-    gps_location_socket = "gpsLocation";
-  }
-
-  this->configure_gnss_source(source);
-  const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration",
-                                                          "carState", "accelerometer", "gyroscope"};
-
-  SubMaster sm(service_list, {}, nullptr, {gps_location_socket});
-  PubMaster pm({"liveLocationKalman"});
-
-  uint64_t cnt = 0;
-  bool filterInitialized = false;
-  const std::vector<std::string> critical_input_services = {"cameraOdometry", "liveCalibration", "accelerometer", "gyroscope"};
-  for (std::string service : critical_input_services) {
-    this->observation_values_invalid.insert({service, 0.0});
-  }
-
-  while (!do_exit) {
-    sm.update();
-    if (filterInitialized){
-      this->observation_timings_invalid_reset();
-      for (const char* service : service_list) {
-        if (sm.updated(service) && sm.valid(service)){
-          const cereal::Event::Reader log = sm[service];
-          this->handle_msg(log);
-        }
-      }
-    } else {
-      filterInitialized = sm.allAliveAndValid();
-    }
-
-    const char* trigger_msg = "cameraOdometry";
-    if (sm.updated(trigger_msg)) {
-      bool inputsOK = sm.allValid() && this->are_inputs_ok();
-      bool gpsOK = this->is_gps_ok();
-      bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"});
-
-      // Log time to first fix
-      if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) {
-        this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time);
-      }
-
-      MessageBuilder msg_builder;
-      kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
-      pm.send("liveLocationKalman", bytes.begin(), bytes.size());
-
-      if (cnt % 1200 == 0 && gpsOK) {  // once a minute
-        VectorXd posGeo = this->get_position_geodetic();
-        std::string lastGPSPosJSON = util::string_format(
-          "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
-        params.putNonBlocking("LastGPSPositionLLK", lastGPSPosJSON);
-      }
-      cnt++;
-    }
-  }
-  return 0;
-}
-
-int main() {
-  util::set_realtime_priority(5);
-
-  Localizer localizer;
-  return localizer.locationd_thread();
-}

sunnypilot/selfdrive/locationd/locationd.h

@@ -1,100 +0,0 @@
-#pragma once
-
-#include <eigen3/Eigen/Dense>
-#include <deque>
-#include <fstream>
-#include <memory>
-#include <map>
-#include <string>
-
-#include "cereal/messaging/messaging.h"
-#include "common/transformations/coordinates.hpp"
-#include "common/transformations/orientation.hpp"
-#include "common/params.h"
-#include "common/swaglog.h"
-#include "common/timing.h"
-#include "common/util.h"
-
-#include "sunnypilot/system/sensord/sensors/constants.h"
-#define VISION_DECIMATION 2
-#define SENSOR_DECIMATION 10
-#include "sunnypilot/selfdrive/locationd/models/live_kf.h"
-
-#define POSENET_STD_HIST_HALF 20
-
-enum LocalizerGnssSource {
-  UBLOX, QCOM
-};
-
-class Localizer {
-public:
-  Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX);
-
-  int locationd_thread();
-
-  void reset_kalman(double current_time = NAN);
-  void reset_kalman(double current_time, const Eigen::VectorXd &init_orient, const Eigen::VectorXd &init_pos, const Eigen::VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R);
-  void reset_kalman(double current_time, const Eigen::VectorXd &init_x, const MatrixXdr &init_P);
-  void finite_check(double current_time = NAN);
-  void time_check(double current_time = NAN);
-  void update_reset_tracker();
-  bool is_gps_ok();
-  bool critical_services_valid(const std::map<std::string, double> &critical_services);
-  bool is_timestamp_valid(double current_time);
-  void determine_gps_mode(double current_time);
-  bool are_inputs_ok();
-  void observation_timings_invalid_reset();
-
-  kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder,
-    bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
-  void build_live_location(cereal::LiveLocationKalman::Builder& fix);
-
-  Eigen::VectorXd get_position_geodetic();
-  Eigen::VectorXd get_state();
-  Eigen::VectorXd get_stdev();
-
-  void handle_msg_bytes(const char *data, const size_t size);
-  void handle_msg(const cereal::Event::Reader& log);
-  void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log);
-  void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset);
-  void handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log);
-  void handle_car_state(double current_time, const cereal::CarState::Reader& log);
-  void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log);
-  void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log);
-
-  void input_fake_gps_observations(double current_time);
-
-private:
-  std::unique_ptr<LiveKalman> kf;
-
-  Eigen::VectorXd calib;
-  MatrixXdr device_from_calib;
-  MatrixXdr calib_from_device;
-  bool calibrated = false;
-
-  double car_speed = 0.0;
-  double last_reset_time = NAN;
-  std::deque<double> posenet_stds;
-
-  std::unique_ptr<LocalCoord> converter;
-
-  int64_t unix_timestamp_millis = 0;
-  double reset_tracker = 0.0;
-  bool device_fell = false;
-  bool gps_mode = false;
-  double first_valid_log_time = NAN;
-  double ttff = NAN;
-  double last_gps_msg = 0;
-  LocalizerGnssSource gnss_source;
-  bool observation_timings_invalid = false;
-  std::map<std::string, double> observation_values_invalid;
-  bool standstill = true;
-  int32_t orientation_reset_count = 0;
-  float gps_std_factor;
-  float gps_variance_factor;
-  float gps_vertical_variance_factor;
-  double gps_time_offset;
-  Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std
-
-  void configure_gnss_source(const LocalizerGnssSource &source);
-};

sunnypilot/selfdrive/locationd/models/.gitignore

@@ -1 +0,0 @@
-generated/