bump

* init

* use internal frame

* exit early if toggle not enabled

* ui init

* ui: replace `static_cast<int>` with `std::nearbyint` for precise rounding

* revert

* update title

* handle live relaxed

* fix

* tweak ui

* toggle behaviors

* lint

* Update torqued_ext.py

* always update

* make sure it's updated properly with offroad states

* fix

* make sure to initialize

---------

Co-authored-by: nayan <nayan8teen@gmail.com>

.github/workflows/selfdrive_tests.yaml

@@ -115,7 +115,9 @@ 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: |
@@ -136,6 +138,7 @@ 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

@@ -146,6 +146,10 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   longitudinalPlanSource @1 :LongitudinalPlanSource;
   smartCruiseControl @2 :SmartCruiseControl;
   speedLimit @3 :SpeedLimit;
+  vTarget @4 :Float32;
+  aTarget @5 :Float32;
+  events @6 :List(OnroadEventSP.Event);
+  e2eAlerts @7 :E2eAlerts;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -160,6 +164,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
 
   struct SmartCruiseControl {
     vision @0 :Vision;
+    map @1 :Map;
 
     struct Vision {
       state @0 :VisionState;
@@ -171,6 +176,14 @@ 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.
@@ -179,16 +192,37 @@ 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 {
@@ -196,11 +230,27 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
       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.
+    }
   }
 
   enum LongitudinalPlanSource {
     cruise @0;
     sccVision @1;
+    sccMap @2;
+    speedLimitAssist @3;
+  }
+
+  struct E2eAlerts {
+    greenLightAlert @0 :Bool;
+    leadDepartAlert @1 :Bool;
   }
 }
 
@@ -243,6 +293,11 @@ struct OnroadEventSP @0xda96579883444c35 {
     pedalPressedAlertOnly @16;
     laneTurnLeft @17;
     laneTurnRight @18;
+    speedLimitPreActive @19;
+    speedLimitActive @20;
+    speedLimitChanged @21;
+    speedLimitPending @22;
+    e2eChime @23;
   }
 }
 
@@ -251,6 +306,7 @@ struct CarParamsSP @0x80ae746ee2596b11 {
   safetyParam @1 : Int16;  # flags for sunnypilot's custom safety flags
   pcmCruiseSpeed @3 :Bool;
   intelligentCruiseButtonManagementAvailable @4 :Bool;
+  enableGasInterceptor @5 :Bool;
 
   neuralNetworkLateralControl @2 :NeuralNetworkLateralControl;
 

cereal/log.capnp

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

cereal/services.py

@@ -89,6 +89,7 @@ _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,19 +148,24 @@ 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
@@ -187,7 +192,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"SunnylinkCache_Users", {PERSISTENT, STRING}},
     {"SunnylinkDongleId", {PERSISTENT, STRING}},
     {"SunnylinkdPid", {PERSISTENT, INT}},
-    {"SunnylinkEnabled", {PERSISTENT, BOOL}},
+    {"SunnylinkEnabled", {PERSISTENT, BOOL, "1"}},
 
     // Backup Manager params
     {"BackupManager_CreateBackup", {PERSISTENT, BOOL}},
@@ -225,10 +230,25 @@ 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"}},
     {"SpeedLimitPolicy", {PERSISTENT | BACKUP, INT, "3"}},
     {"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"}},
 };

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'])
+    self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP', 'longitudinalPlanSP'])
     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.params)
+    set_alternative_experience(self.CP, self.CP_SP, self.params)
     set_car_specific_params(self.CP, self.CP_SP, self.params)
 
     # Dynamic Experimental Control
@@ -216,6 +216,7 @@ 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,6 +53,7 @@ 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:
@@ -104,6 +105,12 @@ 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)
+    LongControl(car_params, car_params_sp)
     if car_params.steerControlType == CarParams.SteerControlType.angle:
       LatControlAngle(car_params, car_params_sp, car_interface)
     elif car_params.lateralTuning.which() == 'pid':

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.LoC = LongControl(self.CP, self.CP_SP)
     self.VM = VehicleModel(self.CP)
     self.LaC: LatControl
     if self.CP.steerControlType == car.CarParams.SteerControlType.angle:

selfdrive/controls/lib/latcontrol_torque.py

@@ -48,6 +48,10 @@ 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,8 +10,11 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
 
-def long_control_state_trans(CP, active, long_control_state, v_ego,
+def long_control_state_trans(CP, CP_SP, 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
@@ -45,8 +48,9 @@ def long_control_state_trans(CP, active, long_control_state, v_ego,
   return long_control_state
 
 class LongControl:
-  def __init__(self, CP):
+  def __init__(self, CP, CP_SP):
     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),
@@ -61,7 +65,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, active, self.long_control_state, CS.vEgo,
+    self.long_control_state = long_control_state_trans(self.CP, self.CP_SP, active, self.long_control_state, CS.vEgo,
                                                        should_stop, CS.brakePressed,
                                                        CS.cruiseState.standstill)
     if self.long_control_state == LongCtrlState.off:

selfdrive/controls/lib/longitudinal_planner.py

@@ -146,7 +146,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
       clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast])
       accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp)
 
-    # Get new v_cruise and a_desired from Smart Cruise Control
+    # Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist
     v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
 
     if force_slow_decel:

selfdrive/controls/plannerd.py

@@ -24,10 +24,11 @@ def main():
   pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
   sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
                             'liveMapDataSP', 'carStateSP', gps_location_service],
-                           poll='modelV2')
+                           poll='carState')
 
   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
+from cereal import car, custom
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans
 
 
@@ -8,49 +8,52 @@ 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, active, current_state, v_ego=0.1,
+    next_state = long_control_state_trans(CP, CP_SP, 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, active, current_state, v_ego=0.1,
+    next_state = long_control_state_trans(CP, CP_SP, 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, active, current_state, v_ego=0.1,
+    next_state = long_control_state_trans(CP, CP_SP, 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, active, current_state, v_ego=1.0,
+    next_state = long_control_state_trans(CP, CP_SP, 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, active, current_state, v_ego=1.0,
+    next_state = long_control_state_trans(CP, CP_SP, 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, active, current_state, v_ego=0.1,
+  next_state = long_control_state_trans(CP, CP_SP, 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, active, current_state, v_ego=0.1,
+  next_state = long_control_state_trans(CP, CP_SP, 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, active, current_state, v_ego=0.1,
+  next_state = long_control_state_trans(CP, CP_SP, 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, active, current_state, v_ego=0.1,
+  next_state = long_control_state_trans(CP, CP_SP, 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, active, current_state, v_ego=0.1,
+  next_state = long_control_state_trans(CP, CP_SP, 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, active, current_state, v_ego=1.0,
+  next_state = long_control_state_trans(CP, CP_SP, 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,6 +11,7 @@ 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
@@ -50,9 +51,10 @@ class TorqueBuckets(PointBuckets):
         break
 
 
-class TorqueEstimator(ParameterEstimator):
+class TorqueEstimator(ParameterEstimator, TorqueEstimatorExt):
   def __init__(self, CP, decimated=False, track_all_points=False):
-    super().__init__()
+    ParameterEstimator.__init__(self)
+    TorqueEstimatorExt.__init__(self, CP)
     self.CP = CP
     self.hist_len = int(HISTORY / DT_MDL)
     self.lag = 0.0
@@ -82,6 +84,8 @@ class TorqueEstimator(ParameterEstimator):
 
     self.calibrator = PoseCalibrator()
 
+    TorqueEstimatorExt.initialize_custom_params(self, decimated)
+
     self.reset()
 
     initial_params = {
@@ -260,6 +264,8 @@ 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

@@ -96,7 +96,7 @@ class SelfdriveD(CruiseHelper):
                                    'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
                                    'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback',
-                                   'modelDataV2SP'] + \
+                                   'modelDataV2SP', 'longitudinalPlanSP'] + \
                                    self.camera_packets + self.sensor_packets + self.gps_packets,
                                   ignore_alive=ignore, ignore_avg_freq=ignore,
                                   ignore_valid=ignore, frequency=int(1/DT_CTRL))
@@ -159,7 +159,7 @@ class SelfdriveD(CruiseHelper):
     self.mads = ModularAssistiveDrivingSystem(self)
     self.icbm = IntelligentCruiseButtonManagement(self.CP, self.CP_SP)
 
-    self.car_events_sp = CarSpecificEventsSP(self.CP, self.params)
+    self.car_events_sp = CarSpecificEventsSP(self.CP, self.CP_SP)
 
     CruiseHelper.__init__(self, self.CP)
 
@@ -205,6 +205,7 @@ class SelfdriveD(CruiseHelper):
 
     if not self.CP.notCar:
       self.events.add_from_msg(self.sm['driverMonitoringState'].events)
+      self.events_sp.add_from_msg(self.sm['longitudinalPlanSP'].events)
 
     # Add car events, ignore if CAN isn't valid
     if CS.canValid:
@@ -444,7 +445,7 @@ class SelfdriveD(CruiseHelper):
           self.events.add(EventName.personalityChanged)
         self.experimental_mode_switched = False
 
-    self.icbm.run(CS, self.sm['carControl'], self.is_metric)
+    self.icbm.run(CS, self.sm['carControl'], self.sm['longitudinalPlanSP'], self.is_metric)
 
   def data_sample(self):
     _car_state = messaging.recv_one(self.car_state_sock)

selfdrive/test/process_replay/migration.py

@@ -28,7 +28,8 @@ 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):
+def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: bool = False, camera_states: bool = False,
+                live_location_kalman: bool = True):
   migrations = [
     migrate_sensorEvents,
     migrate_carParams,
@@ -37,7 +38,6 @@ 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,6 +51,8 @@ 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=["livePose"],
+    subs=["liveLocationKalman", "livePose"],
     ignore=["logMonoTime"],
     should_recv_callback=MessageBasedRcvCallback("cameraOdometry"),
     tolerance=NUMPY_TOLERANCE,

selfdrive/ui/qt/onroad/hud.cc

@@ -47,10 +47,11 @@ 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,6 +24,7 @@ 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",
@@ -50,6 +51,8 @@ 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 = [
@@ -90,9 +93,13 @@ 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
+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_util = qt_util
 
 Export('sp_widgets_src', 'sp_qt_src', "sp_qt_util")

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

@@ -0,0 +1,63 @@
+/**
+ * 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

@@ -0,0 +1,42 @@
+/**
+ * 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

@@ -0,0 +1,40 @@
+/**
+ * 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

@@ -0,0 +1,28 @@
+/**
+ * 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,12 +52,16 @@ void MadsSettings::updateToggles(bool _offroad) {
   );
 
   auto cp_bytes = params.get("CarParamsPersistent");
-  if (!cp_bytes.empty()) {
+  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>();
 
-    if (isBrandInList(CP.getBrand(), mads_limited_settings_brands)) {
+    if (madsLimitedSettings(CP, CP_SP)) {
       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,7 +12,16 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
 
-const std::vector<std::string> mads_limited_settings_brands = {"rivian", "tesla"};
+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;
+}
 
 enum class MadsSteeringMode {
   REMAIN_ACTIVE = 0,

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

@@ -10,14 +10,15 @@
 NeuralNetworkLateralControl::NeuralNetworkLateralControl() :
   ParamControl("NeuralNetworkLateralControl", tr("Neural Network Lateral Control (NNLC)"),  "", "") {
   setConfirmation(true, false);
-  updateToggle();
+  updateToggle(offroad);
 }
 
-void NeuralNetworkLateralControl::updateToggle() {
+void NeuralNetworkLateralControl::updateToggle(bool _offroad) {
   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()) {
@@ -31,7 +32,7 @@ void NeuralNetworkLateralControl::updateToggle() {
     if (CP.getSteerControlType() == cereal::CarParams::SteerControlType::ANGLE) {
       params.remove("NeuralNetworkLateralControl");
       setDescription(nnffDescriptionBuilder(STATUS_NOT_AVAILABLE));
-      setEnabled(false);
+      allowed = false;
     } else {
       QString nn_model_name = QString::fromStdString(CP_SP.getNeuralNetworkLateralControl().getModel().getName());
       QString nn_fuzzy = CP_SP.getNeuralNetworkLateralControl().getFuzzyFingerprint() ?
@@ -56,4 +57,11 @@ void NeuralNetworkLateralControl::updateToggle() {
   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,10 +20,11 @@ public:
   NeuralNetworkLateralControl();
 
 public slots:
-  void updateToggle();
+  void updateToggle(bool _offroad);
 
 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

@@ -0,0 +1,58 @@
+/**
+ * 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

@@ -0,0 +1,27 @@
+/**
+ * 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

@@ -0,0 +1,81 @@
+/**
+ * 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

@@ -0,0 +1,36 @@
+/**
+ * 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,6 +75,36 @@ 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);
@@ -86,12 +116,10 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
       nnlcToggle->hideDescription();
     }
 
-    nnlcToggle->updateToggle();
+    nnlcToggle->updateToggle(offroad);
+    updateToggles(offroad);
   });
 
-  toggleOffroadOnly = {
-    madsToggle, nnlcToggle,
-  };
   QObject::connect(uiState(), &UIState::offroadTransition, this, &LateralPanel::updateToggles);
 
   sunnypilotScroller = new ScrollViewSP(list, this);
@@ -100,6 +128,7 @@ 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 {
@@ -120,7 +149,7 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
 }
 
 void LateralPanel::showEvent(QShowEvent *event) {
-  nnlcToggle->updateToggle();
+  nnlcToggle->updateToggle(offroad);
   updateToggles(offroad);
 }
 
@@ -129,27 +158,40 @@ void LateralPanel::hideEvent(QHideEvent *event) {
 }
 
 void LateralPanel::updateToggles(bool _offroad) {
-  for (auto *toggle : toggleOffroadOnly) {
-    toggle->setEnabled(_offroad);
-  }
-
+  bool torque_allowed = true;
   auto cp_bytes = params.get("CarParamsPersistent");
-  if (!cp_bytes.empty()) {
+  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>();
 
-    if (isBrandInList(CP.getBrand(), mads_limited_settings_brands)) {
+    if (madsLimitedSettings(CP, CP_SP)) {
       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,6 +15,7 @@
 #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"
@@ -35,7 +36,6 @@ private:
   QStackedLayout* main_layout = nullptr;
   QWidget* sunnypilotScreen = nullptr;
   ScrollViewSP *sunnypilotScroller = nullptr;
-  std::vector<ParamControl *> toggleOffroadOnly;
   bool offroad;
 
   ParamControl *madsToggle;
@@ -45,6 +45,9 @@ 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

@@ -37,9 +37,13 @@ inline const QString SpeedLimitSourcePolicyTexts[]{
 enum class SpeedLimitMode {
   OFF,
   INFORMATION,
+  WARNING,
+  ASSIST,
 };
 
 inline const QString SpeedLimitModeTexts[]{
   QObject::tr("Off"),
   QObject::tr("Information"),
+  QObject::tr("Warning"),
+  QObject::tr("Assist"),
 };

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

@@ -4,6 +4,7 @@
  * 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"
 
@@ -38,8 +39,9 @@ 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() {
@@ -48,6 +50,7 @@ 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

@@ -10,6 +10,7 @@
 #include "selfdrive/ui/sunnypilot/ui.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
@@ -25,6 +26,7 @@ signals:
 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

@@ -27,14 +27,16 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent)
   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 Mode"),
+    tr("Speed Limit"),
     "",
     "",
     speed_limit_mode_texts,
-    385);
+    380);
   list->addItem(speed_limit_mode_settings);
 
   list->addItem(horizontal_line());
@@ -42,10 +44,12 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent)
 
   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());
   });
@@ -91,7 +95,8 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent)
   connect(speed_limit_offset_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitSettings::refresh);
 
   refresh();
-  subPanelLayout->addWidget(list);
+  speedLimitScroller = new ScrollViewSP(list, this);
+  subPanelLayout->addWidget(speedLimitScroller);
   addWidget(subPanelFrame);
   addWidget(speedLimitPolicyScreen);
   setCurrentWidget(subPanelFrame);
@@ -103,6 +108,25 @@ void SpeedLimitSettings::refresh() {
   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));
 
@@ -120,6 +144,13 @@ void SpeedLimitSettings::refresh() {
     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();
 }

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

@@ -12,6 +12,7 @@
 #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
@@ -26,6 +27,7 @@ signals:
 
 private:
   Params params;
+  ScrollViewSP *speedLimitScroller;
   QFrame *subPanelFrame;
   ButtonParamControlSP *speed_limit_mode_settings;
   PushButtonSP *speedLimitSource;
@@ -55,15 +57,40 @@ private:
   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::INFORMATION) {
+    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")
+    return QString("%1<br>%2<br>%3<br>%4")
         .arg(off_str)
-        .arg(info_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,6 +50,13 @@ 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);
 
@@ -134,6 +141,7 @@ 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

@@ -32,6 +32,7 @@ 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,6 +13,7 @@
 
 #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"
@@ -87,6 +88,7 @@ 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,21 +25,48 @@ 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."),
-      "../assets/offroad/icon_monitoring.png",
+      "",
+      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.")),
+      "",
       false,
     },
   };

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

@@ -11,7 +11,17 @@
 #include "selfdrive/ui/qt/util.h"
 
 
-HudRendererSP::HudRendererSP() {}
+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});
+}
 
 void HudRendererSP::updateState(const UIState &s) {
   HudRenderer::updateState(s);
@@ -30,13 +40,21 @@ void HudRendererSP::updateState(const UIState &s) {
 
   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 < 2) {
+    if (speedLimitAheadDistance < speedLimitAheadDistancePrev && speedLimitAheadValidFrame < SPEED_LIMIT_AHEAD_VALID_FRAME_THRESHOLD) {
       speedLimitAheadValidFrame++;
     } else if (speedLimitAheadDistance > speedLimitAheadDistancePrev && speedLimitAheadValidFrame > 0) {
       speedLimitAheadValidFrame--;
@@ -92,26 +110,50 @@ 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;  // reserved for 2 icons
+    int y2_offset = -140;
 
+    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, y1_offset, "SCC-V");
+      drawSmartCruiseControlOnroadIcon(p, surface_rect, x_offset, y_scc_v, "SCC-V");
     }
+    smartCruiseControlVisionFrame = smartCruiseControlVisionActive ? (smartCruiseControlVisionFrame + 1) : 0;
 
-    if (smartCruiseControlVisionActive) {
-      smartCruiseControlVisionFrame++;
-    } else {
-      smartCruiseControlVisionFrame = 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");
     }
+    smartCruiseControlMapFrame = smartCruiseControlMapActive ? (smartCruiseControlMapFrame + 1) : 0;
 
     // Bottom Dev UI
     if (devUiInfo == 2) {
@@ -134,11 +176,61 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
     }
 
     // Speed Limit
-    if (speedLimitMode != SpeedLimitMode::OFF) {
-      drawSpeedLimitSigns(p);
-      drawUpcomingSpeedLimit(p);
+    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) {
@@ -327,23 +419,27 @@ void HudRendererSP::drawStandstillTimer(QPainter &p, int x, int y) {
   }
 }
 
-void HudRendererSP::drawSpeedLimitSigns(QPainter &p) {
-  QString speedLimitStr = speedLimit > 0 ? QString::number(std::nearbyint(speedLimit)) : "---";
+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));
+    speedLimitSubText = (speedLimitOffset > 0 ? "" : "-") + QString::number(std::nearbyint(speedLimitOffset));
   }
 
-  // Position next to MAX 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);
+  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
@@ -364,9 +460,8 @@ void HudRendererSP::drawSpeedLimitSigns(QPainter &p) {
 
     // Red border ring with color coding
     QRect red_ring = circle_rect;
-    QColor ring_color = QColor(255, 0, 0, alpha);
 
-    p.setBrush(ring_color);
+    p.setBrush(red_color);
     p.drawEllipse(red_ring);
 
     // Center white circle for text
@@ -378,13 +473,12 @@ void HudRendererSP::drawSpeedLimitSigns(QPainter &p) {
     // Speed value, smaller font for 3+ digits
     int font_size = (speedLimitStr.size() >= 3) ? 70 : 85;
     p.setFont(InterFont(font_size, QFont::Bold));
-    QColor speed_color = QColor(0, 0, 0, alpha);
 
     p.setPen(speed_color);
     p.drawText(center_circle, Qt::AlignCenter, speedLimitStr);
 
     // Offset value in small circular box
-    if (!speedLimitSubText.isEmpty()) {
+    if (!speedLimitSubText.isEmpty() && hasSpeedLimit) {
       int offset_circle_size = circle_size * 0.4;
       int overlap = offset_circle_size * 0.25;
       QRect offset_circle_rect(
@@ -398,7 +492,7 @@ void HudRendererSP::drawSpeedLimitSigns(QPainter &p) {
       p.setBrush(QColor(0, 0, 0, alpha));
       p.drawEllipse(offset_circle_rect);
 
-      p.setFont(InterFont(offset_circle_size * 0.45, QFont::Bold));
+      p.setFont(InterFont(offset_circle_size * speedLimitSubTextFactor, QFont::Bold));
       p.setPen(QColor(255, 255, 255, alpha));
       p.drawText(offset_circle_rect, Qt::AlignCenter, speedLimitSubText);
     }
@@ -424,13 +518,12 @@ void HudRendererSP::drawSpeedLimitSigns(QPainter &p) {
 
     // Speed value with color coding
     p.setFont(InterFont(90, QFont::Bold));
-    QColor speed_color = QColor(0, 0, 0, alpha);
 
     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()) {
+    if (!speedLimitSubText.isEmpty() && hasSpeedLimit) {
       int offset_box_size = sign_rect.width() * 0.4;
       int overlap = offset_box_size * 0.25;
       QRect offset_box_rect(
@@ -445,7 +538,7 @@ void HudRendererSP::drawSpeedLimitSigns(QPainter &p) {
       p.setBrush(QColor(0, 0, 0, alpha));
       p.drawRoundedRect(offset_box_rect, corner_radius, corner_radius);
 
-      p.setFont(InterFont(offset_box_size * 0.45, QFont::Bold));
+      p.setFont(InterFont(offset_box_size * speedLimitSubTextFactor, QFont::Bold));
       p.setPen(QColor(255, 255, 255, alpha));
       p.drawText(offset_box_rect, Qt::AlignCenter, speedLimitSubText);
     }
@@ -514,3 +607,127 @@ void HudRendererSP::drawUpcomingSpeedLimit(QPainter &p) {
   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

@@ -11,6 +11,8 @@
 #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
 
@@ -29,8 +31,12 @@ 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);
+  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;
@@ -66,12 +72,38 @@ 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,8 +25,10 @@ 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,6 +7,7 @@
 
 #pragma once
 
+#include <cmath>
 #include <map>
 #include <optional>
 #include <string>
@@ -500,7 +501,7 @@ private:
 
   int getParamValueScaled() {
     const auto param_value = QString::fromStdString(params.get(key));
-    return static_cast<int>(param_value.toFloat() * 100);
+    return std::nearbyint(param_value.toFloat() * 100.0f);
   }
 
   void setParamValueScaled(const int new_value) {

selfdrive/ui/sunnypilot/ui.cc

@@ -11,6 +11,15 @@
 
 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) {
@@ -53,7 +62,22 @@ 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 = params.getBool("SpeedLimitMode");
+  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,6 +61,7 @@ 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

@@ -11,4 +11,8 @@ 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,6 +204,13 @@ 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,2 +1,3 @@
 SConscript(['modeld/SConscript'])
-SConscript(['modeld_v2/SConscript'])
+SConscript(['modeld_v2/SConscript'])
+SConscript(['selfdrive/locationd/SConscript'])

sunnypilot/mads/helpers.py

@@ -9,6 +9,10 @@ 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:
@@ -17,20 +21,25 @@ class MadsSteeringModeOnBrake:
   DISENGAGE = 2
 
 
-def get_mads_limited_brands(CP: structs.CarParams) -> bool:
-  return CP.brand in ("rivian", "tesla")
+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 read_steering_mode_param(CP: structs.CarParams, params: Params):
-  if get_mads_limited_brands(CP):
+def read_steering_mode_param(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params):
+  if get_mads_limited_brands(CP, CP_SP):
     return MadsSteeringModeOnBrake.DISENGAGE
 
   return params.get("MadsSteeringMode", return_default=True)
 
 
-def set_alternative_experience(CP: structs.CarParams, params: Params):
+def set_alternative_experience(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params):
   enabled = params.get_bool("Mads")
-  steering_mode = read_steering_mode_param(CP, params)
+  steering_mode = read_steering_mode_param(CP, CP_SP, params)
 
   if enabled:
     CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS
@@ -53,9 +62,12 @@ 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/Tesla, identify consistent signals for MADS toggling
-  mads_partial_support = get_mads_limited_brands(CP)
+  # 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)
   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, get_mads_limited_brands
+from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param, MADS_NO_ACC_MAIN_BUTTON
 from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT
 
 State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
@@ -27,6 +27,7 @@ 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
@@ -43,14 +44,16 @@ 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 get_mads_limited_brands(self.CP):
+    if self.CP.brand in MADS_NO_ACC_MAIN_BUTTON:
       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.params)
+    self.steering_mode_on_brake = read_steering_mode_param(self.CP, self.CP_SP, self.params)
     self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode")
 
   def read_params(self):
@@ -58,6 +61,7 @@ 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
 
@@ -102,12 +106,13 @@ class ModularAssistiveDrivingSystem:
 
   def update_events(self, CS: structs.CarState):
     if not self.selfdrive.enabled and self.enabled:
-      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 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.wrongGear) and (CS.vEgo < 2.5 or CS.gearShifter == GearShifter.reverse):
         self.replace_event(EventName.wrongGear, EventNameSP.silentWrongGear)
         self.transition_paused_state()
@@ -126,7 +131,6 @@ 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,25 +4,23 @@ 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
 
-from cereal import messaging
-from openpilot.common.gps import get_gps_location_service
+import cereal.messaging as messaging
 from openpilot.common.params import Params
-from openpilot.sunnypilot.navd.helpers import Coordinate, coordinate_from_param
+from openpilot.sunnypilot.navd.helpers import coordinate_from_param
 
 
 class BaseMapData(ABC):
   def __init__(self):
     self.params = Params()
 
-    self.gps_location_service = get_gps_location_service(self.params)
-    self.sm = messaging.SubMaster(['livePose', 'carControl'] + [self.gps_location_service])
+    self.sm = messaging.SubMaster(['liveLocationKalman'])
     self.pm = messaging.PubMaster(['liveMapDataSP'])
 
-    self.last_position = coordinate_from_param("LastGPSPosition", self.params)
-    self.last_altitude = None
+    self.localizer_valid = False
+    self.last_bearing = None
+    self.last_position = coordinate_from_param("LastGPSPositionLLK", self.params)
 
   @abstractmethod
   def update_location(self) -> None:
@@ -40,24 +38,12 @@ 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.all_checks(service_list=[self.gps_location_service, 'livePose'])
+    mapd_sp_send.valid = self.sm['liveLocationKalman'].gpsOK
     live_map_data = mapd_sp_send.liveMapDataSP
 
     live_map_data.speedLimitValid = bool(speed_limit > 0)
@@ -70,7 +56,6 @@ class BaseMapData(ABC):
     self.pm.send('liveMapDataSP', mapd_sp_send)
 
   def tick(self) -> None:
-    self.sm.update()
-    self.get_current_location()
+    self.sm.update(0)
     self.update_location()
     self.publish()

sunnypilot/mapd/live_map_data/osm_map_data.py

@@ -5,8 +5,10 @@ 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
@@ -15,19 +17,27 @@ 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:
-    if self.last_position is None or self.last_altitude is 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:
       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/selfdrive/assets/images/green_light.png

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

sunnypilot/selfdrive/assets/images/lead_depart.png

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

sunnypilot/selfdrive/assets/img_minus_arrow_down.png

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

sunnypilot/selfdrive/assets/img_plus_arrow_up.png

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

sunnypilot/selfdrive/assets/offroad/icon_display.png

@@ -0,0 +1,3 @@
+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, params):
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
     self.CP = CP
-    self.params = params
+    self.CP_SP = CP_SP
 
     self.low_speed_alert = False
 
@@ -37,9 +37,15 @@ class CarSpecificEventsSP:
         # TODO-SP: add 1 m/s hysteresis
         if CS.vEgo >= self.CP.minEnableSpeed:
           self.low_speed_alert = False
-        if CS.gearShifter != GearShifter.drive:
+        if self.CP.minEnableSpeed >= 14.5 and 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,18 +6,24 @@ See the LICENSE.md file in the root directory for more details.
 """
 import numpy as np
 
-from cereal import car
+from cereal import car, custom
 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:
@@ -32,11 +38,12 @@ 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
@@ -47,6 +54,16 @@ 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)
@@ -89,3 +106,33 @@ 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

@@ -49,19 +49,11 @@ class IntelligentCruiseButtonManagement:
   def v_cruise_equal(self) -> bool:
     return self.v_target == self.v_cruise_cluster
 
-  def update_calculations(self, CS: car.CarState) -> None:
+  def update_calculations(self, CS: car.CarState, LP_SP: custom.LongitudinalPlanSP) -> 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
 
-    # 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_ms_last = apply_hysteresis(LP_SP.vTarget, 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)
@@ -116,20 +108,18 @@ class IntelligentCruiseButtonManagement:
   def update_readiness(self, CS: car.CarState, CC: car.CarControl) -> None:
     update_manual_button_timers(CS, self.cruise_button_timers)
 
-    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
+    ready = CC.enabled 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, is_metric: bool) -> None:
+  def run(self, CS: car.CarState, CC: car.CarControl, LP_SP: custom.LongitudinalPlanSP, is_metric: bool) -> None:
     if self.CP_SP.pcmCruiseSpeed:
       return
 
     self.is_metric = is_metric
 
-    self.update_calculations(CS)
+    self.update_calculations(CS, LP_SP)
     self.update_readiness(CS, CC)
 
     self.cruise_button = self.update_state_machine()

sunnypilot/selfdrive/car/interfaces.py

@@ -22,8 +22,18 @@ def log_fingerprint(CP: structs.CarParams) -> None:
     sentry.capture_fingerprint(CP.carFingerprint, CP.brand)
 
 
-def _initialize_neural_network_lateral_control(CI: CarInterfaceBase, CP: structs.CarParams, CP_SP: structs.CarParamsSP,
-                                               params: Params = None, enabled: bool = False) -> None:
+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:
   if params is None:
     params = Params()
 
@@ -35,13 +45,12 @@ def _initialize_neural_network_lateral_control(CI: CarInterfaceBase, CP: structs
   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:
@@ -52,12 +61,19 @@ 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
 
-  _initialize_neural_network_lateral_control(CI, CP, CP_SP, params)
+  enforce_torque = _enforce_torque_lateral_control(CP, params)
+  nnlc_enabled = _initialize_neural_network_lateral_control(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/controls/lib/e2e_alerts_helper.py

@@ -0,0 +1,58 @@
+"""
+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,11 +6,13 @@ 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):
+class LatControlTorqueExt(NeuralNetworkLateralControl, LatControlTorqueExtOverride):
   def __init__(self, lac_torque, CP, CP_SP, CI):
-    super().__init__(lac_torque, CP, CP_SP, CI)
+    NeuralNetworkLateralControl.__init__(self, lac_torque, CP, CP_SP, CI)
+    LatControlTorqueExtOverride.__init__(self, CP)
 
   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

@@ -0,0 +1,34 @@
+"""
+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,22 +7,34 @@ 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.selfdrived.events import EventsSP
 from openpilot.sunnypilot.models.helpers import get_active_bundle
 
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
-Source = custom.LongitudinalPlanSP.LongitudinalPlanSource
+LongitudinalPlanSource = 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 = Source.cruise
+    self.source = LongitudinalPlanSource.cruise
+    self.e2e_alerts_helper = E2EAlertsHelper()
+
+    self.output_v_target = 0.
+    self.output_a_target = 0.
 
   @property
   def mlsim(self) -> bool:
@@ -36,23 +48,39 @@ 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]:
-    self.scc.update(sm, v_ego, a_ego, v_cruise)
+    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.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)
+
     targets = {
-      Source.cruise: (v_cruise, a_ego),
-      Source.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target)
+      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),
     }
 
     self.source = min(targets, key=lambda k: targets[k][0])
-    v_target, a_target = targets[self.source]
-
-    return v_target, a_target
+    self.output_v_target, self.output_a_target = targets[self.source]
+    return self.output_v_target, self.output_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')
@@ -61,6 +89,9 @@ 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
     dec = longitudinalPlanSP.dec
@@ -70,7 +101,7 @@ class LongitudinalPlannerSP:
 
     # Smart Cruise Control
     smartCruiseControl = longitudinalPlanSP.smartCruiseControl
-    # Vision Turn Speed Control
+    # Vision Control
     sccVision = smartCruiseControl.vision
     sccVision.state = self.scc.vision.state
     sccVision.vTarget = float(self.scc.vision.output_v_target)
@@ -79,13 +110,36 @@ 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
 
     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 = 0
+    self.frame = -1
 
   def update(self, params: Params) -> None:
     self.frame += 1

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

@@ -0,0 +1,9 @@
+"""
+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

@@ -0,0 +1,261 @@
+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,14 @@ 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, v_ego: float, a_ego: float, v_cruise: float) -> None:
-    long_enabled = sm['carControl'].enabled
-    long_override = sm['carControl'].cruiseControl.override
-
+  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

@@ -0,0 +1,58 @@
+"""
+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,19 +8,17 @@ 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.
 
@@ -73,7 +71,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
 
@@ -114,7 +112,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

@@ -6,3 +6,14 @@ 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

@@ -19,3 +19,10 @@ 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

@@ -0,0 +1,19 @@
+"""
+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

@@ -0,0 +1,397 @@
+"""
+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

@@ -26,6 +26,9 @@ class SpeedLimitResolver:
   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
@@ -54,6 +57,27 @@ class SpeedLimitResolver:
     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)
@@ -149,4 +173,6 @@ class SpeedLimitResolver:
     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

@@ -0,0 +1,241 @@
+"""
+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/locationd/.gitignore

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

sunnypilot/selfdrive/locationd/SConscript

@@ -0,0 +1,37 @@
+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

@@ -0,0 +1,750 @@
+#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

@@ -0,0 +1,100 @@
+#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

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

sunnypilot/selfdrive/locationd/models/car_kf.py

@@ -0,0 +1,180 @@
+#!/usr/bin/env python3
+import math
+import sys
+from typing import Any
+
+import numpy as np
+
+from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind
+from openpilot.common.swaglog import cloudlog
+
+from rednose.helpers.kalmanfilter import KalmanFilter
+
+if __name__ == '__main__':  # Generating sympy
+  import sympy as sp
+  from rednose.helpers.ekf_sym import gen_code
+else:
+  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
+
+
+i = 0
+
+def _slice(n):
+  global i
+  s = slice(i, i + n)
+  i += n
+
+  return s
+
+
+class States:
+  # Vehicle model params
+  STIFFNESS = _slice(1)  # [-]
+  STEER_RATIO = _slice(1)  # [-]
+  ANGLE_OFFSET = _slice(1)  # [rad]
+  ANGLE_OFFSET_FAST = _slice(1)  # [rad]
+
+  VELOCITY = _slice(2)  # (x, y) [m/s]
+  YAW_RATE = _slice(1)  # [rad/s]
+  STEER_ANGLE = _slice(1)  # [rad]
+  ROAD_ROLL = _slice(1)  # [rad]
+
+
+class CarKalman(KalmanFilter):
+  name = 'car'
+
+  initial_x = np.array([
+    1.0,
+    15.0,
+    0.0,
+    0.0,
+
+    10.0, 0.0,
+    0.0,
+    0.0,
+    0.0
+  ])
+
+  # process noise
+  Q = np.diag([
+    (.05 / 100)**2,
+    .01**2,
+    math.radians(0.02)**2,
+    math.radians(0.25)**2,
+
+    .1**2, .01**2,
+    math.radians(0.1)**2,
+    math.radians(0.1)**2,
+    math.radians(1)**2,
+  ])
+  P_initial = Q.copy()
+
+  obs_noise: dict[int, Any] = {
+    ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
+    ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
+    ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
+    ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
+    ObservationKind.STIFFNESS: np.atleast_2d(0.5**2),
+    ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
+  }
+
+  global_vars = [
+    'mass',
+    'rotational_inertia',
+    'center_to_front',
+    'center_to_rear',
+    'stiffness_front',
+    'stiffness_rear',
+  ]
+
+  @staticmethod
+  def generate_code(generated_dir):
+    dim_state = CarKalman.initial_x.shape[0]
+    name = CarKalman.name
+
+    # Linearized single-track lateral dynamics, equations 7.211-7.213
+    # Massimo Guiggiani, The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars
+    # Springer Cham, 2023. doi: https://doi.org/10.1007/978-3-031-06461-6
+
+    # globals
+    global_vars = [sp.Symbol(name) for name in CarKalman.global_vars]
+    m, j, aF, aR, cF_orig, cR_orig = global_vars
+
+    # make functions and jacobians with sympy
+    # state variables
+    state_sym = sp.MatrixSymbol('state', dim_state, 1)
+    state = sp.Matrix(state_sym)
+
+    # Vehicle model constants
+    sf = state[States.STIFFNESS, :][0, 0]
+
+    cF, cR = sf * cF_orig, sf * cR_orig
+    angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
+    angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
+    theta = state[States.ROAD_ROLL, :][0, 0]
+    sa = state[States.STEER_ANGLE, :][0, 0]
+
+    sR = state[States.STEER_RATIO, :][0, 0]
+    u, v = state[States.VELOCITY, :]
+    r = state[States.YAW_RATE, :][0, 0]
+
+    A = sp.Matrix(np.zeros((2, 2)))
+    A[0, 0] = -(cF + cR) / (m * u)
+    A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
+    A[1, 0] = -(cF * aF - cR * aR) / (j * u)
+    A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
+
+    B = sp.Matrix(np.zeros((2, 1)))
+    B[0, 0] = cF / m / sR
+    B[1, 0] = (cF * aF) / j / sR
+
+    C = sp.Matrix(np.zeros((2, 1)))
+    C[0, 0] = ACCELERATION_DUE_TO_GRAVITY
+    C[1, 0] = 0
+
+    x = sp.Matrix([v, r])  # lateral velocity, yaw rate
+    x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta
+
+    dt = sp.Symbol('dt')
+    state_dot = sp.Matrix(np.zeros((dim_state, 1)))
+    state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
+    state_dot[States.YAW_RATE.start, 0] = x_dot[1]
+
+    # Basic descretization, 1st order integrator
+    # Can be pretty bad if dt is big
+    f_sym = state + dt * state_dot
+
+    #
+    # Observation functions
+    #
+    obs_eqs = [
+      [sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
+      [sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
+      [sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None],
+      [sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
+      [sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
+      [sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
+      [sp.Matrix([sf]), ObservationKind.STIFFNESS, None],
+      [sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
+    ]
+
+    gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
+
+  def __init__(self, generated_dir):
+    dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
+    self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
+                              dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
+
+  def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
+    self.filter.set_global("mass", mass)
+    self.filter.set_global("rotational_inertia", rotational_inertia)
+    self.filter.set_global("center_to_front", center_to_front)
+    self.filter.set_global("center_to_rear", center_to_rear)
+    self.filter.set_global("stiffness_front", stiffness_front)
+    self.filter.set_global("stiffness_rear", stiffness_rear)
+
+
+if __name__ == "__main__":
+  generated_dir = sys.argv[2]
+  CarKalman.generate_code(generated_dir)

sunnypilot/selfdrive/locationd/models/constants.py

@@ -0,0 +1,92 @@
+import os
+
+GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
+
+class ObservationKind:
+  UNKNOWN = 0
+  NO_OBSERVATION = 1
+  GPS_NED = 2
+  ODOMETRIC_SPEED = 3
+  PHONE_GYRO = 4
+  GPS_VEL = 5
+  PSEUDORANGE_GPS = 6
+  PSEUDORANGE_RATE_GPS = 7
+  SPEED = 8
+  NO_ROT = 9
+  PHONE_ACCEL = 10
+  ORB_POINT = 11
+  ECEF_POS = 12
+  CAMERA_ODO_TRANSLATION = 13
+  CAMERA_ODO_ROTATION = 14
+  ORB_FEATURES = 15
+  MSCKF_TEST = 16
+  FEATURE_TRACK_TEST = 17
+  LANE_PT = 18
+  IMU_FRAME = 19
+  PSEUDORANGE_GLONASS = 20
+  PSEUDORANGE_RATE_GLONASS = 21
+  PSEUDORANGE = 22
+  PSEUDORANGE_RATE = 23
+  ECEF_VEL = 35
+  ECEF_ORIENTATION_FROM_GPS = 32
+  NO_ACCEL = 33
+  ORB_FEATURES_WIDE = 34
+
+  ROAD_FRAME_XY_SPEED = 24  # (x, y) [m/s]
+  ROAD_FRAME_YAW_RATE = 25  # [rad/s]
+  STEER_ANGLE = 26  # [rad]
+  ANGLE_OFFSET_FAST = 27  # [rad]
+  STIFFNESS = 28  # [-]
+  STEER_RATIO = 29  # [-]
+  ROAD_FRAME_X_SPEED = 30  # (x) [m/s]
+  ROAD_ROLL = 31  # [rad]
+
+  names = [
+    'Unknown',
+    'No observation',
+    'GPS NED',
+    'Odometric speed',
+    'Phone gyro',
+    'GPS velocity',
+    'GPS pseudorange',
+    'GPS pseudorange rate',
+    'Speed',
+    'No rotation',
+    'Phone acceleration',
+    'ORB point',
+    'ECEF pos',
+    'camera odometric translation',
+    'camera odometric rotation',
+    'ORB features',
+    'MSCKF test',
+    'Feature track test',
+    'Lane ecef point',
+    'imu frame eulers',
+    'GLONASS pseudorange',
+    'GLONASS pseudorange rate',
+    'pseudorange',
+    'pseudorange rate',
+
+    'Road Frame x,y speed',
+    'Road Frame yaw rate',
+    'Steer Angle',
+    'Fast Angle Offset',
+    'Stiffness',
+    'Steer Ratio',
+    'Road Frame x speed',
+    'Road Roll',
+    'ECEF orientation from GPS',
+    'NO accel',
+    'ORB features wide camera',
+    'ECEF_VEL',
+  ]
+
+  @classmethod
+  def to_string(cls, kind):
+    return cls.names[kind]
+
+
+SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
+           ObservationKind.PSEUDORANGE_RATE_GPS,
+           ObservationKind.PSEUDORANGE_GLONASS,
+           ObservationKind.PSEUDORANGE_RATE_GLONASS]

sunnypilot/selfdrive/locationd/models/live_kf.cc

@@ -0,0 +1,122 @@
+#include "sunnypilot/selfdrive/locationd/models/live_kf.h"
+
+using namespace EKFS;
+using namespace Eigen;
+
+Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) {
+  return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols());
+}
+
+Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) {
+  return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols());
+}
+
+std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) {
+  std::vector<Eigen::Map<Eigen::VectorXd>> res;
+  for (const Eigen::VectorXd &vec : vec_vec) {
+    res.push_back(get_mapvec(vec));
+  }
+  return res;
+}
+
+std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) {
+  std::vector<Eigen::Map<MatrixXdr>> res;
+  for (const MatrixXdr &mat : mat_vec) {
+    res.push_back(get_mapmat(mat));
+  }
+  return res;
+}
+
+LiveKalman::LiveKalman() {
+  this->dim_state = live_initial_x.rows();
+  this->dim_state_err = live_initial_P_diag.rows();
+
+  this->initial_x = live_initial_x;
+  this->initial_P = live_initial_P_diag.asDiagonal();
+  this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal();
+  this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal();
+  this->reset_orientation_P = live_reset_orientation_diag.asDiagonal();
+  this->Q = live_Q_diag.asDiagonal();
+  for (auto& pair : live_obs_noise_diag) {
+    this->obs_noise[pair.first] = pair.second.asDiagonal();
+  }
+
+  // init filter
+  this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
+    get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
+    std::vector<int>{3}, std::vector<std::string>(), 0.8);
+}
+
+void LiveKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) {
+  MatrixXdr covs = covs_diag.asDiagonal();
+  this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
+}
+
+void LiveKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
+  this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
+}
+
+void LiveKalman::init_state(const VectorXd &state, double filter_time) {
+  MatrixXdr covs = this->filter->covs();
+  this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
+}
+
+VectorXd LiveKalman::get_x() {
+  return this->filter->state();
+}
+
+MatrixXdr LiveKalman::get_P() {
+  return this->filter->covs();
+}
+
+double LiveKalman::get_filter_time() {
+  return this->filter->get_filter_time();
+}
+
+std::vector<MatrixXdr> LiveKalman::get_R(int kind, int n) {
+  std::vector<MatrixXdr> R;
+  for (int i = 0; i < n; i++) {
+    R.push_back(this->obs_noise[kind]);
+  }
+  return R;
+}
+
+std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
+  std::optional<Estimate> r;
+  if (R.size() == 0) {
+    R = this->get_R(kind, meas.size());
+  }
+  r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
+  return r;
+}
+
+void LiveKalman::predict(double t) {
+  this->filter->predict(t);
+}
+
+const Eigen::VectorXd &LiveKalman::get_initial_x() {
+  return this->initial_x;
+}
+
+const MatrixXdr &LiveKalman::get_initial_P() {
+  return this->initial_P;
+}
+
+const MatrixXdr &LiveKalman::get_fake_gps_pos_cov() {
+  return this->fake_gps_pos_cov;
+}
+
+const MatrixXdr &LiveKalman::get_fake_gps_vel_cov() {
+  return this->fake_gps_vel_cov;
+}
+
+const MatrixXdr &LiveKalman::get_reset_orientation_P() {
+  return this->reset_orientation_P;
+}
+
+MatrixXdr LiveKalman::H(const VectorXd &in) {
+  assert(in.size() == 6);
+  Matrix<double, 3, 6, Eigen::RowMajor> res;
+  this->filter->get_extra_routine("H")((double*)in.data(), res.data());
+  return res;
+}

sunnypilot/selfdrive/locationd/models/live_kf.h

@@ -0,0 +1,66 @@
+#pragma once
+
+#include <string>
+#include <cmath>
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Dense>
+
+#include "generated/live_kf_constants.h"
+#include "rednose/helpers/ekf_sym.h"
+
+#define EARTH_GM 3.986005e14  // m^3/s^2 (gravitational constant * mass of earth)
+
+using namespace EKFS;
+
+Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec);
+Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat);
+std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec);
+std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec);
+
+class LiveKalman {
+public:
+  LiveKalman();
+
+  void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time);
+  void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time);
+  void init_state(const Eigen::VectorXd &state, double filter_time);
+
+  Eigen::VectorXd get_x();
+  MatrixXdr get_P();
+  double get_filter_time();
+  std::vector<MatrixXdr> get_R(int kind, int n);
+
+  std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {});
+  std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
+  std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
+  std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
+  void predict(double t);
+
+  const Eigen::VectorXd &get_initial_x();
+  const MatrixXdr &get_initial_P();
+  const MatrixXdr &get_fake_gps_pos_cov();
+  const MatrixXdr &get_fake_gps_vel_cov();
+  const MatrixXdr &get_reset_orientation_P();
+
+  MatrixXdr H(const Eigen::VectorXd &in);
+
+private:
+  std::string name = "live";
+
+  std::shared_ptr<EKFSym> filter;
+
+  int dim_state;
+  int dim_state_err;
+
+  Eigen::VectorXd initial_x;
+  MatrixXdr initial_P;
+  MatrixXdr fake_gps_pos_cov;
+  MatrixXdr fake_gps_vel_cov;
+  MatrixXdr reset_orientation_P;
+  MatrixXdr Q;  // process noise
+  std::unordered_map<int, MatrixXdr> obs_noise;
+};

sunnypilot/selfdrive/locationd/models/live_kf.py

@@ -0,0 +1,242 @@
+#!/usr/bin/env python3
+
+import sys
+import os
+import numpy as np
+
+from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind
+
+import sympy as sp
+import inspect
+from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
+from rednose.helpers.ekf_sym import gen_code
+
+EARTH_GM = 3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)
+
+
+def numpy2eigenstring(arr):
+  assert(len(arr.shape) == 1)
+  arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '')
+  return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()"
+
+
+class States:
+  ECEF_POS = slice(0, 3)  # x, y and z in ECEF in meters
+  ECEF_ORIENTATION = slice(3, 7)  # quat for pose of phone in ecef
+  ECEF_VELOCITY = slice(7, 10)  # ecef velocity in m/s
+  ANGULAR_VELOCITY = slice(10, 13)  # roll, pitch and yaw rates in device frame in radians/s
+  GYRO_BIAS = slice(13, 16)  # roll, pitch and yaw biases
+  ACCELERATION = slice(16, 19)  # Acceleration in device frame in m/s**2
+  ACC_BIAS = slice(19, 22)  # Acceletometer bias in m/s**2
+
+  # Error-state has different slices because it is an ESKF
+  ECEF_POS_ERR = slice(0, 3)
+  ECEF_ORIENTATION_ERR = slice(3, 6)  # euler angles for orientation error
+  ECEF_VELOCITY_ERR = slice(6, 9)
+  ANGULAR_VELOCITY_ERR = slice(9, 12)
+  GYRO_BIAS_ERR = slice(12, 15)
+  ACCELERATION_ERR = slice(15, 18)
+  ACC_BIAS_ERR = slice(18, 21)
+
+
+class LiveKalman:
+  name = 'live'
+
+  initial_x = np.array([3.88e6, -3.37e6, 3.76e6,
+                        0.42254641, -0.31238054, -0.83602975, -0.15788347,  # NED [0,0,0] -> ECEF Quat
+                        0, 0, 0,
+                        0, 0, 0,
+                        0, 0, 0,
+                        0, 0, 0,
+                        0, 0, 0])
+
+  # state covariance
+  initial_P_diag = np.array([10**2, 10**2, 10**2,
+                             0.01**2, 0.01**2, 0.01**2,
+                             10**2, 10**2, 10**2,
+                             1**2, 1**2, 1**2,
+                             1**2, 1**2, 1**2,
+                             100**2, 100**2, 100**2,
+                             0.01**2, 0.01**2, 0.01**2])
+
+  # state covariance when resetting midway in a segment
+  reset_orientation_diag = np.array([1**2, 1**2, 1**2])
+
+  # fake observation covariance, to ensure the uncertainty estimate of the filter is under control
+  fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2])
+  fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2])
+
+  # process noise
+  Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
+                     0.001**2, 0.001**2, 0.001**2,
+                     0.01**2, 0.01**2, 0.01**2,
+                     0.1**2, 0.1**2, 0.1**2,
+                     (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
+                     3**2, 3**2, 3**2,
+                     0.005**2, 0.005**2, 0.005**2])
+
+  obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
+                    ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
+                    ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
+                    ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]),
+                    ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
+                    ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
+                    ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]),
+                    ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])}
+
+  @staticmethod
+  def generate_code(generated_dir):
+    name = LiveKalman.name
+    dim_state = LiveKalman.initial_x.shape[0]
+    dim_state_err = LiveKalman.initial_P_diag.shape[0]
+
+    state_sym = sp.MatrixSymbol('state', dim_state, 1)
+    state = sp.Matrix(state_sym)
+    x, y, z = state[States.ECEF_POS, :]
+    q = state[States.ECEF_ORIENTATION, :]
+    v = state[States.ECEF_VELOCITY, :]
+    vx, vy, vz = v
+    omega = state[States.ANGULAR_VELOCITY, :]
+    vroll, vpitch, vyaw = omega
+    roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
+    acceleration = state[States.ACCELERATION, :]
+    acc_bias = state[States.ACC_BIAS, :]
+
+    dt = sp.Symbol('dt')
+
+    # calibration and attitude rotation matrices
+    quat_rot = quat_rotate(*q)
+
+    # Got the quat predict equations from here
+    # A New Quaternion-Based Kalman Filter for
+    # Real-Time Attitude Estimation Using the Two-Step
+    # Geometrically-Intuitive Correction Algorithm
+    A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw],
+                         [vroll, 0, vyaw, -vpitch],
+                         [vpitch, -vyaw, 0, vroll],
+                         [vyaw, vpitch, -vroll, 0]])
+    q_dot = A * q
+
+    # Time derivative of the state as a function of state
+    state_dot = sp.Matrix(np.zeros((dim_state, 1)))
+    state_dot[States.ECEF_POS, :] = v
+    state_dot[States.ECEF_ORIENTATION, :] = q_dot
+    state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
+
+    # Basic descretization, 1st order intergrator
+    # Can be pretty bad if dt is big
+    f_sym = state + dt * state_dot
+
+    state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
+    state_err = sp.Matrix(state_err_sym)
+    quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
+    v_err = state_err[States.ECEF_VELOCITY_ERR, :]
+    omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
+    acceleration_err = state_err[States.ACCELERATION_ERR, :]
+
+    # Time derivative of the state error as a function of state error and state
+    quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
+    q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
+    state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
+    state_err_dot[States.ECEF_POS_ERR, :] = v_err
+    state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
+    state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
+    f_err_sym = state_err + dt * state_err_dot
+
+    # Observation matrix modifier
+    H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
+    H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start)
+    H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
+    H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop)
+
+    # these error functions are defined so that say there
+    # is a nominal x and true x:
+    # true x = err_function(nominal x, delta x)
+    # delta x = inv_err_function(nominal x, true x)
+    nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
+    true_x = sp.MatrixSymbol('true_x', dim_state, 1)
+    delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
+
+    err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
+    delta_quat = sp.Matrix(np.ones(4))
+    delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :])
+    err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
+    err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
+    err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
+
+    inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
+    inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0])
+    delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
+    inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
+    inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0])
+
+    eskf_params = [[err_function_sym, nom_x, delta_x],
+                   [inv_err_function_sym, nom_x, true_x],
+                   H_mod_sym, f_err_sym, state_err_sym]
+    #
+    # Observation functions
+    #
+    h_gyro_sym = sp.Matrix([
+      vroll + roll_bias,
+      vpitch + pitch_bias,
+      vyaw + yaw_bias])
+
+    pos = sp.Matrix([x, y, z])
+    gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
+    h_acc_sym = (gravity + acceleration + acc_bias)
+    h_acc_stationary_sym = acceleration
+    h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
+    h_pos_sym = sp.Matrix([x, y, z])
+    h_vel_sym = sp.Matrix([vx, vy, vz])
+    h_orientation_sym = q
+    h_relative_motion = sp.Matrix(quat_rot.T * v)
+
+    obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
+               [h_phone_rot_sym, ObservationKind.NO_ROT, None],
+               [h_acc_sym, ObservationKind.PHONE_ACCEL, None],
+               [h_pos_sym, ObservationKind.ECEF_POS, None],
+               [h_vel_sym, ObservationKind.ECEF_VEL, None],
+               [h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None],
+               [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
+               [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
+               [h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
+
+    # this returns a sympy routine for the jacobian of the observation function of the local vel
+    in_vec = sp.MatrixSymbol('in_vec', 6, 1)  # roll, pitch, yaw, vx, vy, vz
+    h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]]))
+    extra_routines = [('H', h.jacobian(in_vec), [in_vec])]
+
+    gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines)
+
+    # write constants to extra header file for use in cpp
+    live_kf_header = "#pragma once\n\n"
+    live_kf_header += "#include <unordered_map>\n"
+    live_kf_header += "#include <eigen3/Eigen/Dense>\n\n"
+    for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)):
+      assert(slc.step is None)  # unsupported
+      live_kf_header += f'#define STATE_{state}_START {slc.start}\n'
+      live_kf_header += f'#define STATE_{state}_END {slc.stop}\n'
+      live_kf_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n'
+    live_kf_header += "\n"
+
+    for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)):
+      live_kf_header += f'#define OBSERVATION_{kind} {val}\n'
+    live_kf_header += "\n"
+
+    live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n"
+    live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n"
+    live_kf_header += f"static const Eigen::VectorXd live_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n"
+    live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n"
+    live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_diag)};\n"
+    live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n"
+    live_kf_header += "static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> live_obs_noise_diag = {\n"
+    for kind, noise in LiveKalman.obs_noise_diag.items():
+      live_kf_header += f"  {{ {kind}, {numpy2eigenstring(noise)} }},\n"
+    live_kf_header += "};\n\n"
+
+    open(os.path.join(generated_dir, "live_kf_constants.h"), 'w').write(live_kf_header)
+
+
+if __name__ == "__main__":
+  generated_dir = sys.argv[2]
+  LiveKalman.generate_code(generated_dir)

sunnypilot/selfdrive/locationd/tests/.gitignore

@@ -0,0 +1 @@
+out/

sunnypilot/selfdrive/locationd/tests/test_locationd.py

@@ -0,0 +1,89 @@
+import pytest
+import json
+import random
+import time
+import capnp
+
+import cereal.messaging as messaging
+from cereal.services import SERVICE_LIST
+from openpilot.common.params import Params
+from openpilot.common.transformations.coordinates import ecef2geodetic
+
+from openpilot.system.manager.process_config import managed_processes
+
+
+class TestLocationdProc:
+  LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration',
+              'accelerometer', 'gyroscope', 'magnetometer']
+
+  def setup_method(self):
+    self.pm = messaging.PubMaster(self.LLD_MSGS)
+
+    self.params = Params()
+    self.params.put_bool("UbloxAvailable", True)
+    managed_processes['locationd_llk'].prepare()
+    managed_processes['locationd_llk'].start()
+
+  def teardown_method(self):
+    managed_processes['locationd_llk'].stop()
+
+  def get_msg(self, name, t):
+    try:
+      msg = messaging.new_message(name)
+    except capnp.lib.capnp.KjException:
+      msg = messaging.new_message(name, 0)
+
+    if name == "gpsLocationExternal":
+      msg.gpsLocationExternal.flags = 1
+      msg.gpsLocationExternal.hasFix = True
+      msg.gpsLocationExternal.verticalAccuracy = 1.0
+      msg.gpsLocationExternal.speedAccuracy = 1.0
+      msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
+      msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
+      msg.gpsLocationExternal.latitude = float(self.lat)
+      msg.gpsLocationExternal.longitude = float(self.lon)
+      msg.gpsLocationExternal.unixTimestampMillis = t * 1e6
+      msg.gpsLocationExternal.altitude = float(self.alt)
+    #if name == "gnssMeasurements":
+    #  msg.gnssMeasurements.measTime = t
+    #  msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
+    #  msg.gnssMeasurements.positionECEF.std = [0,0,0]
+    #  msg.gnssMeasurements.positionECEF.valid = True
+    #  msg.gnssMeasurements.velocityECEF.value = []
+    #  msg.gnssMeasurements.velocityECEF.std = [0,0,0]
+    #  msg.gnssMeasurements.velocityECEF.valid = True
+    elif name == 'cameraOdometry':
+      msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
+      msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
+      msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
+      msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
+    msg.logMonoTime = t
+    msg.valid = True
+    return msg
+
+  def test_params_gps(self):
+    random.seed(123489234)
+    self.params.remove('LastGPSPositionLLK')
+
+    self.x = -2710700 + (random.random() * 1e5)
+    self.y = -4280600 + (random.random() * 1e5)
+    self.z = 3850300 + (random.random() * 1e5)
+    self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z])
+
+    # get fake messages at the correct frequency, listed in services.py
+    msgs = []
+    for sec in range(65):
+      for name in self.LLD_MSGS:
+        for j in range(int(SERVICE_LIST[name].frequency)):
+          msgs.append(self.get_msg(name, int((sec + j / SERVICE_LIST[name].frequency) * 1e9)))
+
+    for msg in sorted(msgs, key=lambda x: x.logMonoTime):
+      self.pm.send(msg.which(), msg)
+      if msg.which() == "cameraOdometry":
+        self.pm.wait_for_readers_to_update(msg.which(), 0.1, dt=0.005)
+    time.sleep(1)  # wait for async params write
+
+    lastGPS = json.loads(self.params.get('LastGPSPositionLLK'))
+    assert lastGPS['latitude'] == pytest.approx(self.lat, abs=0.001)
+    assert lastGPS['longitude'] == pytest.approx(self.lon, abs=0.001)
+    assert lastGPS['altitude'] == pytest.approx(self.alt, abs=0.001)

sunnypilot/selfdrive/locationd/torqued_ext.py

@@ -0,0 +1,65 @@
+"""
+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 car
+
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
+
+RELAXED_MIN_BUCKET_POINTS = np.array([0, 200, 300, 500, 500, 300, 200, 0])
+
+ALLOWED_CARS = ['toyota', 'hyundai', 'rivian', 'honda']
+
+
+
+class TorqueEstimatorExt:
+  def __init__(self, CP: car.CarParams):
+    self.CP = CP
+    self._params = Params()
+    self.frame = -1
+
+    self.enforce_torque_control_toggle = self._params.get_bool("EnforceTorqueControl")  # only during init
+    self.use_params = self.CP.brand in ALLOWED_CARS and self.CP.lateralTuning.which() == 'torque'
+    self.use_live_torque_params = self._params.get_bool("LiveTorqueParamsToggle")
+    self.torque_override_enabled = self._params.get_bool("TorqueParamsOverrideEnabled")
+    self.min_bucket_points = RELAXED_MIN_BUCKET_POINTS
+    self.factor_sanity = 0.0
+    self.friction_sanity = 0.0
+    self.offline_latAccelFactor = 0.0
+    self.offline_friction = 0.0
+
+  def initialize_custom_params(self, decimated=False):
+    self.update_use_params()
+
+    if self.enforce_torque_control_toggle:
+      if self._params.get_bool("LiveTorqueParamsRelaxedToggle"):
+        self.min_bucket_points = RELAXED_MIN_BUCKET_POINTS / (10 if decimated else 1)
+        self.factor_sanity = 0.5 if decimated else 1.0
+        self.friction_sanity = 0.8 if decimated else 1.0
+
+      if self._params.get_bool("CustomTorqueParams"):
+        self.offline_latAccelFactor = float(self._params.get("TorqueParamsOverrideLatAccelFactor", return_default=True))
+        self.offline_friction = float(self._params.get("TorqueParamsOverrideFriction", return_default=True))
+
+  def _update_params(self):
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.use_live_torque_params = self._params.get_bool("LiveTorqueParamsToggle")
+      self.torque_override_enabled = self._params.get_bool("TorqueParamsOverrideEnabled")
+
+  def update_use_params(self):
+    self._update_params()
+
+    if self.enforce_torque_control_toggle:
+      if self.torque_override_enabled:
+        self.use_params = False
+      else:
+        self.use_params = self.use_live_torque_params
+
+    self.frame += 1

sunnypilot/selfdrive/selfdrived/events.py

@@ -1,6 +1,10 @@
+import cereal.messaging as messaging
 from cereal import log, car, custom
+from openpilot.common.constants import CV
 from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \
   NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert
+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.helpers import compare_cluster_target
 
 
 AlertSize = log.SelfdriveState.AlertSize
@@ -14,6 +18,49 @@ EventNameSP = custom.OnroadEventSP.EventName
 EVENT_NAME_SP = {v: k for k, v in EventNameSP.schema.enumerants.items()}
 
 
+def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  speedLimit = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimit
+  speed = round(speedLimit * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))
+  message = f'Adjusting to {speed} {"km/h" if metric else "mph"} speed limit'
+  return Alert(
+    message,
+    "",
+    AlertStatus.normal, AlertSize.small,
+    Priority.LOW, VisualAlert.none, AudibleAlert.none, 4.)
+
+
+def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH
+  speed_limit_final_last = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimitFinalLast
+  speed_limit_final_last_conv = round(speed_limit_final_last * speed_conv)
+
+  if CP.openpilotLongitudinalControl and CP.pcmCruise:
+    # PCM long
+    cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[metric]
+    pcm_long_required_max = cst_low if speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[metric] else cst_high
+    pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv)
+    speed_unit = "km/h" if metric else "mph"
+    alert_2_str = f"Manually change set speed to {pcm_long_required_max_set_speed_conv} {speed_unit} to activate"
+  else:
+    # Non PCM long
+    v_cruise_cluster = CS.vCruiseCluster * CV.KPH_TO_MS
+
+    req_plus, req_minus = compare_cluster_target(v_cruise_cluster, speed_limit_final_last, metric)
+    arrow_str = ""
+    if req_plus:
+      arrow_str = "RES/+"
+    elif req_minus:
+      arrow_str = "SET/-"
+
+    alert_2_str = f"Operate the {arrow_str} cruise control button to activate"
+
+  return Alert(
+    "Speed Limit Assist: Activation Required",
+    alert_2_str,
+    AlertStatus.normal, AlertSize.mid,
+    Priority.LOW, VisualAlert.none, AudibleAlert.none, .1)
+
+
 class EventsSP(EventsBase):
   def __init__(self):
     super().__init__()
@@ -148,5 +195,41 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
       "",
       AlertStatus.normal, AlertSize.small,
       Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
-  }
+  },
+
+  EventNameSP.speedLimitActive: {
+    ET.WARNING: Alert(
+      "Automatically adjusting to the posted speed limit",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.),
+  },
+
+  EventNameSP.speedLimitChanged: {
+    ET.WARNING: Alert(
+      "Set speed changed",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.),
+  },
+
+  EventNameSP.speedLimitPreActive: {
+    ET.WARNING: speed_limit_pre_active_alert,
+  },
+
+  EventNameSP.speedLimitPending: {
+    ET.WARNING: Alert(
+      "Automatically adjusting to the last speed limit",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.),
+  },
+
+  EventNameSP.e2eChime: {
+    ET.PERMANENT: Alert(
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.MID, VisualAlert.none, AudibleAlert.prompt, 0.1),
+  },
 }

sunnypilot/system/sensord/.gitignore

@@ -0,0 +1 @@
+sensord